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

Recursive newton euler inverse dynamics solver. More...

#include <src/chainidsolver_recursive_newton_euler.hpp>

Inheritance diagram for KDL::ChainIdSolver_RNE:
Inheritance graph
[legend]
Collaboration diagram for KDL::ChainIdSolver_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

 ChainIdSolver_RNE (const Chain &chain, Vector grav)
 Constructor for the solver, it will allocate all the necessary memory. More...
 
 ~ChainIdSolver_RNE ()
 
int CartToJnt (const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &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 Attributes

const Chainchain
 
unsigned int nj
 
unsigned int ns
 
std::vector< FrameX
 
std::vector< TwistS
 
std::vector< Twistv
 
std::vector< Twista
 
std::vector< Wrenchf
 
Twist ag
 

Detailed Description

Recursive newton euler inverse dynamics solver.

The algorithm implementation is based on the book "Rigid Body Dynamics Algorithms" of Roy Featherstone, 2008 (ISBN:978-0-387-74314-1) See page 96 for the pseudo-code.

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.

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

§ ChainIdSolver_RNE()

KDL::ChainIdSolver_RNE::ChainIdSolver_RNE ( const Chain chain,
Vector  grav 
)

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

Parameters
chainThe kinematic chain to calculate the inverse dynamics for, an internal copy will be made.
gravThe gravity vector to use during the calculation.

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

§ ~ChainIdSolver_RNE()

KDL::ChainIdSolver_RNE::~ChainIdSolver_RNE ( )
inline

Member Function Documentation

§ CartToJnt()

int KDL::ChainIdSolver_RNE::CartToJnt ( const JntArray q,
const JntArray q_dot,
const JntArray q_dotdot,
const Wrenches 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::ChainIdSolver.

References a, ag, 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(), nj, KDL::Joint::None, ns, KDL::Segment::pose(), KDL::JntArray::rows(), S, KDL::Segment::twist(), v, and X.

Referenced by KDL::ChainDynParam::JntToCoriolis(), KDL::ChainDynParam::JntToGravity(), and ~ChainIdSolver_RNE().

§ getError()

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

Return the latest error.

References KDL::SolverI::error.

§ strError()

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

§ updateInternalDataStructures()

void KDL::ChainIdSolver_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 a, chain, f, KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), nj, ns, S, v, and X.

Referenced by KDL::ChainDynParam::updateInternalDataStructures(), and ~ChainIdSolver_RNE().

Member Data Documentation

§ a

std::vector<Twist> KDL::ChainIdSolver_RNE::a
private

§ ag

Twist KDL::ChainIdSolver_RNE::ag
private

Referenced by CartToJnt(), and ChainIdSolver_RNE().

§ chain

const Chain& KDL::ChainIdSolver_RNE::chain
private

§ error

int KDL::SolverI::error
protectedinherited

§ f

std::vector<Wrench> KDL::ChainIdSolver_RNE::f
private

§ nj

unsigned int KDL::ChainIdSolver_RNE::nj
private

§ ns

unsigned int KDL::ChainIdSolver_RNE::ns
private

§ S

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

§ v

std::vector<Twist> KDL::ChainIdSolver_RNE::v
private

§ X

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

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