#include <mbd_reach_cone.h>

Inheritance diagram for OpenTissue::mbd::ReachCone< mbd_types >:

Classes
struct	cone_constraint_type
Public Types
typedef mbd_types::math_policy	math_policy
typedef mbd_types::math_policy::real_type	real_type
typedef mbd_types::math_policy::size_type	size_type
typedef mbd_types::math_policy::value_traits	value_traits
typedef mbd_types::math_policy::vector3_type	vector3_type
typedef mbd_types::math_policy::vector_range	vector_range
typedef mbd_types::math_policy::idx_vector_range	idx_vector_range
typedef mbd_types::math_policy::matrix_range	matrix_range
typedef mbd_types::math_policy::vector_type	vector_type
typedef mbd_types::math_policy::quaternion_type	quaternion_type
Public Member Functions
	ReachCone ()
virtual	~ReachCone ()
void	add_cone (vector3_type const &n_local)
void	evaluate (quaternion_type const &Q, vector3_type const &s_wcs, vector3_type const &s_local)
size_type	get_number_of_jacobian_rows () const
void	get_linear_jacobian_A (matrix_range &J, size_type const &offset) const
void	get_linear_jacobian_B (matrix_range &J, size_type const &offset) const
void	get_angular_jacobian_A (matrix_range &J, size_type const &offset) const
void	get_angular_jacobian_B (matrix_range &J, size_type const &offset) const
void	get_stabilization_term (vector_range &b_error, size_type const &offset) const
void	get_low_limits (vector_range &lo, size_type const &offset) const
void	get_high_limits (vector_range &hi, size_type const &offset) const
void	get_dependency_indices (idx_vector_range &dep, size_type const &offset) const
void	get_dependency_factors (vector_range &factors, size_type const &offset) const
void	set_regularization (vector_range const &gamma, size_type const &offset)
void	get_regularization (vector_range &gamma, size_type const &offset) const
void	set_solution (vector_range const &solution, size_type const &offset)
void	get_solution (vector_range &solution, size_type const &offset) const
Protected Types
typedef std::vector < cone_constraint_type >	constraint_container
typedef std::vector< vector3_type >	normal_container
Protected Attributes
constraint_container	m_contraints
normal_container	m_cone_normals
size_type	m_cone_sides
	The number of cone sides.
size_type	m_rows
	Number of jacobian rows.
vector_type	m_gamma
	Local vector of constraint force mixing terms.
vector_type	m_solution
	Local solution vector, ie. vector of lagrange multipliers.

Detailed Description

template<typename mbd_types>
class OpenTissue::mbd::ReachCone< mbd_types >

A Reach Cone Limit.

The reach cone modelling idea was inspired from the work

{wilhelms.van_gelder.01, author = {Jane Wilhelms and Allen Van Gelder}, title = {Fast and easy reach-cone joint limits}, journal = {J. Graph. Tools}, volume = {6}, number = {2}, year = {2001}, issn = {1086-7651}, pages = {27--41}, publisher = {A. K. Peters, Ltd.}, address = {Natick, MA, USA}, }

See http://jgt.akpeters.com/papers/WilhelmsVanGelder01/ also have a look at

{WilhelmsVanGelder01, author = "Jane Wilhelms and Allen Van Gelder", title = "Fast and Easy Reach-Cone joint_type Limits", journal = "journal of graphics tools", volume = "6", number = "2", pages = "27-41", year = "2001", }

Our modelling method is different, but the ideas are similar. In this implementation a reach cone is simply a collection of angular joint limits.

Initially the joint limits are specified by a face-plane normal of each side of the cone. This is simply another way of specifying an angular limit on a ``rotational'' joint type. When the method ReachCone::evaluate is invoked active cone constraints are converted into the typical representation of an angular joint limit (rotation axis and error angle).

During simulation the cone sides are evaluated (see ReachCone::evaluate) to see whether they are active or not. All active cone side constraints are kept in a local member of the class (see ReachCone::m_contraints) and used to create the rows of the constraint jacobian matrix (see ReachCone::get_angular_jacobian_X) and error terms (see ReachCone::get_stabilization_term).

IMPORTANT: Remember that the joint axis of a joint wrt. a given body is specified by the third axis of the joint frame (see JointSocket::get_joint_axis_local) attached to the given body.

A joint in its zero (rest) position has the joint frames of body A and B in complete alignment. Thus in the world frame the z-axis of the joint frame on body B is aligned with the z-axis of the joint frame on body A.

Thus when specifying cone planes, these are described such that the joint axis wrt. body A (i.e. z-axis of joint frame on body A) is inside the cone. During simulation it is tested if the current joint axis wrt. body B has tipped outside the cone which is kept fixed wrt. to the joint frame on body A.

