KDL  1.3.0
chainiksolverpos_lma.hpp
Go to the documentation of this file.
1 #ifndef KDL_CHAINIKSOLVERPOS_GN_HPP
2 #define KDL_CHAINIKSOLVERPOS_GN_HPP
3 
8 /**************************************************************************
9  begin : May 2012
10  copyright : (C) 2012 Erwin Aertbelien
11  email : firstname.lastname@mech.kuleuven.ac.be
12 
13  History (only major changes)( AUTHOR-Description ) :
14 
15  ***************************************************************************
16  * This library is free software; you can redistribute it and/or *
17  * modify it under the terms of the GNU Lesser General Public *
18  * License as published by the Free Software Foundation; either *
19  * version 2.1 of the License, or (at your option) any later version. *
20  * *
21  * This library is distributed in the hope that it will be useful, *
22  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
23  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
24  * Lesser General Public License for more details. *
25  * *
26  * You should have received a copy of the GNU Lesser General Public *
27  * License along with this library; if not, write to the Free Software *
28  * Foundation, Inc., 59 Temple Place, *
29  * Suite 330, Boston, MA 02111-1307 USA *
30  * *
31  ***************************************************************************/
32 
33 
34 #include "chainiksolver.hpp"
35 #include "chain.hpp"
36 #include <Eigen/Dense>
37 
38 namespace KDL
39 {
40 
65 {
66 private:
67  typedef double ScalarType;
68  typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,Eigen::Dynamic> MatrixXq;
69  typedef Eigen::Matrix<ScalarType,Eigen::Dynamic,1> VectorXq;
70 public:
71 
93  const KDL::Chain& _chain,
94  const Eigen::Matrix<double,6,1>& _L,
95  double _eps=1E-5,
96  int _maxiter=500,
97  double _eps_joints=1E-15
98  );
99 
106  const KDL::Chain& _chain,
107  double _eps=1E-5,
108  int _maxiter=500,
109  double _eps_joints=1E-15
110  );
111 
123  virtual int CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out);
124 
128  virtual ~ChainIkSolverPos_LMA();
129 
135  void compute_fwdpos(const VectorXq& q);
136 
142  void compute_jacobian(const VectorXq& q);
143 
148  void display_jac(const KDL::JntArray& jval);
149 
150 
151 
152 
153 public:
154 
155 
160 
165 
170 
174  double lastRotDiff;
175 
180 
187 
200 
205 private:
206  // additional specification of the inverse position kinematics problem:
207 
208 
209  unsigned int maxiter;
210  double eps;
211  double eps_joints;
212  Eigen::Matrix<ScalarType,6,1> L;
214 
215 
216  // state of compute_fwdpos and compute_jacobian:
217  std::vector<KDL::Frame> T_base_jointroot;
218  std::vector<KDL::Frame> T_base_jointtip;
219  // need 2 vectors because of the somewhat strange definition of segment.hpp
220  // you could also recompute jointtip out of jointroot,
221  // but then you'll need more expensive cos/sin functions.
222 
223 
224  // the following are state of CartToJnt that is pre-allocated:
225 
229  Eigen::LDLT<MatrixXq> ldlt;
230  Eigen::JacobiSVD<MatrixXq> svd;
234 };
235 
236 
237 
238 
239 
240 }; // namespace KDL
241 
242 
243 
244 
245 
246 
247 #endif
ChainIkSolverPos_LMA(const KDL::Chain &_chain, const Eigen::Matrix< double, 6, 1 > &_L, double _eps=1E-5, int _maxiter=500, double _eps_joints=1E-15)
constructs an ChainIkSolverPos_LMA solver.
Definition: chainiksolverpos_lma.cpp:50
This abstract class encapsulates the inverse position solver for a KDL::Chain.
Definition: chainiksolver.hpp:42
unsigned int maxiter
Definition: chainiksolverpos_lma.hpp:209
Eigen::JacobiSVD< MatrixXq > svd
Definition: chainiksolverpos_lma.hpp:230
KDL::Frame T_base_head
for internal use only.
Definition: chainiksolverpos_lma.hpp:199
double lastRotDiff
contains the last value for the (unweighted) rotational difference after an execution of CartToJnt...
Definition: chainiksolverpos_lma.hpp:174
std::vector< KDL::Frame > T_base_jointtip
Definition: chainiksolverpos_lma.hpp:218
This class encapsulates a serial kinematic interconnection structure.
Definition: chain.hpp:35
double eps_joints
Definition: chainiksolverpos_lma.hpp:211
bool display_information
display information on each iteration step to the console.
Definition: chainiksolverpos_lma.hpp:204
void compute_jacobian(const VectorXq &q)
for internal use only.
Definition: chainiksolverpos_lma.cpp:131
MatrixXq A
Definition: chainiksolverpos_lma.hpp:227
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
void display_jac(const KDL::JntArray &jval)
for internal use only.
Definition: chainiksolverpos_lma.cpp:150
VectorXq lastSV
contains the last values for the singular values of the weighted Jacobian after an execution of CartT...
Definition: chainiksolverpos_lma.hpp:179
double eps
Definition: chainiksolverpos_lma.hpp:210
const KDL::Chain & chain
Definition: chainiksolverpos_lma.hpp:213
VectorXq q_new
Definition: chainiksolverpos_lma.hpp:232
MatrixXq jac
for internal use only.
Definition: chainiksolverpos_lma.hpp:186
Definition: articulatedbodyinertia.cpp:28
Solver for the inverse position kinematics that uses Levenberg-Marquardt.
Definition: chainiksolverpos_lma.hpp:64
int lastNrOfIter
contains the last number of iterations for an execution of CartToJnt.
Definition: chainiksolverpos_lma.hpp:159
Eigen::Matrix< ScalarType, 6, 1 > L
Definition: chainiksolverpos_lma.hpp:212
double lastDifference
contains the last value for after an execution of CartToJnt.
Definition: chainiksolverpos_lma.hpp:164
VectorXq grad
for internal use only.
Definition: chainiksolverpos_lma.hpp:193
void compute_fwdpos(const VectorXq &q)
for internal use only.
Definition: chainiksolverpos_lma.cpp:114
double lastTransDiff
contains the last value for the (unweighted) translational difference after an execution of CartToJnt...
Definition: chainiksolverpos_lma.hpp:169
Eigen::LDLT< MatrixXq > ldlt
Definition: chainiksolverpos_lma.hpp:229
VectorXq tmp
Definition: chainiksolverpos_lma.hpp:228
VectorXq q
Definition: chainiksolverpos_lma.hpp:226
std::vector< KDL::Frame > T_base_jointroot
Definition: chainiksolverpos_lma.hpp:217
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
virtual int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &T_base_goal, KDL::JntArray &q_out)
computes the inverse position kinematics.
Definition: chainiksolverpos_lma.cpp:160
VectorXq diffq
Definition: chainiksolverpos_lma.hpp:231
Eigen::Matrix< ScalarType, Eigen::Dynamic, Eigen::Dynamic > MatrixXq
Definition: chainiksolverpos_lma.hpp:68
virtual ~ChainIkSolverPos_LMA()
destructor.
Definition: chainiksolverpos_lma.cpp:112
VectorXq original_Aii
Definition: chainiksolverpos_lma.hpp:233
Eigen::Matrix< ScalarType, Eigen::Dynamic, 1 > VectorXq
Definition: chainiksolverpos_lma.hpp:69
double ScalarType
Definition: chainiksolverpos_lma.hpp:67