$search
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