Go to the documentation of this file.00001 #ifndef KDL_CHAINIKSOLVERPOS_GN_HPP
00002 #define KDL_CHAINIKSOLVERPOS_GN_HPP
00003
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
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
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
00217 std::vector<KDL::Frame> T_base_jointroot;
00218 std::vector<KDL::Frame> T_base_jointtip;
00219
00220
00221
00222
00223
00224
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 };
00241
00242
00243
00244
00245
00246
00247 #endif