Solver for the inverse position kinematics that uses Levenberg-Marquardt. More...

#include <src/chainiksolverpos_lma.hpp>

Inheritance diagram for KDL::ChainIkSolverPos_LMA:

Collaboration diagram for KDL::ChainIkSolverPos_LMA:

[legend]

Public Types
enum	{ E_DEGRADED = +1, E_NOERROR = 0, E_NO_CONVERGE = -1, E_UNDEFINED = -2 }

Public Member Functions
	ChainIkSolverPos_LMA (const KDL::Chain &_chain, const Eigen::Matrix< double, 6, 1 > &_L, double _eps=1E-5, int _maxiter=500, double _eps_joints=1E-15)
	constructs an ChainIkSolverPos_LMA solver. More...

	ChainIkSolverPos_LMA (const KDL::Chain &_chain, double _eps=1E-5, int _maxiter=500, double _eps_joints=1E-15)
	identical the full constructor for ChainIkSolverPos_LMA, but provides for a default weight matrix. More...

virtual int	CartToJnt (const KDL::JntArray &q_init, const KDL::Frame &T_base_goal, KDL::JntArray &q_out)
	computes the inverse position kinematics. More...

virtual	~ChainIkSolverPos_LMA ()
	destructor. More...

void	compute_fwdpos (const VectorXq &q)
	for internal use only. More...

void	compute_jacobian (const VectorXq &q)
	for internal use only. More...

void	display_jac (const KDL::JntArray &jval)
	for internal use only. More...

virtual int	getError () const
	Return the latest error. More...

virtual const char *	strError (const int error) const
	Return a description of the latest error. More...

Public Attributes
int	lastNrOfIter
	contains the last number of iterations for an execution of CartToJnt. More...

double	lastDifference
	contains the last value for \( E \) after an execution of CartToJnt. More...

double	lastTransDiff
	contains the last value for the (unweighted) translational difference after an execution of CartToJnt. More...

double	lastRotDiff
	contains the last value for the (unweighted) rotational difference after an execution of CartToJnt. More...

VectorXq	lastSV
	contains the last values for the singular values of the weighted Jacobian after an execution of CartToJnt. More...

MatrixXq	jac
	for internal use only. More...

VectorXq	grad
	for internal use only. More...

KDL::Frame	T_base_head
	for internal use only. More...

bool	display_information
	display information on each iteration step to the console. More...

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

Private Types
typedef double	ScalarType

typedef Eigen::Matrix< ScalarType, Eigen::Dynamic, Eigen::Dynamic >	MatrixXq

typedef Eigen::Matrix< ScalarType, Eigen::Dynamic, 1 >	VectorXq

Private Attributes
unsigned int	maxiter

double	eps

double	eps_joints

Eigen::Matrix< ScalarType, 6, 1 >	L

const KDL::Chain &	chain

std::vector< KDL::Frame >	T_base_jointroot

std::vector< KDL::Frame >	T_base_jointtip

VectorXq	q

MatrixXq	A

VectorXq	tmp

Eigen::LDLT< MatrixXq >	ldlt

Eigen::JacobiSVD< MatrixXq >	svd

VectorXq	diffq

VectorXq	q_new

VectorXq	original_Aii

Detailed Description

Solver for the inverse position kinematics that uses Levenberg-Marquardt.

The robustness and speed of this solver is improved in several ways:

by using a Levenberg-Marquardt method that automatically adapts the damping when computing the inverse damped least squares inverse velocity kinematics.
by using an internal implementation of forward position kinematics and the Jacobian kinematics. This implementation is more numerically robust, is able to cache previous computations, and implements an \( \mathcal{O}(N) \) algorithm for the computation of the Jacobian (with \(N\), the number of joints, and for a fixed size task space).
by providing a way to specify the weights in task space, you can weigh rotations wrt translations. This is important e.g. to specify that rotations do not matter for the problem at hand, or to specify how important you judge rotations w.r.t. translations, typically in S.I.-units, ([m],[rad]), the rotations are over-specified, this can be avoided using the weight matrix. Weights also make the solver more robust .
only the constructors call memory allocation.

De general principles behind the optimisation is inspired on: Jorge Nocedal, Stephen J. Wright, Numerical Optimization,Springer-Verlag New York, 1999.

Member Typedef Documentation

typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,Eigen::Dynamic> KDL::ChainIkSolverPos_LMA::MatrixXq

private

typedef double KDL::ChainIkSolverPos_LMA::ScalarType

private

typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,1> KDL::ChainIkSolverPos_LMA::VectorXq

private

Member Enumeration Documentation

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

