KDL  1.3.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 }
 

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 int getError () const
 Return the latest error. More...
 

Static Public Attributes

static const int E_SVD_FAILED = -100
 
static const int E_CONVERGE_PINV_SINGULAR = +100
 Child SVD failed. More...
 

Protected Attributes

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

Private Attributes

const Chain chain
 
ChainJntToJacSolver jnt2jac
 
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) )

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 KDL::Jacobian::columns(), KDL::JntArray::data, E_CONVERGE_PINV_SINGULAR, KDL::SolverI::E_NOERROR, E_SVD_FAILED, eps, KDL::SolverI::error, jac, jnt2jac, KDL::ChainJntToJacSolver::JntToJac(), maxiter, nrZeroSigmas, KDL::Jacobian::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(), and svdResult.

§ 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_SVD_FAILED, and KDL::SolverI::strError().

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

Child SVD failed.

solution converged but (pseudo)inverse is singular

Referenced by CartToJnt().

§ E_SVD_FAILED

const int KDL::ChainIkSolverVel_pinv::E_SVD_FAILED = -100
static

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

Referenced by CartToJnt().

§ jnt2jac

ChainJntToJacSolver KDL::ChainIkSolverVel_pinv::jnt2jac
private

Referenced by CartToJnt().

§ maxiter

int KDL::ChainIkSolverVel_pinv::maxiter
private

Referenced by CartToJnt().

§ nrZeroSigmas

unsigned int KDL::ChainIkSolverVel_pinv::nrZeroSigmas
private

Referenced by CartToJnt(), and getNrZeroSigmas().

§ S

JntArray KDL::ChainIkSolverVel_pinv::S
private

Referenced by CartToJnt().

§ svd

SVD_HH KDL::ChainIkSolverVel_pinv::svd
private

Referenced by CartToJnt().

§ svdResult

int KDL::ChainIkSolverVel_pinv::svdResult
private

Referenced by CartToJnt(), and getSVDResult().

§ tmp

JntArray KDL::ChainIkSolverVel_pinv::tmp
private

Referenced by CartToJnt().

§ U

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

Referenced by CartToJnt().

§ V

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

Referenced by CartToJnt().


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