#include <chainiksolvervel_wdls.hpp>

Inheritance diagram for KDL::ChainIkSolverVel_wdls:

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

Static Public Attributes
static const int	E_CONVERGE_PINV_SINGULAR = +100
	solution converged but (pseudo)inverse is singular

Additional Inherited Members
Public Types inherited from KDL::SolverI
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 }
Protected Attributes inherited from KDL::SolverI
int	error
	Latest error, initialized to E_NOERROR in constructor.

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.

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

chain	the chain to calculate the inverse velocity kinematics for
eps	if a singular value is below this value, its inverse is set to zero, default: 0.00001
maxiter	maximum iterations for the svd calculation, default: 150

◆ ~ChainIkSolverVel_wdls()

KDL::ChainIkSolverVel_wdls::~ChainIkSolverVel_wdls ( )

Member Function Documentation

◆ CartToJnt() [1/2]

virtual int KDL::ChainIkSolverVel_wdls::CartToJnt	(	const JntArray &	,
		const FrameVel &	,
		JntArrayVel &	)

inlinevirtual

not (yet) implemented.

Implements KDL::ChainIkSolverVel.

◆ CartToJnt() [2/2]

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

◆ getEps()

double KDL::ChainIkSolverVel_wdls::getEps ( ) const

inline

Request the value of eps

◆ getLambda()

double KDL::ChainIkSolverVel_wdls::getLambda ( ) const

inline

Request the value of lambda for the minimum

◆ getLambdaScaled()

double KDL::ChainIkSolverVel_wdls::getLambdaScaled ( ) const

inline

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

◆ 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

◆ getSigma()

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

Request the six singular values of the Jacobian

◆ getSigmaMin()

double KDL::ChainIkSolverVel_wdls::getSigmaMin ( ) const

inline

Request the minimum of the first six singular values

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

◆ setEps()

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

Set eps

◆ setLambda()

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

Set lambda

◆ setMaxIter()

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

Set maxIter

◆ setWeightJS()

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

Set the joint space weighting matrix

Parameters

weight_js joint 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

◆ setWeightTS()

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

Set the task space weighting matrix

Parameters

weight_ts task 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

◆ strError()

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

◆ updateInternalDataStructures()

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

Member Data Documentation

◆ E_CONVERGE_PINV_SINGULAR

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

static

solution converged but (pseudo)inverse is singular

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

build/orocos_kdl_install/include/kdl/chainiksolvervel_wdls.hpp

Public Member Functions

Static Public Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ ChainIkSolverVel_wdls()

◆ ~ChainIkSolverVel_wdls()

Member Function Documentation

◆ CartToJnt() [1/2]

◆ CartToJnt() [2/2]

◆ getEps()

◆ getLambda()

◆ getLambdaScaled()

◆ getNrZeroSigmas()

◆ getSigma()

◆ getSigmaMin()

◆ getSVDResult()

◆ setEps()

◆ setLambda()

◆ setMaxIter()

◆ setWeightJS()

◆ setWeightTS()

◆ strError()

◆ updateInternalDataStructures()

Member Data Documentation

◆ E_CONVERGE_PINV_SINGULAR