Defines an articulated body for use with reduced-coordinate dynamics algorithms. More...

#include <RCArticulatedBody.h>

Inheritance diagram for RC_ARTICULATED_BODY:

Public Types
enum	ForwardDynamicsAlgorithmType { eFeatherstone, eCRB }

Public Types inherited from DYNAMIC_BODY
enum	GeneralizedCoordinateType { eEuler, eSpatial }

Public Member Functions
	RC_ARTICULATED_BODY ()
	Default constructor. More...

virtual void	reset_accumulators ()
	Resets the force and torque accumulators for all links and joints in the rigid body.

virtual void	update_link_poses ()
	Updates the transforms of the links based on the current joint positions. More...

virtual void	update_link_velocities ()
	Updates the link velocities.

virtual void	apply_impulse (const SMOMENTUM &w, boost::shared_ptr< RIGIDBODY > link)
	Abstract method for applying an impulse to this articulated body. More...

virtual void	calc_fwd_dyn ()
	Computes the forward dynamics. More...

boost::shared_ptr < RC_ARTICULATED_BODY >	get_this ()

boost::shared_ptr< const RC_ARTICULATED_BODY >	get_this () const

virtual void	set_generalized_forces (const SHAREDVECTORN &gf)
	Sets the generalized forces for the articulated body.

virtual void	set_generalized_forces (const VECTORN &gf)
	Sets the generalized forces on the body.

virtual void	add_generalized_force (const SHAREDVECTORN &gf)
	Adds a generalized force to the articulated body.

virtual void	add_generalized_force (const VECTORN &gf)
	Adds a generalized force to the body.

virtual void	apply_generalized_impulse (const SHAREDVECTORN &gj)
	Applies a generalized impulse to the articulated body.

virtual void	apply_generalized_impulse (const VECTORN &gj)
	Applies a generalized impulse to the body.

virtual void	set_generalized_coordinates_euler (const SHAREDVECTORN &gc)
	Sets the generalized position of this body.

virtual void	set_generalized_coordinates_euler (const VECTORN &gc)
	Sets the generalized coordinates of this body (using Euler parameters for any rigid body orientations, if relevant)

virtual void	set_generalized_velocity (DYNAMIC_BODY::GeneralizedCoordinateType gctype, const SHAREDVECTORN &gv)
	Sets the generalized velocity of this body.

virtual void	set_generalized_velocity (DYNAMIC_BODY::GeneralizedCoordinateType gctype, const VECTORN &gv)
	Sets the generalized velocity of this body.

virtual SHAREDMATRIXN &	get_generalized_inertia (SHAREDMATRIXN &M)
	Gets the generalized inertia of this body.

virtual MATRIXN &	get_generalized_inertia (MATRIXN &M)

virtual SHAREDVECTORN &	get_generalized_forces (SHAREDVECTORN &f)
	Gets the generalized forces on this body. More...

virtual VECTORN &	get_generalized_forces (VECTORN &f)
	Gets the external forces on this body. More...

virtual SHAREDVECTORN &	convert_to_generalized_force (boost::shared_ptr< SINGLE_BODY > body, const SFORCE &w, SHAREDVECTORN &gf)
	Converts a force to a generalized force. More...

virtual VECTORN &	convert_to_generalized_force (boost::shared_ptr< SINGLE_BODY > body, const SFORCE &w, VECTORN &gf)
	Converts a force to a generalized force. More...

virtual unsigned	num_generalized_coordinates (DYNAMIC_BODY::GeneralizedCoordinateType gctype) const
	Gets the number of generalized coordinates for this body.

virtual void	set_links_and_joints (const std::vector< boost::shared_ptr< RIGIDBODY > > &links, const std::vector< boost::shared_ptr< JOINT > > &joints)

virtual unsigned	num_joint_dof_implicit () const
	Gets the number of degrees of freedom for implicit constraints.

virtual unsigned	num_joint_dof_explicit () const
	Gets the number of degrees-of-freedom permitted by explicit constraints.

