pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-prismatic-unaligned.hpp
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
3 // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_joint_prismatic_unaligned_hpp__
7 #define __pinocchio_joint_prismatic_unaligned_hpp__
8 
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/multibody/joint/joint-base.hpp"
11 #include "pinocchio/multibody/joint/joint-translation.hpp"
12 #include "pinocchio/multibody/constraint.hpp"
13 #include "pinocchio/spatial/inertia.hpp"
14 #include "pinocchio/math/matrix.hpp"
15 
16 namespace pinocchio
17 {
18 
19  template<typename Scalar, int Options=0> struct MotionPrismaticUnalignedTpl;
21 
22  template<typename Scalar, int Options>
23  struct SE3GroupAction< MotionPrismaticUnalignedTpl<Scalar,Options> >
24  {
26  };
27 
28  template<typename Scalar, int Options, typename MotionDerived>
29  struct MotionAlgebraAction< MotionPrismaticUnalignedTpl<Scalar,Options>, MotionDerived>
30  {
32  };
33 
34  template<typename _Scalar, int _Options>
35  struct traits< MotionPrismaticUnalignedTpl<_Scalar,_Options> >
36  {
37  typedef _Scalar Scalar;
38  enum { Options = _Options };
39  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
40  typedef Eigen::Matrix<Scalar,6,1,Options> Vector6;
41  typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
42  typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
43  typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
44  typedef Vector3 AngularType;
45  typedef Vector3 LinearType;
46  typedef const Vector3 ConstAngularType;
47  typedef const Vector3 ConstLinearType;
48  typedef Matrix6 ActionMatrixType;
50  typedef MotionPlain PlainReturnType;
51  enum {
52  LINEAR = 0,
53  ANGULAR = 3
54  };
55  }; // traits MotionPrismaticUnalignedTpl
56 
57  template<typename _Scalar, int _Options>
59  : MotionBase < MotionPrismaticUnalignedTpl<_Scalar,_Options> >
60  {
61  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
62  MOTION_TYPEDEF_TPL(MotionPrismaticUnalignedTpl);
63 
65 
66  template<typename Vector3Like, typename S2>
67  MotionPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis,
68  const S2 & v)
69  : m_axis(axis), m_v(v)
70  { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }
71 
72  inline PlainReturnType plain() const
73  {
74  return PlainReturnType(m_axis*m_v,
75  PlainReturnType::Vector3::Zero());
76  }
77 
78  template<typename OtherScalar>
79  MotionPrismaticUnalignedTpl __mult__(const OtherScalar & alpha) const
80  {
81  return MotionPrismaticUnalignedTpl(m_axis,alpha*m_v);
82  }
83 
84  template<typename Derived>
85  void addTo(MotionDense<Derived> & other) const
86  {
87  other.linear() += m_axis * m_v;
88  }
89 
90  template<typename Derived>
91  void setTo(MotionDense<Derived> & other) const
92  {
93  other.linear().noalias() = m_axis*m_v;
94  other.angular().setZero();
95  }
96 
97  template<typename S2, int O2, typename D2>
98  void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
99  {
100  v.linear().noalias() = m_v * (m.rotation() * m_axis); // TODO: check efficiency
101  v.angular().setZero();
102  }
103 
104  template<typename S2, int O2>
105  MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
106  {
107  MotionPlain res;
108  se3Action_impl(m,res);
109  return res;
110  }
111 
112  template<typename S2, int O2, typename D2>
113  void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
114  {
115  // Linear
116  v.linear().noalias() = m_v * (m.rotation().transpose() * m_axis);
117 
118  // Angular
119  v.angular().setZero();
120  }
121 
122  template<typename S2, int O2>
123  MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
124  {
125  MotionPlain res;
126  se3ActionInverse_impl(m,res);
127  return res;
128  }
129 
130  template<typename M1, typename M2>
131  void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
132  {
133  // Linear
134  mout.linear().noalias() = v.angular().cross(m_axis);
135  mout.linear() *= m_v;
136 
137  // Angular
138  mout.angular().setZero();
139  }
140 
141  template<typename M1>
142  MotionPlain motionAction(const MotionDense<M1> & v) const
143  {
144  MotionPlain res;
145  motionAction(v,res);
146  return res;
147  }
148 
149  bool isEqual_impl(const MotionPrismaticUnalignedTpl & other) const
150  {
151  return m_axis == other.m_axis && m_v == other.m_v;
152  }
153 
154  const Scalar & linearRate() const { return m_v; }
155  Scalar & linearRate() { return m_v; }
156 
157  const Vector3 & axis() const { return m_axis; }
158  Vector3 & axis() { return m_axis; }
159 
160  protected:
161 
162  Vector3 m_axis;
163  Scalar m_v;
164  }; // struct MotionPrismaticUnalignedTpl
165 
166  template<typename Scalar, int Options, typename MotionDerived>
167  inline typename MotionDerived::MotionPlain
169  {
170  typedef typename MotionDerived::MotionPlain ReturnType;
171  return ReturnType(m1.linearRate() * m1.axis() + m2.linear(), m2.angular());
172  }
173 
174  template<typename MotionDerived, typename S2, int O2>
175  inline typename MotionDerived::MotionPlain
176  operator^(const MotionDense<MotionDerived> & m1,
178  {
179  return m2.motionAction(m1);
180  }
181 
182  template<typename Scalar, int Options> struct ConstraintPrismaticUnalignedTpl;
183 
184  template<typename _Scalar, int _Options>
185  struct traits< ConstraintPrismaticUnalignedTpl<_Scalar,_Options> >
186  {
187  typedef _Scalar Scalar;
188  enum { Options = _Options };
189  enum {
190  LINEAR = 0,
191  ANGULAR = 3
192  };
194  typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
195  typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
196  typedef DenseBase MatrixReturnType;
197  typedef const DenseBase ConstMatrixReturnType;
198 
199  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
200  }; // traits ConstraintPrismaticUnalignedTpl
201 
202  template<typename Scalar, int Options>
204  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
205 
206  template<typename Scalar, int Options, typename MotionDerived>
207  struct MotionAlgebraAction< ConstraintPrismaticUnalignedTpl<Scalar,Options>,MotionDerived >
208  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
209 
210  template<typename Scalar, int Options, typename ForceDerived>
211  struct ConstraintForceOp< ConstraintPrismaticUnalignedTpl<Scalar,Options>, ForceDerived>
212  {
213  typedef typename traits< ConstraintPrismaticUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
214  typedef Eigen::Matrix<typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<ForceDerived>::ConstAngularType),1,1,Options> ReturnType;
215  };
216 
217  template<typename Scalar, int Options, typename ForceSet>
219  {
220  typedef typename traits< ConstraintPrismaticUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
222  typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type
223  >::type ReturnType;
224  };
225 
226  template<typename _Scalar, int _Options>
228  : ConstraintBase< ConstraintPrismaticUnalignedTpl<_Scalar,_Options> >
229  {
230  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
231  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintPrismaticUnalignedTpl)
232 
233  enum { NV = 1 };
234 
235  typedef typename traits<ConstraintPrismaticUnalignedTpl>::Vector3 Vector3;
236 
238 
239  template<typename Vector3Like>
240  ConstraintPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
241  : m_axis(axis)
242  { EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3); }
243 
244  template<typename Vector1Like>
245  JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
246  {
247  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
248  return JointMotion(m_axis,v[0]);
249  }
250 
251  template<typename S1, int O1>
253  se3Action(const SE3Tpl<S1,O1> & m) const
254  {
256  MotionRef<DenseBase> v(res);
257  v.linear().noalias() = m.rotation()*m_axis;
258  v.angular().setZero();
259  return res;
260  }
261 
262  template<typename S1, int O1>
264  se3ActionInverse(const SE3Tpl<S1,O1> & m) const
265  {
267  MotionRef<DenseBase> v(res);
268  v.linear().noalias() = m.rotation().transpose()*m_axis;
269  v.angular().setZero();
270  return res;
271  }
272 
273  int nv_impl() const { return NV; }
274 
276  {
278  TransposeConst(const ConstraintPrismaticUnalignedTpl & ref) : ref(ref) {}
279 
280  template<typename ForceDerived>
281  typename ConstraintForceOp<ConstraintPrismaticUnalignedTpl,ForceDerived>::ReturnType
282  operator* (const ForceDense<ForceDerived> & f) const
283  {
284  typedef typename ConstraintForceOp<ConstraintPrismaticUnalignedTpl,ForceDerived>::ReturnType ReturnType;
285  ReturnType res;
286  res[0] = ref.axis().dot(f.linear());
287  return res;
288  }
289 
290  /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
291  template<typename ForceSet>
292  typename ConstraintForceSetOp<ConstraintPrismaticUnalignedTpl,ForceSet>::ReturnType
293  operator*(const Eigen::MatrixBase<ForceSet> & F)
294  {
295  EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
296  /* Return ax.T * F[1:3,:] */
297  return ref.axis().transpose() * F.template middleRows<3>(LINEAR);
298  }
299 
300  };
301 
302  TransposeConst transpose() const { return TransposeConst(*this); }
303 
304  /* CRBA joint operators
305  * - ForceSet::Block = ForceSet
306  * - ForceSet operator* (Inertia Y,Constraint S)
307  * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
308  * - SE3::act(ForceSet::Block)
309  */
310  DenseBase matrix_impl() const
311  {
312  DenseBase S;
313  S.template segment<3>(LINEAR) = m_axis;
314  S.template segment<3>(ANGULAR).setZero();
315  return S;
316  }
317 
318  template<typename MotionDerived>
319  DenseBase motionAction(const MotionDense<MotionDerived> & v) const
320  {
321  DenseBase res;
322  res.template segment<3>(LINEAR).noalias() = v.angular().cross(m_axis);
323  res.template segment<3>(ANGULAR).setZero();
324 
325  return res;
326  }
327 
328  const Vector3 & axis() const { return m_axis; }
329  Vector3 & axis() { return m_axis; }
330 
331  bool isEqual(const ConstraintPrismaticUnalignedTpl & other) const
332  {
333  return m_axis == other.m_axis;
334  }
335 
336  protected:
337 
338  Vector3 m_axis;
339 
340  }; // struct ConstraintPrismaticUnalignedTpl
341 
342  template<typename S1, int O1,typename S2, int O2>
344  {
345  typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
346  };
347 
348  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
349  namespace impl
350  {
351  template<typename S1, int O1, typename S2, int O2>
353  {
354  typedef InertiaTpl<S1,O1> Inertia;
356  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
357 
358  static inline ReturnType run(const Inertia & Y,
359  const Constraint & cpu)
360  {
361  ReturnType res;
362  /* YS = [ m -mcx ; mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */
363  const S1 & m = Y.mass();
364  const typename Inertia::Vector3 & c = Y.lever();
365 
366  res.template segment<3>(Constraint::LINEAR).noalias() = m*cpu.axis();
367  res.template segment<3>(Constraint::ANGULAR).noalias() = c.cross(res.template segment<3>(Constraint::LINEAR));
368 
369  return res;
370  }
371  };
372  } // namespace impl
373 
374  template<typename M6Like, typename Scalar, int Options>
375  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticUnalignedTpl<Scalar,Options> >
376  {
377  typedef typename SizeDepType<3>::ColsReturn<M6Like>::ConstType M6LikeCols;
378  typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst;
379 
381  typedef typename Constraint::Vector3 Vector3;
382  typedef const typename MatrixMatrixProduct<M6LikeColsNonConst,Vector3>::type ReturnType;
383  };
384 
385  /* [ABA] operator* (Inertia Y,Constraint S) */
386  namespace impl
387  {
388  template<typename M6Like, typename Scalar, int Options>
389  struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintPrismaticUnalignedTpl<Scalar,Options> >
390  {
392  typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
393  static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
394  const Constraint & cru)
395  {
396  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
397  return Y.derived().template middleCols<3>(Constraint::LINEAR) * cru.axis();
398  }
399  };
400  } // namespace impl
401 
402  template<typename Scalar, int Options> struct JointPrismaticUnalignedTpl;
403 
404  template<typename _Scalar, int _Options>
405  struct traits< JointPrismaticUnalignedTpl<_Scalar,_Options> >
406  {
407  enum {
408  NQ = 1,
409  NV = 1
410  };
411  typedef _Scalar Scalar;
412  enum { Options = _Options };
419 
420  // [ABA]
421  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
422  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
423  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
424 
425  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
426 
427  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
428  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
429  };
430 
431  template<typename Scalar, int Options>
432  struct traits< JointDataPrismaticUnalignedTpl<Scalar,Options> >
434 
435  template<typename _Scalar, int _Options>
437  : public JointDataBase< JointDataPrismaticUnalignedTpl<_Scalar,_Options> >
438  {
439  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
441  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
442  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
443 
444  Transformation_t M;
445  Constraint_t S;
446  Motion_t v;
447  Bias_t c;
448 
449  // [ABA] specific data
450  U_t U;
451  D_t Dinv;
452  UD_t UDinv;
453 
455  : M(Transformation_t::Vector3::Zero())
456  , S(Constraint_t::Vector3::Zero())
457  , v(Constraint_t::Vector3::Zero(),(Scalar)0)
458  , U(U_t::Zero())
459  , Dinv(D_t::Zero())
460  , UDinv(UD_t::Zero())
461  {}
462 
463  template<typename Vector3Like>
464  JointDataPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
465  : M()
466  , S(axis)
467  , v(axis,(Scalar)NAN)
468  , U(), Dinv(), UDinv()
469  {}
470 
471  static std::string classname() { return std::string("JointDataPrismaticUnaligned"); }
472  std::string shortname() const { return classname(); }
473 
474  }; // struct JointDataPrismaticUnalignedTpl
475 
476  template<typename Scalar, int Options>
477  struct traits< JointModelPrismaticUnalignedTpl<Scalar,Options> >
479 
480  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelPrismaticUnalignedTpl);
481  template<typename _Scalar, int _Options>
483  : public JointModelBase< JointModelPrismaticUnalignedTpl<_Scalar,_Options> >
484  {
485  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
487  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
488 
490  using Base::id;
491  using Base::idx_q;
492  using Base::idx_v;
493  using Base::setIndexes;
494 
495  typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;
496 
498  JointModelPrismaticUnalignedTpl(const Scalar & x,
499  const Scalar & y,
500  const Scalar & z)
501  : axis(x,y,z)
502  {
503  axis.normalize();
504  assert(isUnitary(axis) && "Translation axis is not unitary");
505  }
506 
507  template<typename Vector3Like>
508  JointModelPrismaticUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
509  : axis(axis)
510  {
511  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
512  assert(isUnitary(axis) && "Translation axis is not unitary");
513  }
514 
515  JointDataDerived createData() const { return JointDataDerived(axis); }
516 
517  using Base::isEqual;
518  bool isEqual(const JointModelPrismaticUnalignedTpl & other) const
519  {
520  return Base::isEqual(other) && axis == other.axis;
521  }
522 
523  template<typename ConfigVector>
524  void calc(JointDataDerived & data,
525  const typename Eigen::MatrixBase<ConfigVector> & qs) const
526  {
527  typedef typename ConfigVector::Scalar Scalar;
528  const Scalar & q = qs[idx_q()];
529 
530  data.M.translation().noalias() = axis * q;
531  }
532 
533  template<typename ConfigVector, typename TangentVector>
534  void calc(JointDataDerived & data,
535  const typename Eigen::MatrixBase<ConfigVector> & qs,
536  const typename Eigen::MatrixBase<TangentVector> & vs) const
537  {
538  calc(data,qs.derived());
539 
540  typedef typename TangentVector::Scalar S2;
541  const S2 & v = vs[idx_v()];
542  data.v.linearRate() = v;
543  }
544 
545  template<typename Matrix6Like>
546  void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
547  {
548  data.U.noalias() = I.template block<6,3> (0,Inertia::LINEAR) * axis;
549  data.Dinv[0] = Scalar(1)/axis.dot(data.U.template segment <3> (Inertia::LINEAR));
550  data.UDinv.noalias() = data.U * data.Dinv;
551 
552  if (update_I)
553  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
554  }
555 
556  static std::string classname() { return std::string("JointModelPrismaticUnaligned"); }
557  std::string shortname() const { return classname(); }
558 
560  template<typename NewScalar>
562  {
564  ReturnType res(axis.template cast<NewScalar>());
565  res.setIndexes(id(),idx_q(),idx_v());
566  return res;
567  }
568 
569  // data
570 
574  Vector3 axis;
575  }; // struct JointModelPrismaticUnalignedTpl
576 
577 } //namespace pinocchio
578 
579 #include <boost/type_traits.hpp>
580 
581 namespace boost
582 {
583  template<typename Scalar, int Options>
584  struct has_nothrow_constructor< ::pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
585  : public integral_constant<bool,true> {};
586 
587  template<typename Scalar, int Options>
588  struct has_nothrow_copy< ::pinocchio::JointModelPrismaticUnalignedTpl<Scalar,Options> >
589  : public integral_constant<bool,true> {};
590 
591  template<typename Scalar, int Options>
592  struct has_nothrow_constructor< ::pinocchio::JointDataPrismaticUnalignedTpl<Scalar,Options> >
593  : public integral_constant<bool,true> {};
594 
595  template<typename Scalar, int Options>
596  struct has_nothrow_copy< ::pinocchio::JointDataPrismaticUnalignedTpl<Scalar,Options> >
597  : public integral_constant<bool,true> {};
598 }
599 
600 
601 #endif // ifndef __pinocchio_joint_prismatic_unaligned_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...
Definition: casadi.hpp:13
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.
Definition: motion.hpp:44
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.
Definition: casadi.hpp:47
bool isUnitary(const Eigen::MatrixBase< MatrixLike > &mat, const typename MatrixLike::RealScalar &prec=Eigen::NumTraits< typename MatrixLike::Scalar >::dummy_precision())
Check whether the input matrix is Unitary within the given precision.
Definition: matrix.hpp:100
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
JointModelPrismaticUnalignedTpl< NewScalar, Options > cast() const
Main pinocchio namespace.
Definition: treeview.dox:24
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:35
ConstLinearType linear() const
Return the linear part of the force vector.
Definition: force-base.hpp:42
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)
 .