Solver for the inverse position kinematics that uses Levenberg-Marquardt. More...
#include <chainiksolverpos_lma.hpp>
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 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::Chain & | chain |
VectorXq | diffq |
double | eps |
double | eps_joints |
Eigen::Matrix< ScalarType, 6, 1 > | L |
Eigen::LDLT< MatrixXq > | ldlt |
unsigned int | maxiter |
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 |
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.
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.
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: , with a diagonal matrix.
_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.
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.
.
Definition at line 79 of file chainiksolverpos_lma.cpp.
KDL::ChainIkSolverPos_LMA::~ChainIkSolverPos_LMA | ( | ) | [virtual] |
destructor.
Definition at line 112 of file chainiksolverpos_lma.cpp.
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.
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). |
Implements KDL::ChainIkSolverPos.
Definition at line 160 of file chainiksolverpos_lma.cpp.
void KDL::ChainIkSolverPos_LMA::compute_fwdpos | ( | const VectorXq & | q | ) |
for internal use only.
Only exposed for test and diagnostic purposes.
Definition at line 114 of file chainiksolverpos_lma.cpp.
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 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.
MatrixXq KDL::ChainIkSolverPos_LMA::A [private] |
Definition at line 227 of file chainiksolverpos_lma.hpp.
const KDL::Chain& KDL::ChainIkSolverPos_LMA::chain [private] |
Definition at line 213 of file chainiksolverpos_lma.hpp.
VectorXq KDL::ChainIkSolverPos_LMA::diffq [private] |
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.
double KDL::ChainIkSolverPos_LMA::eps [private] |
Definition at line 210 of file chainiksolverpos_lma.hpp.
double KDL::ChainIkSolverPos_LMA::eps_joints [private] |
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 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.
VectorXq KDL::ChainIkSolverPos_LMA::q [private] |
Definition at line 226 of file chainiksolverpos_lma.hpp.
VectorXq KDL::ChainIkSolverPos_LMA::q_new [private] |
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.
std::vector<KDL::Frame> KDL::ChainIkSolverPos_LMA::T_base_jointroot [private] |
Definition at line 217 of file chainiksolverpos_lma.hpp.
std::vector<KDL::Frame> KDL::ChainIkSolverPos_LMA::T_base_jointtip [private] |
Definition at line 218 of file chainiksolverpos_lma.hpp.
VectorXq KDL::ChainIkSolverPos_LMA::tmp [private] |
Definition at line 228 of file chainiksolverpos_lma.hpp.