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

#include <chainiksolverpos_lma.hpp>

Inheritance diagram for KDL::ChainIkSolverPos_LMA:

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

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

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

void	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. More...

void	display_jac (const KDL::JntArray &jval)
	for internal use only. Only exposed for test and diagnostic purposes. More...

virtual const char *	strError (const int error) const

void	updateInternalDataStructures ()

virtual	~ChainIkSolverPos_LMA ()
	destructor. More...

Public Member Functions inherited from KDL::ChainIkSolverPos
virtual	~ChainIkSolverPos ()

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

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

VectorXq	grad
	for internal use only. More...

MatrixXq	jac
	for internal use only. More...

double	lastDifference
	contains the last value for after an execution of CartToJnt. More...

int	lastNrOfIter
	contains the last number of iterations for 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...

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

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

Static Public Attributes
static const int	E_GRADIENT_JOINTS_TOO_SMALL = -100

static const int	E_INCREMENT_JOINTS_TOO_SMALL = -101

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

typedef double	ScalarType

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

Private Attributes
MatrixXq	A

const KDL::Chain &	chain

VectorXq	diffq

double	eps

double	eps_joints

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

Eigen::LDLT< MatrixXq >	ldlt

unsigned int	maxiter

unsigned int	nj

unsigned int	ns

VectorXq	original_Aii

VectorXq	q

VectorXq	q_new

Eigen::JacobiSVD< MatrixXq >	svd

std::vector< KDL::Frame >	T_base_jointroot

std::vector< KDL::Frame >	T_base_jointtip

VectorXq	tmp

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

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

Definition at line 64 of file chainiksolverpos_lma.hpp.

Member Typedef Documentation

◆ MatrixXq

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

private

Definition at line 68 of file chainiksolverpos_lma.hpp.

◆ ScalarType

typedef double KDL::ChainIkSolverPos_LMA::ScalarType

private

Definition at line 67 of file chainiksolverpos_lma.hpp.

◆ VectorXq

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

private

Definition at line 69 of file chainiksolverpos_lma.hpp.

Constructor & Destructor Documentation

◆ ChainIkSolverPos_LMA() [1/2]

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 chosen 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 .
_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.

Definition at line 50 of file chainiksolverpos_lma.cpp.

◆ ChainIkSolverPos_LMA() [2/2]

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)$ .

Definition at line 84 of file chainiksolverpos_lma.cpp.

◆ ~ChainIkSolverPos_LMA()

KDL::ChainIkSolverPos_LMA::~ChainIkSolverPos_LMA ( )

virtual

destructor.

Definition at line 139 of file chainiksolverpos_lma.cpp.

Member Function Documentation

◆ CartToJnt()

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: E_NOERROR if successful, E_GRADIENT_JOINTS_TOO_SMALL the gradient of towards the joints is to small, E_INCREMENT_JOINTS_TOO_SMALL if joint position increments are to small, E_MAX_ITER_EXCEEDED if number of iterations is exceeded.

Implements KDL::ChainIkSolverPos.

Definition at line 187 of file chainiksolverpos_lma.cpp.

◆ compute_fwdpos()

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

for internal use only.

Only exposed for test and diagnostic purposes.

Definition at line 141 of file chainiksolverpos_lma.cpp.

◆ compute_jacobian()

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.

Definition at line 158 of file chainiksolverpos_lma.cpp.

◆ display_jac()

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

for internal use only. Only exposed for test and diagnostic purposes.

Definition at line 177 of file chainiksolverpos_lma.cpp.

◆ strError()

const char * KDL::ChainIkSolverPos_LMA::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.

Definition at line 317 of file chainiksolverpos_lma.cpp.

◆ updateInternalDataStructures()

void KDL::ChainIkSolverPos_LMA::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::ChainIkSolverPos.

Definition at line 122 of file chainiksolverpos_lma.cpp.

Member Data Documentation

◆ A

MatrixXq KDL::ChainIkSolverPos_LMA::A

private

Definition at line 236 of file chainiksolverpos_lma.hpp.

◆ chain

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

private

Definition at line 160 of file chainiksolverpos_lma.hpp.

◆ diffq

VectorXq KDL::ChainIkSolverPos_LMA::diffq

private

Definition at line 240 of file chainiksolverpos_lma.hpp.

◆ display_information

bool KDL::ChainIkSolverPos_LMA::display_information

display information on each iteration step to the console.

Definition at line 215 of file chainiksolverpos_lma.hpp.

◆ E_GRADIENT_JOINTS_TOO_SMALL

const int KDL::ChainIkSolverPos_LMA::E_GRADIENT_JOINTS_TOO_SMALL = -100

static

Definition at line 72 of file chainiksolverpos_lma.hpp.

◆ E_INCREMENT_JOINTS_TOO_SMALL

