KDL  1.3.0
Public Types | Public Member Functions | Public Attributes | Protected Attributes | Private Types | Private Attributes | List of all members
KDL::ChainIkSolverPos_LMA Class Reference

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

#include <src/chainiksolverpos_lma.hpp>

Inheritance diagram for KDL::ChainIkSolverPos_LMA:
Inheritance graph
[legend]
Collaboration diagram for KDL::ChainIkSolverPos_LMA:
Collaboration graph
[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::Chainchain
 
std::vector< KDL::FrameT_base_jointroot
 
std::vector< KDL::FrameT_base_jointtip
 
VectorXq q
 
MatrixXq A
 
VectorXq tmp
 
Eigen::LDLT< MatrixXqldlt
 
Eigen::JacobiSVD< MatrixXqsvd
 
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:

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

Member Typedef Documentation

§ MatrixXq

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

§ ScalarType

typedef double KDL::ChainIkSolverPos_LMA::ScalarType
private

§ VectorXq

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

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

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 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
_chainspecifies the kinematic chain.
_Lspecifies the "square root" of the weight (diagonal) matrix in task space. This diagonal matrix is specified as a vector.
_epsspecifies the desired accuracy in task space; after weighing with the weight matrix, it is applied on \(E\).
_maxiterspecifies the maximum number of iterations.
_eps_jointsspecifies 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.

§ 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) \).

References L.

§ ~ChainIkSolverPos_LMA()

KDL::ChainIkSolverPos_LMA::~ChainIkSolverPos_LMA ( )
virtual

destructor.

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_initinitial joint position.
T_base_goalgoal position expressed with respect to the robot base.
q_outjoint 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().

§ compute_fwdpos()

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

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

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

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

§ getError()

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

Return the latest error.

References KDL::SolverI::error.

§ strError()

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

Member Data Documentation

§ A

MatrixXq KDL::ChainIkSolverPos_LMA::A
private

§ chain

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

Referenced by compute_fwdpos(), and compute_jacobian().

§ diffq

VectorXq KDL::ChainIkSolverPos_LMA::diffq
private

Referenced by CartToJnt().

§ display_information

bool KDL::ChainIkSolverPos_LMA::display_information

display information on each iteration step to the console.

Referenced by CartToJnt().

§ eps

double KDL::ChainIkSolverPos_LMA::eps
private

Referenced by CartToJnt().

§ eps_joints

double KDL::ChainIkSolverPos_LMA::eps_joints
private

Referenced by CartToJnt().

§ error

int KDL::SolverI::error
protectedinherited

§ grad

VectorXq KDL::ChainIkSolverPos_LMA::grad

for internal use only.

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

Referenced by CartToJnt().

§ jac

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

§ L

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

Referenced by CartToJnt(), and ChainIkSolverPos_LMA().

§ lastDifference

double KDL::ChainIkSolverPos_LMA::lastDifference

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

Referenced by CartToJnt().

§ lastNrOfIter

int KDL::ChainIkSolverPos_LMA::lastNrOfIter

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

Referenced by CartToJnt().

§ lastRotDiff

double KDL::ChainIkSolverPos_LMA::lastRotDiff

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

Referenced by CartToJnt().

§ lastSV

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

§ lastTransDiff

double KDL::ChainIkSolverPos_LMA::lastTransDiff

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

Referenced by CartToJnt().

§ ldlt

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

§ maxiter

unsigned int KDL::ChainIkSolverPos_LMA::maxiter
private

Referenced by CartToJnt().

§ original_Aii

VectorXq KDL::ChainIkSolverPos_LMA::original_Aii
private

Referenced by CartToJnt().

§ q

VectorXq KDL::ChainIkSolverPos_LMA::q
private

§ q_new

VectorXq KDL::ChainIkSolverPos_LMA::q_new
private

Referenced by CartToJnt().

§ svd

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

Referenced by CartToJnt(), and display_jac().

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

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

§ T_base_jointroot

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

Referenced by compute_fwdpos(), and compute_jacobian().

§ T_base_jointtip

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

Referenced by compute_fwdpos(), and compute_jacobian().

§ tmp

VectorXq KDL::ChainIkSolverPos_LMA::tmp
private

Referenced by CartToJnt().


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