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


orocos_kdl
Author(s):
autogenerated on Sat Mar 13 2021 03:16:37