#ifndef KDL_CHAINIKSOLVERPOS_GN_HPP #define KDL_CHAINIKSOLVERPOS_GN_HPP /** \file chainiksolverpos_lma.hpp \brief computing inverse position kinematics using Levenberg-Marquardt. */ /************************************************************************** begin : May 2012 copyright : (C) 2012 Erwin Aertbelien email : firstname.lastname@mech.kuleuven.ac.be History (only major changes)( AUTHOR-Description ) : *************************************************************************** * This library is free software; you can redistribute it and/or * * modify it under the terms of the GNU Lesser General Public * * License as published by the Free Software Foundation; either * * version 2.1 of the License, or (at your option) any later version. * * * * This library is distributed in the hope that it will be useful, * * but WITHOUT ANY WARRANTY; without even the implied warranty of * * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU * * Lesser General Public License for more details. * * * * You should have received a copy of the GNU Lesser General Public * * License along with this library; if not, write to the Free Software * * Foundation, Inc., 59 Temple Place, * * Suite 330, Boston, MA 02111-1307 USA * * * ***************************************************************************/ #include "chainiksolver.hpp" #include "chain.hpp" #include namespace KDL { /** * \brief 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 \f$ \mathcal{O}(N) \f$ * algorithm for the computation of the Jacobian (with \f$N\f$, 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. * \ingroup KinematicFamily */ class ChainIkSolverPos_LMA : public KDL::ChainIkSolverPos { private: typedef double ScalarType; typedef Eigen::Matrix MatrixXq; typedef Eigen::Matrix VectorXq; public: static const int E_GRADIENT_JOINTS_TOO_SMALL = -100; static const int E_INCREMENT_JOINTS_TOO_SMALL = -101; /** * \brief 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: * \f$ E = \Delta \mathbf{x}^T \mathbf{L} \mathbf{L}^T \Delta \mathbf{x} \f$, with \f$\mathbf{L}\f$ a diagonal matrix. * * \param _chain specifies the kinematic chain. * \param _l specifies the "square root" of the weight (diagonal) matrix in task space. This diagonal matrix is specified as a vector. * \param _eps specifies the desired accuracy in task space; after weighing with * the weight matrix, it is applied on \f$E\f$. * \param _maxiter specifies the maximum number of iterations. * \param _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. */ ChainIkSolverPos_LMA( const KDL::Chain& _chain, const Eigen::Matrix& _l, double _eps=1E-5, int _maxiter=500, double _eps_joints=1E-15 ); /** * \brief identical the full constructor for ChainIkSolverPos_LMA, but provides for a default weight matrix. * * \f$\mathbf{L} = \mathrm{diag}\left( \begin{bmatrix} 1 & 1 & 1 & 0.01 & 0.01 & 0.01 \end{bmatrix} \right) \f$. */ ChainIkSolverPos_LMA( const KDL::Chain& _chain, double _eps=1E-5, int _maxiter=500, double _eps_joints=1E-15 ); /** * \brief computes the inverse position kinematics. * * \param q_init initial joint position. * \param T_base_goal goal position expressed with respect to the robot base. * \param q_out joint position that achieves the specified goal position (if successful). * \return E_NOERROR if successful, * E_GRADIENT_JOINTS_TOO_SMALL the gradient of \f$ E \f$ 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. */ virtual int CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out); /** * \brief destructor. */ virtual ~ChainIkSolverPos_LMA(); /** * \brief for internal use only. * * Only exposed for test and diagnostic purposes. */ void compute_fwdpos(const VectorXq& q); /** * \brief for internal use only. * Only exposed for test and diagnostic purposes. * compute_fwdpos(q) should always have been called before. */ void compute_jacobian(const VectorXq& q); /** * \brief for internal use only. * Only exposed for test and diagnostic purposes. */ void display_jac(const KDL::JntArray& jval); /// @copydoc KDL::SolverI::updateInternalDataStructures void updateInternalDataStructures(); /// @copydoc KDL::SolverI::strError() virtual const char* strError(const int error) const; private: const KDL::Chain& chain; unsigned int nj; unsigned int ns; public: /** * \brief contains the last number of iterations for an execution of CartToJnt. */ int lastNrOfIter; /** * \brief contains the last value for \f$ E \f$ after an execution of CartToJnt. */ double lastDifference; /** * \brief contains the last value for the (unweighted) translational difference after an execution of CartToJnt. */ double lastTransDiff; /** * \brief contains the last value for the (unweighted) rotational difference after an execution of CartToJnt. */ double lastRotDiff; /** * \brief contains the last values for the singular values of the weighted Jacobian after an execution of CartToJnt. */ VectorXq lastSV; /** * \brief for internal use only. * * contains the last value for the Jacobian after an execution of compute_jacobian. */ MatrixXq jac; /** * \brief for internal use only. * * contains the gradient of the error criterion after an execution of CartToJnt. */ VectorXq grad; /** * \brief 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. */ KDL::Frame T_base_head; /** * \brief display information on each iteration step to the console. */ bool display_information; private: // additional specification of the inverse position kinematics problem: unsigned int maxiter; double eps; double eps_joints; Eigen::Matrix L; // state of compute_fwdpos and compute_jacobian: std::vector T_base_jointroot; std::vector T_base_jointtip; // need 2 vectors because of the somewhat strange definition of segment.hpp // you could also recompute jointtip out of jointroot, // but then you'll need more expensive cos/sin functions. // the following are state of CartToJnt that is pre-allocated: VectorXq q; MatrixXq A; VectorXq tmp; Eigen::LDLT ldlt; Eigen::JacobiSVD svd; VectorXq diffq; VectorXq q_new; VectorXq original_Aii; }; } // namespace KDL #endif