Represents a single rigid body. More...

#include <RigidBody.h>

Inheritance diagram for RIGIDBODY:

Public Types
enum	Compliance { eRigid, eCompliant }

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

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

void	add_force (const SFORCE &w)
	Adds a force to the body.

void	set_pose (const POSE3 &pose)
	Sets the current 3D pose for this rigid body. More...

void	apply_impulse (const SMOMENTUM &w)
	Applies a impulse to this link. More...

virtual void	rotate (const QUAT &q)
	Rotates the rigid body.

virtual void	translate (const ORIGIN3 &o)
	Translates the rigid body.

virtual void	calc_fwd_dyn ()
	Computes the forward dynamics for this body.

const SPATIAL_RB_INERTIA &	get_inertia ()
	Gets the body inertia.

void	set_inertia (const SPATIAL_RB_INERTIA &J)
	Sets the rigid body inertia for this body.

bool	is_child_link (boost::shared_ptr< const RIGIDBODY > query) const

bool	is_descendant_link (boost::shared_ptr< const RIGIDBODY > query) const

const SVELOCITY &	get_velocity ()
	Gets the current body velocity.

void	set_velocity (const SVELOCITY &xd)
	Sets the velocity of this body.

void	set_accel (const SACCEL &xdd)
	Sets the acceleration of this body.

virtual const SACCEL &	get_accel ()
	Gets the current body acceleration.

void	set_velocity (const SACCEL &xdd)

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

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

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

virtual MATRIXN &	get_generalized_inertia (MATRIXN &M)

virtual SHAREDVECTORN &	get_generalized_coordinates_euler (SHAREDVECTORN &gc)
	Gets the generalized position of this rigid 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 rigid 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 generalized acceleration of this body.

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

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

virtual void	apply_generalized_impulse (const SHAREDVECTORN &gf)
	Applies a generalized impulse to this rigid 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 coordinates of this rigid 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_acceleration (const SHAREDVECTORN &ga)
	Sets the generalized acceleration of this rigid body.

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

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

virtual SHAREDVECTORN &	get_generalized_forces (SHAREDVECTORN &f)
	Gets the generalized inertia of this rigid body.

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 generalized inertia of this rigid body.

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

SHAREDMATRIXN &	transpose_solve_generalized_inertia_single (const SHAREDMATRIXN &B, SHAREDMATRIXN &X)
	Solves using the generalized inertia matrix (does not call articulated body version)

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

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

boost::shared_ptr< RIGIDBODY >	get_parent_link () const
	Gets the first parent link of this link; returns NULL if there is no parent.

boost::shared_ptr< JOINT >	get_inner_joint_explicit () const
	Gets the explicit inner joint of this link; returns NULL if there is no explicit inner joint. More...

void	add_inner_joint (boost::shared_ptr< JOINT > j)

void	add_outer_joint (boost::shared_ptr< JOINT > j)

void	remove_inner_joint (boost::shared_ptr< JOINT > joint)

void	remove_outer_joint (boost::shared_ptr< JOINT > joint)

virtual REAL	calc_kinetic_energy (boost::shared_ptr< const POSE3 > P=boost::shared_ptr< const POSE3 >())
	Calculates the kinetic energy of the body in an arbitrary frame.

virtual VECTOR3	calc_point_vel (const VECTOR3 &p) const
	Calculates the velocity of a point on this rigid body in the body frame.

bool	is_base () const
	Determines whether this link is the base.

bool	is_ground () const
	Determines whether this link is a "ground" (fixed link)

virtual boost::shared_ptr < const POSE3 >	get_computation_frame () const
	Gets the frame in which kinematics and dynamics computations occur.

virtual void	set_computation_frame_type (ReferenceFrameType rftype)
	(Re)sets the computation frame

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)

const SFORCE &	sum_forces ()
	Gets the current sum of forces on this body.

void	reset_accumulators ()
	Resets the force accumulators on this body.

SFORCE	calc_euler_torques ()
	Computes the torques (w x Jw) that come from the Euler component of the Newton-Euler equations.

void	set_enabled (bool flag)
	Sets the body to enabled / disabled. More...

void	invalidate_pose_vectors ()
	Invalidates pose quantities.

template<class OutputIterator >
OutputIterator	get_parent_links (OutputIterator begin) const

template<class OutputIterator >
OutputIterator	get_child_links (OutputIterator begin) const

boost::shared_ptr< RIGIDBODY >	get_this ()
	Gets the shared pointer for this

boost::shared_ptr< const RIGIDBODY >	get_this () const
	Gets the shared const pointer for this

boost::shared_ptr< const POSE3 >	get_pose () const
	Gets the current pose of this body.

boost::shared_ptr< const POSE3 >	get_gc_pose () const
	Gets the frame for generalized coordinates.

boost::shared_ptr< const POSE3 >	get_mixed_pose () const

REAL	calc_mass () const
	Synonym for get_mass() (implements SingleBody::calc_mass())

virtual REAL	get_mass () const
	Gets the mass of this body.

virtual bool	is_enabled () const
	Gets whether this body is enabled.

boost::shared_ptr < ARTICULATED_BODY >	get_articulated_body () const
	Gets the articulated body corresponding to this body. More...

virtual void	set_articulated_body (boost::shared_ptr< ARTICULATED_BODY > body)
	Sets the articulated body corresponding to this body. More...

unsigned	num_child_links () const
	Gets the number of child links of this link.

