pinocchio  3.5.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-helical-unaligned.hpp
1 //
2 // Copyright (c) 2022-2023 INRIA
3 //
4 
5 #ifndef __pinocchio_multibody_joint_helical_unaligned_hpp__
6 #define __pinocchio_multibody_joint_helical_unaligned_hpp__
7 
8 #include "pinocchio/fwd.hpp"
9 #include "pinocchio/multibody/joint/joint-base.hpp"
10 #include "pinocchio/multibody/joint-motion-subspace.hpp"
11 #include "pinocchio/spatial/inertia.hpp"
12 
13 #include "pinocchio/math/matrix.hpp"
14 #include "pinocchio/math/rotation.hpp"
15 
16 namespace pinocchio
17 {
18 
19  template<typename Scalar, int Options>
20  struct MotionHelicalUnalignedTpl;
21 
22  template<typename Scalar, int Options>
23  struct SE3GroupAction<MotionHelicalUnalignedTpl<Scalar, Options>>
24  {
26  };
27 
28  template<typename Scalar, int Options, typename MotionDerived>
29  struct MotionAlgebraAction<MotionHelicalUnalignedTpl<Scalar, Options>, MotionDerived>
30  {
32  };
33 
34  template<typename _Scalar, int _Options>
35  struct traits<MotionHelicalUnalignedTpl<_Scalar, _Options>>
36  {
37  typedef _Scalar Scalar;
38  enum
39  {
40  Options = _Options
41  };
42  typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
43  typedef Eigen::Matrix<Scalar, 6, 1, Options> Vector6;
44  typedef Eigen::Matrix<Scalar, 4, 4, Options> Matrix4;
45  typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
46  typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Vector6) ToVectorConstReturnType;
47  typedef typename PINOCCHIO_EIGEN_REF_TYPE(Vector6) ToVectorReturnType;
48  typedef Vector3 AngularType;
49  typedef Vector3 LinearType;
50  typedef const Vector3 ConstAngularType;
51  typedef const Vector3 ConstLinearType;
52  typedef Matrix6 ActionMatrixType;
55  typedef Matrix4 HomogeneousMatrixType;
56  enum
57  {
58  LINEAR = 0,
59  ANGULAR = 3
60  };
61  }; // traits MotionHelicalUnalignedTpl
62 
63  template<typename _Scalar, int _Options>
64  struct MotionHelicalUnalignedTpl : MotionBase<MotionHelicalUnalignedTpl<_Scalar, _Options>>
65  {
66  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
67 
68  MOTION_TYPEDEF_TPL(MotionHelicalUnalignedTpl);
69 
71  {
72  }
73 
74  template<typename Vector3Like, typename OtherScalar>
76  const Eigen::MatrixBase<Vector3Like> & axis, const OtherScalar & w, const OtherScalar & v)
77  : m_axis(axis)
78  , m_w(w)
79  , m_v(v)
80  {
81  }
82 
83  inline PlainReturnType plain() const
84  {
85  return PlainReturnType(m_axis * m_v, m_axis * m_w);
86  }
87 
88  template<typename OtherScalar>
89  MotionHelicalUnalignedTpl __mult__(const OtherScalar & alpha) const
90  {
91  return MotionHelicalUnalignedTpl(m_axis, alpha * m_w, alpha * m_v);
92  }
93 
94  template<typename MotionDerived>
95  void setTo(MotionDense<MotionDerived> & m) const
96  {
97  for (Eigen::DenseIndex k = 0; k < 3; ++k)
98  {
99  m.angular().noalias() = m_axis * m_w;
100  m.linear().noalias() = m_axis * m_v;
101  }
102  }
103 
104  template<typename MotionDerived>
105  inline void addTo(MotionDense<MotionDerived> & v) const
106  {
107  v.angular() += m_axis * m_w;
108  v.linear() += m_axis * m_v;
109  }
110 
111  template<typename S2, int O2, typename D2>
112  inline void se3Action_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
113  {
114  v.angular().noalias() = m_w * m.rotation() * m_axis;
115  v.linear().noalias() = m.translation().cross(v.angular()) + m_v * m.rotation() * m_axis;
116  }
117 
118  template<typename S2, int O2>
119  MotionPlain se3Action_impl(const SE3Tpl<S2, O2> & m) const
120  {
121  MotionPlain res;
122  se3Action_impl(m, res);
123  return res;
124  }
125 
126  template<typename S2, int O2, typename D2>
127  void se3ActionInverse_impl(const SE3Tpl<S2, O2> & m, MotionDense<D2> & v) const
128  {
129  // Linear
130  v.angular().noalias() = m_axis.cross(m.translation());
131  v.angular() *= m_w;
132  v.linear().noalias() =
133  m.rotation().transpose() * v.angular() + m_v * (m.rotation().transpose() * m_axis);
134 
135  // Angular
136  v.angular().noalias() = m.rotation().transpose() * m_axis * m_w;
137  }
138 
139  template<typename S2, int O2>
140  MotionPlain se3ActionInverse_impl(const SE3Tpl<S2, O2> & m) const
141  {
142  MotionPlain res;
143  se3ActionInverse_impl(m, res);
144  return res;
145  }
146 
147  template<typename M1, typename M2>
148  void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
149  {
150  // Linear
151  mout.linear().noalias() = v.linear().cross(m_axis);
152  mout.linear() *= m_w;
153  mout.angular().noalias() = v.angular().cross(m_axis);
154  mout.angular() *= m_v;
155  mout.linear() += mout.angular();
156 
157  // Angular
158  mout.angular().noalias() = v.angular().cross(m_axis);
159  mout.angular() *= m_w;
160  }
161 
162  template<typename M1>
163  MotionPlain motionAction(const MotionDense<M1> & v) const
164  {
165  MotionPlain res;
166  motionAction(v, res);
167  return res;
168  }
169 
170  Scalar & angularRate()
171  {
172  return m_w;
173  }
174  const Scalar & angularRate() const
175  {
176  return m_w;
177  }
178 
179  Scalar & linearRate()
180  {
181  return m_v;
182  }
183  const Scalar & linearRate() const
184  {
185  return m_v;
186  }
187 
188  Vector3 & axis()
189  {
190  return m_axis;
191  }
192  const Vector3 & axis() const
193  {
194  return m_axis;
195  }
196 
197  bool isEqual_impl(const MotionHelicalUnalignedTpl & other) const
198  {
199  return internal::comparison_eq(m_axis, other.m_axis)
200  && internal::comparison_eq(m_w, other.m_w) && internal::comparison_eq(m_v, other.m_v);
201  }
202 
203  protected:
204  Vector3 m_axis;
205  Scalar m_w, m_v;
206 
207  }; // struct MotionHelicalUnalignedTpl
208 
209  template<typename S1, int O1, typename MotionDerived>
210  typename MotionDerived::MotionPlain
211  operator+(const MotionHelicalUnalignedTpl<S1, O1> & m1, const MotionDense<MotionDerived> & m2)
212  {
213  typename MotionDerived::MotionPlain res(m2);
214  res += m1;
215  return res;
216  }
217 
218  template<typename MotionDerived, typename S2, int O2>
219  EIGEN_STRONG_INLINE typename MotionDerived::MotionPlain
220  operator^(const MotionDense<MotionDerived> & m1, const MotionHelicalUnalignedTpl<S2, O2> & m2)
221  {
222  return m2.motionAction(m1);
223  }
224 
225  template<typename Scalar, int Options>
226  struct JointMotionSubspaceHelicalUnalignedTpl;
227 
228  template<typename Scalar, int Options>
230  {
231  typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
232  };
233 
234  template<typename Scalar, int Options, typename MotionDerived>
235  struct MotionAlgebraAction<JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>, MotionDerived>
236  {
237  typedef Eigen::Matrix<Scalar, 6, 1, Options> ReturnType;
238  };
239 
240  template<typename Scalar, int Options, typename ForceDerived>
241  struct ConstraintForceOp<JointMotionSubspaceHelicalUnalignedTpl<Scalar, Options>, ForceDerived>
242  {
243  typedef typename Eigen::Matrix<Scalar, 1, 1> ReturnType;
244  };
245 
246  template<typename Scalar, int Options, typename ForceSet>
248  {
249  typedef typename Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> ReturnType;
250  };
251 
252  template<typename _Scalar, int _Options>
254  {
255  typedef _Scalar Scalar;
256  enum
257  {
258  Options = _Options
259  };
260  enum
261  {
262  LINEAR = 0,
263  ANGULAR = 3
264  };
265 
267  typedef Eigen::Matrix<Scalar, 1, 1, Options> JointForce;
268  typedef Eigen::Matrix<Scalar, 6, 1, Options> DenseBase;
269  typedef Eigen::Matrix<Scalar, 1, 1, Options> ReducedSquaredMatrix;
270 
271  typedef DenseBase MatrixReturnType;
272  typedef const DenseBase ConstMatrixReturnType;
273 
274  typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
275 
276  typedef typename ReducedSquaredMatrix::IdentityReturnType StDiagonalMatrixSOperationReturnType;
277  }; // traits JointMotionSubspaceHelicalUnalignedTpl
278 
279  template<typename _Scalar, int _Options>
281  : JointMotionSubspaceBase<JointMotionSubspaceHelicalUnalignedTpl<_Scalar, _Options>>
282  {
283  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
284 
285  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceHelicalUnalignedTpl)
286  enum
287  {
288  NV = 1
289  };
290 
292  {
293  }
294 
296 
297  template<typename Vector3Like>
299  const Eigen::MatrixBase<Vector3Like> & axis, const Scalar & h)
300  : m_axis(axis)
301  , m_pitch(h)
302  {
303  }
304 
305  template<typename Vector1Like>
306  JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
307  {
308  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like, 1);
309  assert(v.size() == 1);
310  return JointMotion(m_axis, v[0], Scalar(v[0] * m_pitch));
311  }
312 
313  template<typename S1, int O1>
314  typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType
315  se3Action(const SE3Tpl<S1, O1> & m) const
316  {
317  typedef
318  typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType ReturnType;
319  ReturnType res;
320  res.template segment<3>(ANGULAR).noalias() = m.rotation() * m_axis;
321  res.template segment<3>(LINEAR).noalias() =
322  m.translation().cross(res.template segment<3>(ANGULAR)) + m_pitch * (m.rotation() * m_axis);
323  return res;
324  }
325 
326  template<typename S1, int O1>
327  typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType
328  se3ActionInverse(const SE3Tpl<S1, O1> & m) const
329  {
330  typedef
331  typename SE3GroupAction<JointMotionSubspaceHelicalUnalignedTpl>::ReturnType ReturnType;
332 
333  ReturnType res;
334  res.template segment<3>(ANGULAR).noalias() = m.rotation().transpose() * m_axis;
335  res.template segment<3>(LINEAR).noalias() =
336  -m.rotation().transpose() * m.translation().cross(m_axis)
337  + m.rotation().transpose() * m_axis * m_pitch;
338 
339  return res;
340  }
341 
342  int nv_impl() const
343  {
344  return NV;
345  }
346 
347  struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceHelicalUnalignedTpl>
348  {
351  : ref(ref)
352  {
353  }
354 
355  template<typename ForceDerived>
356  typename ConstraintForceOp<JointMotionSubspaceHelicalUnalignedTpl, ForceDerived>::ReturnType
357  operator*(const ForceDense<ForceDerived> & f) const
358  {
359  typedef typename ConstraintForceOp<
360  JointMotionSubspaceHelicalUnalignedTpl, ForceDerived>::ReturnType ReturnType;
361  ReturnType res;
362  res[0] = ref.axis().dot(f.angular()) + ref.axis().dot(f.linear()) * ref.m_pitch;
363  return res;
364  }
365 
367  template<typename Derived>
368  typename ConstraintForceSetOp<JointMotionSubspaceHelicalUnalignedTpl, Derived>::ReturnType
369  operator*(const Eigen::MatrixBase<Derived> & F) const
370  {
371  assert(F.rows() == 6);
372  return (
373  ref.axis().transpose() * F.template middleRows<3>(ANGULAR)
374  + (ref.axis().transpose() * F.template middleRows<3>(LINEAR) * ref.m_pitch));
375  }
376  }; // struct TransposeConst
377 
378  TransposeConst transpose() const
379  {
380  return TransposeConst(*this);
381  }
382 
383  /* CRBA joint operators
384  * - ForceSet::Block = ForceSet
385  * - ForceSet operator* (Inertia Y,Constraint S)
386  * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
387  * - SE3::act(ForceSet::Block)
388  */
389  DenseBase matrix_impl() const
390  {
391  DenseBase S;
392  S.template segment<3>(LINEAR) = m_axis * m_pitch;
393  S.template segment<3>(ANGULAR) = m_axis;
394  return S;
395  }
396 
397  template<typename MotionDerived>
398  typename MotionAlgebraAction<JointMotionSubspaceHelicalUnalignedTpl, MotionDerived>::ReturnType
399  motionAction(const MotionDense<MotionDerived> & m) const
400  {
401  const typename MotionDerived::ConstLinearType v = m.linear();
402  const typename MotionDerived::ConstAngularType w = m.angular();
403 
404  DenseBase res;
405  res.template segment<3>(LINEAR).noalias() = v.cross(m_axis);
406  res.template segment<3>(ANGULAR).noalias() = w.cross(m_axis * m_pitch);
407  res.template segment<3>(LINEAR).noalias() += res.template segment<3>(ANGULAR);
408  res.template segment<3>(ANGULAR).noalias() = w.cross(m_axis);
409 
410  return res;
411  }
412 
413  bool isEqual(const JointMotionSubspaceHelicalUnalignedTpl & other) const
414  {
415  return internal::comparison_eq(m_axis, other.m_axis)
416  && internal::comparison_eq(m_pitch, other.m_pitch);
417  }
418 
419  Vector3 & axis()
420  {
421  return m_axis;
422  }
423  const Vector3 & axis() const
424  {
425  return m_axis;
426  }
427 
428  Scalar & h()
429  {
430  return m_pitch;
431  }
432  const Scalar & h() const
433  {
434  return m_pitch;
435  }
436 
437  Vector3 m_axis;
438  Scalar m_pitch;
439 
440  }; // struct JointMotionSubspaceHelicalUnalignedTpl
441 
442  template<typename _Scalar, int _Options>
443  Eigen::Matrix<_Scalar, 1, 1, _Options> operator*(
444  const typename JointMotionSubspaceHelicalUnalignedTpl<_Scalar, _Options>::TransposeConst &
445  S_transpose,
446  const JointMotionSubspaceHelicalUnalignedTpl<_Scalar, _Options> & S)
447  {
448  Eigen::Matrix<_Scalar, 1, 1, _Options> res;
449  res(0) = (S_transpose.ref.axis() * S_transpose.ref.h()).dot(S.axis() * S.h())
450  + (S_transpose.ref.axis().dot(S.axis()));
451  return res;
452  }
453 
454  template<typename S1, int O1, typename S2, int O2>
456  {
457  typedef Eigen::Matrix<S2, 6, 1, O2> ReturnType;
458  };
459 
460  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
461  namespace impl
462  {
463  template<typename S1, int O1, typename S2, int O2>
465  {
466  typedef InertiaTpl<S1, O1> Inertia;
468  typedef typename JointMotionSubspaceHelicalUnalignedTpl<S2, O2>::Vector3 Vector3;
469  typedef typename MultiplicationOp<Inertia, Constraint>::ReturnType ReturnType;
470  static inline ReturnType run(const Inertia & Y, const Constraint & cru)
471  {
472  ReturnType res;
473  /* YS = [ m -mcx ; mcx I-mcxcx ] [ h ; w ] = [mh-mcxw ; mcxh+Iw-mcxcxw ] */
474 
475  const S2 & m_pitch = cru.h();
476  const typename Inertia::Scalar & m = Y.mass();
477  const typename Inertia::Vector3 & c = Y.lever();
478  const typename Inertia::Symmetric3 & I = Y.inertia();
479 
480  res.template segment<3>(Inertia::LINEAR) = -m * c.cross(cru.axis());
481  res.template segment<3>(Inertia::ANGULAR).noalias() = I * cru.axis();
482  res.template segment<3>(Inertia::ANGULAR) += c.cross(cru.axis()) * m * m_pitch;
483  res.template segment<3>(Inertia::ANGULAR) +=
484  c.cross(res.template segment<3>(Inertia::LINEAR));
485  res.template segment<3>(Inertia::LINEAR) += m * m_pitch * cru.axis();
486  return res;
487  }
488  };
489  } // namespace impl
490 
491  template<typename M6Like, typename Scalar, int Options>
493  Eigen::MatrixBase<M6Like>,
495  {
496  typedef const Eigen::Matrix<Scalar, 6, 1> ReturnType;
497  };
498 
499  /* [ABA] operator* (Inertia Y,Constraint S) */
500  namespace impl
501  {
502  template<typename M6Like, typename Scalar, int Options>
504  Eigen::MatrixBase<M6Like>,
506  {
508  typedef Eigen::Matrix<Scalar, 6, 1> ReturnType;
509  static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y, const Constraint & cru)
510  {
511  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like, 6, 6);
512  return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.axis()
513  + Y.derived().template middleCols<3>(Constraint::LINEAR) * cru.axis() * cru.h();
514  }
515  };
516  } // namespace impl
517 
518  template<typename Scalar, int Options>
520 
521  template<typename _Scalar, int _Options>
522  struct traits<JointHelicalUnalignedTpl<_Scalar, _Options>>
523  {
524  enum
525  {
526  NQ = 1,
527  NV = 1,
528  NVExtended = 1
529  };
530  typedef _Scalar Scalar;
531  enum
532  {
533  Options = _Options
534  };
541 
542  // [ABA]
543  typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
544  typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
545  typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
546 
547  typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
548  typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
549 
550  typedef boost::mpl::true_ is_mimicable_t;
551 
552  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
553  };
554 
555  template<typename _Scalar, int _Options>
556  struct traits<JointDataHelicalUnalignedTpl<_Scalar, _Options>>
557  {
559  typedef _Scalar Scalar;
560  };
561 
562  template<typename _Scalar, int _Options>
563  struct traits<JointModelHelicalUnalignedTpl<_Scalar, _Options>>
564  {
566  typedef _Scalar Scalar;
567  };
568 
569  template<typename _Scalar, int _Options>
571  : public JointDataBase<JointDataHelicalUnalignedTpl<_Scalar, _Options>>
572  {
573  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
575  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
576  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
577 
578  ConfigVector_t joint_q;
579  TangentVector_t joint_v;
580 
581  Constraint_t S;
582  Transformation_t M;
583  Motion_t v;
584  Bias_t c;
585 
586  // [ABA] specific data
587  U_t U;
588  D_t Dinv;
589  UD_t UDinv;
590  D_t StU;
591 
593  : joint_q(ConfigVector_t::Zero())
594  , joint_v(TangentVector_t::Zero())
595  , S(Constraint_t::Vector3::Zero(), (Scalar)0)
596  , M(Transformation_t::Identity())
597  , v(Constraint_t::Vector3::Zero(), (Scalar)0, (Scalar)0)
598  , U(U_t::Zero())
599  , Dinv(D_t::Zero())
600  , UDinv(UD_t::Zero())
601  , StU(D_t::Zero())
602  {
603  }
604 
605  template<typename Vector3Like>
606  JointDataHelicalUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
607  : joint_q(ConfigVector_t::Zero())
608  , joint_v(TangentVector_t::Zero())
609  , S(axis, (Scalar)0)
610  , M(Transformation_t::Identity())
611  , v(axis, (Scalar)0, (Scalar)0)
612  , U(U_t::Zero())
613  , Dinv(D_t::Zero())
614  , UDinv(UD_t::Zero())
615  , StU(D_t::Zero())
616  {
617  }
618 
619  static std::string classname()
620  {
621  return std::string("JointDataHelicalUnaligned");
622  }
623  std::string shortname() const
624  {
625  return classname();
626  }
627 
628  }; // struct JointDataHelicalUnalignedTpl
629 
630  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelHelicalUnalignedTpl);
631  template<typename _Scalar, int _Options>
633  : public JointModelBase<JointModelHelicalUnalignedTpl<_Scalar, _Options>>
634  {
635  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
637  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
638  typedef Eigen::Matrix<Scalar, 3, 1, _Options> Vector3;
639 
641  using Base::id;
642  using Base::idx_q;
643  using Base::idx_v;
644  using Base::idx_vExtended;
645  using Base::setIndexes;
646 
648  {
649  }
650 
651  template<typename Vector3Like>
652  JointModelHelicalUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
653  : axis(axis)
654  , m_pitch((Scalar)0)
655  {
656  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
657  assert(isUnitary(axis) && "Rotation axis is not unitary");
658  }
659 
661  const Scalar & x, const Scalar & y, const Scalar & z, const Scalar & h)
662  : axis(x, y, z)
663  , m_pitch(h)
664  {
665  normalize(axis);
666  assert(isUnitary(axis) && "Rotation axis is not unitary");
667  }
668 
669  template<typename Vector3Like>
670  JointModelHelicalUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis, const Scalar & h)
671  : axis(axis)
672  , m_pitch(h)
673  {
674  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
675  assert(isUnitary(axis) && "Rotation axis is not unitary");
676  }
677 
678  const std::vector<bool> hasConfigurationLimit() const
679  {
680  return {true, true};
681  }
682 
683  const std::vector<bool> hasConfigurationLimitInTangent() const
684  {
685  return {true, true};
686  }
687 
688  JointDataDerived createData() const
689  {
690  return JointDataDerived();
691  }
692 
693  using Base::isEqual;
694  bool isEqual(const JointModelHelicalUnalignedTpl & other) const
695  {
696  return Base::isEqual(other) && internal::comparison_eq(axis, other.axis)
697  && internal::comparison_eq(m_pitch, other.m_pitch);
698  }
699 
700  template<typename ConfigVector>
701  void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
702  {
703  data.joint_q[0] = qs[idx_q()];
704 
705  toRotationMatrix(axis, data.joint_q[0], data.M.rotation());
706  data.M.translation() = axis * data.joint_q[0] * m_pitch;
707  data.S.h() = m_pitch;
708  data.S.axis() = axis;
709  }
710 
711  template<typename TangentVector>
712  void
713  calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
714  const
715  {
716  data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
717  data.v.axis() = axis;
718  data.v.linearRate() = static_cast<Scalar>(vs[idx_v()] * m_pitch);
719  }
720 
721  template<typename ConfigVector, typename TangentVector>
722  void calc(
723  JointDataDerived & data,
724  const typename Eigen::MatrixBase<ConfigVector> & qs,
725  const typename Eigen::MatrixBase<TangentVector> & vs) const
726  {
727  calc(data, qs.derived());
728  data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
729  data.v.axis() = axis;
730  data.v.linearRate() = static_cast<Scalar>(vs[idx_v()] * m_pitch);
731  }
732 
733  template<typename VectorLike, typename Matrix6Like>
734  void calc_aba(
735  JointDataDerived & data,
736  const Eigen::MatrixBase<VectorLike> & armature,
737  const Eigen::MatrixBase<Matrix6Like> & I,
738  const bool update_I) const
739  {
740  data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis
741  + m_pitch * I.template middleCols<3>(Motion::LINEAR) * axis;
742  data.StU[0] = axis.dot(data.U.template segment<3>(Motion::ANGULAR))
743  + m_pitch * axis.dot(data.U.template segment<3>(Motion::LINEAR)) + armature[0];
744  data.Dinv[0] = Scalar(1) / data.StU[0];
745  data.UDinv.noalias() = data.U * data.Dinv;
746  if (update_I)
747  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
748  }
749 
750  static std::string classname()
751  {
752  return std::string("JointModelHelicalUnaligned");
753  }
754  std::string shortname() const
755  {
756  return classname();
757  }
758 
760  template<typename NewScalar>
762  {
764  ReturnType res(axis.template cast<NewScalar>(), ScalarCast<NewScalar, Scalar>::cast(m_pitch));
765  res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended());
766  return res;
767  }
768 
769  Vector3 axis;
770  Scalar m_pitch;
771 
772  }; // struct JointModelHelicalUnalignedTpl
773 
774 } // namespace pinocchio
775 
776 #include <boost/type_traits.hpp>
777 
778 namespace boost
779 {
780  template<typename Scalar, int Options>
781  struct has_nothrow_constructor<::pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options>>
782  : public integral_constant<bool, true>
783  {
784  };
785 
786  template<typename Scalar, int Options>
787  struct has_nothrow_copy<::pinocchio::JointModelHelicalUnalignedTpl<Scalar, Options>>
788  : public integral_constant<bool, true>
789  {
790  };
791 
792  template<typename Scalar, int Options>
793  struct has_nothrow_constructor<::pinocchio::JointDataHelicalUnalignedTpl<Scalar, Options>>
794  : public integral_constant<bool, true>
795  {
796  };
797 
798  template<typename Scalar, int Options>
799  struct has_nothrow_copy<::pinocchio::JointDataHelicalUnalignedTpl<Scalar, Options>>
800  : public integral_constant<bool, true>
801  {
802  };
803 } // namespace boost
804 
805 #endif // ifndef __pinocchio_multibody_joint_helical_unaligned_hpp__
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:47
ConstLinearType linear() const
Return the linear part of the force vector.
Definition: force-base.hpp:57
Main pinocchio namespace.
Definition: treeview.dox:11
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:155
JointDataTpl< Scalar, Options, JointCollectionTpl >::TangentVector_t joint_v(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConfigVisitor to get the joint velocity vector.
JointDataTpl< Scalar, Options, JointCollectionTpl >::ConfigVector_t joint_q(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConfigVisitor to get the joint configuration vector.
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
void toRotationMatrix(const Eigen::MatrixBase< Vector3 > &axis, const Scalar &cos_value, const Scalar &sin_value, const Eigen::MatrixBase< Matrix3 > &res)
Computes a rotation matrix from a vector and values of sin and cos orientations values.
Definition: rotation.hpp:26
Blank type.
Definition: fwd.hpp:77
Return type of the Constraint::Transpose * Force operation.
Return type of the Constraint::Transpose * ForceSet operation.
JointModelHelicalUnalignedTpl< NewScalar, Options > cast() const
ConstraintForceSetOp< JointMotionSubspaceHelicalUnalignedTpl, Derived >::ReturnType operator*(const Eigen::MatrixBase< Derived > &F) const
[CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
Return type of the ation of a Motion onto an object of type D.
Definition: motion.hpp:46
Forward declaration of the multiplication operation return type. Should be overloaded,...
Definition: binary-op.hpp:15
Cast scalar type from type FROM to type TO.
Definition: fwd.hpp:106
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:72