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

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

#include <src/chainfksolvervel_recursive.hpp>

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

 ChainFkSolverVel_recursive (const Chain &chain)
 
 ~ChainFkSolverVel_recursive ()
 
virtual int JntToCart (const JntArrayVel &q_in, FrameVel &out, int segmentNr=-1)
 Calculate forward position and velocity kinematics, from joint coordinates to cartesian coordinates. More...
 
virtual int JntToCart (const JntArrayVel &q_in, std::vector< FrameVel > &out, int segmentNr=-1)
 Calculate forward position and velocity kinematics, from joint coordinates to cartesian coordinates. 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 and velocity kinematics algorithm to calculate the position and velocity 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

§ ChainFkSolverVel_recursive()

KDL::ChainFkSolverVel_recursive::ChainFkSolverVel_recursive ( const Chain chain)

§ ~ChainFkSolverVel_recursive()

KDL::ChainFkSolverVel_recursive::~ChainFkSolverVel_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::ChainFkSolverVel_recursive::JntToCart ( const JntArrayVel q_in,
FrameVel out,
int  segmentNr = -1 
)
virtual

Calculate forward position and velocity kinematics, from joint coordinates to cartesian coordinates.

Parameters
q_ininput joint coordinates (position and velocity)
outoutput cartesian coordinates (position and velocity)
Returns
if < 0 something went wrong

Implements KDL::ChainFkSolverVel.

References chain, KDL::SolverI::E_NOERROR, KDL::SolverI::E_OUT_OF_RANGE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, KDL::Segment::getJoint(), KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), KDL::Chain::getSegment(), KDL::Joint::getType(), KDL::FrameVel::Identity(), KDL::Joint::None, KDL::Segment::pose(), KDL::JntArrayVel::q, KDL::JntArrayVel::qdot, KDL::JntArray::rows(), and KDL::Segment::twist().

§ JntToCart() [2/2]

int KDL::ChainFkSolverVel_recursive::JntToCart ( const JntArrayVel q_in,
std::vector< FrameVel > &  out,
int  segmentNr = -1 
)
virtual

Calculate forward position and velocity kinematics, from joint coordinates to cartesian coordinates.

Parameters
q_ininput joint coordinates (position and velocity)
outoutput cartesian coordinates for all segments (position and velocity)
Returns
if < 0 something went wrong

Implements KDL::ChainFkSolverVel.

References chain, KDL::Segment::getJoint(), KDL::Chain::getNrOfJoints(), KDL::Chain::getNrOfSegments(), KDL::Chain::getSegment(), KDL::Joint::getType(), KDL::Joint::None, KDL::Segment::pose(), KDL::JntArrayVel::q, KDL::JntArrayVel::qdot, KDL::JntArray::rows(), and KDL::Segment::twist().

§ strError()

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

§ updateInternalDataStructures()

virtual void KDL::ChainFkSolverVel_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::ChainFkSolverVel.

References chain.

Member Data Documentation

§ chain

const Chain& KDL::ChainFkSolverVel_recursive::chain
private

§ error

int KDL::SolverI::error
protectedinherited

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