bool	is_end_effector () const
	Gets whether this body is an end-effector (i.e., the number of child links is zero) in an articulated body.

void	clear_inner_joints ()
	Removes all inner joints from this link.

void	clear_outer_joints ()
	Removes all outer joints from this link.

unsigned	get_index () const
	Gets the link index (returns std::numeric_limits<unsigned>::max() if not set)

void	set_index (unsigned index)
	Sets the link index. More...

const std::set < boost::shared_ptr< JOINT > > &	get_inner_joints () const
	Gets the set of inner joints for this link.

const std::set < boost::shared_ptr< JOINT > > &	get_outer_joints () const
	Gets the list of outer joints for this link.

Public Member Functions inherited from SINGLE_BODY
virtual boost::shared_ptr < DYNAMIC_BODY >	get_super_body () const
	Gets the "super" body.

REAL	calc_point_vel (const VECTOR3 &point, const VECTOR3 &dir)
	Calculates the velocity at a point on the body in a given direction.

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

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.

virtual VECTORN &	get_generalized_acceleration (VECTORN &ga)
	Gets 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 VECTORN &b, VECTORN &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
Compliance	compliance
	Compliance value, determines event type.

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

Protected Member Functions
void	update_mixed_pose ()
	Updates the center-of-mass center / global aligned frame. More...

Protected Attributes
bool	_xddj_valid
	Indicates whether inner joint frame acceleration is valid (up-to-date)

SPATIAL_RB_INERTIA	_Jj
	Spatial rigid body inertia matrix (inner joint frame)

SVELOCITY	_xdj
	Velocity (inner joint frame)

SFORCE	_forcej
	Cumulative force on the body (inner joint frame)

SACCEL	_xddj
	Acceleration (inner joint frame)

boost::shared_ptr< POSE3 >	_F
	reference pose for this body

boost::shared_ptr< POSE3 >	_F2
	secondary pose for this body

boost::weak_ptr< ARTICULATED_BODY >	_abody
	Pointer to articulated body (if this body is a link)

std::set< boost::shared_ptr < JOINT > >	_inner_joints
	Inner joints and associated data.

std::set< boost::shared_ptr < JOINT > >	_outer_joints
	Outer joints and associated data.

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	ARTICULATED_BODY

class	RC_ARTICULATED_BODY

class	MCARTICULATED_BODY

class	JOINT

Detailed Description

Represents a single rigid body.

Contains information needed to represent a rigid body, including position and velocity (both linear and angular), mass, center of mass, inertia matrix, collision data, and visualization data. This class is used for both non-articulated and articulated rigid bodies, though not all member data may be used in the latter.

Todo:: implement rest matrix

Constructor & Destructor Documentation

RIGIDBODY::RIGIDBODY ( )

Default constructor.

Constructs a rigid body with zero mass, zero inertia tensor, and center of mass at [0,0,0] with position at [0,0,0], identity orientation, and zero linear and angular velocity. Body is enabled by default.

References _F, _F2, DYNAMIC_BODY::_rftype, _xddj_valid, compliance, SPATIAL_RB_INERTIA::pose, and SVECTOR6::pose.

Member Function Documentation

void RIGIDBODY::apply_impulse ( const SMOMENTUM & w )

virtual

Applies a impulse to this link.

Parameters

w	the impulse as a force

Implements SINGLE_BODY.

References _abody, _F, _F2, _xdj, get_computation_frame(), get_inertia(), get_inner_joint_explicit(), get_pose(), get_this(), SPATIAL_RB_INERTIA::inverse_mult(), is_base(), SVECTOR6::pose, and reset_accumulators().

virtual SHAREDVECTORN& RIGIDBODY::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& RIGIDBODY::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().

boost::shared_ptr<ARTICULATED_BODY> RIGIDBODY::get_articulated_body ( ) const

inlinevirtual

Gets the articulated body corresponding to this body.

Returns: a pointer to the articulated body, or NULL if this body is not a link an articulated body

Implements SINGLE_BODY.

References _abody.

virtual VECTORN& RIGIDBODY::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().

shared_ptr< JOINT > RIGIDBODY::get_inner_joint_explicit ( ) const

Gets the explicit inner joint of this link; returns NULL if there is no explicit inner joint.

Throws an exception if this link has multiple explicit inner joints

References _inner_joints.

Referenced by add_force(), apply_impulse(), get_accel(), get_computation_frame(), get_inertia(), get_velocity(), set_accel(), set_inertia(), set_velocity(), and sum_forces().

virtual void RIGIDBODY::set_articulated_body ( boost::shared_ptr< ARTICULATED_BODY > body )

inlinevirtual

Sets the articulated body corresponding to this body.

Parameters

body	a pointer to the articulated body or NULL if this body is not a link in an articulated body

References _abody.

void RIGIDBODY::set_enabled ( bool flag )

Sets the body to enabled / disabled.

If the body is disabled, the linear and angular velocity are set to zero, and the body will not be updated if it is attempted to integrate its equations of motion.

References _xddj, _xddj_valid, and _xdj.

void RIGIDBODY::set_index ( unsigned index )

inline

Sets the link index.

This is set automatically by the articulated body. Users should not change this index or unknown behavior will result.

void RIGIDBODY::set_pose ( const POSE3 & p )

Sets the current 3D pose for this rigid body.

Also updates the transforms for associated visualization and collision data.

References _F, POSE3::rpose, and update_mixed_pose().

void RIGIDBODY::update_mixed_pose ( )

protected

Updates the center-of-mass center / global aligned frame.