Cheetah Software  1.0
FloatingBaseModel.cpp
Go to the documentation of this file.
1 
19 #include "Math/orientation_tools.h"
20 
22 #include <stdio.h>
23 #include <stdexcept>
24 #include <string>
25 #include <vector>
26 
27 using namespace ori;
28 using namespace spatial;
29 using namespace std;
30 
39 template <typename T>
41  const Vec3<T> &force_ics_at_contact,
42  DVec<T> &dstate_out) {
43  forwardKinematics();
44  updateArticulatedBodies();
45  updateForcePropagators();
46  udpateQddEffects();
47 
48  size_t i_opsp = _gcParent.at(gc_index);
49  size_t i = i_opsp;
50 
51  dstate_out = DVec<T>::Zero(_nDof);
52 
53  // Rotation to absolute coords
54  Mat3<T> Rai = _Xa[i].template block<3, 3>(0, 0).transpose();
55  Mat6<T> Xc = createSXform(Rai, _gcLocation.at(gc_index));
56 
57  // D is one column of an extended force propagator matrix (See Wensing, 2012
58  // ICRA)
59  SVec<T> F = Xc.transpose().template rightCols<3>() * force_ics_at_contact;
60 
61  T LambdaInv = 0;
62  T tmp = 0;
63 
64  // from tips to base
65  while (i > 5) {
66  tmp = F.dot(_S[i]);
67  LambdaInv += tmp * tmp / _d[i];
68  dstate_out.tail(_nDof - 6) += _qdd_from_subqdd.col(i - 6) * tmp / _d[i];
69 
70  // Apply force propagator (see Pat's ICRA 2012 paper)
71  // essentially, since the joint is articulated, only a portion of the force
72  // is felt on the predecessor. So, while Xup^T sends a force backwards as if
73  // the joint was locked, ChiUp^T sends the force backward as if the joint
74  // were free
75  F = _ChiUp[i].transpose() * F;
76  i = _parents[i];
77  }
78 
79  dstate_out.head(6) = _invIA5.solve(F);
80  LambdaInv += F.dot(dstate_out.head(6));
81  dstate_out.tail(_nDof - 6) += _qdd_from_base_accel * dstate_out.head(6);
82 
83  return LambdaInv;
84 }
85 
97 template <typename T>
99  if (_qddEffectsUpToDate) return;
100  updateForcePropagators();
101  _qdd_from_base_accel.setZero();
102  _qdd_from_subqdd.setZero();
103 
104  // Pass for force props
105  // This loop is semi-equivalent to a cholesky factorization on H
106  // akin to Featherstone's sparse operational space algo
107  // These computations are for treating the joint rates like a task space
108  // To do so, F computes the dynamic effect of torues onto bodies down the tree
109  //
110  for (size_t i = 6; i < _nDof; i++) {
111  _qdd_from_subqdd(i - 6, i - 6) = 1;
112  SVec<T> F = (_ChiUp[i].transpose() - _Xup[i].transpose()) * _S[i];
113  size_t j = _parents[i];
114  while (j > 5) {
115  _qdd_from_subqdd(i - 6, j - 6) = _S[j].dot(F);
116  F = _ChiUp[j].transpose() * F;
117  j = _parents[j];
118  }
119  _qdd_from_base_accel.row(i - 6) = F.transpose();
120  }
121  _qddEffectsUpToDate = true;
122 }
123 
128 template <typename T>
130  if (_forcePropagatorsUpToDate) return;
131  updateArticulatedBodies();
132  for (size_t i = 6; i < _nDof; i++) {
133  _ChiUp[i] = _Xup[i] - _S[i] * _Utot[i].transpose() / _d[i];
134  }
135  _forcePropagatorsUpToDate = true;
136 }
137 
141 template <typename T>
143  if (_articulatedBodiesUpToDate) return;
144 
145  forwardKinematics();
146 
147  _IA[5] = _Ibody[5].getMatrix();
148 
149  // loop 1, down the tree
150  for (size_t i = 6; i < _nDof; i++) {
151  _IA[i] = _Ibody[i].getMatrix(); // initialize
152  Mat6<T> XJrot = jointXform(_jointTypes[i], _jointAxes[i],
153  _state.q[i - 6] * _gearRatios[i]);
154  _Xuprot[i] = XJrot * _Xrot[i];
155  _Srot[i] = _S[i] * _gearRatios[i];
156  }
157 
158  // Pat's magic principle of least constraint (Guass too!)
159  for (size_t i = _nDof - 1; i >= 6; i--) {
160  _U[i] = _IA[i] * _S[i];
161  _Urot[i] = _Irot[i].getMatrix() * _Srot[i];
162  _Utot[i] = _Xup[i].transpose() * _U[i] + _Xuprot[i].transpose() * _Urot[i];
163 
164  _d[i] = _Srot[i].transpose() * _Urot[i];
165  _d[i] += _S[i].transpose() * _U[i];
166 
167  // articulated inertia recursion
168  Mat6<T> Ia = _Xup[i].transpose() * _IA[i] * _Xup[i] +
169  _Xuprot[i].transpose() * _Irot[i].getMatrix() * _Xuprot[i] -
170  _Utot[i] * _Utot[i].transpose() / _d[i];
171  _IA[_parents[i]] += Ia;
172  }
173 
174  _invIA5.compute(_IA[5]);
175  _articulatedBodiesUpToDate = true;
176 }
177 
178 // parents, gr, jtype, Xtree, I, Xrot, Irot,
179 
184 template <typename T>
186  if (count != 1 && count != 6) {
187  throw std::runtime_error(
188  "addDynamicsVars must be called with count=1 (joint) or count=6 "
189  "(base).\n");
190  }
191 
192  Mat6<T> eye6 = Mat6<T>::Identity();
193  SVec<T> zero6 = SVec<T>::Zero();
194  Mat6<T> zero66 = Mat6<T>::Zero();
195 
196  SpatialInertia<T> zeroInertia(zero66);
197  for (int i = 0; i < count; i++) {
198  _v.push_back(zero6);
199  _vrot.push_back(zero6);
200  _a.push_back(zero6);
201  _arot.push_back(zero6);
202  _avp.push_back(zero6);
203  _avprot.push_back(zero6);
204  _c.push_back(zero6);
205  _crot.push_back(zero6);
206  _S.push_back(zero6);
207  _Srot.push_back(zero6);
208  _f.push_back(zero6);
209  _frot.push_back(zero6);
210  _fvp.push_back(zero6);
211  _fvprot.push_back(zero6);
212  _ag.push_back(zero6);
213  _agrot.push_back(zero6);
214  _IC.push_back(zeroInertia);
215  _Xup.push_back(eye6);
216  _Xuprot.push_back(eye6);
217  _Xa.push_back(eye6);
218 
219  _ChiUp.push_back(eye6);
220  _d.push_back(0.);
221  _u.push_back(0.);
222  _IA.push_back(eye6);
223 
224  _U.push_back(zero6);
225  _Urot.push_back(zero6);
226  _Utot.push_back(zero6);
227  _pA.push_back(zero6);
228  _pArot.push_back(zero6);
229  _externalForces.push_back(zero6);
230  }
231 
232  _J.push_back(D6Mat<T>::Zero(6, _nDof));
233  _Jdqd.push_back(SVec<T>::Zero());
234 
235  resizeSystemMatricies();
236 }
237 
241 template <typename T>
243  _H.setZero(_nDof, _nDof);
244  _C.setZero(_nDof, _nDof);
245  _Cqd.setZero(_nDof);
246  _G.setZero(_nDof);
247  for (size_t i = 0; i < _J.size(); i++) {
248  _J[i].setZero(6, _nDof);
249  _Jdqd[i].setZero();
250  }
251 
252  for (size_t i = 0; i < _Jc.size(); i++) {
253  _Jc[i].setZero(3, _nDof);
254  _Jcdqd[i].setZero();
255  }
256  _qdd_from_subqdd.resize(_nDof - 6, _nDof - 6);
257  _qdd_from_base_accel.resize(_nDof - 6, 6);
258  _state.q = DVec<T>::Zero(_nDof - 6);
259  _state.qd = DVec<T>::Zero(_nDof - 6);
260 }
261 
266 template <typename T>
268  if (_nDof) {
269  throw std::runtime_error("Cannot add base multiple times!\n");
270  }
271 
272  Mat6<T> eye6 = Mat6<T>::Identity();
273  Mat6<T> zero6 = Mat6<T>::Zero();
274  SpatialInertia<T> zeroInertia(zero6);
275  // the floating base has 6 DOFs
276 
277  _nDof = 6;
278  for (size_t i = 0; i < 6; i++) {
279  _parents.push_back(0);
280  _gearRatios.push_back(0);
281  _jointTypes.push_back(JointType::Nothing); // doesn't actually matter
282  _jointAxes.push_back(CoordinateAxis::X); // doesn't actually matter
283  _Xtree.push_back(eye6);
284  _Ibody.push_back(zeroInertia);
285  _Xrot.push_back(eye6);
286  _Irot.push_back(zeroInertia);
287  _bodyNames.push_back("N/A");
288  }
289 
290  _jointTypes[5] = JointType::FloatingBase;
291  _Ibody[5] = inertia;
292  _gearRatios[5] = 1;
293  _bodyNames[5] = "Floating Base";
294 
295  addDynamicsVars(6);
296 }
297 
304 template <typename T>
305 void FloatingBaseModel<T>::addBase(T mass, const Vec3<T> &com,
306  const Mat3<T> &I) {
307  SpatialInertia<T> IS(mass, com, I);
308  addBase(IS);
309 }
310 
318 template <typename T>
320  const Vec3<T> &location,
321  bool isFoot) {
322  if ((size_t)bodyID >= _nDof) {
323  throw std::runtime_error(
324  "addGroundContactPoint got invalid bodyID: " + std::to_string(bodyID) +
325  " nDofs: " + std::to_string(_nDof) + "\n");
326  }
327 
328  // std::cout << "pt-add: " << location.transpose() << "\n";
329  _gcParent.push_back(bodyID);
330  _gcLocation.push_back(location);
331 
332  Vec3<T> zero3 = Vec3<T>::Zero();
333 
334  _pGC.push_back(zero3);
335  _vGC.push_back(zero3);
336 
337  D3Mat<T> J(3, _nDof);
338  J.setZero();
339 
340  _Jc.push_back(J);
341  _Jcdqd.push_back(zero3);
342  //_compute_contact_info.push_back(false);
343  _compute_contact_info.push_back(true);
344 
345  // add foot to foot list
346  if (isFoot) {
347  _footIndicesGC.push_back(_nGroundContact);
348  _compute_contact_info[_nGroundContact] = true;
349  }
350 
351  resizeSystemMatricies();
352  return _nGroundContact++;
353 }
354 
359 template <typename T>
361  const Vec3<T> &dims) {
362  // addGroundContactPoint(bodyId, Vec3<T>( dims(0), dims(1), dims(2))/2);
363  // addGroundContactPoint(bodyId, Vec3<T>(-dims(0), dims(1), dims(2))/2);
364  // addGroundContactPoint(bodyId, Vec3<T>( dims(0), -dims(1), dims(2))/2);
365  // addGroundContactPoint(bodyId, Vec3<T>(-dims(0), -dims(1), dims(2))/2);
366 
367  addGroundContactPoint(bodyId, Vec3<T>(dims(0), dims(1), 0.) / 2);
368  addGroundContactPoint(bodyId, Vec3<T>(-dims(0), dims(1), 0.) / 2);
369  addGroundContactPoint(bodyId, Vec3<T>(dims(0), -dims(1), 0.) / 2);
370  addGroundContactPoint(bodyId, Vec3<T>(-dims(0), -dims(1), 0.) / 2);
371 
372  addGroundContactPoint(bodyId, Vec3<T>(dims(0), dims(1), -dims(2)) / 2);
373  addGroundContactPoint(bodyId, Vec3<T>(-dims(0), dims(1), -dims(2)) / 2);
374  addGroundContactPoint(bodyId, Vec3<T>(dims(0), -dims(1), -dims(2)) / 2);
375  addGroundContactPoint(bodyId, Vec3<T>(-dims(0), -dims(1), -dims(2)) / 2);
376 }
377 
391 template <typename T>
393  const SpatialInertia<T> &rotorInertia,
394  T gearRatio, int parent, JointType jointType,
395  CoordinateAxis jointAxis,
396  const Mat6<T> &Xtree, const Mat6<T> &Xrot) {
397  if ((size_t)parent >= _nDof) {
398  throw std::runtime_error(
399  "addBody got invalid parent: " + std::to_string(parent) +
400  " nDofs: " + std::to_string(_nDof) + "\n");
401  }
402 
403  _parents.push_back(parent);
404  _gearRatios.push_back(gearRatio);
405  _jointTypes.push_back(jointType);
406  _jointAxes.push_back(jointAxis);
407  _Xtree.push_back(Xtree);
408  _Xrot.push_back(Xrot);
409  _Ibody.push_back(inertia);
410  _Irot.push_back(rotorInertia);
411  _nDof++;
412 
413  addDynamicsVars(1);
414 
415  return _nDof;
416 }
417 
431 template <typename T>
433  const MassProperties<T> &rotorInertia,
434  T gearRatio, int parent, JointType jointType,
435  CoordinateAxis jointAxis,
436  const Mat6<T> &Xtree, const Mat6<T> &Xrot) {
437  return addBody(SpatialInertia<T>(inertia), SpatialInertia<T>(rotorInertia),
438  gearRatio, parent, jointType, jointAxis, Xtree, Xrot);
439 }
440 
441 template <typename T>
443  if (_nDof != _parents.size())
444  throw std::runtime_error("Invalid dof and parents length");
445 }
446 
451 template <typename T>
453  T totalMass = 0;
454  for (size_t i = 0; i < _nDof; i++) {
455  totalMass += _Ibody[i].getMass();
456  }
457  return totalMass;
458 }
459 
464 template <typename T>
466  T totalMass = 0;
467  for (size_t i = 0; i < _nDof; i++) {
468  totalMass += _Irot[i].getMass();
469  }
470  return totalMass;
471 }
472 
478 template <typename T>
480  if (_kinematicsUpToDate) return;
481 
482  // calculate joint transformations
483  _Xup[5] = createSXform(quaternionToRotationMatrix(_state.bodyOrientation),
484  _state.bodyPosition);
485  _v[5] = _state.bodyVelocity;
486  for (size_t i = 6; i < _nDof; i++) {
487  // joint xform
488  Mat6<T> XJ = jointXform(_jointTypes[i], _jointAxes[i], _state.q[i - 6]);
489  _Xup[i] = XJ * _Xtree[i];
490  _S[i] = jointMotionSubspace<T>(_jointTypes[i], _jointAxes[i]);
491  SVec<T> vJ = _S[i] * _state.qd[i - 6];
492  // total velocity of body i
493  _v[i] = _Xup[i] * _v[_parents[i]] + vJ;
494 
495  // Same for rotors
496  Mat6<T> XJrot = jointXform(_jointTypes[i], _jointAxes[i],
497  _state.q[i - 6] * _gearRatios[i]);
498  _Srot[i] = _S[i] * _gearRatios[i];
499  SVec<T> vJrot = _Srot[i] * _state.qd[i - 6];
500  _Xuprot[i] = XJrot * _Xrot[i];
501  _vrot[i] = _Xuprot[i] * _v[_parents[i]] + vJrot;
502 
503  // Coriolis accelerations
504  _c[i] = motionCrossProduct(_v[i], vJ);
505  _crot[i] = motionCrossProduct(_vrot[i], vJrot);
506  }
507 
508  // calculate from absolute transformations
509  for (size_t i = 5; i < _nDof; i++) {
510  if (_parents[i] == 0) {
511  _Xa[i] = _Xup[i]; // float base
512  } else {
513  _Xa[i] = _Xup[i] * _Xa[_parents[i]];
514  }
515  }
516 
517  // ground contact points
518  // // TODO : we end up inverting the same Xa a few times (like for the 8
519  // points on the body). this isn't super efficient.
520  for (size_t j = 0; j < _nGroundContact; j++) {
521  if (!_compute_contact_info[j]) continue;
522  size_t i = _gcParent.at(j);
523  Mat6<T> Xai = invertSXform(_Xa[i]); // from link to absolute
524  SVec<T> vSpatial = Xai * _v[i];
525 
526  // foot position in world
527  _pGC.at(j) = sXFormPoint(Xai, _gcLocation.at(j));
528  _vGC.at(j) = spatialToLinearVelocity(vSpatial, _pGC.at(j));
529  }
530  _kinematicsUpToDate = true;
531 }
532 
533 // template <typename T>
534 // void FloatingBaseModel<T>::getPositionVelocity(
535 // const int & link_idx, const Vec3<T> & local_pos,
536 // Vec3<T> & link_pos, Vec3<T> & link_vel) const {
537 
538 // Mat6<T> Xai = invertSXform(_Xa[link_idx]); // from link to absolute
539 // link_pos = sXFormPoint(Xai, local_pos);
540 // link_vel = spatialToLinearVelocity(Xai*_v[link_idx], link_pos);
541 //}
542 
547 template <typename T>
549  forwardKinematics();
550  biasAccelerations();
551 
552  for (size_t k = 0; k < _nGroundContact; k++) {
553  _Jc[k].setZero();
554  _Jcdqd[k].setZero();
555 
556  // Skip it if we don't care about it
557  if (!_compute_contact_info[k]) continue;
558 
559  size_t i = _gcParent.at(k);
560 
561  // Rotation to absolute coords
562  Mat3<T> Rai = _Xa[i].template block<3, 3>(0, 0).transpose();
563  Mat6<T> Xc = createSXform(Rai, _gcLocation.at(k));
564 
565  // Bias acceleration
566  SVec<T> ac = Xc * _avp[i];
567  SVec<T> vc = Xc * _v[i];
568 
569  // Correct to classical
570  _Jcdqd[k] = spatialToLinearAcceleration(ac, vc);
571 
572  // rows for linear velcoity in the world
573  D3Mat<T> Xout = Xc.template bottomRows<3>();
574 
575  // from tips to base
576  while (i > 5) {
577  _Jc[k].col(i) = Xout * _S[i];
578  Xout = Xout * _Xup[i];
579  i = _parents[i];
580  }
581  _Jc[k].template leftCols<6>() = Xout;
582  }
583 }
584 
589 template <typename T>
591  if (_biasAccelerationsUpToDate) return;
592  forwardKinematics();
593  // velocity product acceelration of base
594  _avp[5] << 0, 0, 0, 0, 0, 0;
595 
596  // from base to tips
597  for (size_t i = 6; i < _nDof; i++) {
598  // Outward kinamtic propagtion
599  _avp[i] = _Xup[i] * _avp[_parents[i]] + _c[i];
600  _avprot[i] = _Xuprot[i] * _avp[_parents[i]] + _crot[i];
601  }
602  _biasAccelerationsUpToDate = true;
603 }
604 
609 template <typename T>
611  compositeInertias();
612 
613  SVec<T> aGravity;
614  aGravity << 0, 0, 0, _gravity[0], _gravity[1], _gravity[2];
615  _ag[5] = _Xup[5] * aGravity;
616 
617  // Gravity comp force is the same as force required to accelerate
618  // oppostite gravity
619  _G.template topRows<6>() = -_IC[5].getMatrix() * _ag[5];
620  for (size_t i = 6; i < _nDof; i++) {
621  _ag[i] = _Xup[i] * _ag[_parents[i]];
622  _agrot[i] = _Xuprot[i] * _ag[_parents[i]];
623 
624  // body and rotor
625  _G[i] = -_S[i].dot(_IC[i].getMatrix() * _ag[i]) -
626  _Srot[i].dot(_Irot[i].getMatrix() * _agrot[i]);
627  }
628  return _G;
629 }
630 
635 template <typename T>
637  biasAccelerations();
638 
639  // Floating base force
640  Mat6<T> Ifb = _Ibody[5].getMatrix();
641  SVec<T> hfb = Ifb * _v[5];
642  _fvp[5] = Ifb * _avp[5] + forceCrossProduct(_v[5], hfb);
643 
644  for (size_t i = 6; i < _nDof; i++) {
645  // Force on body i
646  Mat6<T> Ii = _Ibody[i].getMatrix();
647  SVec<T> hi = Ii * _v[i];
648  _fvp[i] = Ii * _avp[i] + forceCrossProduct(_v[i], hi);
649 
650  // Force on rotor i
651  Mat6<T> Ir = _Irot[i].getMatrix();
652  SVec<T> hr = Ir * _vrot[i];
653  _fvprot[i] = Ir * _avprot[i] + forceCrossProduct(_vrot[i], hr);
654  }
655 
656  for (size_t i = _nDof - 1; i > 5; i--) {
657  // Extract force along the joints
658  _Cqd[i] = _S[i].dot(_fvp[i]) + _Srot[i].dot(_fvprot[i]);
659 
660  // Propage force down the tree
661  _fvp[_parents[i]] += _Xup[i].transpose() * _fvp[i];
662  _fvp[_parents[i]] += _Xuprot[i].transpose() * _fvprot[i];
663  }
664 
665  // Force on floating base
666  _Cqd.template topRows<6>() = _fvp[5];
667  return _Cqd;
668 }
669 
670 template <typename T>
672  forwardKinematics();
673  Mat3<T> Rai = _Xa[link_idx].template block<3, 3>(0, 0);
674  Rai.transposeInPlace();
675  return Rai;
676 }
677 
678 
679 template <typename T>
681 {
682  forwardKinematics();
683  Mat6<T> Xai = invertSXform(_Xa[link_idx]); // from link to absolute
684  Vec3<T> link_pos = sXFormPoint(Xai, Vec3<T>::Zero());
685  return link_pos;
686 }
687 
688 template <typename T>
689 Vec3<T> FloatingBaseModel<T>::getPosition(const int link_idx, const Vec3<T> & local_pos)
690 {
691  forwardKinematics();
692  Mat6<T> Xai = invertSXform(_Xa[link_idx]); // from link to absolute
693  Vec3<T> link_pos = sXFormPoint(Xai, local_pos);
694  return link_pos;
695 }
696 
697 template <typename T>
699  const Vec3<T> &point) {
700  forwardAccelerationKinematics();
701  Mat3<T> R = getOrientation(link_idx);
702  return R * spatialToLinearAcceleration(_a[link_idx], _v[link_idx], point);
703 }
704 
705 template <typename T>
707  forwardAccelerationKinematics();
708  Mat3<T> R = getOrientation(link_idx);
709  return R * spatialToLinearAcceleration(_a[link_idx], _v[link_idx], Vec3<T>::Zero());
710 }
711 
712 
713 template <typename T>
715  const Vec3<T> &point) {
716  forwardKinematics();
717  Mat3<T> Rai = getOrientation(link_idx);
718  return Rai * spatialToLinearVelocity(_v[link_idx], point);
719 }
720 
721 template <typename T>
723  forwardKinematics();
724  Mat3<T> Rai = getOrientation(link_idx);
725  return Rai * spatialToLinearVelocity(_v[link_idx], Vec3<T>::Zero());
726 }
727 
728 
729 
730 template <typename T>
732  forwardKinematics();
733  Mat3<T> Rai = getOrientation(link_idx);
734  // Vec3<T> v3 =
735  return Rai * _v[link_idx].template head<3>();
736  ;
737 }
738 
739 template <typename T>
741  forwardAccelerationKinematics();
742  Mat3<T> Rai = getOrientation(link_idx);
743  return Rai * _a[link_idx].template head<3>();
744 }
745 
752 template <typename T>
754  if (_compositeInertiasUpToDate) return;
755 
756  forwardKinematics();
757  // initialize
758  for (size_t i = 5; i < _nDof; i++) {
759  _IC[i].setMatrix(_Ibody[i].getMatrix());
760  }
761 
762  // backward loop
763  for (size_t i = _nDof - 1; i > 5; i--) {
764  // Propagate inertia down the tree
765  _IC[_parents[i]].addMatrix(_Xup[i].transpose() * _IC[i].getMatrix() *
766  _Xup[i]);
767  _IC[_parents[i]].addMatrix(_Xuprot[i].transpose() * _Irot[i].getMatrix() *
768  _Xuprot[i]);
769  }
770  _compositeInertiasUpToDate = true;
771 }
772 
777 template <typename T>
779  compositeInertias();
780  _H.setZero();
781 
782  // Top left corner is the locked inertia of the whole system
783  _H.template topLeftCorner<6, 6>() = _IC[5].getMatrix();
784 
785  for (size_t j = 6; j < _nDof; j++) {
786  // f = spatial force required for a unit qdd_j
787  SVec<T> f = _IC[j].getMatrix() * _S[j];
788  SVec<T> frot = _Irot[j].getMatrix() * _Srot[j];
789 
790  _H(j, j) = _S[j].dot(f) + _Srot[j].dot(frot);
791 
792  // Propagate down the tree
793  f = _Xup[j].transpose() * f + _Xuprot[j].transpose() * frot;
794  size_t i = _parents[j];
795  while (i > 5) {
796  // in here f is expressed in frame {i}
797  _H(i, j) = _S[i].dot(f);
798  _H(j, i) = _H(i, j);
799 
800  // Propagate down the tree
801  f = _Xup[i].transpose() * f;
802  i = _parents[i];
803  }
804 
805  // Force on floating base
806  _H.template block<6, 1>(0, j) = f;
807  _H.template block<1, 6>(j, 0) = f.adjoint();
808  }
809  return _H;
810 }
811 
812 template <typename T>
814  if (_accelerationsUpToDate) {
815  return;
816  }
817 
818  forwardKinematics();
819  biasAccelerations();
820 
821  // Initialize gravity with model info
822  SVec<T> aGravity = SVec<T>::Zero();
823  aGravity.template tail<3>() = _gravity;
824 
825  // Spatial force for floating base
826  _a[5] = -_Xup[5] * aGravity + _dState.dBodyVelocity;
827 
828  // loop through joints
829  for (size_t i = 6; i < _nDof; i++) {
830  // spatial acceleration
831  _a[i] = _Xup[i] * _a[_parents[i]] + _S[i] * _dState.qdd[i - 6] + _c[i];
832  _arot[i] =
833  _Xuprot[i] * _a[_parents[i]] + _Srot[i] * _dState.qdd[i - 6] + _crot[i];
834  }
835  _accelerationsUpToDate = true;
836 }
837 
844 template <typename T>
846  const FBModelStateDerivative<T> &dState) {
847  setDState(dState);
848  forwardAccelerationKinematics();
849 
850  // Spatial force for floating base
851  SVec<T> hb = _Ibody[5].getMatrix() * _v[5];
852  _f[5] = _Ibody[5].getMatrix() * _a[5] + forceCrossProduct(_v[5], hb);
853 
854  // loop through joints
855  for (size_t i = 6; i < _nDof; i++) {
856  // spatial momentum
857  SVec<T> hi = _Ibody[i].getMatrix() * _v[i];
858  SVec<T> hr = _Irot[i].getMatrix() * _vrot[i];
859 
860  // spatial force
861  _f[i] = _Ibody[i].getMatrix() * _a[i] + forceCrossProduct(_v[i], hi);
862  _frot[i] =
863  _Irot[i].getMatrix() * _arot[i] + forceCrossProduct(_vrot[i], hr);
864  }
865 
866  DVec<T> genForce(_nDof);
867  for (size_t i = _nDof - 1; i > 5; i--) {
868  // Pull off compoents of force along the joint
869  genForce[i] = _S[i].dot(_f[i]) + _Srot[i].dot(_frot[i]);
870 
871  // Propagate down the tree
872  _f[_parents[i]] += _Xup[i].transpose() * _f[i];
873  _f[_parents[i]] += _Xuprot[i].transpose() * _frot[i];
874  }
875  genForce.template head<6>() = _f[5];
876  return genForce;
877 }
878 
879 template <typename T>
881  FBModelStateDerivative<T> &dstate) {
882  (void)tau;
883  forwardKinematics();
884  updateArticulatedBodies();
885 
886  // create spatial vector for gravity
887  SVec<T> aGravity;
888  aGravity << 0, 0, 0, _gravity[0], _gravity[1], _gravity[2];
889 
890  // float-base articulated inertia
891  SVec<T> ivProduct = _Ibody[5].getMatrix() * _v[5];
892  _pA[5] = forceCrossProduct(_v[5], ivProduct);
893 
894  // loop 1, down the tree
895  for (size_t i = 6; i < _nDof; i++) {
896  ivProduct = _Ibody[i].getMatrix() * _v[i];
897  _pA[i] = forceCrossProduct(_v[i], ivProduct);
898 
899  // same for rotors
900  SVec<T> vJrot = _Srot[i] * _state.qd[i - 6];
901  _vrot[i] = _Xuprot[i] * _v[_parents[i]] + vJrot;
902  _crot[i] = motionCrossProduct(_vrot[i], vJrot);
903  ivProduct = _Irot[i].getMatrix() * _vrot[i];
904  _pArot[i] = forceCrossProduct(_vrot[i], ivProduct);
905  }
906 
907  // adjust pA for external forces
908  for (size_t i = 5; i < _nDof; i++) {
909  // TODO add if statement (avoid these calculations if the force is zero)
910  Mat3<T> R = rotationFromSXform(_Xa[i]);
911  Vec3<T> p = translationFromSXform(_Xa[i]);
912  Mat6<T> iX = createSXform(R.transpose(), -R * p);
913  _pA[i] = _pA[i] - iX.transpose() * _externalForces.at(i);
914  }
915 
916  // Pat's magic principle of least constraint
917  for (size_t i = _nDof - 1; i >= 6; i--) {
918  _u[i] = tau[i - 6] - _S[i].transpose() * _pA[i] -
919  _Srot[i].transpose() * _pArot[i] - _U[i].transpose() * _c[i] -
920  _Urot[i].transpose() * _crot[i];
921 
922  // articulated inertia recursion
923  SVec<T> pa =
924  _Xup[i].transpose() * (_pA[i] + _IA[i] * _c[i]) +
925  _Xuprot[i].transpose() * (_pArot[i] + _Irot[i].getMatrix() * _crot[i]) +
926  _Utot[i] * _u[i] / _d[i];
927  _pA[_parents[i]] += pa;
928  }
929 
930  // include gravity and compute acceleration of floating base
931  SVec<T> a0 = -aGravity;
932  SVec<T> ub = -_pA[5];
933  _a[5] = _Xup[5] * a0;
934  SVec<T> afb = _invIA5.solve(ub - _IA[5].transpose() * _a[5]);
935  _a[5] += afb;
936 
937  // joint accelerations
938  dstate.qdd = DVec<T>(_nDof - 6);
939  for (size_t i = 6; i < _nDof; i++) {
940  dstate.qdd[i - 6] =
941  (_u[i] - _Utot[i].transpose() * _a[_parents[i]]) / _d[i];
942  _a[i] = _Xup[i] * _a[_parents[i]] + _S[i] * dstate.qdd[i - 6] + _c[i];
943  }
944 
945  // output
946  RotMat<T> Rup = rotationFromSXform(_Xup[5]);
947  dstate.dBodyPosition =
948  Rup.transpose() * _state.bodyVelocity.template block<3, 1>(3, 0);
949  dstate.dBodyVelocity = afb;
950  // qdd is set in the for loop above
951 }
952 
961 template <typename T>
963  const Vec3<T> &force_ics_at_contact,
964  FBModelStateDerivative<T> &dstate_out) {
965  forwardKinematics();
966  updateArticulatedBodies();
967  updateForcePropagators();
968  udpateQddEffects();
969 
970  size_t i_opsp = _gcParent.at(gc_index);
971  size_t i = i_opsp;
972 
973  dstate_out.qdd.setZero();
974 
975  // Rotation to absolute coords
976  Mat3<T> Rai = _Xa[i].template block<3, 3>(0, 0).transpose();
977  Mat6<T> Xc = createSXform(Rai, _gcLocation.at(gc_index));
978 
979  // D is one column of an extended force propagator matrix (See Wensing, 2012
980  // ICRA)
981  SVec<T> F = Xc.transpose().template rightCols<3>() * force_ics_at_contact;
982 
983  double LambdaInv = 0;
984  double tmp = 0;
985 
986  // from tips to base
987  while (i > 5) {
988  tmp = F.dot(_S[i]);
989  LambdaInv += tmp * tmp / _d[i];
990  dstate_out.qdd += _qdd_from_subqdd.col(i - 6) * tmp / _d[i];
991 
992  // Apply force propagator (see Pat's ICRA 2012 paper)
993  // essentially, since the joint is articulated, only a portion of the force
994  // is felt on the predecessor. So, while Xup^T sends a force backwards as if
995  // the joint was locked, ChiUp^T sends the force backward as if the joint
996  // were free
997  F = _ChiUp[i].transpose() * F;
998  i = _parents[i];
999  }
1000 
1001  // TODO: Only carry out the QR once within update Aritculated Bodies
1002  dstate_out.dBodyVelocity = _invIA5.solve(F);
1003  LambdaInv += F.dot(dstate_out.dBodyVelocity);
1004  dstate_out.qdd += _qdd_from_base_accel * dstate_out.dBodyVelocity;
1005 
1006  return LambdaInv;
1007 }
1008 
1016 template <typename T>
1018  const Vec3<T> &force_ics_at_contact) {
1019  forwardKinematics();
1020  updateArticulatedBodies();
1021  updateForcePropagators();
1022 
1023  size_t i_opsp = _gcParent.at(gc_index);
1024  size_t i = i_opsp;
1025 
1026  // Rotation to absolute coords
1027  Mat3<T> Rai = _Xa[i].template block<3, 3>(0, 0).transpose();
1028  Mat6<T> Xc = createSXform(Rai, _gcLocation.at(gc_index));
1029 
1030  // D is one column of an extended force propagator matrix (See Wensing, 2012
1031  // ICRA)
1032  SVec<T> F = Xc.transpose().template rightCols<3>() * force_ics_at_contact;
1033 
1034  double LambdaInv = 0;
1035  double tmp = 0;
1036 
1037  // from tips to base
1038  while (i > 5) {
1039  tmp = F.dot(_S[i]);
1040  LambdaInv += tmp * tmp / _d[i];
1041 
1042  // Apply force propagator (see Pat's ICRA 2012 paper)
1043  // essentially, since the joint is articulated, only a portion of the force
1044  // is felt on the predecessor. So, while Xup^T sends a force backwards as if
1045  // the joint was locked, ChiUp^T sends the force backward as if the joint
1046  // were free
1047  F = _ChiUp[i].transpose() * F;
1048  i = _parents[i];
1049  }
1050  LambdaInv += F.dot(_invIA5.solve(F));
1051  return LambdaInv;
1052 }
1053 
1064 template <typename T>
1066  const int gc_index, const D6Mat<T> &force_directions) {
1067  forwardKinematics();
1068  updateArticulatedBodies();
1069  updateForcePropagators();
1070 
1071  size_t i_opsp = _gcParent.at(gc_index);
1072  size_t i = i_opsp;
1073 
1074  // Rotation to absolute coords
1075  Mat3<T> Rai = _Xa[i].template block<3, 3>(0, 0).transpose();
1076  Mat6<T> Xc = createSXform(Rai, _gcLocation.at(gc_index));
1077 
1078  // D is a subslice of an extended force propagator matrix (See Wensing, 2012
1079  // ICRA)
1080  D6Mat<T> D = Xc.transpose() * force_directions;
1081 
1082  size_t m = force_directions.cols();
1083 
1084  DMat<T> LambdaInv = DMat<T>::Zero(m, m);
1085  DVec<T> tmp = DVec<T>::Zero(m);
1086 
1087  // from tips to base
1088  while (i > 5) {
1089  tmp = D.transpose() * _S[i];
1090  LambdaInv += tmp * tmp.transpose() / _d[i];
1091 
1092  // Apply force propagator (see Pat's ICRA 2012 paper)
1093  // essentially, since the joint is articulated, only a portion of the force
1094  // is felt on the predecessor. So, while Xup^T sends a force backwards as if
1095  // the joint was locked, ChiUp^T sends the force backward as if the joint
1096  // were free
1097  D = _ChiUp[i].transpose() * D;
1098  i = _parents[i];
1099  }
1100 
1101  // TODO: Only carry out the QR once within update Aritculated Bodies
1102  LambdaInv += D.transpose() * _invIA5.solve(D);
1103 
1104  return LambdaInv;
1105 }
1106 
1107 template class FloatingBaseModel<double>;
1108 template class FloatingBaseModel<float>;
DVec< T > inverseDynamics(const FBModelStateDerivative< T > &dState)
Mat3< T > getOrientation(const int link_idx)
auto spatialToLinearVelocity(const Eigen::MatrixBase< T > &v, const Eigen::MatrixBase< T2 > &x)
Definition: spatial.h:261
void runABA(const DVec< T > &tau, FBModelStateDerivative< T > &dstate)
Vec3< T > getAngularVelocity(const int link_idx)
auto forceCrossProduct(const Eigen::MatrixBase< T > &a, const Eigen::MatrixBase< T > &b)
Definition: spatial.h:91
typename Eigen::Matrix< T, 6, Eigen::Dynamic > D6Mat
Definition: cppTypes.h:110
typename Eigen::Matrix< T, 6, 6 > Mat6
Definition: cppTypes.h:70
Mat6< T > jointXform(JointType joint, CoordinateAxis axis, T q)
Definition: spatial.h:219
EIGEN_MAKE_ALIGNED_OPERATOR_NEW Vec3< T > dBodyPosition
typename Eigen::Matrix< T, 3, 3 > Mat3
Definition: cppTypes.h:54
DVec< T > generalizedCoriolisForce()
int addGroundContactPoint(int bodyID, const Vec3< T > &location, bool isFoot=false)
typename Eigen::Matrix< T, Eigen::Dynamic, Eigen::Dynamic > DMat
Definition: cppTypes.h:106
typename Eigen::Matrix< T, 3, 1 > Vec3
Definition: cppTypes.h:26
Vec3< T > getLinearAcceleration(const int link_idx, const Vec3< T > &point)
auto translationFromSXform(const Eigen::MatrixBase< T > &X)
Definition: spatial.h:168
auto spatialToLinearAcceleration(const Eigen::MatrixBase< T > &a, const Eigen::MatrixBase< T2 > &v)
Definition: spatial.h:289
auto sXFormPoint(const Eigen::MatrixBase< T > &X, const Eigen::MatrixBase< T2 > &p)
Definition: spatial.h:329
typename Eigen::Matrix< T, 3, Eigen::Dynamic > D3Mat
Definition: cppTypes.h:114
typename Eigen::Matrix< T, 6, 1 > SVec
Definition: cppTypes.h:62
Vec3< T > getPosition(const int link_idx, const Vec3< T > &local_pos)
auto createSXform(const Eigen::MatrixBase< T > &R, const Eigen::MatrixBase< T2 > &r)
Definition: spatial.h:140
int addBody(const SpatialInertia< T > &inertia, const SpatialInertia< T > &rotorInertia, T gearRatio, int parent, JointType jointType, CoordinateAxis jointAxis, const Mat6< T > &Xtree, const Mat6< T > &Xrot)
auto motionCrossProduct(const Eigen::MatrixBase< T > &a, const Eigen::MatrixBase< T > &b)
Definition: spatial.h:73
JointType
Definition: spatial.h:21
DMat< T > invContactInertia(const int gc_index, const D6Mat< T > &force_directions)
Vec3< T > getAngularAcceleration(const int link_idx)
T applyTestForce(const int gc_index, const Vec3< T > &force_ics_at_contact, FBModelStateDerivative< T > &dstate_out)
Utility functions for 3D rotations.
typename Eigen::Matrix< T, Eigen::Dynamic, 1 > DVec
Definition: cppTypes.h:102
Mat3< typename T::Scalar > quaternionToRotationMatrix(const Eigen::MatrixBase< T > &q)
void addGroundContactBoxPoints(int bodyId, const Vec3< T > &dims)
typename Eigen::Matrix< T, 10, 1 > MassProperties
Definition: cppTypes.h:98
Vec3< T > getLinearVelocity(const int link_idx, const Vec3< T > &point)
void addDynamicsVars(int count)
DVec< T > generalizedGravityForce()
typename Eigen::Matrix< T, 3, 3 > RotMat
Definition: cppTypes.h:18
auto rotationFromSXform(const Eigen::MatrixBase< T > &X)
Definition: spatial.h:157
Implementation of Rigid Body Floating Base model data structure.
void addBase(const SpatialInertia< T > &inertia)
MX f(const MX &x, const MX &u)
auto invertSXform(const Eigen::MatrixBase< T > &X)
Definition: spatial.h:181