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

#include <chainiksolvervel_wdls.hpp>

Inheritance diagram for KDL::ChainIkSolverVel_wdls:
Inheritance graph

Public Member Functions

virtual int CartToJnt (const JntArray &q_in, const Twist &v_in, JntArray &qdot_out)
virtual int CartToJnt (const JntArray &q_init, const FrameVel &v_in, JntArrayVel &q_out)
 ChainIkSolverVel_wdls (const Chain &chain, double eps=0.00001, int maxiter=150)
double getEps () const
double getLambda () const
double getLambdaScaled () const
unsigned int getNrZeroSigmas () const
int getSigma (Eigen::VectorXd &Sout)
double getSigmaMin () const
int getSVDResult () const
void setEps (const double eps_in)
void setLambda (const double lambda)
void setMaxIter (const int maxiter_in)
int setWeightJS (const Eigen::MatrixXd &Mq)
int setWeightTS (const Eigen::MatrixXd &Mx)
virtual const char * strError (const int error) const
virtual void updateInternalDataStructures ()
 ~ChainIkSolverVel_wdls ()
- Public Member Functions inherited from KDL::ChainIkSolverVel
virtual ~ChainIkSolverVel ()
- Public Member Functions inherited from KDL::SolverI
virtual int getError () const
 Return the latest error. More...
 SolverI ()
 Initialize latest error to E_NOERROR. More...
virtual ~SolverI ()

Static Public Attributes

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

Private Attributes

const Chainchain
double eps
Jacobian jac
ChainJntToJacSolver jnt2jac
double lambda
double lambda_scaled
int maxiter
unsigned int nj
unsigned int nrZeroSigmas
Eigen::VectorXd S
double sigmaMin
int svdResult
Eigen::VectorXd tmp
Eigen::MatrixXd tmp_jac
Eigen::MatrixXd tmp_jac_weight1
Eigen::MatrixXd tmp_jac_weight2
Eigen::MatrixXd tmp_js
Eigen::MatrixXd tmp_ts
Eigen::MatrixXd U
Eigen::MatrixXd V
Eigen::MatrixXd weight_js
Eigen::MatrixXd weight_ts

Additional Inherited Members

