KDL  1.3.0
Public 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>

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)
 

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< Wrenchwrenchnull
 
std::vector< FrameX
 
std::vector< TwistS
 
std::vector< ArticulatedBodyInertiaIc
 
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.

Constructor & Destructor Documentation

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

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

KDL::ChainDynParam::~ChainDynParam ( )
virtual

Member Function Documentation

int KDL::ChainDynParam::JntToCoriolis ( const JntArray &  q,
const JntArray &  q_dot,
JntArray &  coriolis 
)
virtual
int KDL::ChainDynParam::JntToGravity ( const JntArray &  q,
JntArray &  gravity 
)
virtual
int KDL::ChainDynParam::JntToMass ( const JntArray &  q,
JntSpaceInertiaMatrix &  H 
)
virtual

Member Data Documentation

Twist KDL::ChainDynParam::ag
private

Referenced by ChainDynParam().

const Chain KDL::ChainDynParam::chain
private

Referenced by JntToMass().

ChainIdSolver_RNE KDL::ChainDynParam::chainidsolver_coriolis
private

Referenced by JntToCoriolis().

ChainIdSolver_RNE KDL::ChainDynParam::chainidsolver_gravity
private

Referenced by JntToGravity().

Wrench KDL::ChainDynParam::F
private

Referenced by JntToMass().

Vector KDL::ChainDynParam::grav
private

Referenced by ChainDynParam().

std::vector<ArticulatedBodyInertia> KDL::ChainDynParam::Ic
private

Referenced by JntToMass().

JntArray KDL::ChainDynParam::jntarraynull
private

Referenced by JntToCoriolis(), and JntToGravity().

unsigned int KDL::ChainDynParam::nj
private

Referenced by JntToMass().

int KDL::ChainDynParam::nr
private
unsigned int KDL::ChainDynParam::ns
private

Referenced by JntToMass().

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

Referenced by JntToMass().

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

Referenced by JntToCoriolis(), and JntToGravity().

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

Referenced by JntToMass().


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