KDL::ChainIkSolverPos_LMA::ChainIkSolverPos_LMA	(	const KDL::Chain &	_chain,
		const Eigen::Matrix< double, 6, 1 > &	_L,
		double	_eps = `1E-5`,
		int	_maxiter = `500`,
		double	_eps_joints = `1E-15`
	)

constructs an ChainIkSolverPos_LMA solver.

The default parameters are choosen to be applicable to industrial-size robots (e.g. 0.5 to 3 meters range in task space), with an accuracy that is more then sufficient for typical industrial applications.

Weights are applied in task space, i.e. the kinematic solver minimizes: \( E = \Delta \mathbf{x}^T \mathbf{L} \mathbf{L}^T \Delta \mathbf{x} \), with \(\mathbf{L}\) a diagonal matrix.

Parameters

_chain	specifies the kinematic chain.
_L	specifies the "square root" of the weight (diagonal) matrix in task space. This diagonal matrix is specified as a vector.
_eps	specifies the desired accuracy in task space; after weighing with the weight matrix, it is applied on \(E\).
_maxiter	specifies the maximum number of iterations.
_eps_joints	specifies that the algorithm has to stop when the computed joint angle increments are smaller then _eps_joints. This is to avoid unnecessary computations up to _maxiter when the joint angle increments are so small that they effectively (in floating point) do not change the joint angles any more. The default is a few digits above numerical accuracy.

KDL::ChainIkSolverPos_LMA::ChainIkSolverPos_LMA	(	const KDL::Chain &	_chain,
		double	_eps = `1E-5`,
		int	_maxiter = `500`,
		double	_eps_joints = `1E-15`
	)

identical the full constructor for ChainIkSolverPos_LMA, but provides for a default weight matrix.

\(\mathbf{L} = \mathrm{diag}\left( \begin{bmatrix} 1 & 1 & 1 & 0.01 & 0.01 & 0.01 \end{bmatrix} \right) \).

References L.

KDL::ChainIkSolverPos_LMA::~ChainIkSolverPos_LMA ( )

virtual

destructor.

Member Function Documentation

int KDL::ChainIkSolverPos_LMA::CartToJnt	(	const KDL::JntArray &	q_init,
		const KDL::Frame &	T_base_goal,
		KDL::JntArray &	q_out
	)

virtual

computes the inverse position kinematics.

Parameters

q_init	initial joint position.
T_base_goal	goal position expressed with respect to the robot base.
q_out	joint position that achieves the specified goal position (if successful).

Returns: 0 if successful, -1 the gradient of \( E \) towards the joints is to small, -2 if joint position increments are to small, -3 if number of iterations is exceeded.

Implements KDL::ChainIkSolverPos.

References compute_fwdpos(), compute_jacobian(), KDL::JntArray::data, KDL::diff(), diffq, display_information, eps, eps_joints, grad, jac, L, lastDifference, lastNrOfIter, lastRotDiff, lastSV, lastTransDiff, maxiter, original_Aii, q, q_new, svd, T_base_head, tmp, and KDL::Twist_to_Eigen().

void KDL::ChainIkSolverPos_LMA::compute_fwdpos ( const VectorXq & q )

for internal use only.

Only exposed for test and diagnostic purposes.

References chain, KDL::Segment::getJoint(), KDL::Chain::getNrOfSegments(), KDL::Chain::getSegment(), KDL::Joint::getType(), KDL::Frame::Identity(), KDL::Joint::None, KDL::Segment::pose(), q, T_base_head, T_base_jointroot, and T_base_jointtip.

Referenced by CartToJnt(), and display_jac().

void KDL::ChainIkSolverPos_LMA::compute_jacobian ( const VectorXq & q )

for internal use only.

Only exposed for test and diagnostic purposes. compute_fwdpos(q) should always have been called before.

References chain, KDL::Segment::getJoint(), KDL::Chain::getNrOfSegments(), KDL::Chain::getSegment(), KDL::Joint::getType(), jac, KDL::Joint::None, KDL::Frame::p, q, T_base_head, T_base_jointroot, T_base_jointtip, and KDL::Segment::twist().

Referenced by CartToJnt(), and display_jac().

void KDL::ChainIkSolverPos_LMA::display_jac ( const KDL::JntArray & jval )

for internal use only.

Only exposed for test and diagnostic purposes.

References compute_fwdpos(), compute_jacobian(), KDL::JntArray::data, jac, q, and svd.

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

inlinevirtualinherited

Return the latest error.

References KDL::SolverI::error.

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

inlinevirtualinherited

Return a description of the latest error.

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

Reimplemented in KDL::ChainIkSolverVel_wdls, KDL::ChainIkSolverVel_pinv, KDL::ChainIkSolverPos_NR, and KDL::ChainJntToJacSolver.

References KDL::SolverI::E_NO_CONVERGE, and KDL::SolverI::E_NOERROR.