- Public Types inherited from KDL::SolverI
enum  {
- Protected Attributes inherited from KDL::SolverI
int error
 Latest error, initialized to E_NOERROR in constructor. More...

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.

Definition at line 63 of file chainiksolvervel_wdls.hpp.

Constructor & Destructor Documentation

KDL::ChainIkSolverVel_wdls::ChainIkSolverVel_wdls ( const Chain chain,
double  eps = 0.00001,
int  maxiter = 150 

Constructor of the solver

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

Definition at line 28 of file chainiksolvervel_wdls.cpp.

KDL::ChainIkSolverVel_wdls::~ChainIkSolverVel_wdls ( )

Definition at line 72 of file chainiksolvervel_wdls.cpp.

Member Function Documentation

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

E_NOERROR=svd solution converged in maxiter E_SVD_FAILED=svd solution failed E_CONVERGE_PINV_SINGULAR=svd solution converged but (pseudo)inverse singular
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.

Definition at line 116 of file chainiksolvervel_wdls.cpp.

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

not (yet) implemented.

Implements KDL::ChainIkSolverVel.

Definition at line 105 of file chainiksolvervel_wdls.hpp.

double KDL::ChainIkSolverVel_wdls::getEps ( ) const

Request the value of eps

Definition at line 193 of file chainiksolvervel_wdls.hpp.

double KDL::ChainIkSolverVel_wdls::getLambda ( ) const

Request the value of lambda for the minimum

Definition at line 198 of file chainiksolvervel_wdls.hpp.

double KDL::ChainIkSolverVel_wdls::getLambdaScaled ( ) const

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

Definition at line 204 of file chainiksolvervel_wdls.hpp.

unsigned int KDL::ChainIkSolverVel_wdls::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

Definition at line 178 of file chainiksolvervel_wdls.hpp.

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

Request the six singular values of the Jacobian

Definition at line 108 of file chainiksolvervel_wdls.cpp.

double KDL::ChainIkSolverVel_wdls::getSigmaMin ( ) const

Request the minimum of the first six singular values

Definition at line 183 of file chainiksolvervel_wdls.hpp.

int KDL::ChainIkSolverVel_wdls::getSVDResult ( ) const

Retrieve the latest return code from the SVD algorithm

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

Definition at line 210 of file chainiksolvervel_wdls.hpp.

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

Set eps

Definition at line 98 of file chainiksolvervel_wdls.cpp.

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

Set lambda

Definition at line 93 of file chainiksolvervel_wdls.cpp.

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

Set maxIter

Definition at line 103 of file chainiksolvervel_wdls.cpp.

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

Set the joint space weighting matrix

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 :
success/error code

Definition at line 76 of file chainiksolvervel_wdls.cpp.

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

Set the task space weighting matrix

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 :
success/error code

Definition at line 86 of file chainiksolvervel_wdls.cpp.

const char * KDL::ChainIkSolverVel_wdls::strError ( const int  error) const

Return a description of the latest error

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

Reimplemented from KDL::SolverI.

Definition at line 218 of file chainiksolvervel_wdls.cpp.

void KDL::ChainIkSolverVel_wdls::updateInternalDataStructures ( )

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.

Definition at line 54 of file chainiksolvervel_wdls.cpp.

Member Data Documentation

const Chain& KDL::ChainIkSolverVel_wdls::chain

Definition at line 219 of file chainiksolvervel_wdls.hpp.

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

solution converged but (pseudo)inverse is singular

Definition at line 67 of file chainiksolvervel_wdls.hpp.

double KDL::ChainIkSolverVel_wdls::eps

Definition at line 226 of file chainiksolvervel_wdls.hpp.

Jacobian KDL::ChainIkSolverVel_wdls::jac

Definition at line 222 of file chainiksolvervel_wdls.hpp.

ChainJntToJacSolver KDL::ChainIkSolverVel_wdls::jnt2jac

Definition at line 220 of file chainiksolvervel_wdls.hpp.

double KDL::ChainIkSolverVel_wdls::lambda

Definition at line 236 of file chainiksolvervel_wdls.hpp.

double KDL::ChainIkSolverVel_wdls::lambda_scaled

Definition at line 237 of file chainiksolvervel_wdls.hpp.

int KDL::ChainIkSolverVel_wdls::maxiter

Definition at line 227 of file chainiksolvervel_wdls.hpp.

unsigned int KDL::ChainIkSolverVel_wdls::nj

Definition at line 221 of file chainiksolvervel_wdls.hpp.

unsigned int KDL::ChainIkSolverVel_wdls::nrZeroSigmas

Definition at line 238 of file chainiksolvervel_wdls.hpp.

Eigen::VectorXd KDL::ChainIkSolverVel_wdls::S

Definition at line 224 of file chainiksolvervel_wdls.hpp.

double KDL::ChainIkSolverVel_wdls::sigmaMin

Definition at line 240 of file chainiksolvervel_wdls.hpp.

int KDL::ChainIkSolverVel_wdls::svdResult

Definition at line 239 of file chainiksolvervel_wdls.hpp.

Eigen::VectorXd KDL::ChainIkSolverVel_wdls::tmp

Definition at line 228 of file chainiksolvervel_wdls.hpp.

Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::tmp_jac

Definition at line 229 of file chainiksolvervel_wdls.hpp.

Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::tmp_jac_weight1

Definition at line 230 of file chainiksolvervel_wdls.hpp.

Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::tmp_jac_weight2

Definition at line 231 of file chainiksolvervel_wdls.hpp.

Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::tmp_js

Definition at line 233 of file chainiksolvervel_wdls.hpp.

Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::tmp_ts

Definition at line 232 of file chainiksolvervel_wdls.hpp.

Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::U

Definition at line 223 of file chainiksolvervel_wdls.hpp.

Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::V

Definition at line 225 of file chainiksolvervel_wdls.hpp.

Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::weight_js

Definition at line 235 of file chainiksolvervel_wdls.hpp.

Eigen::MatrixXd KDL::ChainIkSolverVel_wdls::weight_ts

Definition at line 234 of file chainiksolvervel_wdls.hpp.

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

autogenerated on Wed Mar 6 2019 03:07:25