orocos_kdl
chainiksolverpos_lma.cpp
Go to the documentation of this file.
1 
6 /**************************************************************************
7  begin : May 2012
8  copyright : (C) 2012 Erwin Aertbelien
9  email : firstname.lastname@mech.kuleuven.ac.be
10 
11  History (only major changes)( AUTHOR-Description ) :
12 
13  ***************************************************************************
14  * This library is free software; you can redistribute it and/or *
15  * modify it under the terms of the GNU Lesser General Public *
16  * License as published by the Free Software Foundation; either *
17  * version 2.1 of the License, or (at your option) any later version. *
18  * *
19  * This library is distributed in the hope that it will be useful, *
20  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
21  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU *
22  * Lesser General Public License for more details. *
23  * *
24  * You should have received a copy of the GNU Lesser General Public *
25  * License along with this library; if not, write to the Free Software *
26  * Foundation, Inc., 59 Temple Place, *
27  * Suite 330, Boston, MA 02111-1307 USA *
28  * *
29  ***************************************************************************/
30 
31 #include "chainiksolverpos_lma.hpp"
32 #include <iostream>
33 
34 namespace KDL {
35 
36 
37 
38 
39 template <typename Derived>
40 inline void Twist_to_Eigen(const KDL::Twist& t,Eigen::MatrixBase<Derived>& e) {
41  e(0)=t.vel.data[0];
42  e(1)=t.vel.data[1];
43  e(2)=t.vel.data[2];
44  e(3)=t.rot.data[0];
45  e(4)=t.rot.data[1];
46  e(5)=t.rot.data[2];
47 }
48 
49 
51  const KDL::Chain& _chain,
52  const Eigen::Matrix<double,6,1>& _l,
53  double _eps,
54  int _maxiter,
55  double _eps_joints
56 ) :
57  chain(_chain),
58  nj(chain.getNrOfJoints()),
59  ns(chain.getNrOfSegments()),
60  lastNrOfIter(0),
61  lastDifference(0),
62  lastTransDiff(0),
63  lastRotDiff(0),
64  lastSV(nj),
65  jac(6, nj),
66  grad(nj),
67  display_information(false),
68  maxiter(_maxiter),
69  eps(_eps),
70  eps_joints(_eps_joints),
71  L(_l.cast<ScalarType>()),
72  T_base_jointroot(nj),
73  T_base_jointtip(nj),
74  q(nj),
75  A(nj, nj),
76  tmp(nj),
77  ldlt(nj),
78  svd(6, nj,Eigen::ComputeThinU | Eigen::ComputeThinV),
79  diffq(nj),
80  q_new(nj),
81  original_Aii(nj)
82 {}
83 
85  const KDL::Chain& _chain,
86  double _eps,
87  int _maxiter,
88  double _eps_joints
89 ) :
90  chain(_chain),
91  nj(chain.getNrOfJoints()),
92  ns(chain.getNrOfSegments()),
93  lastNrOfIter(0),
94  lastDifference(0),
95  lastTransDiff(0),
96  lastRotDiff(0),
97  lastSV(nj>6?6:nj),
98  jac(6, nj),
99  grad(nj),
100  display_information(false),
101  maxiter(_maxiter),
102  eps(_eps),
103  eps_joints(_eps_joints),
104  T_base_jointroot(nj),
105  T_base_jointtip(nj),
106  q(nj),
107  A(nj, nj),
108  ldlt(nj),
109  svd(6, nj,Eigen::ComputeThinU | Eigen::ComputeThinV),
110  diffq(nj),
111  q_new(nj),
112  original_Aii(nj)
113 {
114  L(0)=1;
115  L(1)=1;
116  L(2)=1;
117  L(3)=0.01;
118  L(4)=0.01;
119  L(5)=0.01;
120 }
121 
123  nj = chain.getNrOfJoints();
125  lastSV.conservativeResize(nj>6?6:nj);
126  jac.conservativeResize(Eigen::NoChange, nj);
127  grad.conservativeResize(nj);
130  q.conservativeResize(nj);
131  A.conservativeResize(nj, nj);
132  ldlt = Eigen::LDLT<MatrixXq>(nj);
133  svd = Eigen::JacobiSVD<MatrixXq>(6, nj,Eigen::ComputeThinU | Eigen::ComputeThinV);
134  diffq.conservativeResize(nj);
135  q_new.conservativeResize(nj);
136  original_Aii.conservativeResize(nj);
137 }
138 
140 
141 void ChainIkSolverPos_LMA::compute_fwdpos(const VectorXq& q) {
142  using namespace KDL;
143  unsigned int jointndx=0;
144  T_base_head = Frame::Identity(); // frame w.r.t. base of head
145  for (unsigned int i=0;i<chain.getNrOfSegments();i++) {
146  const Segment& segment = chain.getSegment(i);
147  if (segment.getJoint().getType()!=Joint::Fixed) {
148  T_base_jointroot[jointndx] = T_base_head;
149  T_base_head = T_base_head * segment.pose(q(jointndx));
150  T_base_jointtip[jointndx] = T_base_head;
151  jointndx++;
152  } else {
153  T_base_head = T_base_head * segment.pose(0.0);
154  }
155  }
156 }
157 
159  using namespace KDL;
160  unsigned int jointndx=0;
161  for (unsigned int i=0;i<chain.getNrOfSegments();i++) {
162  const Segment& segment = chain.getSegment(i);
163  if (segment.getJoint().getType()!=Joint::Fixed) {
164  // compute twist of the end effector motion caused by joint [jointndx]; expressed in base frame, with vel. ref. point equal to the end effector
165  KDL::Twist t = ( T_base_jointroot[jointndx].M * segment.twist(q(jointndx),1.0) ).RefPoint( T_base_head.p - T_base_jointtip[jointndx].p);
166  jac(0,jointndx)=t[0];
167  jac(1,jointndx)=t[1];
168  jac(2,jointndx)=t[2];
169  jac(3,jointndx)=t[3];
170  jac(4,jointndx)=t[4];
171  jac(5,jointndx)=t[5];
172  jointndx++;
173  }
174  }
175 }
176 
178  VectorXq q;
179  q = jval.data.cast<ScalarType>();
180  compute_fwdpos(q);
182  svd.compute(jac);
183  std::cout << "Singular values : " << svd.singularValues().transpose()<<"\n";
184 }
185 
186 
187 int ChainIkSolverPos_LMA::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& T_base_goal, KDL::JntArray& q_out) {
188  if (nj != chain.getNrOfJoints())
189  return (error = E_NOT_UP_TO_DATE);
190 
191  if (nj != q_init.rows() || nj != q_out.rows())
192  return (error = E_SIZE_MISMATCH);
193 
194  using namespace KDL;
195  double v = 2;
196  double tau = 10;
197  double rho;
198  double lambda;
199  Twist t;
200  double delta_pos_norm;
201  Eigen::Matrix<ScalarType,6,1> delta_pos;
202  Eigen::Matrix<ScalarType,6,1> delta_pos_new;
203 
204 
205  q=q_init.data.cast<ScalarType>();
206  compute_fwdpos(q);
207  Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
208  delta_pos=L.asDiagonal()*delta_pos;
209  delta_pos_norm = delta_pos.norm();
210  if (delta_pos_norm<eps) {
211  lastNrOfIter =0 ;
212  Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
213  lastDifference = delta_pos.norm();
214  lastTransDiff = delta_pos.topRows(3).norm();
215  lastRotDiff = delta_pos.bottomRows(3).norm();
216  svd.compute(jac);
217  original_Aii = svd.singularValues();
218  lastSV = svd.singularValues();
219  q_out.data = q.cast<double>();
220  return (error = E_NOERROR);
221  }
223  jac = L.asDiagonal()*jac;
224 
225  lambda = tau;
226  double dnorm = 1;
227  for (unsigned int i=0;i<maxiter;++i) {
228 
229  svd.compute(jac);
230  original_Aii = svd.singularValues();
231  for (unsigned int j=0;j<original_Aii.rows();++j) {
232  original_Aii(j) = original_Aii(j)/( original_Aii(j)*original_Aii(j)+lambda);
233 
234  }
235  tmp = svd.matrixU().transpose()*delta_pos;
236  tmp = original_Aii.cwiseProduct(tmp);
237  diffq = svd.matrixV()*tmp;
238  grad = jac.transpose()*delta_pos;
239  if (display_information) {
240  std::cout << "------- iteration " << i << " ----------------\n"
241  << " q = " << q.transpose() << "\n"
242  << " weighted jac = \n" << jac << "\n"
243  << " lambda = " << lambda << "\n"
244  << " eigenvalues = " << svd.singularValues().transpose() << "\n"
245  << " difference = " << delta_pos.transpose() << "\n"
246  << " difference norm= " << delta_pos_norm << "\n"
247  << " proj. on grad. = " << grad << "\n";
248  std::cout << std::endl;
249  }
250  dnorm = diffq.lpNorm<Eigen::Infinity>();
251  if (dnorm < eps_joints) {
252  lastDifference = delta_pos_norm;
253  lastNrOfIter = i;
254  lastSV = svd.singularValues();
255  q_out.data = q.cast<double>();
256  compute_fwdpos(q);
257  Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
258  lastTransDiff = delta_pos.topRows(3).norm();
259  lastRotDiff = delta_pos.bottomRows(3).norm();
261  }
262 
263 
264  if (grad.transpose()*grad < eps_joints*eps_joints ) {
265  compute_fwdpos(q);
266  Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
267  lastDifference = delta_pos_norm;
268  lastTransDiff = delta_pos.topRows(3).norm();
269  lastRotDiff = delta_pos.bottomRows(3).norm();
270  lastSV = svd.singularValues();
271  lastNrOfIter = i;
272  q_out.data = q.cast<double>();
274  }
275 
276  q_new = q+diffq;
278  Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos_new );
279  delta_pos_new = L.asDiagonal()*delta_pos_new;
280  double delta_pos_new_norm = delta_pos_new.norm();
281  rho = delta_pos_norm*delta_pos_norm - delta_pos_new_norm*delta_pos_new_norm;
282  rho /= diffq.transpose()*(lambda*diffq + grad);
283  if (rho > 0) {
284  q = q_new;
285  delta_pos = delta_pos_new;
286  delta_pos_norm = delta_pos_new_norm;
287  if (delta_pos_norm<eps) {
288  Twist_to_Eigen( diff( T_base_head, T_base_goal), delta_pos );
289  lastDifference = delta_pos_norm;
290  lastTransDiff = delta_pos.topRows(3).norm();
291  lastRotDiff = delta_pos.bottomRows(3).norm();
292  lastSV = svd.singularValues();
293  lastNrOfIter = i;
294  q_out.data = q.cast<double>();
295  return (error = E_NOERROR);
296  }
298  jac = L.asDiagonal()*jac;
299  double tmp=2*rho-1;
300  lambda = lambda*max(1/3.0, 1-tmp*tmp*tmp);
301  v = 2;
302  } else {
303  lambda = lambda*v;
304  v = 2*v;
305  }
306  }
307  lastDifference = delta_pos_norm;
308  lastTransDiff = delta_pos.topRows(3).norm();
309  lastRotDiff = delta_pos.bottomRows(3).norm();
310  lastSV = svd.singularValues();
312  q_out.data = q.cast<double>();
313  return (error = E_MAX_ITERATIONS_EXCEEDED);
314 
315 }
316 
317 const char* ChainIkSolverPos_LMA::strError(const int error) const
318  {
319  if (E_GRADIENT_JOINTS_TOO_SMALL == error) return "The gradient of E towards the joints is to small";
320  else if (E_INCREMENT_JOINTS_TOO_SMALL == error) return "The joint position increments are to small";
321  else return SolverI::strError(error);
322  }
323 
324 }//namespace KDL
KDL::ChainIkSolverPos_LMA::strError
virtual const char * strError(const int error) const
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:165
std::vector::resize
T resize(T... args)
KDL::ChainIkSolverPos_LMA::jac
MatrixXq jac
for internal use only.
Definition: chainiksolverpos_lma.hpp:243
KDL::Frame::Identity
static Frame Identity()
Definition: frames.inl:700
KDL::SolverI::strError
virtual const char * strError(const int error) const
Definition: solveri.hpp:125
KDL::Twist::rot
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:726
KDL::JntArray
Definition: jntarray.hpp:69
KDL::ChainIkSolverPos_LMA::A
MatrixXq A
Definition: chainiksolverpos_lma.hpp:282
KDL::Segment::getJoint
const Joint & getJoint() const
Definition: segment.hpp:118
KDL::ChainIkSolverPos_LMA::display_jac
void display_jac(const KDL::JntArray &jval)
for internal use only. Only exposed for test and diagnostic purposes.
Definition: chainiksolverpos_lma.cpp:200
KDL::Frame::p
Vector p
origine of the Frame
Definition: frames.hpp:574
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::Joint::Fixed
@ Fixed
Definition: joint.hpp:47
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::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
iostream
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::Chain::getNrOfSegments
unsigned int getNrOfSegments() const
Definition: chain.hpp:76
KDL::ChainIkSolverPos_LMA::tmp
VectorXq tmp
Definition: chainiksolverpos_lma.hpp:283
std::cout
KDL::ChainIkSolverPos_LMA::compute_jacobian
void compute_jacobian(const VectorXq &q)
for internal use only. Only exposed for test and diagnostic purposes. compute_fwdpos(q) should always...
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:723
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
Definition: jntarray.cpp:70
chainiksolverpos_lma.hpp
computing inverse position kinematics using Levenberg-Marquardt.
KDL::Chain::getSegment
const Segment & getSegment(unsigned int nr) const
Definition: chain.cpp:67
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
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
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
KDL::max
double max(double a, double b)
Definition: utility.h:204
KDL::Segment::twist
Twist twist(const double &q, const double &qdot) const
Definition: segment.cpp:62
KDL::JntArray::data
Eigen::VectorXd data
Definition: jntarray.hpp:72
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:725
KDL::Segment
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body ine...
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
std::endl
T endl(T... args)
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()
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
Definition: joint.hpp:159
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::diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
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::Chain
This class encapsulates a serial kinematic interconnection structure. It is built out of segments.
Definition: chain.hpp:35
KDL::ChainIkSolverPos_LMA::ScalarType
double ScalarType
Definition: chainiksolverpos_lma.hpp:113
KDL::Chain::getNrOfJoints
unsigned int getNrOfJoints() const
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