Referenced by KDL::ChainJntToJacSolver::strError(), KDL::ChainIkSolverPos_NR::strError(), KDL::ChainIkSolverVel_pinv::strError(), and KDL::ChainIkSolverVel_wdls::strError().

Member Data Documentation

MatrixXq KDL::ChainIkSolverPos_LMA::A

private

const KDL::Chain& KDL::ChainIkSolverPos_LMA::chain

private

Referenced by compute_fwdpos(), and compute_jacobian().

VectorXq KDL::ChainIkSolverPos_LMA::diffq

private

Referenced by CartToJnt().

bool KDL::ChainIkSolverPos_LMA::display_information

display information on each iteration step to the console.

Referenced by CartToJnt().

double KDL::ChainIkSolverPos_LMA::eps

private

Referenced by CartToJnt().

double KDL::ChainIkSolverPos_LMA::eps_joints

private

Referenced by CartToJnt().

int KDL::SolverI::error

protectedinherited

Latest error, initialized to E_NOERROR in constructor.

Referenced by KDL::ChainIkSolverPos_NR::CartToJnt(), KDL::ChainIkSolverVel_pinv::CartToJnt(), KDL::ChainIkSolverVel_wdls::CartToJnt(), KDL::SolverI::getError(), KDL::ChainIkSolverVel_pinv::getSVDResult(), KDL::ChainIkSolverVel_wdls::getSVDResult(), and KDL::ChainJntToJacSolver::JntToJac().

VectorXq KDL::ChainIkSolverPos_LMA::grad

for internal use only.

contains the gradient of the error criterion after an execution of CartToJnt.

Referenced by CartToJnt().

MatrixXq KDL::ChainIkSolverPos_LMA::jac

for internal use only.

contains the last value for the Jacobian after an execution of compute_jacobian.

Referenced by CartToJnt(), compute_jacobian(), and display_jac().

Eigen::Matrix<ScalarType,6,1> KDL::ChainIkSolverPos_LMA::L

private

Referenced by CartToJnt(), and ChainIkSolverPos_LMA().

double KDL::ChainIkSolverPos_LMA::lastDifference

contains the last value for \( E \) after an execution of CartToJnt.

Referenced by CartToJnt().

int KDL::ChainIkSolverPos_LMA::lastNrOfIter

contains the last number of iterations for an execution of CartToJnt.

Referenced by CartToJnt().

double KDL::ChainIkSolverPos_LMA::lastRotDiff

contains the last value for the (unweighted) rotational difference after an execution of CartToJnt.

Referenced by CartToJnt().

VectorXq KDL::ChainIkSolverPos_LMA::lastSV

contains the last values for the singular values of the weighted Jacobian after an execution of CartToJnt.

Referenced by CartToJnt().

double KDL::ChainIkSolverPos_LMA::lastTransDiff

contains the last value for the (unweighted) translational difference after an execution of CartToJnt.

Referenced by CartToJnt().

Eigen::LDLT<MatrixXq> KDL::ChainIkSolverPos_LMA::ldlt

private

unsigned int KDL::ChainIkSolverPos_LMA::maxiter

private

Referenced by CartToJnt().

VectorXq KDL::ChainIkSolverPos_LMA::original_Aii

private

Referenced by CartToJnt().

VectorXq KDL::ChainIkSolverPos_LMA::q

private

Referenced by CartToJnt(), compute_fwdpos(), compute_jacobian(), and display_jac().

VectorXq KDL::ChainIkSolverPos_LMA::q_new

private

Referenced by CartToJnt().

Eigen::JacobiSVD<MatrixXq> KDL::ChainIkSolverPos_LMA::svd

private

Referenced by CartToJnt(), and display_jac().

KDL::Frame KDL::ChainIkSolverPos_LMA::T_base_head

for internal use only.

contains the last value for the position of the tip of the robot (head) with respect to the base, after an execution of compute_jacobian.

Referenced by CartToJnt(), compute_fwdpos(), and compute_jacobian().

std::vector<KDL::Frame> KDL::ChainIkSolverPos_LMA::T_base_jointroot

private

Referenced by compute_fwdpos(), and compute_jacobian().

std::vector<KDL::Frame> KDL::ChainIkSolverPos_LMA::T_base_jointtip

private

Referenced by compute_fwdpos(), and compute_jacobian().

VectorXq KDL::ChainIkSolverPos_LMA::tmp

private

Referenced by CartToJnt().

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

src/chainiksolverpos_lma.hpp
src/chainiksolverpos_lma.cpp

Public Types

Public Member Functions

Public Attributes

Protected Attributes

Private Types

Private Attributes

Detailed Description

Member Typedef Documentation

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation