Implementation of a method to calculate the matrices H (inertia),C(coriolis) and G(gravitation) for the calculation torques out of the pose and derivatives. More...

#include <src/chaindynparam.hpp>

Inheritance diagram for KDL::ChainDynParam:

[legend]

Collaboration diagram for KDL::ChainDynParam:

[legend]

Public Types
enum	{ E_DEGRADED = +1 , E_NOERROR = 0 , E_NO_CONVERGE = -1 , E_UNDEFINED = -2 , E_NOT_UP_TO_DATE = -3 , E_SIZE_MISMATCH = -4 , E_MAX_ITERATIONS_EXCEEDED = -5 , E_OUT_OF_RANGE = -6 , E_NOT_IMPLEMENTED = -7 , E_SVD_FAILED = -8 }

Public Member Functions
	ChainDynParam (const Chain &chain, Vector _grav)

virtual	~ChainDynParam ()

virtual int	JntToCoriolis (const JntArray &q, const JntArray &q_dot, JntArray &coriolis)

virtual int	JntToMass (const JntArray &q, JntSpaceInertiaMatrix &H)

virtual int	JntToGravity (const JntArray &q, JntArray &gravity)

virtual void	updateInternalDataStructures ()
	Update the internal data structures. More...

virtual int	getError () const
	Return the latest error. More...

virtual const char *	strError (const int error) const
	Return a description of the latest error. More...

Protected Attributes
int	error
	Latest error, initialized to E_NOERROR in constructor. More...

Private Attributes
const Chain &	chain

int	nr

unsigned int	nj

unsigned int	ns

Vector	grav

Vector	vectornull

JntArray	jntarraynull

ChainIdSolver_RNE	chainidsolver_coriolis

ChainIdSolver_RNE	chainidsolver_gravity

std::vector< Wrench >	wrenchnull

std::vector< Frame >	X

std::vector< Twist >	S

std::vector< ArticulatedBodyInertia, Eigen::aligned_allocator< ArticulatedBodyInertia > >	Ic

Wrench	F

Twist	ag

Detailed Description

Implementation of a method to calculate the matrices H (inertia),C(coriolis) and G(gravitation) for the calculation torques out of the pose and derivatives.

(inverse dynamics)

The algorithm implementation for H is based on the book "Rigid Body Dynamics Algorithms" of Roy Featherstone, 2008 (ISBN:978-0-387-74314-1) See page 107 for the pseudo-code. This algorithm is extended for the use of fixed joints

It calculates the joint-space inertia matrix, given the motion of the joints (q,qdot,qdotdot), external forces on the segments (expressed in the segments reference frame) and the dynamical parameters of the segments.

Member Enumeration Documentation

◆ anonymous enum

anonymous enum

inherited

Enumerator
E_DEGRADED	Converged but degraded solution (e.g. WDLS with psuedo-inverse singular)
E_NOERROR	No error.
E_NO_CONVERGE	Failed to converge.
E_UNDEFINED	Undefined value (e.g. computed a NAN, or tan(90 degrees) )
E_NOT_UP_TO_DATE	Chain size changed.
E_SIZE_MISMATCH	Input size does not match internal state.
E_MAX_ITERATIONS_EXCEEDED	Maximum number of iterations exceeded.
E_OUT_OF_RANGE	Requested index out of range.
E_NOT_IMPLEMENTED	Not yet implemented.
E_SVD_FAILED	Internal svd calculation failed.

Constructor & Destructor Documentation

◆ ChainDynParam()

KDL::ChainDynParam::ChainDynParam	(	const Chain &	chain,
		Vector	_grav
	)

References ag, grav, and KDL::Vector::Zero().

◆ ~ChainDynParam()

KDL::ChainDynParam::~ChainDynParam ( )

virtual

Member Function Documentation

◆ getError()

virtual int KDL::SolverI::getError ( ) const

inlinevirtualinherited

Return the latest error.

References KDL::SolverI::error.

◆ JntToCoriolis()

int KDL::ChainDynParam::JntToCoriolis	(	const JntArray &	q,
		const JntArray &	q_dot,
		JntArray &	coriolis
	)

virtual

References KDL::ChainIdSolver_RNE::CartToJnt(), chainidsolver_coriolis, jntarraynull, KDL::SetToZero(), and wrenchnull.

Referenced by KDL::ChainExternalWrenchEstimator::JntToExtWrench().

◆ JntToGravity()

int KDL::ChainDynParam::JntToGravity	(	const JntArray &	q,
		JntArray &	gravity
	)

virtual

References KDL::ChainIdSolver_RNE::CartToJnt(), chainidsolver_gravity, jntarraynull, KDL::SetToZero(), and wrenchnull.

Referenced by KDL::ChainExternalWrenchEstimator::JntToExtWrench().

◆ JntToMass()

int KDL::ChainDynParam::JntToMass	(	const JntArray &	q,
		JntSpaceInertiaMatrix &	H
	)

virtual

◆ strError()

virtual const char* KDL::SolverI::strError ( const int error ) const

inlinevirtualinherited

Return a description of the latest error.

Returns: if error is known then a description of error, otherwise "UNKNOWN ERROR"

Reimplemented in KDL::ChainJntToJacDotSolver, KDL::ChainIkSolverVel_wdls, KDL::ChainIkSolverVel_pinv, KDL::ChainIkSolverPos_NR_JL, KDL::ChainIkSolverPos_NR, KDL::ChainIkSolverPos_LMA, and KDL::ChainExternalWrenchEstimator.

References KDL::SolverI::E_DEGRADED, KDL::SolverI::E_MAX_ITERATIONS_EXCEEDED, KDL::SolverI::E_NO_CONVERGE, KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_IMPLEMENTED, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_OUT_OF_RANGE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::E_SVD_FAILED, KDL::SolverI::E_UNDEFINED, and KDL::SolverI::error.

Referenced by KDL::ChainExternalWrenchEstimator::strError(), KDL::ChainIkSolverPos_LMA::strError(), KDL::ChainIkSolverPos_NR::strError(), KDL::ChainIkSolverPos_NR_JL::strError(), KDL::ChainIkSolverVel_pinv::strError(), KDL::ChainIkSolverVel_wdls::strError(), and KDL::ChainJntToJacDotSolver::strError().

◆ updateInternalDataStructures()

void KDL::ChainDynParam::updateInternalDataStructures ( )

virtual

Update the internal data structures.

This is required if the number of segments or number of joints of a chain/tree have changed. This provides a single point of contact for solver memory allocations.

Implements KDL::SolverI.

References chain, chainidsolver_coriolis, chainidsolver_gravity, KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), Ic, jntarraynull, nj, ns, KDL::JntArray::resize(), S, KDL::ChainIdSolver_RNE::updateInternalDataStructures(), wrenchnull, X, and KDL::Wrench::Zero().