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

Implementation of a inverse velocity kinematics algorithm based on the generalize pseudo inverse to calculate the velocity transformation from Cartesian to joint space of a general KDL::Chain. More...

#include <src/chainiksolvervel_pinv.hpp>

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

 ChainIkSolverVel_pinv (const Chain &chain, double eps=0.00001, int maxiter=150)
 Constructor of the solver. More...
 
 ~ChainIkSolverVel_pinv ()
 
virtual int CartToJnt (const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
 Find an output joint velocity qdot_out, given a starting joint pose q_init and a desired cartesian velocity v_in. More...
 
virtual int CartToJnt (const JntArray &q_init, const FrameVel &v_in, JntArrayVel &q_out)
 not (yet) implemented. More...
 
unsigned int getNrZeroSigmas () const
 Retrieve the number of singular values of the jacobian that are < eps; if the number of near zero singular values is > jac.col()-jac.row(), then the jacobian pseudoinverse is singular. More...
 
int getSVDResult () const
 Retrieve the latest return code from the SVD algorithm. More...
 
virtual const char * strError (const int error) const
 Return a description of the latest error. More...
 
virtual void updateInternalDataStructures ()
 Update the internal data structures. More...
 
virtual int getError () const
 Return the latest error. More...
 

Static Public Attributes

static const int E_CONVERGE_PINV_SINGULAR = +100
 solution converged but (pseudo)inverse is singular More...
 

Protected Attributes

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

Private Attributes

const Chainchain
 
ChainJntToJacSolver jnt2jac
 
unsigned int nj
 
Jacobian jac
 
SVD_HH svd
 
std::vector< JntArrayU
 
JntArray S
 
std::vector< JntArrayV
 
JntArray tmp
 
double eps
 
int maxiter
 
unsigned int nrZeroSigmas
 
int svdResult
 

Detailed Description

Implementation of a inverse velocity kinematics algorithm based on the generalize pseudo inverse to calculate the velocity transformation from Cartesian to joint space of a general KDL::Chain.

It uses a svd-calculation based on householders rotations.

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

§ ChainIkSolverVel_pinv()

KDL::ChainIkSolverVel_pinv::ChainIkSolverVel_pinv ( const Chain chain,
double  eps = 0.00001,
int  maxiter = 150 
)
explicit

Constructor of the solver.

Parameters
chainthe chain to calculate the inverse velocity kinematics for
epsif a singular value is below this value, its inverse is set to zero, default: 0.00001
maxitermaximum iterations for the svd calculation, default: 150

§ ~ChainIkSolverVel_pinv()

KDL::ChainIkSolverVel_pinv::~ChainIkSolverVel_pinv ( )

Member Function Documentation

§ CartToJnt() [1/2]

int KDL::ChainIkSolverVel_pinv::CartToJnt ( const JntArray q_in,
const Twist v_in,
JntArray qdot_out 
)
virtual

Find an output joint velocity qdot_out, given a starting joint pose q_init and a desired cartesian velocity v_in.

Returns
E_NOERROR=solution converged to <eps in maxiter E_SVD_FAILED=SVD computation failed E_CONVERGE_PINV_SINGULAR=solution converged but (pseudo)inverse is singular
Note
if E_CONVERGE_PINV_SINGULAR returned then converged and can continue motion, but have degraded solution
If E_SVD_FAILED returned, then getSvdResult() returns the error code from the SVD algorithm.

Implements KDL::ChainIkSolverVel.

References chain, KDL::Jacobian::columns(), KDL::JntArray::data, E_CONVERGE_PINV_SINGULAR, KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::E_SVD_FAILED, eps, KDL::SolverI::error, KDL::Chain::getNrOfJoints(), jac, jnt2jac, KDL::ChainJntToJacSolver::JntToJac(), maxiter, nj, nrZeroSigmas, KDL::Jacobian::rows(), KDL::JntArray::rows(), S, svd, svdResult, tmp, U, and V.

§ CartToJnt() [2/2]

virtual int KDL::ChainIkSolverVel_pinv::CartToJnt ( const JntArray q_init,
const FrameVel v_in,
JntArrayVel q_out 
)
inlinevirtual

not (yet) implemented.

Implements KDL::ChainIkSolverVel.

§ getError()

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

Return the latest error.

References KDL::SolverI::error.

§ getNrZeroSigmas()

unsigned int KDL::ChainIkSolverVel_pinv::getNrZeroSigmas ( ) const
inline

Retrieve the number of singular values of the jacobian that are < eps; if the number of near zero singular values is > jac.col()-jac.row(), then the jacobian pseudoinverse is singular.

References nrZeroSigmas.

§ getSVDResult()

int KDL::ChainIkSolverVel_pinv::getSVDResult ( ) const
inline

Retrieve the latest return code from the SVD algorithm.

Returns
0 if CartToJnt() not yet called, otherwise latest SVD result code.

References KDL::SolverI::error, strError(), svdResult, and updateInternalDataStructures().

§ strError()

const char * KDL::ChainIkSolverVel_pinv::strError ( const int  error) const
virtual

Return a description of the latest error.

Returns
if error is known then a description of error, otherwise "UNKNOWN ERROR"

Reimplemented from KDL::SolverI.

References E_CONVERGE_PINV_SINGULAR, and KDL::SolverI::strError().

Referenced by getSVDResult().

§ updateInternalDataStructures()

void KDL::ChainIkSolverVel_pinv::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::ChainIkSolverVel.

References chain, KDL::Chain::getNrOfJoints(), jac, jnt2jac, nj, KDL::Jacobian::resize(), KDL::JntArray::resize(), S, svd, tmp, U, KDL::ChainJntToJacSolver::updateInternalDataStructures(), and V.

Referenced by getSVDResult().

Member Data Documentation

§ chain

const Chain& KDL::ChainIkSolverVel_pinv::chain
private

§ E_CONVERGE_PINV_SINGULAR

const int KDL::ChainIkSolverVel_pinv::E_CONVERGE_PINV_SINGULAR = +100
static

solution converged but (pseudo)inverse is singular

Referenced by CartToJnt(), and strError().

§ eps

double KDL::ChainIkSolverVel_pinv::eps
private

Referenced by CartToJnt().

§ error

int KDL::SolverI::error
protectedinherited

§ jac

Jacobian KDL::ChainIkSolverVel_pinv::jac
private

§ jnt2jac

ChainJntToJacSolver KDL::ChainIkSolverVel_pinv::jnt2jac
private

§ maxiter

int KDL::ChainIkSolverVel_pinv::maxiter
private

Referenced by CartToJnt().

§ nj

unsigned int KDL::ChainIkSolverVel_pinv::nj
private

§ nrZeroSigmas

unsigned int KDL::ChainIkSolverVel_pinv::nrZeroSigmas
private

Referenced by CartToJnt(), and getNrZeroSigmas().

§ S

JntArray KDL::ChainIkSolverVel_pinv::S
private

§ svd

SVD_HH KDL::ChainIkSolverVel_pinv::svd
private

§ svdResult

int KDL::ChainIkSolverVel_pinv::svdResult
private

Referenced by CartToJnt(), and getSVDResult().

§ tmp

JntArray KDL::ChainIkSolverVel_pinv::tmp
private

§ U

std::vector<JntArray> KDL::ChainIkSolverVel_pinv::U
private

§ V

std::vector<JntArray> KDL::ChainIkSolverVel_pinv::V
private

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