chainiksolverpos_lma.cpp
Go to the documentation of this file.
1 
6 /**************************************************************************
7  begin : May 2012
8  copyright : (C) 2020 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),
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),
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);
128  T_base_jointroot.resize(nj);
129  T_base_jointtip.resize(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 
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);
181  compute_jacobian(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
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 class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body...
Definition: segment.hpp:46
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
Vector vel
The velocity of that point.
Definition: frames.hpp:722
unsigned int getNrOfSegments() const
Definition: chain.hpp:76
Chain size changed.
Definition: solveri.hpp:97
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...
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
Definition: frames.hpp:1130
Maximum number of iterations exceeded.
Definition: solveri.hpp:101
This class represents an fixed size array containing joint values of a KDL::Chain.
Definition: jntarray.hpp:69
const Segment & getSegment(unsigned int nr) const
Definition: chain.cpp:68
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...
computing inverse position kinematics using Levenberg-Marquardt.
Input size does not match internal state.
Definition: solveri.hpp:99
unsigned int rows() const
Definition: jntarray.cpp:72
static const int E_GRADIENT_JOINTS_TOO_SMALL
const Joint & getJoint() const
Definition: segment.hpp:118
represents both translational and rotational velocities.
Definition: frames.hpp:720
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
MatrixXq jac
for internal use only.
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
double data[3]
Definition: frames.hpp:163
int lastNrOfIter
contains the last number of iterations for an execution of CartToJnt.
Frame pose(const double &q) const
Definition: segment.cpp:57
Eigen::VectorXd data
Definition: jntarray.hpp:72
Vector p
origine of the Frame
Definition: frames.hpp:572
const JointType & getType() const
Definition: joint.hpp:159
Eigen::Matrix< ScalarType, 6, 1 > L
double lastDifference
contains the last value for after an execution of CartToJnt.
void Twist_to_Eigen(const KDL::Twist &t, Eigen::MatrixBase< Derived > &e)
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
static Frame Identity()
Definition: frames.hpp:701
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
static const int E_INCREMENT_JOINTS_TOO_SMALL
virtual const char * strError(const int error) const
Definition: solveri.hpp:125
virtual int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &T_base_goal, KDL::JntArray &q_out)
computes the inverse position kinematics.
Twist twist(const double &q, const double &qdot) const
Definition: segment.cpp:62
virtual const char * strError(const int error) const
virtual ~ChainIkSolverPos_LMA()
destructor.
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
double max(double a, double b)
Definition: utility.h:198
Eigen::Matrix< ScalarType, Eigen::Dynamic, 1 > VectorXq


orocos_kdl
Author(s):
autogenerated on Wed Jul 1 2020 03:17:39