chainiksolverpos_lma.hpp
Go to the documentation of this file.
00001 #ifndef KDL_CHAINIKSOLVERPOS_GN_HPP
00002 #define KDL_CHAINIKSOLVERPOS_GN_HPP
00003 
00008 /**************************************************************************
00009     begin                : May 2012
00010     copyright            : (C) 2012 Erwin Aertbelien
00011     email                : firstname.lastname@mech.kuleuven.ac.be
00012 
00013  History (only major changes)( AUTHOR-Description ) :
00014 
00015  ***************************************************************************
00016  *   This library is free software; you can redistribute it and/or         *
00017  *   modify it under the terms of the GNU Lesser General Public            *
00018  *   License as published by the Free Software Foundation; either          *
00019  *   version 2.1 of the License, or (at your option) any later version.    *
00020  *                                                                         *
00021  *   This library is distributed in the hope that it will be useful,       *
00022  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00023  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00024  *   Lesser General Public License for more details.                       *
00025  *                                                                         *
00026  *   You should have received a copy of the GNU Lesser General Public      *
00027  *   License along with this library; if not, write to the Free Software   *
00028  *   Foundation, Inc., 59 Temple Place,                                    *
00029  *   Suite 330, Boston, MA  02111-1307  USA                                *
00030  *                                                                         *
00031  ***************************************************************************/
00032 
00033 
00034 #include "chainiksolver.hpp"
00035 #include "chain.hpp"
00036 #include <Eigen/Dense>
00037 
00038 namespace KDL
00039 {
00040 
00064 class ChainIkSolverPos_LMA : public KDL::ChainIkSolverPos
00065 {
00066 private:
00067         typedef double ScalarType;
00068     typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,Eigen::Dynamic> MatrixXq;
00069     typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,1> VectorXq;
00070 public:
00071 
00072     static const int E_GRADIENT_JOINTS_TOO_SMALL = -100;
00073     static const int E_INCREMENT_JOINTS_TOO_SMALL = -101;
00074 
00095     ChainIkSolverPos_LMA(
00096                 const KDL::Chain& _chain,
00097                 const Eigen::Matrix<double,6,1>& _L,
00098                 double _eps=1E-5,
00099                 int _maxiter=500,
00100                 double _eps_joints=1E-15
00101     );
00102 
00108     ChainIkSolverPos_LMA(
00109                 const KDL::Chain& _chain,
00110                 double _eps=1E-5,
00111                 int _maxiter=500,
00112                 double _eps_joints=1E-15
00113     );
00114 
00126     virtual int CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out);
00127 
00131     virtual ~ChainIkSolverPos_LMA();
00132 
00138     void compute_fwdpos(const VectorXq& q);
00139 
00145     void compute_jacobian(const VectorXq& q);
00146 
00151     void display_jac(const KDL::JntArray& jval);
00152 
00153 
00155     virtual const char* strError(const int error) const;
00156 
00157 private:
00158     const KDL::Chain& chain;
00159     unsigned int nj;
00160     unsigned int ns;
00161 
00162 public:
00163 
00164 
00168     int lastNrOfIter;
00169 
00173     double lastDifference;
00174 
00178     double lastTransDiff;
00179 
00183     double lastRotDiff;
00184 
00188     VectorXq lastSV;
00189 
00195     MatrixXq jac;
00196 
00202     VectorXq grad;
00208     KDL::Frame T_base_head;
00209 
00213     bool display_information;
00214 private:
00215     // additional specification of the inverse position kinematics problem:
00216     unsigned int maxiter;
00217     double eps;
00218     double eps_joints;
00219     Eigen::Matrix<ScalarType,6,1> L;
00220 
00221 
00222 
00223     // state of compute_fwdpos and compute_jacobian:
00224     std::vector<KDL::Frame> T_base_jointroot;
00225     std::vector<KDL::Frame> T_base_jointtip;
00226                                         // need 2 vectors because of the somewhat strange definition of segment.hpp
00227                                         // you could also recompute jointtip out of jointroot,
00228                                 // but then you'll need more expensive cos/sin functions.
00229 
00230 
00231     // the following are state of CartToJnt that is pre-allocated:
00232 
00233     VectorXq q;
00234     MatrixXq A;
00235     VectorXq tmp;
00236     Eigen::LDLT<MatrixXq> ldlt;
00237     Eigen::JacobiSVD<MatrixXq> svd;
00238     VectorXq diffq;
00239     VectorXq q_new;
00240     VectorXq original_Aii;
00241 };
00242 
00243 
00244 
00245 
00246 
00247 }; // namespace KDL
00248 
00249 
00250 
00251 
00252 
00253 
00254 #endif


orocos_kdl
Author(s):
autogenerated on Wed Dec 6 2017 03:40:30