Public Member Functions | Public Attributes | Private Types | Private Attributes
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]

List of all members.

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.
 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.
 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.
void compute_fwdpos (const VectorXq &q)
 for internal use only.
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.
void display_jac (const KDL::JntArray &jval)
 for internal use only. Only exposed for test and diagnostic purposes.
virtual ~ChainIkSolverPos_LMA ()
 destructor.

Public Attributes

bool display_information
 display information on each iteration step to the console.
VectorXq grad
 for internal use only.
MatrixXq jac
 for internal use only.
double lastDifference
 contains the last value for $ E $ after an execution of CartToJnt.
int lastNrOfIter
 contains the last number of iterations for an execution of CartToJnt.
double lastRotDiff
 contains the last value for the (unweighted) rotational difference after an execution of CartToJnt.
VectorXq lastSV
 contains the last values for the singular values of the weighted Jacobian after an execution of CartToJnt.
double lastTransDiff
 contains the last value for the (unweighted) translational difference after an execution of CartToJnt.
KDL::Frame T_base_head
 for internal use only.

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

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 64 of file chainiksolverpos_lma.hpp.


Member Typedef Documentation

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

Definition at line 68 of file chainiksolverpos_lma.hpp.

typedef double KDL::ChainIkSolverPos_LMA::ScalarType [private]

Definition at line 67 of file chainiksolverpos_lma.hpp.

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

Definition at line 69 of file chainiksolverpos_lma.hpp.


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:
_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 50 of file chainiksolverpos_lma.cpp.

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 79 of file chainiksolverpos_lma.cpp.

destructor.

Definition at line 112 of file chainiksolverpos_lma.cpp.


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

Definition at line 160 of file chainiksolverpos_lma.cpp.

for internal use only.

Only exposed for test and diagnostic purposes.

Definition at line 114 of file chainiksolverpos_lma.cpp.

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

Definition at line 131 of file chainiksolverpos_lma.cpp.

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

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

Definition at line 150 of file chainiksolverpos_lma.cpp.


Member Data Documentation

Definition at line 227 of file chainiksolverpos_lma.hpp.

Definition at line 213 of file chainiksolverpos_lma.hpp.

Definition at line 231 of file chainiksolverpos_lma.hpp.

display information on each iteration step to the console.

Definition at line 204 of file chainiksolverpos_lma.hpp.

Definition at line 210 of file chainiksolverpos_lma.hpp.

Definition at line 211 of file chainiksolverpos_lma.hpp.

for internal use only.

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

Definition at line 193 of file chainiksolverpos_lma.hpp.

for internal use only.

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

Definition at line 186 of file chainiksolverpos_lma.hpp.

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

Definition at line 212 of file chainiksolverpos_lma.hpp.

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

Definition at line 164 of file chainiksolverpos_lma.hpp.

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

Definition at line 159 of file chainiksolverpos_lma.hpp.

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

Definition at line 174 of file chainiksolverpos_lma.hpp.

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

Definition at line 179 of file chainiksolverpos_lma.hpp.

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

Definition at line 169 of file chainiksolverpos_lma.hpp.

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

Definition at line 229 of file chainiksolverpos_lma.hpp.

unsigned int KDL::ChainIkSolverPos_LMA::maxiter [private]

Definition at line 209 of file chainiksolverpos_lma.hpp.

Definition at line 233 of file chainiksolverpos_lma.hpp.

Definition at line 226 of file chainiksolverpos_lma.hpp.

Definition at line 232 of file chainiksolverpos_lma.hpp.

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

Definition at line 230 of file chainiksolverpos_lma.hpp.

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 199 of file chainiksolverpos_lma.hpp.

Definition at line 217 of file chainiksolverpos_lma.hpp.

Definition at line 218 of file chainiksolverpos_lma.hpp.

Definition at line 228 of file chainiksolverpos_lma.hpp.


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


orocos_kdl
Author(s):
autogenerated on Mon Oct 6 2014 03:11:17