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 
00154     void updateInternalDataStructures();
00155 
00157     virtual const char* strError(const int error) const;
00158 
00159 private:
00160     const KDL::Chain& chain;
00161     unsigned int nj;
00162     unsigned int ns;
00163 
00164 public:
00165 
00166 
00170     int lastNrOfIter;
00171 
00175     double lastDifference;
00176 
00180     double lastTransDiff;
00181 
00185     double lastRotDiff;
00186 
00190     VectorXq lastSV;
00191 
00197     MatrixXq jac;
00198 
00204     VectorXq grad;
00210     KDL::Frame T_base_head;
00211 
00215     bool display_information;
00216 private:
00217     // additional specification of the inverse position kinematics problem:
00218     unsigned int maxiter;
00219     double eps;
00220     double eps_joints;
00221     Eigen::Matrix<ScalarType,6,1> L;
00222 
00223 
00224 
00225     // state of compute_fwdpos and compute_jacobian:
00226     std::vector<KDL::Frame> T_base_jointroot;
00227     std::vector<KDL::Frame> T_base_jointtip;
00228                                         // need 2 vectors because of the somewhat strange definition of segment.hpp
00229                                         // you could also recompute jointtip out of jointroot,
00230                                 // but then you'll need more expensive cos/sin functions.
00231 
00232 
00233     // the following are state of CartToJnt that is pre-allocated:
00234 
00235     VectorXq q;
00236     MatrixXq A;
00237     VectorXq tmp;
00238     Eigen::LDLT<MatrixXq> ldlt;
00239     Eigen::JacobiSVD<MatrixXq> svd;
00240     VectorXq diffq;
00241     VectorXq q_new;
00242     VectorXq original_Aii;
00243 };
00244 
00245 
00246 
00247 
00248 
00249 }; // namespace KDL
00250 
00251 
00252 
00253 
00254 
00255 
00256 #endif


orocos_kdl
Author(s):
autogenerated on Tue May 15 2018 03:06:08