KDL  1.4.0
Public Member Functions | Private Types | Private Member Functions | Private Attributes | List of all members
KDL::ChainDynParam Class Reference

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:
Inheritance graph
[legend]
Collaboration diagram for KDL::ChainDynParam:
Collaboration graph
[legend]

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...
 

Private 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
}
 

Private Member Functions

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...
 

Private Attributes

const Chainchain
 
int nr
 
unsigned int nj
 
unsigned int ns
 
Vector grav
 
Vector vectornull
 
JntArray jntarraynull
 
ChainIdSolver_RNE chainidsolver_coriolis
 
ChainIdSolver_RNE chainidsolver_gravity
 
std::vector< Wrenchwrenchnull
 
std::vector< FrameX
 
std::vector< TwistS
 
std::vector< ArticulatedBodyInertia, Eigen::aligned_allocator< ArticulatedBodyInertia > > Ic
 
Wrench F
 
Twist ag
 
int error
 Latest error, initialized to E_NOERROR in constructor. More...
 

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

§ JntToGravity()

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

§ JntToMass()

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

§ strError()

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

§ 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().

Member Data Documentation

§ ag

Twist KDL::ChainDynParam::ag
private

Referenced by ChainDynParam().

§ chain

const Chain& KDL::ChainDynParam::chain
private

§ chainidsolver_coriolis

ChainIdSolver_RNE KDL::ChainDynParam::chainidsolver_coriolis
private

§ chainidsolver_gravity

ChainIdSolver_RNE KDL::ChainDynParam::chainidsolver_gravity
private

§ error

int KDL::SolverI::error
protectedinherited

§ F

Wrench KDL::ChainDynParam::F
private

Referenced by JntToMass().

§ grav

Vector KDL::ChainDynParam::grav
private

Referenced by ChainDynParam().

§ Ic

std::vector<ArticulatedBodyInertia, Eigen::aligned_allocator<ArticulatedBodyInertia> > KDL::ChainDynParam::Ic
private

§ jntarraynull

JntArray KDL::ChainDynParam::jntarraynull
private

§ nj

unsigned int KDL::ChainDynParam::nj
private

§ nr

int KDL::ChainDynParam::nr
private

§ ns

unsigned int KDL::ChainDynParam::ns
private

§ S

std::vector<Twist> KDL::ChainDynParam::S
private

§ vectornull

Vector KDL::ChainDynParam::vectornull
private

§ wrenchnull

std::vector<Wrench> KDL::ChainDynParam::wrenchnull
private

§ X

std::vector<Frame> KDL::ChainDynParam::X
private

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