KDL  1.4.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 
64 class ChainIkSolverPos_LMA : public KDL::ChainIkSolverPos
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:
160  const KDL::Chain& chain;
161  unsigned int nj;
162  unsigned int ns;
163 
164 public:
165 
166 
170  int lastNrOfIter;
171 
175  double lastDifference;
176 
180  double lastTransDiff;
181 
185  double lastRotDiff;
186 
191 
197  MatrixXq jac;
198 
204  VectorXq grad;
211 
215  bool display_information;
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;
237  VectorXq tmp;
238  Eigen::LDLT<MatrixXq> ldlt;
239  Eigen::JacobiSVD<MatrixXq> svd;
240  VectorXq diffq;
241  VectorXq q_new;
243 };
244 
245 
246 
247 
248 
249 }; // namespace KDL
250 
251 
252 
253 
254 
255 
256 #endif
KDL::ChainIkSolverPos_LMA::strError
virtual const char * strError(const int error) const
Return a description of the latest error.
Definition: chainiksolverpos_lma.cpp:340
KDL::ChainIkSolverPos_LMA::T_base_jointtip
std::vector< KDL::Frame > T_base_jointtip
Definition: chainiksolverpos_lma.hpp:273
KDL::ChainIkSolverPos_LMA::svd
Eigen::JacobiSVD< MatrixXq > svd
Definition: chainiksolverpos_lma.hpp:285
KDL::Vector::data
double data[3]
Definition: frames.hpp:163
chain.hpp
KDL::ChainIkSolverPos_LMA::jac
MatrixXq jac
for internal use only.
Definition: chainiksolverpos_lma.hpp:243
KDL::Frame::Identity
static Frame Identity()
Definition: frames.inl:695
KDL::SolverI::strError
virtual const char * strError(const int error) const
Return a description of the latest error.
Definition: solveri.hpp:125
KDL::Twist::rot
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
KDL::JntArray
Definition: jntarray.hpp:69
KDL::ChainIkSolverPos_LMA::A
MatrixXq A
Definition: chainiksolverpos_lma.hpp:282
KDL::Segment::getJoint
const Joint & getJoint() const
Request the joint of the segment.
Definition: segment.hpp:118
KDL::ChainIkSolverPos_LMA::display_jac
void display_jac(const KDL::JntArray &jval)
for internal use only.
Definition: chainiksolverpos_lma.cpp:200
KDL::Frame::p
Vector p
origine of the Frame
Definition: frames.hpp:572
KDL::ChainIkSolverPos_LMA::maxiter
unsigned int maxiter
Definition: chainiksolverpos_lma.hpp:264
KDL::ChainIkSolverPos_LMA::T_base_head
KDL::Frame T_base_head
for internal use only.
Definition: chainiksolverpos_lma.hpp:256
KDL::ChainIkSolverPos_LMA::diffq
VectorXq diffq
Definition: chainiksolverpos_lma.hpp:286
KDL::ChainIkSolverPos_LMA::eps
double eps
Definition: chainiksolverpos_lma.hpp:265
KDL::SolverI::E_NOT_UP_TO_DATE
@ E_NOT_UP_TO_DATE
Chain size changed.
Definition: solveri.hpp:97
KDL::ChainIkSolverPos_LMA::lastRotDiff
double lastRotDiff
contains the last value for the (unweighted) rotational difference after an execution of CartToJnt.
Definition: chainiksolverpos_lma.hpp:231
KDL
Definition: articulatedbodyinertia.cpp:28
KDL::Chain::getNrOfSegments
unsigned int getNrOfSegments() const
Request the total number of segments in the chain.
Definition: chain.hpp:76
KDL::ChainIkSolverPos_LMA::tmp
VectorXq tmp
Definition: chainiksolverpos_lma.hpp:283
KDL::ChainIkSolverPos_LMA::compute_jacobian
void compute_jacobian(const VectorXq &q)
for internal use only.
Definition: chainiksolverpos_lma.cpp:181
KDL::ChainIkSolverPos_LMA::q_new
VectorXq q_new
Definition: chainiksolverpos_lma.hpp:287
KDL::SolverI::E_MAX_ITERATIONS_EXCEEDED
@ E_MAX_ITERATIONS_EXCEEDED
Maximum number of iterations exceeded.
Definition: solveri.hpp:101
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:720
KDL::Twist_to_Eigen
void Twist_to_Eigen(const KDL::Twist &t, Eigen::MatrixBase< Derived > &e)
Definition: chainiksolverpos_lma.cpp:63
KDL::ChainIkSolverPos_LMA::nj
unsigned int nj
Definition: chainiksolverpos_lma.hpp:207
KDL::JntArray::rows
unsigned int rows() const
Returns the number of rows (size) of the array.
Definition: jntarray.cpp:72
chainiksolverpos_lma.hpp
computing inverse position kinematics using Levenberg-Marquardt.
chainiksolver.hpp
KDL::Chain::getSegment
const Segment & getSegment(unsigned int nr) const
Request the nr'd segment of the chain.
Definition: chain.cpp:68
KDL::SolverI::E_SIZE_MISMATCH
@ E_SIZE_MISMATCH
Input size does not match internal state.
Definition: solveri.hpp:99
KDL::ChainIkSolverPos_LMA::display_information
bool display_information
display information on each iteration step to the console.
Definition: chainiksolverpos_lma.hpp:261
KDL::Segment::pose
Frame pose(const double &q) const
Request the pose of the segment, given the joint position q.
Definition: segment.cpp:57
KDL::ChainIkSolverPos_LMA::compute_fwdpos
void compute_fwdpos(const VectorXq &q)
for internal use only.
Definition: chainiksolverpos_lma.cpp:164
KDL::ChainIkSolverPos_LMA::E_GRADIENT_JOINTS_TOO_SMALL
static const int E_GRADIENT_JOINTS_TOO_SMALL
Definition: chainiksolverpos_lma.hpp:118
KDL::Frame
Definition: frames.hpp:570
KDL::Segment::twist
Twist twist(const double &q, const double &qdot) const
Request the 6D-velocity of the tip of the segment, given the joint position q and the joint velocity ...
Definition: segment.cpp:62
KDL::JntArray::data
Eigen::VectorXd data
Definition: jntarray.hpp:72
KDL::Joint::None
@ None
Definition: joint.hpp:47
KDL::ChainIkSolverPos_LMA::chain
const KDL::Chain & chain
Definition: chainiksolverpos_lma.hpp:206
KDL::Twist::vel
Vector vel
The velocity of that point.
Definition: frames.hpp:722
KDL::Segment
Definition: segment.hpp:46
KDL::ChainIkSolverPos_LMA::ldlt
Eigen::LDLT< MatrixXq > ldlt
Definition: chainiksolverpos_lma.hpp:284
KDL::ChainIkSolverPos_LMA::L
Eigen::Matrix< ScalarType, 6, 1 > L
Definition: chainiksolverpos_lma.hpp:267
KDL::ChainIkSolverPos_LMA::lastSV
VectorXq lastSV
contains the last values for the singular values of the weighted Jacobian after an execution of CartT...
Definition: chainiksolverpos_lma.hpp:236
KDL::ChainIkSolverPos_LMA::T_base_jointroot
std::vector< KDL::Frame > T_base_jointroot
Definition: chainiksolverpos_lma.hpp:272
KDL::ChainIkSolverPos_LMA::~ChainIkSolverPos_LMA
virtual ~ChainIkSolverPos_LMA()
destructor.
Definition: chainiksolverpos_lma.cpp:162
KDL::ChainIkSolverPos_LMA::ns
unsigned int ns
Definition: chainiksolverpos_lma.hpp:208
KDL::ChainIkSolverPos_LMA::lastTransDiff
double lastTransDiff
contains the last value for the (unweighted) translational difference after an execution of CartToJnt...
Definition: chainiksolverpos_lma.hpp:226
KDL::ChainIkSolverPos_LMA::VectorXq
Eigen::Matrix< ScalarType, Eigen::Dynamic, 1 > VectorXq
Definition: chainiksolverpos_lma.hpp:115
KDL::ChainIkSolverPos_LMA::updateInternalDataStructures
void updateInternalDataStructures()
Update the internal data structures.
Definition: chainiksolverpos_lma.cpp:145
KDL::ChainIkSolverPos_LMA::lastDifference
double lastDifference
contains the last value for after an execution of CartToJnt.
Definition: chainiksolverpos_lma.hpp:221
KDL::ChainIkSolverPos_LMA::grad
VectorXq grad
for internal use only.
Definition: chainiksolverpos_lma.hpp:250
KDL::Joint::getType
const JointType & getType() const
Request the type of the joint.
Definition: joint.hpp:159
KDL::ChainIkSolverPos_LMA::ChainIkSolverPos_LMA
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:73
KDL::ChainIkSolverPos_LMA::CartToJnt
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:210
KDL::ChainIkSolverPos
Definition: chainiksolver.hpp:42
KDL::diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
determines the difference of vector b with vector a.
KDL::SolverI::E_NOERROR
@ E_NOERROR
No error.
Definition: solveri.hpp:91
KDL::ChainIkSolverPos_LMA::eps_joints
double eps_joints
Definition: chainiksolverpos_lma.hpp:266
KDL::ChainIkSolverPos_LMA::E_INCREMENT_JOINTS_TOO_SMALL
static const int E_INCREMENT_JOINTS_TOO_SMALL
Definition: chainiksolverpos_lma.hpp:119
KDL::ChainIkSolverPos_LMA::MatrixXq
Eigen::Matrix< ScalarType, Eigen::Dynamic, Eigen::Dynamic > MatrixXq
Definition: chainiksolverpos_lma.hpp:114
KDL::Chain
Definition: chain.hpp:35
KDL::ChainIkSolverPos_LMA::ScalarType
double ScalarType
Definition: chainiksolverpos_lma.hpp:113
KDL::Chain::getNrOfJoints
unsigned int getNrOfJoints() const
Request the total number of joints in the chain.
Definition: chain.hpp:71
KDL::ChainIkSolverPos_LMA::original_Aii
VectorXq original_Aii
Definition: chainiksolverpos_lma.hpp:288
KDL::SolverI::error
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
KDL::ChainIkSolverPos_LMA::lastNrOfIter
int lastNrOfIter
contains the last number of iterations for an execution of CartToJnt.
Definition: chainiksolverpos_lma.hpp:216
KDL::ChainIkSolverPos_LMA::q
VectorXq q
Definition: chainiksolverpos_lma.hpp:281