KDL  1.5.1
Public Types | Public Member Functions | Protected Attributes | Private Member Functions | Private Attributes | List of all members
KDL::TreeIdSolver_RNE Class Reference

Recursive newton euler inverse dynamics solver for kinematic trees. More...

#include <src/treeidsolver_recursive_newton_euler.hpp>

Inheritance diagram for KDL::TreeIdSolver_RNE:
Inheritance graph
[legend]
Collaboration diagram for KDL::TreeIdSolver_RNE:
Collaboration graph
[legend]

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. More...
 
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. More...
 
virtual void updateInternalDataStructures ()
 Update the internal data structures. More...
 
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...
 

Protected Attributes

int error
 Latest error, initialized to E_NOERROR in constructor. More...
 

Private Member Functions

void initAuxVariables ()
 Helper function to initialize private members X, S, v, a, f. More...
 
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. More...
 

Private Attributes

const Treetree
 
unsigned int nj
 
unsigned int ns
 
std::map< std::string, FrameX
 
std::map< std::string, TwistS
 
std::map< std::string, Twistv
 
std::map< std::string, Twista
 
std::map< std::string, Wrenchf
 
Twist ag
 

Detailed Description

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,

See also
ChainIdSolver_RNE. The main difference is the use of STL maps instead of vectors to represent external wrenches (as well as internal variables exploited during the recursion).

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

◆ TreeIdSolver_RNE()

KDL::TreeIdSolver_RNE::TreeIdSolver_RNE ( const Tree tree,
Vector  grav 
)

Constructor for the solver, it will allocate all the necessary memory.

Parameters
treeThe kinematic tree to calculate the inverse dynamics for, an internal reference will be stored.
gravThe gravity vector to use during the calculation.

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

Member Function Documentation

◆ CartToJnt()

int KDL::TreeIdSolver_RNE::CartToJnt ( const JntArray q,
const JntArray q_dot,
const JntArray q_dotdot,
const WrenchMap f_ext,
JntArray torques 
)
virtual

Function to calculate from Cartesian forces to joint torques.

Input parameters;

Parameters
qThe current joint positions
q_dotThe current joint velocities
q_dotdotThe current joint accelerations
f_extThe external forces (no gravity) on the segments Output parameters:
torquesthe 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.

◆ getError()

virtual int KDL::SolverI::getError ( ) const
inlinevirtualinherited

Return the latest error.

References KDL::SolverI::error.

◆ initAuxVariables()

void KDL::TreeIdSolver_RNE::initAuxVariables ( )
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().

◆ rne_step()

void KDL::TreeIdSolver_RNE::rne_step ( SegmentMap::const_iterator  segment,
const JntArray q,
const JntArray q_dot,
const JntArray q_dotdot,
const WrenchMap f_ext,
JntArray torques 
)
private

◆ strError()

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

◆ updateInternalDataStructures()

void KDL::TreeIdSolver_RNE::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 KDL::Tree::getNrOfJoints(), KDL::Tree::getNrOfSegments(), initAuxVariables(), nj, ns, and tree.

Member Data Documentation

◆ a

std::map<std::string,Twist> KDL::TreeIdSolver_RNE::a
private

Referenced by initAuxVariables(), and rne_step().

◆ ag

Twist KDL::TreeIdSolver_RNE::ag
private

Referenced by rne_step(), and TreeIdSolver_RNE().

◆ error

int KDL::SolverI::error
protectedinherited

Latest error, initialized to E_NOERROR in constructor.

Referenced by KDL::ChainIkSolverVel_pinv_givens::CartToJnt(), KDL::ChainIdSolver_RNE::CartToJnt(), CartToJnt(), KDL::ChainFdSolver_RNE::CartToJnt(), KDL::ChainIkSolverVel_pinv_nso::CartToJnt(), KDL::ChainIkSolverPos_NR::CartToJnt(), KDL::ChainIkSolverVel_pinv::CartToJnt(), KDL::ChainIkSolverPos_NR_JL::CartToJnt(), KDL::ChainIkSolverVel_wdls::CartToJnt(), KDL::ChainIkSolverPos_LMA::CartToJnt(), KDL::ChainHdSolver_Vereshchagin::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::ChainJntToJacSolver::setLockedJoints(), KDL::ChainJntToJacDotSolver::setLockedJoints(), KDL::ChainIkSolverVel_pinv_nso::setOptPos(), KDL::ChainIkSolverVel_wdls::setWeightJS(), KDL::ChainIkSolverVel_pinv_nso::setWeights(), KDL::ChainIkSolverVel_wdls::setWeightTS(), KDL::ChainIkSolverPos_NR::strError(), KDL::ChainIkSolverVel_pinv::strError(), KDL::ChainExternalWrenchEstimator::strError(), KDL::ChainIkSolverPos_NR_JL::strError(), KDL::ChainJntToJacDotSolver::strError(), KDL::SolverI::strError(), KDL::ChainIkSolverPos_LMA::strError(), and KDL::ChainIkSolverVel_wdls::strError().

◆ f

std::map<std::string,Wrench> KDL::TreeIdSolver_RNE::f
private

Referenced by initAuxVariables(), and rne_step().

◆ nj

unsigned int KDL::TreeIdSolver_RNE::nj
private

◆ ns

unsigned int KDL::TreeIdSolver_RNE::ns
private

◆ S

std::map<std::string,Twist> KDL::TreeIdSolver_RNE::S
private

Referenced by initAuxVariables(), and rne_step().

◆ tree

const Tree& KDL::TreeIdSolver_RNE::tree
private

◆ v

std::map<std::string,Twist> KDL::TreeIdSolver_RNE::v
private

Referenced by initAuxVariables(), and rne_step().

◆ X

std::map<std::string,Frame> KDL::TreeIdSolver_RNE::X
private

Referenced by initAuxVariables(), and rne_step().


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