KDL 1.5.1
|
Recursive newton euler inverse dynamics solver for kinematic trees. More...
#include <src/treeidsolver_recursive_newton_euler.hpp>
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 | |
TreeIdSolver_RNE (const Tree &tree, Vector grav) | |
Constructor for the solver, it will allocate all the necessary memory. | |
int | CartToJnt (const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap &f_ext, JntArray &torques) |
Function to calculate from Cartesian forces to joint torques. | |
virtual void | updateInternalDataStructures () |
Update the internal data structures. | |
virtual int | getError () const |
Return the latest error. | |
virtual const char * | strError (const int error) const |
Return a description of the latest error. | |
Protected Attributes | |
int | error |
Latest error, initialized to E_NOERROR in constructor. | |
Private Member Functions | |
void | initAuxVariables () |
Helper function to initialize private members X, S, v, a, f. | |
void | rne_step (SegmentMap::const_iterator segment, const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const WrenchMap &f_ext, JntArray &torques) |
One recursion step. | |
Private Attributes | |
const Tree & | tree |
unsigned int | nj |
unsigned int | ns |
std::map< std::string, Frame > | X |
std::map< std::string, Twist > | S |
std::map< std::string, Twist > | v |
std::map< std::string, Twist > | a |
std::map< std::string, Wrench > | f |
Twist | ag |
Recursive newton euler inverse dynamics solver for kinematic trees.
It calculates the torques for the joints, 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.
This is an extension of the inverse dynamic solver for kinematic chains,
|
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 for the solver, it will allocate all the necessary memory.
tree | The kinematic tree to calculate the inverse dynamics for, an internal reference will be stored. |
grav | The gravity vector to use during the calculation. |
References ag, initAuxVariables(), and KDL::Vector::Zero().
|
virtual |
Function to calculate from Cartesian forces to joint torques.
Input parameters;
q | The current joint positions |
q_dot | The current joint velocities |
q_dotdot | The current joint accelerations |
f_ext | The external forces (no gravity) on the segments Output parameters: |
torques | the resulting torques for the joints |
Implements KDL::TreeIdSolver.
References KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, KDL::Tree::getNrOfJoints(), KDL::Tree::getNrOfSegments(), KDL::Tree::getRootSegment(), nj, ns, rne_step(), KDL::JntArray::rows(), and tree.
|
inlinevirtualinherited |
Return the latest error.
References KDL::SolverI::error.
|
private |
Helper function to initialize private members X, S, v, a, f.
References a, f, KDL::Tree::getSegments(), S, tree, v, and X.
Referenced by TreeIdSolver_RNE(), and updateInternalDataStructures().
|
private |
One recursion step.
References a, ag, dot(), f, KDL::Joint::Fixed, KDL::Joint::getInertia(), KDL::Segment::getInertia(), KDL::Segment::getJoint(), KDL::Tree::getRootSegment(), GetTreeElementChildren, GetTreeElementParent, GetTreeElementQNr, GetTreeElementSegment, KDL::Joint::getType(), KDL::Segment::pose(), rne_step(), S, tree, KDL::Segment::twist(), v, and X.
Referenced by CartToJnt(), and rne_step().
|
inlinevirtualinherited |
Return a description of the latest error.
Reimplemented in KDL::ChainExternalWrenchEstimator, KDL::ChainIkSolverPos_LMA, KDL::ChainIkSolverPos_NR, KDL::ChainIkSolverPos_NR_JL, KDL::ChainIkSolverVel_pinv, KDL::ChainIkSolverVel_wdls, and KDL::ChainJntToJacDotSolver.
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().
|
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 KDL::Tree::getNrOfJoints(), KDL::Tree::getNrOfSegments(), initAuxVariables(), nj, ns, and tree.
|
private |
Referenced by initAuxVariables(), and rne_step().
|
private |
Referenced by rne_step(), and TreeIdSolver_RNE().
|
protectedinherited |
Latest error, initialized to E_NOERROR in constructor.
Referenced by KDL::ChainIdSolver_RNE::CartToJnt(), CartToJnt(), KDL::ChainFdSolver_RNE::CartToJnt(), KDL::ChainHdSolver_Vereshchagin::CartToJnt(), KDL::ChainIkSolverVel_pinv::CartToJnt(), KDL::ChainIkSolverVel_pinv_givens::CartToJnt(), KDL::ChainIkSolverVel_pinv_nso::CartToJnt(), KDL::ChainIkSolverVel_wdls::CartToJnt(), KDL::ChainIkSolverPos_NR::CartToJnt(), KDL::ChainIkSolverPos_NR_JL::CartToJnt(), KDL::ChainIkSolverPos_LMA::CartToJnt(), KDL::SolverI::getError(), KDL::ChainIkSolverVel_wdls::getSigma(), KDL::ChainFkSolverPos_recursive::JntToCart(), KDL::ChainFkSolverVel_recursive::JntToCart(), KDL::ChainExternalWrenchEstimator::JntToExtWrench(), KDL::ChainJntToJacSolver::JntToJac(), KDL::ChainJntToJacDotSolver::JntToJacDot(), KDL::ChainDynParam::JntToMass(), KDL::ChainExternalWrenchEstimator::setInitialMomentum(), KDL::ChainIkSolverPos_NR_JL::setJointLimits(), KDL::ChainJntToJacDotSolver::setLockedJoints(), KDL::ChainJntToJacSolver::setLockedJoints(), KDL::ChainIkSolverVel_pinv_nso::setOptPos(), KDL::ChainIkSolverVel_wdls::setWeightJS(), KDL::ChainIkSolverVel_pinv_nso::setWeights(), KDL::ChainIkSolverVel_wdls::setWeightTS(), KDL::ChainExternalWrenchEstimator::strError(), KDL::ChainIkSolverPos_LMA::strError(), KDL::ChainIkSolverPos_NR::strError(), KDL::ChainIkSolverPos_NR_JL::strError(), KDL::ChainIkSolverVel_pinv::strError(), KDL::ChainIkSolverVel_wdls::strError(), KDL::ChainJntToJacDotSolver::strError(), and KDL::SolverI::strError().
|
private |
Referenced by initAuxVariables(), and rne_step().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by CartToJnt(), and updateInternalDataStructures().
|
private |
Referenced by initAuxVariables(), and rne_step().
|
private |
Referenced by CartToJnt(), initAuxVariables(), rne_step(), and updateInternalDataStructures().
|
private |
Referenced by initAuxVariables(), and rne_step().
|
private |
Referenced by initAuxVariables(), and rne_step().