void	set_floating_base (bool flag)
	Sets whether the base of this body is "floating" (or fixed)

virtual void	set_computation_frame_type (ReferenceFrameType rftype)
	Sets the computation frame type.

virtual VECTORN &	solve_generalized_inertia (const VECTORN &b, VECTORN &x)
	Solves using the inverse generalized inertia.

virtual SHAREDMATRIXN &	transpose_solve_generalized_inertia (const SHAREDMATRIXN &B, SHAREDMATRIXN &X)
	Solves the transpose using a generalized inertia matrix.

virtual SHAREDVECTORN &	solve_generalized_inertia (const SHAREDVECTORN &v, SHAREDVECTORN &result)
	Solves using a generalized inertia matrix.

virtual SHAREDMATRIXN &	solve_generalized_inertia (const SHAREDMATRIXN &m, SHAREDMATRIXN &result)
	Solves using a generalized inertia matrix.

virtual boost::shared_ptr < const POSE3 >	get_gc_pose () const
	Gets the frame used for generalized coordinate calculations.

virtual void	validate_position_variables ()
	Validates position variables.

virtual SHAREDVECTORN &	get_generalized_coordinates_euler (SHAREDVECTORN &gc)
	Gets the generalized coordinates of this body.

virtual VECTORN &	get_generalized_coordinates_euler (VECTORN &gc)
	Gets the generalized coordinates of this body (using Euler parameters for any rigid body orientations, if relevant)

virtual SHAREDVECTORN &	get_generalized_velocity (DYNAMIC_BODY::GeneralizedCoordinateType gctype, SHAREDVECTORN &gv)
	Gets the generalized velocity of this body.

virtual VECTORN &	get_generalized_velocity (DYNAMIC_BODY::GeneralizedCoordinateType gctype, VECTORN &gv)
	Gets the generalized velocity of this body.

virtual SHAREDVECTORN &	get_generalized_acceleration (SHAREDVECTORN &ga)
	Gets the derivative of the velocity state vector for this articulated body. More...

void	set_generalized_acceleration (const SHAREDVECTORN &a)
	Sets the generalized acceleration for this body.

virtual VECTORN &	get_generalized_acceleration (VECTORN &ga)
	Gets the generalized acceleration of this body.

template<class V >
void	get_generalized_acceleration_generic (V &ga)
	Gets the derivative of the velocity state vector for this articulated body. More...

template<class V >
void	get_generalized_coordinates_euler_generic (V &gc)
	Gets the generalized coordinates of this body.

template<class V >
void	set_generalized_coordinates_euler_generic (V &gc)
	Sets the generalized position of this body.

template<class V >
void	set_generalized_velocity_generic (DYNAMIC_BODY::GeneralizedCoordinateType gctype, V &gv)
	Sets the generalized velocity of this body.

template<class V >
void	get_generalized_velocity_generic (DYNAMIC_BODY::GeneralizedCoordinateType gctype, V &gv)
	Gets the generalized velocity of this body.

virtual bool	is_floating_base () const
	Gets whether the base of this body is fixed or "floating".

virtual unsigned	num_joint_dof () const
	Gets the number of DOF of the explicit joints in the body, not including floating base DOF.

virtual boost::shared_ptr < RIGIDBODY >	get_base_link () const
	Gets the base link.

virtual const std::vector < boost::shared_ptr< JOINT > > &	get_explicit_joints () const
	Gets the vector of explicit joint constraints.

virtual const std::vector < boost::shared_ptr< JOINT > > &	get_implicit_joints () const
	Gets the vector of explicit joint constraints.

Public Member Functions inherited from ARTICULATED_BODY
unsigned	num_constraint_eqns_explicit () const
	Gets the number of explicit joint constraint equations.

unsigned	num_constraint_eqns_implicit () const
	Gets the number of implicit joint constraint equations.

virtual void	rotate (const QUAT &q)
	Transforms all links in the articulated body by the given transform. More...

virtual void	translate (const ORIGIN3 &o)
	Transforms all links in the articulated body by the given transform. More...