const int KDL::ChainIkSolverPos_LMA::E_INCREMENT_JOINTS_TOO_SMALL = -101

static

Definition at line 73 of file chainiksolverpos_lma.hpp.

◆ eps

double KDL::ChainIkSolverPos_LMA::eps

private

Definition at line 219 of file chainiksolverpos_lma.hpp.

◆ eps_joints

double KDL::ChainIkSolverPos_LMA::eps_joints

private

Definition at line 220 of file chainiksolverpos_lma.hpp.

◆ grad

VectorXq KDL::ChainIkSolverPos_LMA::grad

for internal use only.

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

Definition at line 204 of file chainiksolverpos_lma.hpp.

◆ jac

MatrixXq KDL::ChainIkSolverPos_LMA::jac

for internal use only.

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

Definition at line 197 of file chainiksolverpos_lma.hpp.

◆ L

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

private

Definition at line 221 of file chainiksolverpos_lma.hpp.

◆ lastDifference

double KDL::ChainIkSolverPos_LMA::lastDifference

contains the last value for $ E $ after an execution of CartToJnt.

Definition at line 175 of file chainiksolverpos_lma.hpp.

◆ lastNrOfIter

int KDL::ChainIkSolverPos_LMA::lastNrOfIter

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

Definition at line 170 of file chainiksolverpos_lma.hpp.

◆ lastRotDiff

double KDL::ChainIkSolverPos_LMA::lastRotDiff

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

Definition at line 185 of file chainiksolverpos_lma.hpp.

◆ lastSV

VectorXq KDL::ChainIkSolverPos_LMA::lastSV

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

Definition at line 190 of file chainiksolverpos_lma.hpp.

◆ lastTransDiff

double KDL::ChainIkSolverPos_LMA::lastTransDiff

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

Definition at line 180 of file chainiksolverpos_lma.hpp.

◆ ldlt

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

private

Definition at line 238 of file chainiksolverpos_lma.hpp.

◆ maxiter

unsigned int KDL::ChainIkSolverPos_LMA::maxiter

private

Definition at line 218 of file chainiksolverpos_lma.hpp.

◆ nj

unsigned int KDL::ChainIkSolverPos_LMA::nj

private

Definition at line 161 of file chainiksolverpos_lma.hpp.

◆ ns

unsigned int KDL::ChainIkSolverPos_LMA::ns

private

Definition at line 162 of file chainiksolverpos_lma.hpp.

◆ original_Aii

VectorXq KDL::ChainIkSolverPos_LMA::original_Aii

private

Definition at line 242 of file chainiksolverpos_lma.hpp.

◆ q

VectorXq KDL::ChainIkSolverPos_LMA::q

private

Definition at line 235 of file chainiksolverpos_lma.hpp.

◆ q_new

VectorXq KDL::ChainIkSolverPos_LMA::q_new

private

Definition at line 241 of file chainiksolverpos_lma.hpp.

◆ svd

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

private

Definition at line 239 of file chainiksolverpos_lma.hpp.

◆ T_base_head

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.

Definition at line 210 of file chainiksolverpos_lma.hpp.

◆ T_base_jointroot

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

private

Definition at line 226 of file chainiksolverpos_lma.hpp.

◆ T_base_jointtip

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

private

Definition at line 227 of file chainiksolverpos_lma.hpp.

◆ tmp

VectorXq KDL::ChainIkSolverPos_LMA::tmp

private

Definition at line 237 of file chainiksolverpos_lma.hpp.

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

Public Member Functions

Public Attributes

Static Public Attributes

Private Types

Private Attributes

Additional Inherited Members

Detailed Description

Member Typedef Documentation

◆ MatrixXq

◆ ScalarType

◆ VectorXq

Constructor & Destructor Documentation

◆ ChainIkSolverPos_LMA() [1/2]

◆ ChainIkSolverPos_LMA() [2/2]

◆ ~ChainIkSolverPos_LMA()

Member Function Documentation

◆ CartToJnt()

◆ compute_fwdpos()

◆ compute_jacobian()

◆ display_jac()

◆ strError()

◆ updateInternalDataStructures()

Member Data Documentation

◆ A

◆ chain

◆ diffq

◆ display_information

◆ E_GRADIENT_JOINTS_TOO_SMALL

◆ E_INCREMENT_JOINTS_TOO_SMALL

◆ eps

◆ eps_joints

◆ grad

◆ jac

◆ L

◆ lastDifference

◆ lastNrOfIter

◆ lastRotDiff

◆ lastSV

◆ lastTransDiff

◆ ldlt

◆ maxiter

◆ nj

◆ ns

◆ original_Aii

◆ q

◆ q_new

◆ svd

◆ T_base_head

◆ T_base_jointroot

◆ T_base_jointtip

◆ tmp