orocos_kdl
Public Member Functions | Public Attributes | Static Public 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 <chainiksolverpos_lma.hpp>

Inheritance diagram for KDL::ChainIkSolverPos_LMA:
Inheritance graph
[legend]

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 \( E \) 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::Chainchain
 
VectorXq diffq
 
double eps
 
double eps_joints
 
Eigen::Matrix< ScalarType, 6, 1 > L
 
Eigen::LDLT< MatrixXqldlt
 
unsigned int maxiter
 
unsigned int nj
 
unsigned int ns
 
VectorXq original_Aii
 
VectorXq q
 
VectorXq q_new
 
Eigen::JacobiSVD< MatrixXqsvd
 
std::vector< KDL::FrameT_base_jointroot
 
std::vector< KDL::FrameT_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:

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

Definition at line 87 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 114 of file chainiksolverpos_lma.hpp.

◆ ScalarType

typedef double KDL::ChainIkSolverPos_LMA::ScalarType
private

Definition at line 113 of file chainiksolverpos_lma.hpp.

◆ VectorXq

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

Definition at line 115 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
_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.

Definition at line 73 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 107 of file chainiksolverpos_lma.cpp.

◆ ~ChainIkSolverPos_LMA()

KDL::ChainIkSolverPos_LMA::~ChainIkSolverPos_LMA ( )
virtual

destructor.

Definition at line 162 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_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
E_NOERROR if successful, E_GRADIENT_JOINTS_TOO_SMALL the gradient of \( E \) 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 210 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 164 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 181 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 200 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 340 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 145 of file chainiksolverpos_lma.cpp.

Member Data Documentation

◆ A

MatrixXq KDL::ChainIkSolverPos_LMA::A
private

Definition at line 282 of file chainiksolverpos_lma.hpp.

◆ chain

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

Definition at line 206 of file chainiksolverpos_lma.hpp.

◆ diffq

VectorXq KDL::ChainIkSolverPos_LMA::diffq
private

Definition at line 286 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 261 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 118 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 119 of file chainiksolverpos_lma.hpp.

◆ eps

double KDL::ChainIkSolverPos_LMA::eps
private

Definition at line 265 of file chainiksolverpos_lma.hpp.

◆ eps_joints

double KDL::ChainIkSolverPos_LMA::eps_joints
private

Definition at line 266 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 250 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 243 of file chainiksolverpos_lma.hpp.

◆ L

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

Definition at line 267 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 221 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 216 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 231 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 236 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 226 of file chainiksolverpos_lma.hpp.

◆ ldlt

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

Definition at line 284 of file chainiksolverpos_lma.hpp.

◆ maxiter

unsigned int KDL::ChainIkSolverPos_LMA::maxiter
private

Definition at line 264 of file chainiksolverpos_lma.hpp.

◆ nj

unsigned int KDL::ChainIkSolverPos_LMA::nj
private

Definition at line 207 of file chainiksolverpos_lma.hpp.

◆ ns

unsigned int KDL::ChainIkSolverPos_LMA::ns
private

Definition at line 208 of file chainiksolverpos_lma.hpp.

◆ original_Aii

VectorXq KDL::ChainIkSolverPos_LMA::original_Aii
private

Definition at line 288 of file chainiksolverpos_lma.hpp.

◆ q

VectorXq KDL::ChainIkSolverPos_LMA::q
private

Definition at line 281 of file chainiksolverpos_lma.hpp.

◆ q_new

VectorXq KDL::ChainIkSolverPos_LMA::q_new
private

Definition at line 287 of file chainiksolverpos_lma.hpp.

◆ svd

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

Definition at line 285 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 256 of file chainiksolverpos_lma.hpp.

◆ T_base_jointroot

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

Definition at line 272 of file chainiksolverpos_lma.hpp.

◆ T_base_jointtip

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

Definition at line 273 of file chainiksolverpos_lma.hpp.

◆ tmp

VectorXq KDL::ChainIkSolverPos_LMA::tmp
private

Definition at line 283 of file chainiksolverpos_lma.hpp.


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