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 
00092     ChainIkSolverPos_LMA(
00093                 const KDL::Chain& _chain,
00094                 const Eigen::Matrix<double,6,1>& _L,
00095                 double _eps=1E-5,
00096                 int _maxiter=500,
00097                 double _eps_joints=1E-15
00098     );
00099 
00105     ChainIkSolverPos_LMA(
00106                 const KDL::Chain& _chain,
00107                 double _eps=1E-5,
00108                 int _maxiter=500,
00109                 double _eps_joints=1E-15
00110     );
00111 
00123     virtual int CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out);
00124 
00128     virtual ~ChainIkSolverPos_LMA();
00129 
00135     void compute_fwdpos(const VectorXq& q);
00136 
00142     void compute_jacobian(const VectorXq& q);
00143 
00148     void display_jac(const KDL::JntArray& jval);
00149 
00150 
00151 
00152 
00153 public:
00154 
00155 
00159     int lastNrOfIter;
00160 
00164     double lastDifference;
00165 
00169     double lastTransDiff;
00170 
00174     double lastRotDiff;
00175 
00179     VectorXq lastSV;
00180 
00186     MatrixXq jac;
00187 
00193     VectorXq grad;
00199     KDL::Frame T_base_head;
00200 
00204     bool display_information;
00205 private:
00206     // additional specification of the inverse position kinematics problem:
00207 
00208 
00209     unsigned int maxiter;
00210     double eps;
00211     double eps_joints;
00212     Eigen::Matrix<ScalarType,6,1> L;
00213     const KDL::Chain& chain;
00214 
00215 
00216     // state of compute_fwdpos and compute_jacobian:
00217     std::vector<KDL::Frame> T_base_jointroot;
00218     std::vector<KDL::Frame> T_base_jointtip;
00219                                         // need 2 vectors because of the somewhat strange definition of segment.hpp
00220                                         // you could also recompute jointtip out of jointroot,
00221                                 // but then you'll need more expensive cos/sin functions.
00222 
00223 
00224     // the following are state of CartToJnt that is pre-allocated:
00225 
00226     VectorXq q;
00227     MatrixXq A;
00228     VectorXq tmp;
00229     Eigen::LDLT<MatrixXq> ldlt;
00230     Eigen::JacobiSVD<MatrixXq> svd;
00231     VectorXq diffq;
00232     VectorXq q_new;
00233     VectorXq original_Aii;
00234 };
00235 
00236 
00237 
00238 
00239 
00240 }; // namespace KDL
00241 
00242 
00243 
00244 
00245 
00246 
00247 #endif


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14