virtual REAL	calc_kinetic_energy (boost::shared_ptr< const POSE3 > P=boost::shared_ptr< const POSE3 >())
	Calculates the combined kinetic energy of all links in this body with respect to the base frame.

boost::shared_ptr< RIGIDBODY >	find_link (const std::string &id) const

boost::shared_ptr< JOINT >	find_joint (const std::string &id) const

void	get_adjacent_links (std::list< sorted_pair< boost::shared_ptr< RIGIDBODY > > > &links) const

void	find_loops (std::vector< unsigned > &loop_indices, std::vector< std::vector< unsigned > > &loop_links) const

virtual MATRIXN &	calc_jacobian (boost::shared_ptr< const POSE3 > source_pose, boost::shared_ptr< const POSE3 > target_pose, boost::shared_ptr< DYNAMIC_BODY > body, MATRIXN &J)
	The Jacobian transforms from the generalized coordinate from to the given frame.

virtual MATRIXN &	calc_jacobian_dot (boost::shared_ptr< const POSE3 > source_pose, boost::shared_ptr< const POSE3 > target_pose, boost::shared_ptr< DYNAMIC_BODY > body, MATRIXN &J)

virtual MATRIXN &	calc_jacobian (boost::shared_ptr< const POSE3 > target_pose, boost::shared_ptr< DYNAMIC_BODY > body, MATRIXN &J)

virtual MATRIXN &	calc_jacobian_dot (boost::shared_ptr< const POSE3 > target_pose, boost::shared_ptr< DYNAMIC_BODY > body, MATRIXN &J)

virtual bool	is_enabled () const
	Articulated bodies are always enabled.

virtual const std::vector < boost::shared_ptr< RIGIDBODY > > &	get_links () const
	Gets the set of links.

virtual const std::vector < boost::shared_ptr< JOINT > > &	get_joints () const
	Gets the set of joints.

boost::shared_ptr < ARTICULATED_BODY >	get_this ()
	Gets shared pointer to this object as type ARTICULATED_BODY.

boost::shared_ptr< const ARTICULATED_BODY >	get_this () const
	Gets shared pointer to this object as type const ArticulateBody.

Public Member Functions inherited from DYNAMIC_BODY
virtual void	validate_velocity_variables ()
	Validates velocity-based variables (potentially dangerous for a user to call)

ReferenceFrameType	get_computation_frame_type () const
	Gets the computation frame type for this body.

virtual void	set_generalized_acceleration (const VECTORN &ga)
	Sets the generalized acceleration of this body.

MATRIXN &	get_generalized_inertia (MATRIXN &M)
	Gets the generalized inertia of this body.

virtual MATRIXN &	solve_generalized_inertia (const MATRIXN &B, MATRIXN &X)
	Solves using the inverse generalized inertia.

virtual MATRIXN &	solve_generalized_inertia (const SHAREDMATRIXN &B, MATRIXN &X)
	Solves using the inverse generalized inertia.

virtual SHAREDMATRIXN &	solve_generalized_inertia (const MATRIXN &B, SHAREDMATRIXN &X)
	Solves using the inverse generalized inertia.

virtual VECTORN &	solve_generalized_inertia (const SHAREDVECTORN &b, VECTORN &x)
	Solves using the inverse generalized inertia.

virtual SHAREDVECTORN &	solve_generalized_inertia (const VECTORN &b, SHAREDVECTORN &x)
	Solves using the inverse generalized inertia.

virtual MATRIXN &	transpose_solve_generalized_inertia (const MATRIXN &B, MATRIXN &X)
	Solves the transpose matrix using the inverse generalized inertia.

virtual MATRIXN &	transpose_solve_generalized_inertia (const SHAREDMATRIXN &B, MATRIXN &X)
	Solves the transpose matrix using the inverse generalized inertia.

virtual SHAREDMATRIXN &	transpose_solve_generalized_inertia (const MATRIXN &B, SHAREDMATRIXN &X)
	Solves the transpose matrix using the inverse generalized inertia.

