chainidsolver_vereshchagin.cpp
Go to the documentation of this file.
1 // Copyright (C) 2020 Ruben Smits <ruben dot smits at intermodalics dot eu>
2 
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
5 // Author: Herman Bruyninckx
6 // Author: Azamat Shakhimardanov
7 // Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
8 // URL: http://www.orocos.org/kdl
9 
10 // This library is free software; you can redistribute it and/or
11 // modify it under the terms of the GNU Lesser General Public
12 // License as published by the Free Software Foundation; either
13 // version 2.1 of the License, or (at your option) any later version.
14 
15 // This library is distributed in the hope that it will be useful,
16 // but WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18 // Lesser General Public License for more details.
19 
20 // You should have received a copy of the GNU Lesser General Public
21 // License along with this library; if not, write to the Free Software
22 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
23 
25 #include "frames_io.hpp"
27 
28 
29 namespace KDL
30 {
31 using namespace Eigen;
32 
33 ChainIdSolver_Vereshchagin::ChainIdSolver_Vereshchagin(const Chain& chain_, Twist root_acc, unsigned int _nc) :
34  chain(chain_), nj(chain.getNrOfJoints()), ns(chain.getNrOfSegments()), nc(_nc),
35  results(ns + 1, segment_info(nc))
36 {
37  acc_root = root_acc;
38 
39  //Provide the necessary memory for computing the inverse of M0
40  nu_sum.resize(nc);
41  M_0_inverse.resize(nc, nc);
42  Um = MatrixXd::Identity(nc, nc);
43  Vm = MatrixXd::Identity(nc, nc);
44  Sm = VectorXd::Ones(nc);
45  tmpm = VectorXd::Ones(nc);
46 }
47 
50  results.resize(ns+1,segment_info(nc));
51 }
52 
53 int ChainIdSolver_Vereshchagin::CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques)
54 {
56  if(ns != chain.getNrOfSegments())
57  return (error = E_NOT_UP_TO_DATE);
58  //Check sizes always
59  if (q.rows() != nj || q_dot.rows() != nj || q_dotdot.rows() != nj || torques.rows() != nj || f_ext.size() != ns)
60  return (error = E_SIZE_MISMATCH);
61  if (alfa.columns() != nc || beta.rows() != nc)
62  return (error = E_SIZE_MISMATCH);
63  //do an upward recursion for position and velocities
64  this->initial_upwards_sweep(q, q_dot, q_dotdot, f_ext);
65  //do an inward recursion for inertia, forces and constraints
66  this->downwards_sweep(alfa, torques);
67  //Solve for the constraint forces
68  this->constraint_calculation(beta);
69  //do an upward recursion to propagate the result
70  this->final_upwards_sweep(q_dotdot, torques);
71  return (error = E_NOERROR);
72 }
73 
74 void ChainIdSolver_Vereshchagin::initial_upwards_sweep(const JntArray &q, const JntArray &qdot, const JntArray &qdotdot, const Wrenches& f_ext)
75 {
76  //if (q.rows() != nj || qdot.rows() != nj || qdotdot.rows() != nj || f_ext.size() != ns)
77  // return -1;
78 
79  unsigned int j = 0;
81  for (unsigned int i = 0; i < ns; i++)
82  {
83  //Express everything in the segments reference frame (body coordinates)
84  //which is at the segments tip, i.e. where the next joint is attached.
85 
86  //Calculate segment properties: X,S,vj,cj
87  const Segment& segment = chain.getSegment(i);
88  segment_info& s = results[i + 1];
89  //The pose between the joint root and the segment tip (tip expressed in joint root coordinates)
90  s.F = segment.pose(q(j)); //X pose of each link in link coord system
91 
92  F_total = F_total * s.F; //X pose of the each link in root coord system
93  s.F_base = F_total; //X pose of the each link in root coord system for getter functions
94 
95  //The velocity due to the joint motion of the segment expressed in the segments reference frame (tip)
96  Twist vj = s.F.M.Inverse(segment.twist(q(j), qdot(j))); //XDot of each link
97  //Twist aj = s.F.M.Inverse(segment.twist(q(j), qdotdot(j))); //XDotDot of each link
98 
99  //The unit velocity due to the joint motion of the segment expressed in the segments reference frame (tip)
100  s.Z = s.F.M.Inverse(segment.twist(q(j), 1.0));
101  //Put Z in the joint root reference frame:
102  s.Z = s.F * s.Z;
103 
104  //The total velocity of the segment expressed in the segments reference frame (tip)
105  if (i != 0)
106  {
107  s.v = s.F.Inverse(results[i].v) + vj; // recursive velocity of each link in segment frame
108  //s.A=s.F.Inverse(results[i].A)+aj;
109  s.A = s.F.M.Inverse(results[i].A);
110  }
111  else
112  {
113  s.v = vj;
114  s.A = s.F.M.Inverse(acc_root);
115  }
116  //c[i] = cj + v[i]xvj (remark: cj=0, since our S is not time dependent in local coordinates)
117  //The velocity product acceleration
118  //std::cout << i << " Initial upward" << s.v << std::endl;
119  s.C = s.v*vj; //This is a cross product: cartesian space BIAS acceleration in local link coord.
120  //Put C in the joint root reference frame
121  s.C = s.F * s.C; //+F_total.M.Inverse(acc_root));
122  //The rigid body inertia of the segment, expressed in the segments reference frame (tip)
123  s.H = segment.getInertia();
124 
125  //wrench of the rigid body bias forces and the external forces on the segment (in body coordinates, tip)
126  //external forces are taken into account through s.U.
127  Wrench FextLocal = F_total.M.Inverse() * f_ext[i];
128  s.U = s.v * (s.H * s.v) - FextLocal; //f_ext[i];
129  if (segment.getJoint().getType() != Joint::Fixed)
130  j++;
131  }
132 
133 }
134 
136 {
137  int j = nj - 1;
138  for (int i = ns; i >= 0; i--)
139  {
140  //Get a handle for the segment we are working on.
141  segment_info& s = results[i];
142  //For segment N,
143  //tilde is in the segment refframe (tip, not joint root)
144  //without tilde is at the joint root (the childs tip!!!)
145  //P_tilde is the articulated body inertia
146  //R_tilde is the sum of external and coriolis/centrifugal forces
147  //M is the (unit) acceleration energy already generated at link i
148  //G is the (unit) magnitude of the constraint forces at link i
149  //E are the (unit) constraint forces due to the constraints
150  if (i == (int)ns)
151  {
152  s.P_tilde = s.H;
153  s.R_tilde = s.U;
154  s.M.setZero();
155  s.G.setZero();
156  //changeBase(alfa_N,F_total.M.Inverse(),alfa_N2);
157  for (unsigned int r = 0; r < 3; r++)
158  for (unsigned int c = 0; c < nc; c++)
159  {
160  //copy alfa constrain force matrix in E~
161  s.E_tilde(r, c) = alfa(r + 3, c);
162  s.E_tilde(r + 3, c) = alfa(r, c);
163  }
164  //Change the reference frame of alfa to the segmentN tip frame
165  //F_Total holds end effector frame, if done per segment bases then constraints could be extended to all segments
166  Rotation base_to_end = F_total.M.Inverse();
167  for (unsigned int c = 0; c < nc; c++)
168  {
169  Wrench col(Vector(s.E_tilde(3, c), s.E_tilde(4, c), s.E_tilde(5, c)),
170  Vector(s.E_tilde(0, c), s.E_tilde(1, c), s.E_tilde(2, c)));
171  col = base_to_end*col;
172  s.E_tilde.col(c) << Vector3d::Map(col.torque.data), Vector3d::Map(col.force.data);
173  }
174  }
175  else
176  {
177  //For all others:
178  //Everything should expressed in the body coordinates of segment i
179  segment_info& child = results[i + 1];
180  //Copy PZ into a vector so we can do matrix manipulations, put torques above forces
181  Vector6d vPZ;
182  vPZ << Vector3d::Map(child.PZ.torque.data), Vector3d::Map(child.PZ.force.data);
183  Matrix6d PZDPZt;
184  PZDPZt.noalias() = vPZ * vPZ.transpose();
185  PZDPZt /= child.D;
186 
187  //equation a) (see Vereshchagin89) PZDPZt=[I,H;H',M]
188  //Azamat:articulated body inertia as in Featherstone (7.19)
189  s.P_tilde = s.H + child.P - ArticulatedBodyInertia(PZDPZt.bottomRightCorner<3,3>(), PZDPZt.topRightCorner<3,3>(), PZDPZt.topLeftCorner<3,3>());
190  //equation b) (see Vereshchagin89)
191  //Azamat: bias force as in Featherstone (7.20)
192  s.R_tilde = s.U + child.R + child.PC + (child.PZ / child.D) * child.u;
193  //equation c) (see Vereshchagin89)
194  s.E_tilde = child.E;
195 
196  //Azamat: equation (c) right side term
197  s.E_tilde.noalias() -= (vPZ * child.EZ.transpose()) / child.D;
198 
199  //equation d) (see Vereshchagin89)
200  s.M = child.M;
201  //Azamat: equation (d) right side term
202  s.M.noalias() -= (child.EZ * child.EZ.transpose()) / child.D;
203 
204  //equation e) (see Vereshchagin89)
205  s.G = child.G;
206  Twist CiZDu = child.C + (child.Z / child.D) * child.u;
207  Vector6d vCiZDu;
208  vCiZDu << Vector3d::Map(CiZDu.rot.data), Vector3d::Map(CiZDu.vel.data);
209  s.G.noalias() += child.E.transpose() * vCiZDu;
210  }
211  if (i != 0)
212  {
213  //Transform all results to joint root coordinates of segment i (== body coordinates segment i-1)
214  //equation a)
215  s.P = s.F * s.P_tilde;
216  //equation b)
217  s.R = s.F * s.R_tilde;
218  //equation c), in matrix: torques above forces, so switch and switch back
219  for (unsigned int c = 0; c < nc; c++)
220  {
221  Wrench col(Vector(s.E_tilde(3, c), s.E_tilde(4, c), s.E_tilde(5, c)),
222  Vector(s.E_tilde(0, c), s.E_tilde(1, c), s.E_tilde(2, c)));
223  col = s.F*col;
224  s.E.col(c) << Vector3d::Map(col.torque.data), Vector3d::Map(col.force.data);
225  }
226 
227  //needed for next recursion
228  s.PZ = s.P * s.Z;
229  s.D = dot(s.Z, s.PZ);
230  s.PC = s.P * s.C;
231 
232  //u=(Q-Z(R+PC)=sum of external forces along the joint axes,
233  //R are the forces coming from the children,
234  //Q is taken zero (do we need to take the previous calculated torques?
235 
236  //projection of coriolis and centrepital forces into joint subspace (0 0 Z)
237  s.totalBias = -dot(s.Z, s.R + s.PC);
238  s.u = torques(j) + s.totalBias;
239 
240  //Matrix form of Z, put rotations above translations
241  Vector6d vZ;
242  vZ << Vector3d::Map(s.Z.rot.data), Vector3d::Map(s.Z.vel.data);
243  s.EZ.noalias() = s.E.transpose() * vZ;
244 
245  if (chain.getSegment(i - 1).getJoint().getType() != Joint::Fixed)
246  j--;
247  }
248  }
249 }
250 
252 {
253  //equation f) nu = M_0_inverse*(beta_N - E0_tilde`*acc0 - G0)
254  //M_0_inverse, always nc*nc symmetric matrix
255  //std::cout<<"M0: "<<results[0].M<<std::endl;
256  //results[0].M-=MatrixXd::Identity(nc,nc);
257  //std::cout<<"augmented M0: "<<results[0].M<<std::endl;
258 
259 
260  //ToDo: Need to check ill conditions
261 
262  //M_0_inverse=results[0].M.inverse();
263  svd_eigen_HH(results[0].M, Um, Sm, Vm, tmpm);
264  //truncated svd, what would sdls, dls physically mean?
265  for (unsigned int i = 0; i < nc; i++)
266  if (Sm(i) < 1e-14)
267  Sm(i) = 0.0;
268  else
269  Sm(i) = 1 / Sm(i);
270 
271  results[0].M.noalias() = Vm * Sm.asDiagonal();
272  M_0_inverse.noalias() = results[0].M * Um.transpose();
273  //results[0].M.ldlt().solve(MatrixXd::Identity(nc,nc),&M_0_inverse);
274  //results[0].M.computeInverse(&M_0_inverse);
275  Vector6d acc;
276  acc << Vector3d::Map(acc_root.rot.data), Vector3d::Map(acc_root.vel.data);
277  nu_sum.noalias() = -(results[0].E_tilde.transpose() * acc);
278  //nu_sum.setZero();
279  nu_sum += beta.data;
280  nu_sum -= results[0].G;
281 
282  //equation f) nu = M_0_inverse*(beta_N - E0_tilde`*acc0 - G0)
283  nu.noalias() = M_0_inverse * nu_sum;
284 }
285 
287 {
288  unsigned int j = 0;
289 
290  for (unsigned int i = 1; i <= ns; i++)
291  {
292  segment_info& s = results[i];
293  //Calculation of joint and segment accelerations
294  //equation g) qdotdot[i] = D^-1*(Q - Z'(R + P(C + acc[i-1]) + E*nu))
295  // = D^-1(u - Z'(P*acc[i-1] + E*nu)
296  Twist a_g;
297  Twist a_p;
298  if (i == 1)
299  {
300  a_p = acc_root;
301  }
302  else
303  {
304  a_p = results[i - 1].acc;
305  }
306 
307  //The contribution of the constraint forces at segment i
308  Vector6d tmp = s.E*nu;
309  Wrench constraint_force = Wrench(Vector(tmp(3), tmp(4), tmp(5)),
310  Vector(tmp(0), tmp(1), tmp(2)));
311 
312  //acceleration components are also computed
313  //Contribution of the acceleration of the parent (i-1)
314  Wrench parent_force = s.P*a_p;
315  double parent_forceProjection = -dot(s.Z, parent_force);
316  double parentAccComp = parent_forceProjection / s.D;
317 
318  //The constraint force and acceleration force projected on the joint axes -> axis torque/force
319  double constraint_torque = -dot(s.Z, constraint_force);
320  //The result should be the torque at this joint
321 
322  torques(j) = constraint_torque;
323  //s.constAccComp = torques(j) / s.D;
324  s.constAccComp = constraint_torque / s.D;
325  s.nullspaceAccComp = s.u / s.D;
326  //total joint space acceleration resulting from accelerations of parent joints, constraint forces and
327  // nullspace forces.
328  q_dotdot(j) = (s.nullspaceAccComp + parentAccComp + s.constAccComp);
329  s.acc = s.F.Inverse(a_p + s.Z * q_dotdot(j) + s.C);//returns acceleration in link distal tip coordinates. For use needs to be transformed
330  if (chain.getSegment(i - 1).getJoint().getType() != Joint::Fixed)
331  j++;
332  }
333 }
334 
335 /*
336 void ChainIdSolver_Vereshchagin::getLinkCartesianPose(Frames& x_base)
337 {
338  for (int i = 0; i < ns; i++)
339  {
340  x_base[i] = results[i + 1].F_base;
341  }
342  return;
343 }
344 
345 void ChainIdSolver_Vereshchagin::getLinkCartesianVelocity(Twists& xDot_base)
346 {
347 
348  for (int i = 0; i < ns; i++)
349  {
350  xDot_base[i] = results[i + 1].F_base.M * results[i + 1].v;
351  }
352  return;
353 }
354 
355 void ChainIdSolver_Vereshchagin::getLinkCartesianAcceleration(Twists& xDotDot_base)
356 {
357 
358  for (int i = 0; i < ns; i++)
359  {
360  xDotDot_base[i] = results[i + 1].F_base.M * results[i + 1].acc;
361  //std::cout << "XDotDot_base[i] " << xDotDot_base[i] << std::endl;
362  }
363  return;
364 }
365 
366 void ChainIdSolver_Vereshchagin::getLinkPose(Frames& x_local)
367 {
368  for (int i = 0; i < ns; i++)
369  {
370  x_local[i] = results[i + 1].F;
371  }
372  return;
373 }
374 
375 void ChainIdSolver_Vereshchagin::getLinkVelocity(Twists& xDot_local)
376 {
377  for (int i = 0; i < ns; i++)
378  {
379  xDot_local[i] = results[i + 1].v;
380  }
381  return;
382 
383 }
384 
385 void ChainIdSolver_Vereshchagin::getLinkAcceleration(Twists& xDotdot_local)
386 {
387  for (int i = 0; i < ns; i++)
388  {
389  xDotdot_local[i] = results[i + 1].acc;
390  }
391  return;
392 
393 }
394 
395 void ChainIdSolver_Vereshchagin::getJointBiasAcceleration(JntArray& bias_q_dotdot)
396 {
397  for (int i = 0; i < ns; i++)
398  {
399  //this is only force
400  double tmp = results[i + 1].totalBias;
401  //this is acceleration
402  bias_q_dotdot(i) = tmp / results[i + 1].D;
403 
404  //s.totalBias = - dot(s.Z, s.R + s.PC);
405  //std::cout << "totalBiasAccComponent" << i << ": " << bias_q_dotdot(i) << std::endl;
406  //bias_q_dotdot(i) = s.totalBias/s.D
407 
408  }
409  return;
410 
411 }
412 
413 void ChainIdSolver_Vereshchagin::getJointConstraintAcceleration(JntArray& constraint_q_dotdot)
414 {
415  for (int i = 0; i < ns; i++)
416  {
417  constraint_q_dotdot(i) = results[i + 1].constAccComp;
418  //double tmp = results[i + 1].u;
419  //s.u = torques(j) + s.totalBias;
420  // std::cout << "s.constraintAccComp" << i << ": " << results[i+1].constAccComp << std::endl;
421  //nullspace_q_dotdot(i) = s.u/s.D
422 
423  }
424  return;
425 
426 
427 }
428 
429 //Check the name it does not seem to be appropriate
430 
431 void ChainIdSolver_Vereshchagin::getJointNullSpaceAcceleration(JntArray& nullspace_q_dotdot)
432 {
433  for (int i = 0; i < ns; i++)
434  {
435  nullspace_q_dotdot(i) = results[i + 1].nullspaceAccComp;
436  //double tmp = results[i + 1].u;
437  //s.u = torques(j) + s.totalBias;
438  //std::cout << "s.nullSpaceAccComp" << i << ": " << results[i+1].nullspaceAccComp << std::endl;
439  //nullspace_q_dotdot(i) = s.u/s.D
440 
441  }
442  return;
443 
444 
445 }
446 
447 //This is not only a bias force energy but also includes generalized forces
448 //change type of parameter G
449 //this method should return array of G's
450 
451 void ChainIdSolver_Vereshchagin::getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G)
452 {
453  for (int i = 0; i < ns; i++)
454  {
455  G = results[i + 1].G;
456  //double tmp = results[i + 1].u;
457  //s.u = torques(j) + s.totalBias;
458  //std::cout << "s.G " << i << ": " << results[i+1].G << std::endl;
459  //nullspace_q_dotdot(i) = s.u/s.D
460 
461  }
462  return;
463 
464 }
465 
466 //this method should return array of R's
467 
468 void ChainIdSolver_Vereshchagin::getLinkBiasForceMatrix(Wrenches& R_tilde)
469 {
470  for (int i = 0; i < ns; i++)
471  {
472  R_tilde[i] = results[i + 1].R_tilde;
473  //Azamat: bias force as in Featherstone (7.20)
474  //s.R_tilde = s.U + child.R + child.PC + child.PZ / child.D * child.u;
475  std::cout << "s.R_tilde " << i << ": " << results[i + 1].R_tilde << std::endl;
476  }
477  return;
478 }
479 
480 */
481 
482 }//namespace
represents rotations in 3 dimensional space.
Definition: frames.hpp:301
unsigned int columns() const
Definition: jacobian.cpp:76
This class encapsulates a simple segment, that is a "rigid body" (i.e., a frame and a rigid body...
Definition: segment.hpp:46
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
Eigen::Matrix< double, 6, 1 > Vector6d
unsigned int getNrOfSegments() const
Definition: chain.hpp:76
Chain size changed.
Definition: solveri.hpp:97
const RigidBodyInertia & getInertia() const
Definition: segment.hpp:128
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
Input size does not match internal state.
Definition: solveri.hpp:99
Vector torque
Torque that is applied at the origin of the current ref frame.
Definition: frames.hpp:882
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Definition: frameacc.hpp:138
unsigned int rows() const
Definition: jntarray.cpp:72
int svd_eigen_HH(const MatrixXd &A, MatrixXd &U, VectorXd &S, MatrixXd &V, VectorXd &tmp, int maxiter, double epsilon)
Rotation M
Orientation of the Frame.
Definition: frames.hpp:573
const Joint & getJoint() const
Definition: segment.hpp:118
represents both translational and rotational velocities.
Definition: frames.hpp:720
int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian &alfa, const JntArray &beta, const Wrenches &f_ext, JntArray &torques)
unsigned int getNrOfJoints() const
Definition: chain.hpp:71
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.hpp:423
6D Inertia of a articulated body
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
double data[3]
Definition: frames.hpp:163
Frame pose(const double &q) const
Definition: segment.cpp:57
void constraint_calculation(const JntArray &beta)
Eigen::VectorXd data
Definition: jntarray.hpp:72
void final_upwards_sweep(JntArray &q_dotdot, JntArray &torques)
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.hpp:638
const JointType & getType() const
Definition: joint.hpp:159
ChainIdSolver_Vereshchagin(const Chain &chain, Twist root_acc, unsigned int nc)
Vector force
Force that is applied at the origin of the current ref frame.
Definition: frames.hpp:881
void downwards_sweep(const Jacobian &alfa, const JntArray &torques)
std::vector< Wrench > Wrenches
void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext)
static Frame Identity()
Definition: frames.hpp:701
represents both translational and rotational acceleration.
Definition: frames.hpp:878
std::vector< segment_info, Eigen::aligned_allocator< segment_info > > results
Twist twist(const double &q, const double &qdot) const
Definition: segment.cpp:62
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
Eigen::Matrix< double, 6, 6 > Matrix6d


orocos_kdl
Author(s):
autogenerated on Tue Sep 1 2020 03:18:51