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
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
00216 unsigned int maxiter;
00217 double eps;
00218 double eps_joints;
00219 Eigen::Matrix<ScalarType,6,1> L;
00220
00221
00222
00223
00224 std::vector<KDL::Frame> T_base_jointroot;
00225 std::vector<KDL::Frame> T_base_jointtip;
00226
00227
00228
00229
00230
00231
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 };
00248
00249
00250
00251
00252
00253
00254 #endif