Public Member Functions inherited from Ravelin::virtual_enable_shared_from_this< DYNAMIC_BODY >
boost::shared_ptr< DYNAMIC_BODY >	shared_from_this ()

boost::shared_ptr< const DYNAMIC_BODY >	shared_from_this () const

Public Attributes
ForwardDynamicsAlgorithmType	algorithm_type
	The forward dynamics algorithm.

Public Attributes inherited from DYNAMIC_BODY
std::string	body_id
	The identifier for this body.

Protected Member Functions
virtual void	compile ()
	Compiles this body (updates the link transforms and velocities)

Protected Attributes
bool	_floating_base
	Whether this body uses a floating base.

unsigned	_n_joint_DOF_explicit
	The number of DOF of the explicit joint constraints in the body (does not include floating base DOF!)

std::vector< boost::shared_ptr < JOINT > >	_ejoints
	The vector of explicit joint constraints.

std::vector< boost::shared_ptr < JOINT > >	_ijoints
	The vector of implicit joint constraints.

bool	_position_invalidated
	Indicates when position data has been invalidated.

CRB_ALGORITHM	_crb
	The CRB algorithm.

FSAB_ALGORITHM	_fsab
	The FSAB algorithm.

boost::shared_ptr< LINALG >	_LA
	Linear algebra object.

Protected Attributes inherited from ARTICULATED_BODY
std::vector< unsigned >	_processed
	Vector for processing links.

std::vector< boost::shared_ptr < RIGIDBODY > >	_links
	The set of links for this articulated body.

std::vector< boost::shared_ptr < JOINT > >	_joints
	The set of joints for this articulated body.

Protected Attributes inherited from DYNAMIC_BODY
ReferenceFrameType	_rftype
	The computation frame type.

VECTORN	gc
	Temporaries for use with integration.

VECTORN	gv

VECTORN	gcgv

VECTORN	xp

VECTORN	xv

VECTORN	xa

Friends
class	CRB_ALGORITHM

class	FSAB_ALGORITHM

Detailed Description

Defines an articulated body for use with reduced-coordinate dynamics algorithms.

Reduced-coordinate articulated bodies cannot rely upon the integrator to automatically update the states (i.e., positions, velocities) of the links, as is done with maximal-coordinate articulated bodies. Rather, the integrator updates the joint positions and velocities; the states are obtained from this reduced-coordinate representation. Notes about concurrency:

It is generally desirable to be able to run forward dynamics and inverse dynamics algorithms concurrently to simulate actual robotic systems. In general, derived classes should not operate on state variables (joint positions, velocities, accelerations and floating base positions, velocites, and accelerations) directly during execution of the algorithm. Rather, derived classes should operate on copies of the state variables, updating the state variables on conclusion of the algorithms.

Constructor & Destructor Documentation

RC_ARTICULATED_BODY::RC_ARTICULATED_BODY ( )

Default constructor.

Constructs a reduced-coordinate articulated body with no joints and no links.

References _crb, _floating_base, _fsab, _LA, _n_joint_DOF_explicit, _position_invalidated, algorithm_type, and set_computation_frame_type().

Member Function Documentation

virtual void RC_ARTICULATED_BODY::apply_impulse	(	const SMOMENTUM &	w,
		boost::shared_ptr< RIGIDBODY >	link
	)

virtual

Abstract method for applying an impulse to this articulated body.

Parameters

w	the impulsive force
link	link in the articulated body where the impulse is applied

Implements ARTICULATED_BODY.

void RC_ARTICULATED_BODY::calc_fwd_dyn ( )

virtual

Computes the forward dynamics.

Given the joint positions and velocities, joint forces, and external forces on the links, determines the joint and link accelerations as well as floating base accelerations (if applicable). The joint accelerations are stored in the individual joints and the link accelerations are stored in the individual links.

Note: only computes the forward dynamics if the state-derivative is no longer valid

Implements DYNAMIC_BODY.

