KDL
1.4.0
|
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>
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 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 |
int | error |
Latest error, initialized to E_NOERROR in constructor. More... | |
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.
|
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. |
References ag, grav, and KDL::Vector::Zero().
|
virtual |
|
inlinevirtualinherited |
Return the latest error.
References KDL::SolverI::error.
|
virtual |
References KDL::ChainIdSolver_RNE::CartToJnt(), chainidsolver_coriolis, jntarraynull, KDL::SetToZero(), and wrenchnull.
References KDL::ChainIdSolver_RNE::CartToJnt(), chainidsolver_gravity, jntarraynull, KDL::SetToZero(), and wrenchnull.
|
virtual |
References chain, KDL::dot(), KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, F, KDL::Segment::getInertia(), KDL::Segment::getJoint(), KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), KDL::Chain::getSegment(), KDL::Joint::getType(), Ic, nj, KDL::Joint::None, ns, KDL::Segment::pose(), KDL::JntArray::rows(), S, KDL::Segment::twist(), and X.
|
inlinevirtualinherited |
Return a description of the latest error.
Reimplemented in KDL::ChainIkSolverVel_wdls, KDL::ChainIkSolverPos_LMA, KDL::ChainJntToJacDotSolver, KDL::ChainIkSolverPos_NR_JL, KDL::ChainIkSolverVel_pinv, and KDL::ChainIkSolverPos_NR.
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::updateInternalDataStructures().
Referenced by KDL::ChainIkSolverPos_NR::strError(), KDL::ChainIkSolverVel_pinv::strError(), KDL::ChainIkSolverPos_NR_JL::strError(), KDL::ChainJntToJacDotSolver::strError(), KDL::ChainIkSolverPos_LMA::strError(), and KDL::ChainIkSolverVel_wdls::strError().
|
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().
|
private |
Referenced by ChainDynParam().
|
private |
Referenced by JntToMass(), and updateInternalDataStructures().
|
private |
Referenced by JntToCoriolis(), and updateInternalDataStructures().
|
private |
Referenced by JntToGravity(), and updateInternalDataStructures().
|
protectedinherited |
Latest error, initialized to E_NOERROR in constructor.
Referenced by KDL::ChainIkSolverVel_pinv_givens::CartToJnt(), KDL::ChainIdSolver_RNE::CartToJnt(), KDL::ChainIkSolverVel_pinv_nso::CartToJnt(), KDL::ChainIdSolver_Vereshchagin::CartToJnt(), KDL::ChainIkSolverPos_NR::CartToJnt(), KDL::ChainIkSolverVel_pinv::CartToJnt(), KDL::ChainIkSolverPos_NR_JL::CartToJnt(), KDL::ChainIkSolverVel_wdls::CartToJnt(), KDL::ChainIkSolverPos_LMA::CartToJnt(), KDL::SolverI::getError(), KDL::ChainIkSolverVel_wdls::getSigma(), KDL::ChainIkSolverVel_pinv::getSVDResult(), KDL::ChainIkSolverVel_wdls::getSVDResult(), KDL::ChainFkSolverPos_recursive::JntToCart(), KDL::ChainFkSolverVel_recursive::JntToCart(), KDL::ChainJntToJacSolver::JntToJac(), KDL::ChainJntToJacDotSolver::JntToJacDot(), JntToMass(), KDL::ChainJntToJacDotSolver::setInternialRepresentation(), KDL::ChainIkSolverPos_NR_JL::setJointLimits(), KDL::ChainJntToJacSolver::setLockedJoints(), KDL::ChainJntToJacDotSolver::setLockedJoints(), KDL::ChainIkSolverVel_pinv_nso::setOptPos(), KDL::ChainIkSolverVel_wdls::setWeightJS(), KDL::ChainIkSolverVel_pinv_nso::setWeights(), and KDL::ChainIkSolverVel_wdls::setWeightTS().
|
private |
Referenced by JntToMass().
|
private |
Referenced by ChainDynParam().
|
private |
Referenced by JntToMass(), and updateInternalDataStructures().
|
private |
Referenced by JntToCoriolis(), JntToGravity(), and updateInternalDataStructures().
|
private |
Referenced by JntToMass(), and updateInternalDataStructures().
|
private |
|
private |
Referenced by JntToMass(), and updateInternalDataStructures().
|
private |
Referenced by JntToMass(), and updateInternalDataStructures().
|
private |
|
private |
Referenced by JntToCoriolis(), JntToGravity(), and updateInternalDataStructures().
|
private |
Referenced by JntToMass(), and updateInternalDataStructures().