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

Implementation of a inverse velocity kinematics algorithm based on the weighted pseudo inverse with damped least-square to calculate the velocity transformation from Cartesian to joint space of a general KDL::Chain. More...

#include <src/chainiksolvervel_wdls.hpp>

Inheritance diagram for KDL::ChainIkSolverVel_wdls:
Inheritance graph
[legend]
Collaboration diagram for KDL::ChainIkSolverVel_wdls:
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_wdls (const Chain &chain, double eps=0.00001, int maxiter=150)
 Constructor of the solver. More...
 
 ~ChainIkSolverVel_wdls ()
 
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...
 
int setWeightJS (const Eigen::MatrixXd &Mq)
 Set the joint space weighting matrix. More...
 
int setWeightTS (const Eigen::MatrixXd &Mx)
 Set the task space weighting matrix. More...
 
void setLambda (const double lambda)
 Set lambda. More...
 
void setEps (const double eps_in)
 Set eps. More...
 
void setMaxIter (const int maxiter_in)
 Set maxIter. More...
 
unsigned int getNrZeroSigmas () const
 Request 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...
 
double getSigmaMin () const
 Request the minimum of the first six singular values. More...
 
int getSigma (Eigen::VectorXd &Sout)
 Request the six singular values of the Jacobian. More...
 
double getEps () const
 Request the value of eps. More...
 
double getLambda () const
 Request the value of lambda for the minimum. More...
 
double getLambdaScaled () const
 Request the scaled value of lambda for the minimum singular value 1-6. 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
 
Eigen::MatrixXd U
 
Eigen::VectorXd S
 
Eigen::MatrixXd V
 
double eps
 
int maxiter
 
Eigen::VectorXd tmp
 
Eigen::MatrixXd tmp_jac
 
Eigen::MatrixXd tmp_jac_weight1
 
Eigen::MatrixXd tmp_jac_weight2
 
Eigen::MatrixXd tmp_ts
 
Eigen::MatrixXd tmp_js
 
Eigen::MatrixXd weight_ts
 
Eigen::MatrixXd weight_js
 
double lambda
 
double lambda_scaled
 
unsigned int nrZeroSigmas
 
int svdResult
 
double sigmaMin
 

Detailed Description

Implementation of a inverse velocity kinematics algorithm based on the weighted pseudo inverse with damped least-square to calculate the velocity transformation from Cartesian to joint space of a general KDL::Chain.

It uses a svd-calculation based on householders rotations.

J# = M_q*Vb*pinv_dls(Db)*Ub'*M_x

where B = Mx*J*Mq

and B = Ub*Db*Vb' is the SVD decomposition of B

Mq and Mx represent, respectively, the joint-space and task-space weighting matrices. Please refer to the documentation of setWeightJS(const Eigen::MatrixXd& Mq) and setWeightTS(const Eigen::MatrixXd& Mx) for details on the effects of these matrices.

For more details on Weighted Pseudo Inverse, see : 1) [Ben Israel 03] A. Ben Israel & T.N.E. Greville. Generalized Inverses : Theory and Applications, second edition. Springer, 2003. ISBN 0-387-00293-6.

2) [Doty 93] K. L. Doty, C. Melchiorri & C. Boniveto. A theory of generalized inverses applied to Robotics. The International Journal of Robotics Research, vol. 12, no. 1, pages 1-19, february 1993.

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_wdls()

