#include <chaindynparam.hpp>
Public Member Functions | |
ChainDynParam (const Chain &chain, Vector _grav) | |
virtual int | JntToCoriolis (const JntArray &q, const JntArray &q_dot, JntArray &coriolis) |
virtual int | JntToGravity (const JntArray &q, JntArray &gravity) |
virtual int | JntToMass (const JntArray &q, JntSpaceInertiaMatrix &H) |
virtual | ~ChainDynParam () |
Private Attributes | |
Twist | ag |
const Chain | chain |
ChainIdSolver_RNE | chainidsolver_coriolis |
ChainIdSolver_RNE | chainidsolver_gravity |
Wrench | F |
Vector | grav |
std::vector < ArticulatedBodyInertia > | Ic |
JntArray | jntarraynull |
unsigned int | nj |
int | nr |
unsigned int | ns |
std::vector< Twist > | S |
Vector | vectornull |
std::vector< Wrench > | wrenchnull |
std::vector< Frame > | X |
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.
Definition at line 46 of file chaindynparam.hpp.
KDL::ChainDynParam::ChainDynParam | ( | const Chain & | chain, |
Vector | _grav | ||
) |
Definition at line 28 of file chaindynparam.cpp.
KDL::ChainDynParam::~ChainDynParam | ( | ) | [virtual] |
Definition at line 134 of file chaindynparam.cpp.
int KDL::ChainDynParam::JntToCoriolis | ( | const JntArray & | q, |
const JntArray & | q_dot, | ||
JntArray & | coriolis | ||
) | [virtual] |
Definition at line 110 of file chaindynparam.cpp.
int KDL::ChainDynParam::JntToGravity | ( | const JntArray & | q, |
JntArray & | gravity | ||
) | [virtual] |
Definition at line 123 of file chaindynparam.cpp.
int KDL::ChainDynParam::JntToMass | ( | const JntArray & | q, |
JntSpaceInertiaMatrix & | H | ||
) | [virtual] |
Definition at line 46 of file chaindynparam.cpp.
Twist KDL::ChainDynParam::ag [private] |
Definition at line 72 of file chaindynparam.hpp.
const Chain KDL::ChainDynParam::chain [private] |
Definition at line 57 of file chaindynparam.hpp.
Definition at line 64 of file chaindynparam.hpp.
Definition at line 65 of file chaindynparam.hpp.
Wrench KDL::ChainDynParam::F [private] |
Definition at line 71 of file chaindynparam.hpp.
Vector KDL::ChainDynParam::grav [private] |
Definition at line 61 of file chaindynparam.hpp.
std::vector<ArticulatedBodyInertia> KDL::ChainDynParam::Ic [private] |
Definition at line 70 of file chaindynparam.hpp.
JntArray KDL::ChainDynParam::jntarraynull [private] |
Definition at line 63 of file chaindynparam.hpp.
unsigned int KDL::ChainDynParam::nj [private] |
Definition at line 59 of file chaindynparam.hpp.
int KDL::ChainDynParam::nr [private] |
Definition at line 58 of file chaindynparam.hpp.
unsigned int KDL::ChainDynParam::ns [private] |
Definition at line 60 of file chaindynparam.hpp.
std::vector<Twist> KDL::ChainDynParam::S [private] |
Definition at line 68 of file chaindynparam.hpp.
Vector KDL::ChainDynParam::vectornull [private] |
Definition at line 62 of file chaindynparam.hpp.
std::vector<Wrench> KDL::ChainDynParam::wrenchnull [private] |
Definition at line 66 of file chaindynparam.hpp.
std::vector<Frame> KDL::ChainDynParam::X [private] |
Definition at line 67 of file chaindynparam.hpp.