Member Typedef Documentation

template<typename mbd_types >

typedef std::vector<cone_constraint_type> OpenTissue::mbd::ReachCone< mbd_types >::constraint_container [protected]

template<typename mbd_types >

typedef mbd_types::math_policy::idx_vector_range OpenTissue::mbd::ReachCone< mbd_types >::idx_vector_range

Reimplemented from OpenTissue::mbd::SubConstraintInterface< mbd_types >.

template<typename mbd_types >

typedef mbd_types::math_policy OpenTissue::mbd::ReachCone< mbd_types >::math_policy

template<typename mbd_types >

typedef mbd_types::math_policy::matrix_range OpenTissue::mbd::ReachCone< mbd_types >::matrix_range

Reimplemented from OpenTissue::mbd::SubConstraintInterface< mbd_types >.

template<typename mbd_types >

typedef std::vector<vector3_type> OpenTissue::mbd::ReachCone< mbd_types >::normal_container [protected]

template<typename mbd_types >

typedef mbd_types::math_policy::quaternion_type OpenTissue::mbd::ReachCone< mbd_types >::quaternion_type

template<typename mbd_types >

typedef mbd_types::math_policy::real_type OpenTissue::mbd::ReachCone< mbd_types >::real_type

Reimplemented from OpenTissue::mbd::SubConstraintInterface< mbd_types >.

template<typename mbd_types >

typedef mbd_types::math_policy::size_type OpenTissue::mbd::ReachCone< mbd_types >::size_type

Reimplemented from OpenTissue::mbd::SubConstraintInterface< mbd_types >.

template<typename mbd_types >

typedef mbd_types::math_policy::value_traits OpenTissue::mbd::ReachCone< mbd_types >::value_traits

Reimplemented from OpenTissue::mbd::CoreConstraintInterface< mbd_types >.

template<typename mbd_types >

typedef mbd_types::math_policy::vector3_type OpenTissue::mbd::ReachCone< mbd_types >::vector3_type

template<typename mbd_types >

typedef mbd_types::math_policy::vector_range OpenTissue::mbd::ReachCone< mbd_types >::vector_range

Reimplemented from OpenTissue::mbd::SubConstraintInterface< mbd_types >.

template<typename mbd_types >

typedef mbd_types::math_policy::vector_type OpenTissue::mbd::ReachCone< mbd_types >::vector_type

Constructor & Destructor Documentation

template<typename mbd_types >

OpenTissue::mbd::ReachCone< mbd_types >::ReachCone ( ) [inline]

template<typename mbd_types >

virtual OpenTissue::mbd::ReachCone< mbd_types >::~ReachCone ( ) [inline, virtual]

Member Function Documentation

template<typename mbd_types >

void OpenTissue::mbd::ReachCone< mbd_types >::add_cone ( vector3_type const & n_local ) [inline]

Add Cone.

Parameters:

n_local

Inward cone face normal in local joint frame of body A.

template<typename mbd_types >

void OpenTissue::mbd::ReachCone< mbd_types >::evaluate	(	quaternion_type const &	Q,
		vector3_type const &	s_wcs,
		vector3_type const &	s_local
	)			`[inline]`

Evaluate Constraints.

This method loops over all the sides of the cone. For each side of the cone the method evaluates whether the joint orientation violates the corresponding cone plane. If this is the case an corresponding angular joint limit is created.

Parameters:

Q

Rotation from local joint frame of body A to WCS. That is something like

bodyA->get_orientation(Q_a); Q = Q_a % socketA->get_joint_frame().Q()

Parameters:

	s_wcs	joint_type axis of body A in wcs
	s_local	joint_type axis of body B in local joint frame of body A.

JF_A = socketA->get_joint_frame().Q() JF_B = socketB->get_joint_frame().Q() bodyA->get_orientation(BF_A); bodyB->get_orientation(BF_B); Q = conj(JF_A) conj(BF_A) BF_B JF_B s_local = Q.rotate(s_b);

template<typename mbd_types >

void OpenTissue::mbd::ReachCone< mbd_types >::get_angular_jacobian_A	(	matrix_range &	J,
		size_type const &	offset
	)			const `[inline, virtual]`

Implements OpenTissue::mbd::SubConstraintInterface< mbd_types >.

template<typename mbd_types >

void OpenTissue::mbd::ReachCone< mbd_types >::get_angular_jacobian_B	(	matrix_range &	J,
		size_type const &	offset
	)			const `[inline, virtual]`

