提交依赖与构建产物
This commit is contained in:
wangdequan
256
kdl_install/include/kdl/chainiksolverpos_lma.hpp
Normal file
256
kdl_install/include/kdl/chainiksolverpos_lma.hpp
Normal file
@@ -0,0 +1,256 @@
|
||||
#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 <Eigen/Dense>
|
||||
|
||||
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. <B>Weights also
|
||||
* make the solver more robust </B>.
|
||||
* - only the constructors call <B>memory allocation</B>.
|
||||
*
|
||||
* 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<ScalarType,Eigen::Dynamic,Eigen::Dynamic> MatrixXq;
|
||||
typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,1> 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; <B>after</B> 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<double,6,1>& _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<ScalarType,6,1> L;
|
||||
|
||||
|
||||
|
||||
// state of compute_fwdpos and compute_jacobian:
|
||||
std::vector<KDL::Frame> T_base_jointroot;
|
||||
std::vector<KDL::Frame> 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<MatrixXq> ldlt;
|
||||
Eigen::JacobiSVD<MatrixXq> svd;
|
||||
VectorXq diffq;
|
||||
VectorXq q_new;
|
||||
VectorXq original_Aii;
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
} // namespace KDL
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user