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 
72  static const int E_GRADIENT_JOINTS_TOO_SMALL = -100;
73  static const int E_INCREMENT_JOINTS_TOO_SMALL = -101;
74 
96  const KDL::Chain& _chain,
97  const Eigen::Matrix<double,6,1>& _L,
98  double _eps=1E-5,
99  int _maxiter=500,
100  double _eps_joints=1E-15
101  );
102 
109  const KDL::Chain& _chain,
110  double _eps=1E-5,
111  int _maxiter=500,
112  double _eps_joints=1E-15
113  );
114 
126  virtual int CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out);
127 
131  virtual ~ChainIkSolverPos_LMA();
132 
138  void compute_fwdpos(const VectorXq& q);
139 
145  void compute_jacobian(const VectorXq& q);
146 
151  void display_jac(const KDL::JntArray& jval);
152 
155 
157  virtual const char* strError(const int error) const;
158 
159 private:
161  unsigned int nj;
162  unsigned int ns;
163 
164 public:
165 
166 
171 
176 
181 
185  double lastRotDiff;
186 
190  VectorXq lastSV;
191 
197  MatrixXq jac;
198 
204  VectorXq grad;
211 
216 private:
217  // additional specification of the inverse position kinematics problem:
218  unsigned int maxiter;
219  double eps;
220  double eps_joints;
221  Eigen::Matrix<ScalarType,6,1> L;
222 
223 
224 
225  // state of compute_fwdpos and compute_jacobian:
226  std::vector<KDL::Frame> T_base_jointroot;
227  std::vector<KDL::Frame> T_base_jointtip;
228  // need 2 vectors because of the somewhat strange definition of segment.hpp
229  // you could also recompute jointtip out of jointroot,
230  // but then you'll need more expensive cos/sin functions.
231 
232 
233  // the following are state of CartToJnt that is pre-allocated:
234 
235  VectorXq q;
236  MatrixXq A;
237  VectorXq tmp;
238  Eigen::LDLT<MatrixXq> ldlt;
239  Eigen::JacobiSVD<MatrixXq> svd;
240  VectorXq diffq;
241  VectorXq q_new;
242  VectorXq original_Aii;
243 };
244 
245 
246 
247 
248 
249 }; // namespace KDL
250 
251 
252 
253 
254 
255 
256 #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.
This abstract class encapsulates the inverse position solver for a KDL::Chain.
Eigen::JacobiSVD< MatrixXq > svd
KDL::Frame T_base_head
for internal use only.
double lastRotDiff
contains the last value for the (unweighted) rotational difference after an execution of CartToJnt...
std::vector< KDL::Frame > T_base_jointtip
This class encapsulates a serial kinematic interconnection structure. It is built out of segments...
Definition: chain.hpp:35
bool display_information
display information on each iteration step to the console.
void compute_jacobian(const VectorXq &q)
for internal use only. Only exposed for test and diagnostic purposes. compute_fwdpos(q) should always...
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. Only exposed for test and diagnostic purposes.
VectorXq lastSV
contains the last values for the singular values of the weighted Jacobian after an execution of CartT...
static const int E_GRADIENT_JOINTS_TOO_SMALL
MatrixXq jac
for internal use only.
Solver for the inverse position kinematics that uses Levenberg-Marquardt.
int lastNrOfIter
contains the last number of iterations for an execution of CartToJnt.
Eigen::Matrix< ScalarType, 6, 1 > L
double lastDifference
contains the last value for after an execution of CartToJnt.
VectorXq grad
for internal use only.
void compute_fwdpos(const VectorXq &q)
for internal use only.
double lastTransDiff
contains the last value for the (unweighted) translational difference after an execution of CartToJnt...
Eigen::LDLT< MatrixXq > ldlt
std::vector< KDL::Frame > T_base_jointroot
virtual const char * strError(const int error) const
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
static const int E_INCREMENT_JOINTS_TOO_SMALL
virtual int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &T_base_goal, KDL::JntArray &q_out)
computes the inverse position kinematics.
Eigen::Matrix< ScalarType, Eigen::Dynamic, Eigen::Dynamic > MatrixXq
virtual ~ChainIkSolverPos_LMA()
destructor.
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
Eigen::Matrix< ScalarType, Eigen::Dynamic, 1 > VectorXq


orocos_kdl
Author(s):
autogenerated on Sat Jun 15 2019 19:07:36