KDL::ChainIkSolverVel_wdls::ChainIkSolverVel_wdls ( 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_wdls()

KDL::ChainIkSolverVel_wdls::~ChainIkSolverVel_wdls ( )

Member Function Documentation

§ CartToJnt() [1/2]

int KDL::ChainIkSolverVel_wdls::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=svd solution converged in maxiter E_SVD_FAILED=svd solution failed E_CONVERGE_PINV_SINGULAR=svd solution converged but (pseudo)inverse 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::Jacobian::data, 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(), lambda, lambda_scaled, maxiter, nj, nrZeroSigmas, KDL::Jacobian::rows(), KDL::JntArray::rows(), S, sigmaMin, svdResult, tmp, tmp_jac_weight1, tmp_jac_weight2, tmp_js, tmp_ts, U, V, weight_js, and weight_ts.

§ CartToJnt() [2/2]

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

not (yet) implemented.

Implements KDL::ChainIkSolverVel.

References lambda, setEps(), setLambda(), setMaxIter(), setWeightJS(), and setWeightTS().

§ getEps()

double KDL::ChainIkSolverVel_wdls::getEps ( ) const
inline

Request the value of eps.

References eps.

§ getError()

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

Return the latest error.

References KDL::SolverI::error.

§ getLambda()

double KDL::ChainIkSolverVel_wdls::getLambda ( ) const
inline

Request the value of lambda for the minimum.

References lambda.

§ getLambdaScaled()

double KDL::ChainIkSolverVel_wdls::getLambdaScaled ( ) const
inline

Request the scaled value of lambda for the minimum singular value 1-6.

References lambda_scaled.

§ getNrZeroSigmas()

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

Request 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.

§ getSigma()

int KDL::ChainIkSolverVel_wdls::getSigma ( Eigen::VectorXd &  Sout)

Request the six singular values of the Jacobian.

References KDL::SolverI::E_NOERROR, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, and S.

Referenced by getSigmaMin().

§ getSigmaMin()

double KDL::ChainIkSolverVel_wdls::getSigmaMin ( ) const
inline

Request the minimum of the first six singular values.

References getSigma(), and sigmaMin.

§ getSVDResult()

int KDL::ChainIkSolverVel_wdls::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().

§ setEps()

void KDL::ChainIkSolverVel_wdls::setEps ( const double  eps_in)

Set eps.

References eps.

Referenced by CartToJnt().

§ setLambda()

void KDL::ChainIkSolverVel_wdls::setLambda ( const double  lambda)

Set lambda.

References lambda.

Referenced by CartToJnt().

§ setMaxIter()

void KDL::ChainIkSolverVel_wdls::setMaxIter ( const int  maxiter_in)

Set maxIter.

References maxiter.

Referenced by CartToJnt().

§ setWeightJS()

int KDL::ChainIkSolverVel_wdls::setWeightJS ( const Eigen::MatrixXd &  Mq)

Set the joint space weighting matrix.

Parameters
weight_jsjoint space weighting symmetric matrix, default : identity. M_q : This matrix being used as a weight for the norm of the joint space speed it HAS TO BE symmetric and positive definite. We can actually deal with matrices containing a symmetric and positive definite block and 0s otherwise. Taking a diagonal matrix as an example, a 0 on the diagonal means that the corresponding joints will not contribute to the motion of the system. On the other hand, the bigger the value, the most the corresponding joint will contribute to the overall motion. The obtained solution q_dot will actually minimize the weighted norm sqrt(q_dot'*(M_q^-2)*q_dot). In the special case we deal with, it does not make sense to invert M_q but what is important is the physical meaning of all this : a joint that has a zero weight in M_q will not contribute to the motion of the system and this is equivalent to saying that it gets an infinite weight in the norm computation. For more detailed explanation : vince.nosp@m.nt.p.nosp@m.adois.nosp@m.@upm.nosp@m.c.fr
Returns
success/error code

References chain, KDL::SolverI::E_NOERROR, KDL::SolverI::E_NOT_UP_TO_DATE, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, KDL::Chain::getNrOfJoints(), nj, and weight_js.

Referenced by CartToJnt().

§ setWeightTS()

int KDL::ChainIkSolverVel_wdls::setWeightTS ( const Eigen::MatrixXd &  Mx)

Set the task space weighting matrix.

Parameters
weight_tstask space weighting symmetric matrix, default: identity M_x : This matrix being used as a weight for the norm of the error (in terms of task space speed) it HAS TO BE symmetric and positive definite. We can actually deal with matrices containing a symmetric and positive definite block and 0s otherwise. Taking a diagonal matrix as an example, a 0 on the diagonal means that the corresponding task coordinate will not be taken into account (ie the corresponding error can be really big). If the rank of the jacobian is equal to the number of task space coordinates which do not have a 0 weight in M_x, the weighting will actually not impact the results (ie there is an exact solution to the velocity inverse kinematics problem). In cases without an exact solution, the bigger the value, the most the corresponding task coordinate will be taken into account (ie the more the corresponding error will be reduced). The obtained solution will minimize the weighted norm sqrt(|x_dot-Jq_dot|'*(M_x^2)*|x_dot-Jq_dot|). For more detailed explanation : vince.nosp@m.nt.p.nosp@m.adois.nosp@m.@upm.nosp@m.c.fr
Returns
success/error code

References KDL::SolverI::E_NOERROR, KDL::SolverI::E_SIZE_MISMATCH, KDL::SolverI::error, and weight_ts.

Referenced by CartToJnt().

§ strError()

const char * KDL::ChainIkSolverVel_wdls::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_wdls::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(), S, tmp, tmp_jac, tmp_jac_weight1, tmp_jac_weight2, tmp_js, U, KDL::ChainJntToJacSolver::updateInternalDataStructures(), V, and weight_js.

Referenced by getSVDResult().

Member Data Documentation

§ chain

const Chain& KDL::ChainIkSolverVel_wdls::chain
private

§ E_CONVERGE_PINV_SINGULAR

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

solution converged but (pseudo)inverse is singular

Referenced by CartToJnt(), and strError().

§ eps

double KDL::ChainIkSolverVel_wdls::eps
private

Referenced by CartToJnt(), getEps(), and setEps().

§ error

int KDL::SolverI::error
protectedinherited

§ jac

Jacobian KDL::ChainIkSolverVel_wdls::jac
private

§ jnt2jac

ChainJntToJacSolver KDL::ChainIkSolverVel_wdls::jnt2jac
private

§ lambda

double KDL::ChainIkSolverVel_wdls::lambda
private

Referenced by CartToJnt(), getLambda(), and setLambda().

§ lambda_scaled

double KDL::ChainIkSolverVel_wdls::lambda_scaled
private

Referenced by CartToJnt(), and getLambdaScaled().

§ maxiter

int KDL::ChainIkSolverVel_wdls::maxiter
private

Referenced by CartToJnt(), and setMaxIter().

§ nj

unsigned int KDL::ChainIkSolverVel_wdls::nj
private

§ nrZeroSigmas

unsigned int KDL::ChainIkSolverVel_wdls::nrZeroSigmas
private

Referenced by CartToJnt(), and getNrZeroSigmas().

§ S

Eigen::VectorXd KDL::ChainIkSolverVel_wdls::S
private

§ sigmaMin

double KDL::ChainIkSolverVel_wdls::sigmaMin
private

Referenced by CartToJnt(), and getSigmaMin().

§ svdResult

int KDL::ChainIkSolverVel_wdls::svdResult
private

Referenced by CartToJnt(), and getSVDResult().

§ tmp

Eigen::VectorXd KDL::ChainIkSolverVel_wdls::tmp
private

§ tmp_jac

Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::tmp_jac
private

§ tmp_jac_weight1

Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::tmp_jac_weight1
private

§ tmp_jac_weight2

Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::tmp_jac_weight2
private

§ tmp_js

Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::tmp_js
private

§ tmp_ts

Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::tmp_ts
private

Referenced by CartToJnt().

§ U

Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::U
private

§ V

Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::V
private

§ weight_js

Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::weight_js
private

§ weight_ts

Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::weight_ts
private

Referenced by CartToJnt(), and setWeightTS().


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