Implements OpenTissue::mbd::SubConstraintInterface< mbd_types >.

template<typename mbd_types >

void OpenTissue::mbd::ReachCone< mbd_types >::get_dependency_factors	(	vector_range &	factors,
		size_type const &	offset
	)			const `[inline, virtual]`

Implements OpenTissue::mbd::SubConstraintInterface< mbd_types >.

template<typename mbd_types >

void OpenTissue::mbd::ReachCone< mbd_types >::get_dependency_indices	(	idx_vector_range &	dep,
		size_type const &	offset
	)			const `[inline, virtual]`

Implements OpenTissue::mbd::SubConstraintInterface< mbd_types >.

template<typename mbd_types >

void OpenTissue::mbd::ReachCone< mbd_types >::get_high_limits	(	vector_range &	hi,
		size_type const &	offset
	)			const `[inline, virtual]`

Implements OpenTissue::mbd::SubConstraintInterface< mbd_types >.

template<typename mbd_types >

void OpenTissue::mbd::ReachCone< mbd_types >::get_linear_jacobian_A	(	matrix_range &	J,
		size_type const &	offset
	)			const `[inline, virtual]`

Implements OpenTissue::mbd::SubConstraintInterface< mbd_types >.

template<typename mbd_types >

void OpenTissue::mbd::ReachCone< mbd_types >::get_linear_jacobian_B	(	matrix_range &	J,
		size_type const &	offset
	)			const `[inline, virtual]`

Implements OpenTissue::mbd::SubConstraintInterface< mbd_types >.

template<typename mbd_types >

void OpenTissue::mbd::ReachCone< mbd_types >::get_low_limits	(	vector_range &	lo,
		size_type const &	offset
	)			const `[inline, virtual]`

Implements OpenTissue::mbd::SubConstraintInterface< mbd_types >.

template<typename mbd_types >

size_type OpenTissue::mbd::ReachCone< mbd_types >::get_number_of_jacobian_rows ( ) const [inline, virtual]

Return Number of Jacobian Rows. The number of jacobian rows corresponds to the number of jacobian variables.

Returns:: Jacobian rows.

Implements OpenTissue::mbd::CoreConstraintInterface< mbd_types >.

template<typename mbd_types >

void OpenTissue::mbd::ReachCone< mbd_types >::get_regularization	(	vector_range &	gamma,
		size_type const &	offset
	)			const `[inline, virtual]`

Implements OpenTissue::mbd::SubConstraintInterface< mbd_types >.

template<typename mbd_types >

void OpenTissue::mbd::ReachCone< mbd_types >::get_solution	(	vector_range &	solution,
		size_type const &	offset
	)			const `[inline, virtual]`

Implements OpenTissue::mbd::SubConstraintInterface< mbd_types >.

template<typename mbd_types >

void OpenTissue::mbd::ReachCone< mbd_types >::get_stabilization_term	(	vector_range &	b_error,
		size_type const &	offset
	)			const `[inline, virtual]`

Implements OpenTissue::mbd::SubConstraintInterface< mbd_types >.

template<typename mbd_types >

void OpenTissue::mbd::ReachCone< mbd_types >::set_regularization	(	vector_range const &	gamma,
		size_type const &	offset
	)			`[inline, virtual]`

Implements OpenTissue::mbd::SubConstraintInterface< mbd_types >.

template<typename mbd_types >

void OpenTissue::mbd::ReachCone< mbd_types >::set_solution	(	vector_range const &	solution,
		size_type const &	offset
	)			`[inline, virtual]`

Implements OpenTissue::mbd::SubConstraintInterface< mbd_types >.

Member Data Documentation

template<typename mbd_types >

normal_container OpenTissue::mbd::ReachCone< mbd_types >::m_cone_normals [protected]

A side of the cone is described as the plane containing the joint position (the pivot point of the rotation) and an inward unit plane normal (normal points to the legal reach region). It is implictly assumed that the ``convex'' inside region of the planes correspond to the reach cone.

template<typename mbd_types >

size_type OpenTissue::mbd::ReachCone< mbd_types >::m_cone_sides [protected]

The number of cone sides.

template<typename mbd_types >

constraint_container OpenTissue::mbd::ReachCone< mbd_types >::m_contraints [protected]

Collection of active cone constraints. Each constraint is basically just an angular joint limit.

template<typename mbd_types >

vector_type OpenTissue::mbd::ReachCone< mbd_types >::m_gamma [protected]

Local vector of constraint force mixing terms.

template<typename mbd_types >

size_type OpenTissue::mbd::ReachCone< mbd_types >::m_rows [protected]

Number of jacobian rows.