pinocchio  2.2.1-dirty
joint-prismatic.hpp
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
3 // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_joint_prismatic_hpp__
7 #define __pinocchio_joint_prismatic_hpp__
8 
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/multibody/joint/joint-base.hpp"
11 #include "pinocchio/multibody/constraint.hpp"
12 #include "pinocchio/spatial/inertia.hpp"
13 #include "pinocchio/spatial/spatial-axis.hpp"
14 #include "pinocchio/utils/axis-label.hpp"
15 
16 namespace pinocchio
17 {
18 
19  template<typename Scalar, int Options, int _axis> struct MotionPrismaticTpl;
20 
21  template<typename Scalar, int Options, int axis>
22  struct SE3GroupAction< MotionPrismaticTpl<Scalar,Options,axis> >
23  {
25  };
26 
27  template<typename Scalar, int Options, int axis, typename MotionDerived>
28  struct MotionAlgebraAction< MotionPrismaticTpl<Scalar,Options,axis>, MotionDerived>
29  {
31  };
32 
33  template<typename _Scalar, int _Options, int _axis>
34  struct traits < MotionPrismaticTpl<_Scalar,_Options,_axis> >
35  {
36  typedef _Scalar Scalar;
37  enum { Options = _Options };
38  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
39  typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
40  typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
41  typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
42  typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
43  typedef Vector3 AngularType;
44  typedef Vector3 LinearType;
45  typedef const Vector3 ConstAngularType;
46  typedef const Vector3 ConstLinearType;
47  typedef Matrix6 ActionMatrixType;
49  typedef MotionPlain PlainReturnType;
50  enum {
51  LINEAR = 0,
52  ANGULAR = 3
53  };
54  }; // struct traits MotionPrismaticTpl
55 
56  template<typename _Scalar, int _Options, int _axis>
57  struct MotionPrismaticTpl
58  : MotionBase < MotionPrismaticTpl<_Scalar,_Options,_axis> >
59  {
60  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
61  MOTION_TYPEDEF_TPL(MotionPrismaticTpl);
62 
63  enum { axis = _axis };
64 
65  typedef SpatialAxis<_axis+LINEAR> Axis;
66  typedef typename Axis::CartesianAxis3 CartesianAxis3;
67 
69  MotionPrismaticTpl(const Scalar & v) : rate(v) {}
70 
71  inline PlainReturnType plain() const { return Axis() * rate; }
72 
73  template<typename OtherScalar>
74  MotionPrismaticTpl __mult__(const OtherScalar & alpha) const
75  {
76  return MotionPrismaticTpl(alpha*rate);
77  }
78 
79  template<typename Derived>
80  void addTo(MotionDense<Derived> & other) const
81  {
82  typedef typename MotionDense<Derived>::Scalar OtherScalar;
83  other.linear()[_axis] += (OtherScalar) rate;
84  }
85 
86  template<typename MotionDerived>
87  void setTo(MotionDense<MotionDerived> & other) const
88  {
89  for(Eigen::DenseIndex k = 0; k < 3; ++k)
90  other.linear()[k] = k == axis ? rate : (Scalar)0;
91  other.angular().setZero();
92  }
93 
94  template<typename S2, int O2, typename D2>
95  void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
96  {
97  v.angular().setZero();
98  v.linear().noalias() = rate * (m.rotation().col(axis));
99  }
100 
101  template<typename S2, int O2>
102  MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
103  {
104  MotionPlain res;
105  se3Action_impl(m,res);
106  return res;
107  }
108 
109  template<typename S2, int O2, typename D2>
110  void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
111  {
112  // Linear
113  v.linear().noalias() = rate * (m.rotation().transpose().col(axis));
114 
115  // Angular
116  v.angular().setZero();
117  }
118 
119  template<typename S2, int O2>
120  MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
121  {
122  MotionPlain res;
123  se3ActionInverse_impl(m,res);
124  return res;
125  }
126 
127  template<typename M1, typename M2>
128  void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
129  {
130  // Linear
131  CartesianAxis3::alphaCross(-rate,v.angular(),mout.linear());
132 
133  // Angular
134  mout.angular().setZero();
135  }
136 
137  template<typename M1>
138  MotionPlain motionAction(const MotionDense<M1> & v) const
139  {
140  MotionPlain res;
141  motionAction(v,res);
142  return res;
143  }
144 
145  //data
146  Scalar rate;
147  }; // struct MotionPrismaticTpl
148 
149  template<typename Scalar, int Options, int axis, typename MotionDerived>
150  typename MotionDerived::MotionPlain
151  operator+(const MotionPrismaticTpl<Scalar,Options,axis> & m1,
152  const MotionDense<MotionDerived> & m2)
153  {
154  typename MotionDerived::MotionPlain res(m2);
155  res += m1;
156  return res;
157  }
158 
159  template<typename MotionDerived, typename S2, int O2, int axis>
160  EIGEN_STRONG_INLINE
161  typename MotionDerived::MotionPlain
162  operator^(const MotionDense<MotionDerived> & m1, const MotionPrismaticTpl<S2,O2,axis> & m2)
163  {
164  return m2.motionAction(m1);
165  }
166 
167  template<typename Scalar, int Options, int axis> struct TransformPrismaticTpl;
168 
169  template<typename _Scalar, int _Options, int _axis>
170  struct traits< TransformPrismaticTpl<_Scalar,_Options,_axis> >
171  {
172  enum {
173  axis = _axis,
174  Options = _Options,
175  LINEAR = 0,
176  ANGULAR = 3
177  };
178  typedef _Scalar Scalar;
180  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
181  typedef Eigen::Matrix<Scalar,3,3,Options> Matrix3;
182  typedef typename Matrix3::IdentityReturnType AngularType;
183  typedef AngularType AngularRef;
184  typedef AngularType ConstAngularRef;
185  typedef Vector3 LinearType;
186  typedef const Vector3 LinearRef;
187  typedef const Vector3 ConstLinearRef;
188  typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
189  typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
190  }; // traits TransformPrismaticTpl
191 
192  template<typename Scalar, int Options, int axis>
193  struct SE3GroupAction< TransformPrismaticTpl<Scalar,Options,axis> >
194  { typedef typename traits <TransformPrismaticTpl<Scalar,Options,axis> >::PlainType ReturnType; };
195 
196  template<typename _Scalar, int _Options, int axis>
197  struct TransformPrismaticTpl : SE3Base< TransformPrismaticTpl<_Scalar,_Options,axis> >
198  {
199  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
200  PINOCCHIO_SE3_TYPEDEF_TPL(TransformPrismaticTpl);
201  typedef typename traits<TransformPrismaticTpl>::PlainType PlainType;
202 
203  typedef SpatialAxis<axis+LINEAR> Axis;
204  typedef typename Axis::CartesianAxis3 CartesianAxis3;
205 
207  TransformPrismaticTpl(const Scalar & displacement)
208  : m_displacement(displacement)
209  {}
210 
211  PlainType plain() const
212  {
213  PlainType res(PlainType::Identity());
214  res.rotation().setIdentity();
215  res.translation()[axis] = m_displacement;
216 
217  return res;
218  }
219 
220  operator PlainType() const { return plain(); }
221 
222  template<typename S2, int O2>
224  se3action(const SE3Tpl<S2,O2> & m) const
225  {
226  typedef typename SE3GroupAction<TransformPrismaticTpl>::ReturnType ReturnType;
227  ReturnType res(m);
228  res.translation()[axis] += m_displacement;
229 
230  return res;
231  }
232 
233  const Scalar & displacement() const { return m_displacement; }
234  Scalar & displacement() { return m_displacement; }
235 
236  ConstLinearRef translation() const { return CartesianAxis3()*displacement(); };
237  AngularType rotation() const { return AngularType(3,3); }
238 
239  protected:
240 
241  Scalar m_displacement;
242  };
243 
244  template<typename Scalar, int Options, int axis> struct ConstraintPrismaticTpl;
245 
246  template<typename _Scalar, int _Options, int axis>
247  struct traits< ConstraintPrismaticTpl<_Scalar,_Options,axis> >
248  {
249  typedef _Scalar Scalar;
250  enum { Options = _Options };
251  enum {
252  LINEAR = 0,
253  ANGULAR = 3
254  };
256  typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
257  typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
258  typedef DenseBase MatrixReturnType;
259  typedef const DenseBase ConstMatrixReturnType;
260  }; // traits ConstraintRevolute
261 
262  template<typename Scalar, int Options, int axis>
263  struct SE3GroupAction< ConstraintPrismaticTpl<Scalar,Options,axis> >
264  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
265 
266  template<typename Scalar, int Options, int axis, typename MotionDerived>
267  struct MotionAlgebraAction< ConstraintPrismaticTpl<Scalar,Options,axis>, MotionDerived >
268  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
269 
270  template<typename Scalar, int Options, int axis, typename ForceDerived>
271  struct ConstraintForceOp< ConstraintPrismaticTpl<Scalar,Options,axis>, ForceDerived>
272  { typedef typename ForceDense<ForceDerived>::ConstLinearType::template ConstFixedSegmentReturnType<1>::Type ReturnType; };
273 
274  template<typename Scalar, int Options, int axis, typename ForceSet>
275  struct ConstraintForceSetOp< ConstraintPrismaticTpl<Scalar,Options,axis>, ForceSet>
276  { typedef typename Eigen::MatrixBase<ForceSet>::ConstRowXpr ReturnType; };
277 
278  template<typename _Scalar, int _Options, int axis>
280  : ConstraintBase < ConstraintPrismaticTpl <_Scalar,_Options,axis> >
281  {
282  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
283  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintPrismaticTpl)
284  enum { NV = 1 };
285 
286  typedef SpatialAxis<LINEAR+axis> Axis;
287 
289 
290  template<typename Vector1Like>
291  JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
292  {
293  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
294  assert(v.size() == 1);
295  return JointMotion(v[0]);
296  }
297 
298  template<typename S2, int O2>
300  se3Action(const SE3Tpl<S2,O2> & m) const
301  {
303  MotionRef<DenseBase> v(res);
304  v.linear() = m.rotation().col(axis);
305  v.angular().setZero();
306  return res;
307  }
308 
309  template<typename S2, int O2>
311  se3ActionInverse(const SE3Tpl<S2,O2> & m) const
312  {
314  MotionRef<DenseBase> v(res);
315  v.linear() = m.rotation().transpose().col(axis);
316  v.angular().setZero();
317  return res;
318  }
319 
320  int nv_impl() const { return NV; }
321 
323  {
324  const ConstraintPrismaticTpl & ref;
325  TransposeConst(const ConstraintPrismaticTpl & ref) : ref(ref) {}
326 
327  template<typename ForceDerived>
328  typename ConstraintForceOp<ConstraintPrismaticTpl,ForceDerived>::ReturnType
329  operator* (const ForceDense<ForceDerived> & f) const
330  { return f.linear().template segment<1>(axis); }
331 
332  /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
333  template<typename Derived>
334  typename ConstraintForceSetOp<ConstraintPrismaticTpl,Derived>::ReturnType
335  operator*(const Eigen::MatrixBase<Derived> & F )
336  {
337  assert(F.rows()==6);
338  return F.row(LINEAR+axis);
339  }
340 
341  }; // struct TransposeConst
342  TransposeConst transpose() const { return TransposeConst(*this); }
343 
344  /* CRBA joint operators
345  * - ForceSet::Block = ForceSet
346  * - ForceSet operator* (Inertia Y,Constraint S)
347  * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
348  * - SE3::act(ForceSet::Block)
349  */
350  DenseBase matrix_impl() const
351  {
352  DenseBase S;
354  v << Axis();
355  return S;
356  }
357 
358  template<typename MotionDerived>
360  motionAction(const MotionDense<MotionDerived> & m) const
361  {
363  MotionRef<DenseBase> v(res);
364  v = m.cross(Axis());
365  return res;
366  }
367 
368  }; // struct ConstraintPrismaticTpl
369 
370  template<typename S1, int O1,typename S2, int O2, int axis>
372  {
373  typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
374  };
375 
376  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
377  namespace impl
378  {
379  template<typename S1, int O1, typename S2, int O2>
381  {
382  typedef InertiaTpl<S1,O1> Inertia;
384  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
385  static inline ReturnType run(const Inertia & Y,
386  const Constraint & /*constraint*/)
387  {
388  ReturnType res;
389 
390  /* Y(:,0) = ( 1,0, 0, 0 , z , -y ) */
391  const S1
392  &m = Y.mass(),
393  &y = Y.lever()[1],
394  &z = Y.lever()[2];
395  res << m, S1(0), S1(0), S1(0), m*z, -m*y;
396 
397  return res;
398  }
399  };
400 
401  template<typename S1, int O1, typename S2, int O2>
403  {
404  typedef InertiaTpl<S1,O1> Inertia;
406  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
407  static inline ReturnType run(const Inertia & Y,
408  const Constraint & /*constraint*/)
409  {
410  ReturnType res;
411 
412  /* Y(:,1) = ( 0,1, 0, -z , 0 , x) */
413  const S1
414  &m = Y.mass(),
415  &x = Y.lever()[0],
416  &z = Y.lever()[2];
417 
418  res << S1(0), m, S1(0), -m*z, S1(0), m*x;
419 
420  return res;
421  }
422  };
423 
424  template<typename S1, int O1, typename S2, int O2>
426  {
427  typedef InertiaTpl<S1,O1> Inertia;
429  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
430  static inline ReturnType run(const Inertia & Y,
431  const Constraint & /*constraint*/)
432  {
433  ReturnType res;
434 
435  /* Y(:,2) = ( 0,0, 1, y , -x , 0) */
436  const S1
437  &m = Y.mass(),
438  &x = Y.lever()[0],
439  &y = Y.lever()[1];
440 
441  res << S1(0), S1(0), m, m*y, -m*x, S1(0);
442 
443  return res;
444  }
445  };
446  } // namespace impl
447 
448  template<typename M6Like,typename S2, int O2, int axis>
449  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticTpl<S2,O2,axis> >
450  {
451  typedef typename M6Like::ConstColXpr ReturnType;
452  };
453 
454  /* [ABA] operator* (Inertia Y,Constraint S) */
455  namespace impl
456  {
457  template<typename M6Like, typename Scalar, int Options, int axis>
458  struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticTpl<Scalar,Options,axis> >
459  {
461  typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
462  static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
463  const Constraint & /*constraint*/)
464  {
465  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
466  return Y.derived().col(Inertia::LINEAR + axis);
467  }
468  };
469  } // namespace impl
470 
471  template<typename _Scalar, int _Options, int _axis>
473  {
474  typedef _Scalar Scalar;
475 
476  enum
477  {
478  Options = _Options,
479  axis = _axis
480  };
481  };
482 
483  template<typename _Scalar, int _Options, int axis>
484  struct traits< JointPrismaticTpl<_Scalar,_Options,axis> >
485  {
486  enum {
487  NQ = 1,
488  NV = 1
489  };
490  typedef _Scalar Scalar;
491  enum { Options = _Options };
498 
499  // [ABA]
500  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
501  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
502  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
503 
504  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
505 
506  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
507  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
508  };
509 
510  template<typename Scalar, int Options, int axis>
511  struct traits< JointDataPrismaticTpl<Scalar,Options,axis> >
513 
514  template<typename Scalar, int Options, int axis>
515  struct traits< JointModelPrismaticTpl<Scalar,Options,axis> >
517 
518  template<typename _Scalar, int _Options, int axis>
519  struct JointDataPrismaticTpl : public JointDataBase< JointDataPrismaticTpl<_Scalar,_Options,axis> >
520  {
521  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
523  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
524  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
525 
526  Constraint_t S;
527  Transformation_t M;
528  Motion_t v;
529  Bias_t c;
530 
531  // [ABA] specific data
532  U_t U;
533  D_t Dinv;
534  UD_t UDinv;
535 
537 
538  static std::string classname() { return std::string("JointDataPrismatic"); }
539  std::string shortname() const { return classname(); }
540 
541  }; // struct JointDataPrismaticTpl
542 
543  template<typename NewScalar, typename Scalar, int Options, int axis>
544  struct CastType< NewScalar, JointModelPrismaticTpl<Scalar,Options,axis> >
545  {
547  };
548 
549  template<typename _Scalar, int _Options, int axis>
551  : public JointModelBase< JointModelPrismaticTpl<_Scalar,_Options,axis> >
552  {
553  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
554  typedef JointPrismaticTpl<_Scalar,_Options,axis> JointDerived;
555  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
556 
558  using Base::id;
559  using Base::idx_q;
560  using Base::idx_v;
561  using Base::setIndexes;
562 
563  JointDataDerived createData() const { return JointDataDerived(); }
564 
565  template<typename ConfigVector>
566  void calc(JointDataDerived & data,
567  const typename Eigen::MatrixBase<ConfigVector> & qs) const
568  {
569  typedef typename ConfigVector::Scalar Scalar;
570  const Scalar & q = qs[idx_q()];
571  data.M.displacement() = q;
572  }
573 
574  template<typename ConfigVector, typename TangentVector>
575  void calc(JointDataDerived & data,
576  const typename Eigen::MatrixBase<ConfigVector> & qs,
577  const typename Eigen::MatrixBase<TangentVector> & vs) const
578  {
579  calc(data,qs.derived());
580 
581  typedef typename TangentVector::Scalar S2;
582  const S2 & v = vs[idx_v()];
583  data.v.rate = v;
584  }
585 
586  template<typename Matrix6Like>
587  void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
588  {
589  data.U = I.col(Inertia::LINEAR + axis);
590  data.Dinv[0] = 1./I(Inertia::LINEAR + axis, Inertia::LINEAR + axis);
591  data.UDinv.noalias() = data.U * data.Dinv[0];
592 
593  if (update_I)
594  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
595  }
596 
597  static std::string classname()
598  {
599  return std::string("JointModelP") + axisLabel<axis>();
600  }
601  std::string shortname() const { return classname(); }
602 
604  template<typename NewScalar>
606  {
608  ReturnType res;
609  res.setIndexes(id(),idx_q(),idx_v());
610  return res;
611  }
612 
613  }; // struct JointModelPrismaticTpl
614 
618 
622 
626 
627 } //namespace pinocchio
628 
629 #include <boost/type_traits.hpp>
630 
631 namespace boost
632 {
633  template<typename Scalar, int Options, int axis>
634  struct has_nothrow_constructor< ::pinocchio::JointModelPrismaticTpl<Scalar,Options,axis> >
635  : public integral_constant<bool,true> {};
636 
637  template<typename Scalar, int Options, int axis>
638  struct has_nothrow_copy< ::pinocchio::JointModelPrismaticTpl<Scalar,Options,axis> >
639  : public integral_constant<bool,true> {};
640 
641  template<typename Scalar, int Options, int axis>
642  struct has_nothrow_constructor< ::pinocchio::JointDataPrismaticTpl<Scalar,Options,axis> >
643  : public integral_constant<bool,true> {};
644 
645  template<typename Scalar, int Options, int axis>
646  struct has_nothrow_copy< ::pinocchio::JointDataPrismaticTpl<Scalar,Options,axis> >
647  : public integral_constant<bool,true> {};
648 }
649 
650 #endif // ifndef __pinocchio_joint_prismatic_hpp__
Forward declaration of the multiplication operation return type. Should be overloaded, otherwise it will procude a compilation error.
Definition: binary-op.hpp:15
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
int idx_v(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxVVisitor to get the index in the full model tangent space corre...
Return type of the Constraint::Transpose * ForceSet operation.
Return type of the ation of a Motion onto an object of type D.
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.
Source from #include <cppad/example/cppad_eigen.hpp>
JointModelPrismaticTpl< NewScalar, Options, axis > cast() const
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
Main pinocchio namespace.
Definition: treeview.dox:24
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:32
ConstLinearType linear() const
Return the linear part of the force vector.
Definition: force-base.hpp:47
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
Definition: spatial/fwd.hpp:43
Return type of the Constraint::Transpose * Force operation.
void calc_aba(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, const Eigen::MatrixBase< Matrix6Type > &I, const bool update_I)
Visit a JointModelTpl and the corresponding JointDataTpl through JointCalcAbaVisitor to...
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
 .