KDL 1.5.1
Loading...
Searching...
No Matches
Public Types | Public Member Functions | Protected Attributes | Private Attributes | List of all members
KDL::ChainFkSolverPos_recursive Class Reference

Implementation of a recursive forward position kinematics algorithm to calculate the position transformation from joint space to Cartesian space of a general kinematic chain (KDL::Chain). More...

#include <src/chainfksolverpos_recursive.hpp>

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

 ChainFkSolverPos_recursive (const Chain &chain)
 
 ~ChainFkSolverPos_recursive ()
 
virtual int JntToCart (const JntArray &q_in, Frame &p_out, int segmentNr=-1)
 Calculate forward position kinematics for a KDL::Chain, from joint coordinates to cartesian pose. More...
 
virtual int JntToCart (const JntArray &q_in, std::vector< Frame > &p_out, int segmentNr=-1)
 Calculate forward position kinematics for a KDL::Chain, from joint coordinates to cartesian pose. 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
 

Detailed Description

Implementation of a recursive forward position kinematics algorithm to calculate the position transformation from joint space to Cartesian space of a general kinematic chain (KDL::Chain).

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

◆ ChainFkSolverPos_recursive()

KDL::ChainFkSolverPos_recursive::ChainFkSolverPos_recursive ( const Chain chain)

◆ ~ChainFkSolverPos_recursive()

KDL::ChainFkSolverPos_recursive::~ChainFkSolverPos_recursive ( )

Member Function Documentation

◆ getError()

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

Return the latest error.

References KDL::SolverI::error.

◆ JntToCart() [1/2]

int KDL::ChainFkSolverPos_recursive::JntToCart ( const JntArray q_in,
Frame p_out,
int  segmentNr = -1 
)
virtual

◆ JntToCart() [2/2]

int KDL::ChainFkSolverPos_recursive::JntToCart ( const JntArray q_in,
std::vector< Frame > &  p_out,
int  segmentNr = -1 
)
virtual

Calculate forward position kinematics for a KDL::Chain, from joint coordinates to cartesian pose.

Parameters
q_ininput joint coordinates
p_outreference to a vector of output cartesian poses for all segments
Returns
if < 0 something went wrong

Implements KDL::ChainFkSolverPos.

References chain, KDL::Joint::Fixed, KDL::Segment::getJoint(), KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), KDL::Chain::getSegment(), KDL::Joint::getType(), KDL::Segment::pose(), and KDL::JntArray::rows().

◆ strError()

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

◆ updateInternalDataStructures()

virtual void KDL::ChainFkSolverPos_recursive::updateInternalDataStructures ( )
inlinevirtual

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::ChainFkSolverPos.

Referenced by KDL::ChainExternalWrenchEstimator::updateInternalDataStructures(), and KDL::ChainJntToJacDotSolver::updateInternalDataStructures().

Member Data Documentation

◆ chain

const Chain& KDL::ChainFkSolverPos_recursive::chain
private

Referenced by JntToCart().

◆ error

int KDL::SolverI::error
protectedinherited

Latest error, initialized to E_NOERROR in constructor.

Referenced by KDL::ChainIdSolver_RNE::CartToJnt(), KDL::TreeIdSolver_RNE::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(), 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().


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