References _crb, _fsab, _position_invalidated, algorithm_type, CRB_ALGORITHM::calc_fwd_dyn(), FSAB_ALGORITHM::calc_fwd_dyn(), and DYNAMIC_BODY::get_computation_frame_type().

virtual SHAREDVECTORN& RC_ARTICULATED_BODY::convert_to_generalized_force	(	boost::shared_ptr< SINGLE_BODY >	body,
		const SFORCE &	w,
		SHAREDVECTORN &	gf
	)

virtual

Converts a force to a generalized force.

Parameters

body	the actual rigid body to which the force/torque is applied (at the center-of-mass)
w	the force
gf	the generalized force, on return

Note: uses the current generalized coordinates

Implements DYNAMIC_BODY.

virtual VECTORN& RC_ARTICULATED_BODY::convert_to_generalized_force	(	boost::shared_ptr< SINGLE_BODY >	body,
		const SFORCE &	w,
		VECTORN &	gf
	)

inlinevirtual

Converts a force to a generalized force.

Parameters

body	the actual rigid body to which the force/torque is applied (at the center-of-mass)
w	the force
gf	the generalized force, on return

Note: uses the current generalized coordinates

Reimplemented from DYNAMIC_BODY.

References DYNAMIC_BODY::convert_to_generalized_force().

SHAREDVECTORN & RC_ARTICULATED_BODY::get_generalized_acceleration ( SHAREDVECTORN & ga )

virtual

Gets the derivative of the velocity state vector for this articulated body.

The state vector consists of the joint-space velocities of the robot as well as the base momentum; therefore, the derivative of the state vector is composed of the joint-space accelerations and base forces (and torques).Gets the derivative of the velocity state vector for this articulated body The state vector consists of the joint-space velocities of the robot as well as the base momentum; therefore, the derivative of the state vector is composed of the joint-space accelerations and base forces (and torques).

Implements DYNAMIC_BODY.

References get_generalized_acceleration_generic().

template<class V >

void RC_ARTICULATED_BODY::get_generalized_acceleration_generic ( V & ga )

Gets the derivative of the velocity state vector for this articulated body.

The state vector consists of the joint-space velocities of the robot as well as the base momentum; therefore, the derivative of the state vector is composed of the joint-space accelerations and base forces (and torques).

References _ejoints, _floating_base, ARTICULATED_BODY::_links, num_generalized_coordinates(), num_joint_dof_explicit(), and SHAREDVECTORN::segment().

Referenced by get_generalized_acceleration().

SHAREDVECTORN & RC_ARTICULATED_BODY::get_generalized_forces ( SHAREDVECTORN & f )

virtual

Gets the generalized forces on this body.

Note: does not add forces from implicit joint constraints!

Implements DYNAMIC_BODY.

References _crb, _ejoints, _floating_base, ARTICULATED_BODY::_links, and CRB_ALGORITHM::calc_generalized_forces().

virtual VECTORN& RC_ARTICULATED_BODY::get_generalized_forces ( VECTORN & f )

inlinevirtual

Gets the external forces on this body.

Note: uses the current generalized coordinates

Reimplemented from DYNAMIC_BODY.

References DYNAMIC_BODY::get_generalized_forces().

void RC_ARTICULATED_BODY::update_link_poses ( )

virtual

Updates the transforms of the links based on the current joint positions.

Note: this doesn't actually calculate other than the joint positions; all links are defined with respect to the joints, which are defined with respect to their inner link

References ARTICULATED_BODY::_joints, ARTICULATED_BODY::_links, _position_invalidated, TRANSFORM3::q, and TRANSFORM3::x.

Referenced by compile(), and set_generalized_coordinates_euler_generic().

The documentation for this class was generated from the following files:

/home/drum/Ravelin.new/include/Ravelin/RCArticulatedBody.h
/home/drum/Ravelin.new/include/Ravelin/RCArticulatedBody.inl
/home/drum/Ravelin.new/src/RCArticulatedBody.cpp

Public Types

Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Friends

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation