Implements Featherstone's algorithm for forward dynamics. More...

#include <FSABAlgorithm.h>

Public Member Functions
boost::shared_ptr < RC_ARTICULATED_BODY >	get_body () const

void	set_body (boost::shared_ptr< RC_ARTICULATED_BODY > body)

void	calc_fwd_dyn ()
	Computes the joint accelerations (forward dynamics) for an articulated body. More...

void	calc_inverse_generalized_inertia_noprecalc (MATRIXN &iM)
	Calculates the inverse generalized inertia matrix.

void	solve_generalized_inertia_noprecalc (SHAREDVECTORN &v)
	Solves the equation Mx = b, where M is the generalized inertia matrix. More...

void	solve_generalized_inertia_noprecalc (SHAREDMATRIXN &Y)
	Solves the equation MX = B, where M is the generalized inertia matrix. More...

void	apply_generalized_impulse (const VECTORN &gj)
	Applies a generalized impulse using the algorithm of Drumwright.

void	apply_impulse (const SMOMENTUM &j, boost::shared_ptr< RIGIDBODY > link)

void	calc_spatial_inertias (boost::shared_ptr< RC_ARTICULATED_BODY > body)

Public Attributes
boost::weak_ptr < RC_ARTICULATED_BODY >	_body
	The body that this algorithm operates on.

std::vector< SACCEL >	_a
	The spatial accelerations.

std::vector< SPATIAL_AB_INERTIA >	_I
	The articulated body inertias.

std::vector< SFORCE >	_Z
	The articulated body spatial zero accelerations.

std::vector< SVELOCITY >	_dv
	Vector of link velocity updates.

std::vector< SACCEL >	_c
	The spatial coriolis vectors.

std::vector< std::vector < SMOMENTUM > >	_Is
	The expressions I*s.

std::vector< MATRIXN >	_sIs
	Cholesky factorizations sIs.

std::vector< MATRIXN >	_usIs
	SVDs of sIs.

std::vector< MATRIXN >	_vsIs

std::vector< VECTORN >	_ssIs

std::vector< bool >	_rank_deficient
	Determines whether the equations for a joint are rank deficient.

std::vector< VECTORN >	_mu
	The temporary expression Q - Is'c - s'*Z.

Friends
class	RC_ARTICULATED_BODY

Detailed Description

Implements Featherstone's algorithm for forward dynamics.

Implements Featherstone's algorithm for articulated bodies. Featherstone's algorithm runs in O(n) time [n = # of joints]. This implementation is based on Brian Mirtich's Ph. D. thesis, and remains pretty consistent with it. There are a couple of changes, to produce a nice implementation. The user need not be concerned with these issues, but they are useful to know for debugging.

Mirtich labels his links from 1..n, and considers the base to be link 0; the total number of links is considered to be n, rather than n+1. I make the total number of links n+1 and treat the links the same as the base. I do this so that the user thinks of the base as a link for purposes of link connectivity.
Mirtich labels his joints from 0..n-1. When labeling the link in Mirtich's style, link i and joint i match up (joint i is link i's inner joint). When labeling the link in my style, joint i-1 is the corresponding joint for link i.

Note that one critical note for manipulator setup is that the base is the first link in the list of links.

Member Function Documentation

void FSAB_ALGORITHM::calc_fwd_dyn ( )

Computes the joint accelerations (forward dynamics) for an articulated body.

Featherstone Algorithm taken from Mirtich's thesis (p. 113). Mirtich's numbering is a little funny, so I decrement his joint indices by one, while leaving his link indices intact.

References _body.

Referenced by RC_ARTICULATED_BODY::calc_fwd_dyn().

void FSAB_ALGORITHM::solve_generalized_inertia_noprecalc ( SHAREDVECTORN & v )

Solves the equation Mx = b, where M is the generalized inertia matrix.

Parameters

v	the vector b on entry, the vector x on return

Precondition: spatial inertias already computed for the body's current configuration

References _body, and apply_generalized_impulse().

Referenced by RC_ARTICULATED_BODY::solve_generalized_inertia(), and RC_ARTICULATED_BODY::transpose_solve_generalized_inertia().

void FSAB_ALGORITHM::solve_generalized_inertia_noprecalc ( SHAREDMATRIXN & Y )

Solves the equation MX = B, where M is the generalized inertia matrix.

Parameters

Y	the matrix B on entry, the matrix X on return

Precondition: spatial inertias already computed for the body's current configuration

References _body, apply_generalized_impulse(), and VECTORN::set_zero().

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

/home/drum/Ravelin.new/include/Ravelin/FSABAlgorithm.h
/home/drum/Ravelin.new/src/FSABAlgorithm.cpp

Public Member Functions

Public Attributes

Friends

Detailed Description

Member Function Documentation