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