pinocchio  2.7.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-revolute-unaligned.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_revolute_unaligned_hpp__
7 #define __pinocchio_joint_revolute_unaligned_hpp__
8 
9 #include "pinocchio/fwd.hpp"
10 #include "pinocchio/multibody/joint/joint-base.hpp"
11 #include "pinocchio/multibody/constraint.hpp"
12 #include "pinocchio/spatial/inertia.hpp"
13 #include "pinocchio/math/matrix.hpp"
14 
15 namespace pinocchio
16 {
17 
18  template<typename Scalar, int Options=0> struct MotionRevoluteUnalignedTpl;
19  typedef MotionRevoluteUnalignedTpl<double> MotionRevoluteUnaligned;
20 
21  template<typename Scalar, int Options>
22  struct SE3GroupAction< MotionRevoluteUnalignedTpl<Scalar,Options> >
23  {
24  typedef MotionTpl<Scalar,Options> ReturnType;
25  };
26 
27  template<typename Scalar, int Options, typename MotionDerived>
28  struct MotionAlgebraAction< MotionRevoluteUnalignedTpl<Scalar,Options>, MotionDerived>
29  {
30  typedef MotionTpl<Scalar,Options> ReturnType;
31  };
32 
33  template<typename _Scalar, int _Options>
34  struct traits< MotionRevoluteUnalignedTpl<_Scalar,_Options> >
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,4,4,Options> Matrix4;
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;
49  typedef Matrix4 HomogeneousMatrixType;
50  typedef MotionTpl<Scalar,Options> MotionPlain;
51  typedef MotionPlain PlainReturnType;
52  enum {
53  LINEAR = 0,
54  ANGULAR = 3
55  };
56  }; // traits MotionRevoluteUnalignedTpl
57 
58  template<typename _Scalar, int _Options>
59  struct MotionRevoluteUnalignedTpl
60  : MotionBase< MotionRevoluteUnalignedTpl<_Scalar,_Options> >
61  {
62  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63  MOTION_TYPEDEF_TPL(MotionRevoluteUnalignedTpl);
64 
65  MotionRevoluteUnalignedTpl() {}
66 
67  template<typename Vector3Like, typename OtherScalar>
68  MotionRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis,
69  const OtherScalar & w)
70  : m_axis(axis)
71  , m_w(w)
72  {}
73 
74  inline PlainReturnType plain() const
75  {
76  return PlainReturnType(PlainReturnType::Vector3::Zero(),
77  m_axis*m_w);
78  }
79 
80  template<typename OtherScalar>
81  MotionRevoluteUnalignedTpl __mult__(const OtherScalar & alpha) const
82  {
83  return MotionRevoluteUnalignedTpl(m_axis,alpha*m_w);
84  }
85 
86  template<typename MotionDerived>
87  inline void addTo(MotionDense<MotionDerived> & v) const
88  {
89  v.angular() += m_axis*m_w;
90  }
91 
92  template<typename Derived>
93  void setTo(MotionDense<Derived> & other) const
94  {
95  other.linear().setZero();
96  other.angular().noalias() = m_axis*m_w;
97  }
98 
99  template<typename S2, int O2, typename D2>
100  void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
101  {
102  // Angular
103  v.angular().noalias() = m_w * m.rotation() * m_axis;
104 
105  // Linear
106  v.linear().noalias() = m.translation().cross(v.angular());
107  }
108 
109  template<typename S2, int O2>
110  MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
111  {
112  MotionPlain res;
113  se3Action_impl(m,res);
114  return res;
115  }
116 
117  template<typename S2, int O2, typename D2>
118  void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
119  {
120  // Linear
121  // TODO: use v.angular() as temporary variable
122  Vector3 v3_tmp;
123  v3_tmp.noalias() = m_axis.cross(m.translation());
124  v3_tmp *= m_w;
125  v.linear().noalias() = m.rotation().transpose() * v3_tmp;
126 
127  // Angular
128  v.angular().noalias() = m.rotation().transpose() * m_axis;
129  v.angular() *= m_w;
130  }
131 
132  template<typename S2, int O2>
133  MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
134  {
135  MotionPlain res;
136  se3ActionInverse_impl(m,res);
137  return res;
138  }
139 
140  template<typename M1, typename M2>
141  void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
142  {
143  // Linear
144  mout.linear().noalias() = v.linear().cross(m_axis);
145  mout.linear() *= m_w;
146 
147  // Angular
148  mout.angular().noalias() = v.angular().cross(m_axis);
149  mout.angular() *= m_w;
150  }
151 
152  template<typename M1>
153  MotionPlain motionAction(const MotionDense<M1> & v) const
154  {
155  MotionPlain res;
156  motionAction(v,res);
157  return res;
158  }
159 
160  bool isEqual_impl(const MotionRevoluteUnalignedTpl & other) const
161  {
162  return m_axis == other.m_axis && m_w == other.m_w;
163  }
164 
165  const Scalar & angularRate() const { return m_w; }
166  Scalar & angularRate() { return m_w; }
167 
168  const Vector3 & axis() const { return m_axis; }
169  Vector3 & axis() { return m_axis; }
170 
171  protected:
172  Vector3 m_axis;
173  Scalar m_w;
174 
175  }; // struct MotionRevoluteUnalignedTpl
176 
177  template<typename S1, int O1, typename MotionDerived>
178  inline typename MotionDerived::MotionPlain
179  operator+(const MotionRevoluteUnalignedTpl<S1,O1> & m1,
180  const MotionDense<MotionDerived> & m2)
181  {
182  typename MotionDerived::MotionPlain res(m2);
183  res += m1;
184  return res;
185  }
186 
187  template<typename MotionDerived, typename S2, int O2>
188  inline typename MotionDerived::MotionPlain
189  operator^(const MotionDense<MotionDerived> & m1,
190  const MotionRevoluteUnalignedTpl<S2,O2> & m2)
191  {
192  return m2.motionAction(m1);
193  }
194 
195  template<typename Scalar, int Options> struct ConstraintRevoluteUnalignedTpl;
196 
197  template<typename _Scalar, int _Options>
198  struct traits< ConstraintRevoluteUnalignedTpl<_Scalar,_Options> >
199  {
200  typedef _Scalar Scalar;
201  enum { Options = _Options };
202  enum {
203  LINEAR = 0,
204  ANGULAR = 3
205  };
206  typedef MotionRevoluteUnalignedTpl<Scalar,Options> JointMotion;
207  typedef Eigen::Matrix<Scalar,1,1,Options> JointForce;
208  typedef Eigen::Matrix<Scalar,6,1,Options> DenseBase;
209  typedef DenseBase MatrixReturnType;
210  typedef const DenseBase ConstMatrixReturnType;
211 
212  typedef Eigen::Matrix<Scalar,3,1,Options> Vector3;
213  }; // traits ConstraintRevoluteUnalignedTpl
214 
215  template<typename Scalar, int Options>
216  struct SE3GroupAction< ConstraintRevoluteUnalignedTpl<Scalar,Options> >
217  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
218 
219  template<typename Scalar, int Options, typename MotionDerived>
220  struct MotionAlgebraAction< ConstraintRevoluteUnalignedTpl<Scalar,Options>, MotionDerived >
221  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
222 
223  template<typename Scalar, int Options, typename ForceDerived>
224  struct ConstraintForceOp< ConstraintRevoluteUnalignedTpl<Scalar,Options>, ForceDerived>
225  {
226  typedef typename traits< ConstraintRevoluteUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
227  typedef Eigen::Matrix<typename PINOCCHIO_EIGEN_DOT_PRODUCT_RETURN_TYPE(Vector3,typename ForceDense<ForceDerived>::ConstAngularType),1,1,Options> ReturnType;
228  };
229 
230  template<typename Scalar, int Options, typename ForceSet>
231  struct ConstraintForceSetOp< ConstraintRevoluteUnalignedTpl<Scalar,Options>, ForceSet>
232  {
233  typedef typename traits< ConstraintRevoluteUnalignedTpl<Scalar,Options> >::Vector3 Vector3;
234  typedef typename MatrixMatrixProduct<Eigen::Transpose<const Vector3>,
235  typename Eigen::MatrixBase<const ForceSet>::template NRowsBlockXpr<3>::Type
236  >::type ReturnType;
237  };
238 
239  template<typename _Scalar, int _Options>
240  struct ConstraintRevoluteUnalignedTpl
241  : ConstraintBase< ConstraintRevoluteUnalignedTpl<_Scalar,_Options> >
242  {
243  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
244  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintRevoluteUnalignedTpl)
245 
246  enum { NV = 1 };
247 
248  typedef typename traits<ConstraintRevoluteUnalignedTpl>::Vector3 Vector3;
249 
250  ConstraintRevoluteUnalignedTpl() {}
251 
252  template<typename Vector3Like>
253  ConstraintRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
254  : m_axis(axis)
255  {}
256 
257  template<typename Vector1Like>
258  JointMotion __mult__(const Eigen::MatrixBase<Vector1Like> & v) const
259  {
260  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector1Like,1);
261  return JointMotion(m_axis,v[0]);
262  }
263 
264  template<typename S1, int O1>
265  typename SE3GroupAction<ConstraintRevoluteUnalignedTpl>::ReturnType
266  se3Action(const SE3Tpl<S1,O1> & m) const
267  {
268  typedef typename SE3GroupAction<ConstraintRevoluteUnalignedTpl>::ReturnType ReturnType;
269 
270  /* X*S = [ R pxR ; 0 R ] [ 0 ; a ] = [ px(Ra) ; Ra ] */
271  ReturnType res;
272  res.template segment<3>(ANGULAR).noalias() = m.rotation() * m_axis;
273  res.template segment<3>(LINEAR).noalias() = m.translation().cross(res.template segment<3>(ANGULAR));
274  return res;
275  }
276 
277  template<typename S1, int O1>
278  typename SE3GroupAction<ConstraintRevoluteUnalignedTpl>::ReturnType
279  se3ActionInverse(const SE3Tpl<S1,O1> & m) const
280  {
281  typedef typename SE3GroupAction<ConstraintRevoluteUnalignedTpl>::ReturnType ReturnType;
282 
283  ReturnType res;
284  res.template segment<3>(ANGULAR).noalias() = m.rotation().transpose() * m_axis;
285  res.template segment<3>(LINEAR).noalias() = -m.rotation().transpose() * m.translation().cross(m_axis);
286  return res;
287  }
288 
289  int nv_impl() const { return NV; }
290 
291  struct TransposeConst
292  {
293  const ConstraintRevoluteUnalignedTpl & ref;
294  TransposeConst(const ConstraintRevoluteUnalignedTpl & ref) : ref(ref) {}
295 
296  template<typename ForceDerived>
297  typename ConstraintForceOp<ConstraintRevoluteUnalignedTpl,ForceDerived>::ReturnType
298  operator*(const ForceDense<ForceDerived> & f) const
299  {
300  typedef typename ConstraintForceOp<ConstraintRevoluteUnalignedTpl,ForceDerived>::ReturnType ReturnType;
301  ReturnType res;
302  res[0] = ref.axis().dot(f.angular());
303  return res;
304  }
305 
306  /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
307  template<typename ForceSet>
308  typename ConstraintForceSetOp<ConstraintRevoluteUnalignedTpl,ForceSet>::ReturnType
309  operator*(const Eigen::MatrixBase<ForceSet> & F)
310  {
311  EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
312  /* Return ax.T * F[3:end,:] */
313  return ref.axis().transpose() * F.template middleRows<3>(ANGULAR);
314  }
315 
316  };
317 
318  TransposeConst transpose() const { return TransposeConst(*this); }
319 
320  /* CRBA joint operators
321  * - ForceSet::Block = ForceSet
322  * - ForceSet operator* (Inertia Y,Constraint S)
323  * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
324  * - SE3::act(ForceSet::Block)
325  */
326  DenseBase matrix_impl() const
327  {
328  DenseBase S;
329  S.template segment<3>(LINEAR).setZero();
330  S.template segment<3>(ANGULAR) = m_axis;
331  return S;
332  }
333 
334  template<typename MotionDerived>
335  typename MotionAlgebraAction<ConstraintRevoluteUnalignedTpl,MotionDerived>::ReturnType
336  motionAction(const MotionDense<MotionDerived> & m) const
337  {
338  const typename MotionDerived::ConstLinearType v = m.linear();
339  const typename MotionDerived::ConstAngularType w = m.angular();
340 
341  DenseBase res;
342  res.template segment<3>(LINEAR).noalias() = v.cross(m_axis);
343  res.template segment<3>(ANGULAR).noalias() = w.cross(m_axis);
344 
345  return res;
346  }
347 
348  const Vector3 & axis() const { return m_axis; }
349  Vector3 & axis() { return m_axis; }
350 
351  bool isEqual(const ConstraintRevoluteUnalignedTpl & other) const
352  {
353  return m_axis == other.m_axis;
354  }
355 
356  protected:
357 
358  Vector3 m_axis;
359 
360  }; // struct ConstraintRevoluteUnalignedTpl
361 
362  template<typename S1, int O1,typename S2, int O2>
363  struct MultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteUnalignedTpl<S2,O2> >
364  {
365  typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
366  };
367 
368  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
369  namespace impl
370  {
371  template<typename S1, int O1, typename S2, int O2>
372  struct LhsMultiplicationOp<InertiaTpl<S1,O1>, ConstraintRevoluteUnalignedTpl<S2,O2> >
373  {
374  typedef InertiaTpl<S1,O1> Inertia;
375  typedef ConstraintRevoluteUnalignedTpl<S2,O2> Constraint;
376  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
377  static inline ReturnType run(const Inertia & Y,
378  const Constraint & cru)
379  {
380  ReturnType res;
381 
382  /* YS = [ m -mcx ; mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */
383  const typename Inertia::Scalar & m = Y.mass();
384  const typename Inertia::Vector3 & c = Y.lever();
385  const typename Inertia::Symmetric3 & I = Y.inertia();
386 
387  res.template segment<3>(Inertia::LINEAR) = -m*c.cross(cru.axis());
388  res.template segment<3>(Inertia::ANGULAR).noalias() = I*cru.axis();
389  res.template segment<3>(Inertia::ANGULAR) += c.cross(res.template segment<3>(Inertia::LINEAR));
390 
391  return res;
392  }
393  };
394  } // namespace impl
395 
396  template<typename M6Like, typename Scalar, int Options>
397  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteUnalignedTpl<Scalar,Options> >
398  {
399  typedef typename SizeDepType<3>::ColsReturn<M6Like>::ConstType M6LikeCols;
400  typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst;
401 
402  typedef ConstraintRevoluteUnalignedTpl<Scalar,Options> Constraint;
403  typedef typename Constraint::Vector3 Vector3;
404  typedef const typename MatrixMatrixProduct<M6LikeColsNonConst,Vector3>::type ReturnType;
405  };
406 
407  /* [ABA] operator* (Inertia Y,Constraint S) */
408  namespace impl
409  {
410  template<typename M6Like, typename Scalar, int Options>
411  struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteUnalignedTpl<Scalar,Options> >
412  {
413  typedef ConstraintRevoluteUnalignedTpl<Scalar,Options> Constraint;
414  typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
415 
416  static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
417  const Constraint & cru)
418  {
419  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
420  return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.axis();
421  }
422  };
423  } // namespace impl
424 
425  template<typename Scalar, int Options> struct JointRevoluteUnalignedTpl;
426 
427  template<typename _Scalar, int _Options>
428  struct traits< JointRevoluteUnalignedTpl<_Scalar,_Options> >
429  {
430  enum {
431  NQ = 1,
432  NV = 1
433  };
434  typedef _Scalar Scalar;
435  enum { Options = _Options };
436  typedef JointDataRevoluteUnalignedTpl<Scalar,Options> JointDataDerived;
437  typedef JointModelRevoluteUnalignedTpl<Scalar,Options> JointModelDerived;
438  typedef ConstraintRevoluteUnalignedTpl<Scalar,Options> Constraint_t;
439  typedef SE3Tpl<Scalar,Options> Transformation_t;
440  typedef MotionRevoluteUnalignedTpl<Scalar,Options> Motion_t;
441  typedef MotionZeroTpl<Scalar,Options> Bias_t;
442 
443  // [ABA]
444  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
445  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
446  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
447 
448  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
449 
450  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
451  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
452 
453  };
454 
455  template<typename Scalar, int Options>
456  struct traits< JointDataRevoluteUnalignedTpl<Scalar,Options> >
457  { typedef JointRevoluteUnalignedTpl<Scalar,Options> JointDerived; };
458 
459  template<typename Scalar, int Options>
460  struct traits <JointModelRevoluteUnalignedTpl<Scalar,Options> >
461  { typedef JointRevoluteUnalignedTpl<Scalar,Options> JointDerived; };
462 
463  template<typename _Scalar, int _Options>
464  struct JointDataRevoluteUnalignedTpl
465  : public JointDataBase< JointDataRevoluteUnalignedTpl<_Scalar,_Options> >
466  {
467  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
468  typedef JointRevoluteUnalignedTpl<_Scalar,_Options> JointDerived;
469  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
470  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
471 
472  Transformation_t M;
473  Constraint_t S;
474  Motion_t v;
475  Bias_t c;
476 
477  // [ABA] specific data
478  U_t U;
479  D_t Dinv;
480  UD_t UDinv;
481 
482  JointDataRevoluteUnalignedTpl()
483  : M(Transformation_t::Identity())
484  , S(Constraint_t::Vector3::Zero())
485  , v(Constraint_t::Vector3::Zero(),(Scalar)0)
486  , U(U_t::Zero())
487  , Dinv(D_t::Zero())
488  , UDinv(UD_t::Zero())
489  {}
490 
491  template<typename Vector3Like>
492  JointDataRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
493  : M(Transformation_t::Identity())
494  , S(axis)
495  , v(axis,(Scalar)NAN)
496  , U(U_t::Zero())
497  , Dinv(D_t::Zero())
498  , UDinv(UD_t::Zero())
499  {}
500 
501  static std::string classname() { return std::string("JointDataRevoluteUnaligned"); }
502  std::string shortname() const { return classname(); }
503 
504  }; // struct JointDataRevoluteUnalignedTpl
505 
506  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelRevoluteUnalignedTpl);
507  template<typename _Scalar, int _Options>
508  struct JointModelRevoluteUnalignedTpl
509  : public JointModelBase< JointModelRevoluteUnalignedTpl<_Scalar,_Options> >
510  {
511  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
512  typedef JointRevoluteUnalignedTpl<_Scalar,_Options> JointDerived;
513  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
514  typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;
515 
516  typedef JointModelBase<JointModelRevoluteUnalignedTpl> Base;
517  using Base::id;
518  using Base::idx_q;
519  using Base::idx_v;
520  using Base::setIndexes;
521 
522  JointModelRevoluteUnalignedTpl(): axis(Vector3::UnitX()) {}
523 
524  JointModelRevoluteUnalignedTpl(const Scalar & x,
525  const Scalar & y,
526  const Scalar & z)
527  : axis(x,y,z)
528  {
529  axis.normalize();
530  assert(isUnitary(axis) && "Rotation axis is not unitary");
531  }
532 
533  template<typename Vector3Like>
534  JointModelRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
535  : axis(axis)
536  {
537  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
538  assert(isUnitary(axis) && "Rotation axis is not unitary");
539  }
540 
541  JointDataDerived createData() const { return JointDataDerived(axis); }
542 
543  using Base::isEqual;
544  bool isEqual(const JointModelRevoluteUnalignedTpl & other) const
545  {
546  return Base::isEqual(other) && axis == other.axis;
547  }
548 
549  const std::vector<bool> hasConfigurationLimit() const
550  {
551  return {true};
552  }
553 
554  const std::vector<bool> hasConfigurationLimitInTangent() const
555  {
556  return {true};
557  }
558 
559  template<typename ConfigVector>
560  void calc(JointDataDerived & data,
561  const typename Eigen::MatrixBase<ConfigVector> & qs) const
562  {
563  typedef typename ConfigVector::Scalar OtherScalar;
564  typedef Eigen::AngleAxis<Scalar> AngleAxis;
565 
566  const OtherScalar & q = qs[idx_q()];
567 
568  data.M.rotation(AngleAxis(q,axis).toRotationMatrix());
569  }
570 
571  template<typename ConfigVector, typename TangentVector>
572  void calc(JointDataDerived & data,
573  const typename Eigen::MatrixBase<ConfigVector> & qs,
574  const typename Eigen::MatrixBase<TangentVector> & vs) const
575  {
576  calc(data,qs.derived());
577 
578  data.v.angularRate() = static_cast<Scalar>(vs[idx_v()]);
579  }
580 
581  template<typename Matrix6Like>
582  void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
583  {
584  data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis;
585  data.Dinv[0] = (Scalar)(1)/axis.dot(data.U.template segment<3>(Motion::ANGULAR));
586  data.UDinv.noalias() = data.U * data.Dinv;
587 
588  if (update_I)
589  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
590  }
591 
592  static std::string classname() { return std::string("JointModelRevoluteUnaligned"); }
593  std::string shortname() const { return classname(); }
594 
596  template<typename NewScalar>
597  JointModelRevoluteUnalignedTpl<NewScalar,Options> cast() const
598  {
599  typedef JointModelRevoluteUnalignedTpl<NewScalar,Options> ReturnType;
600  ReturnType res(axis.template cast<NewScalar>());
601  res.setIndexes(id(),idx_q(),idx_v());
602  return res;
603  }
604 
605  // data
606 
610  Vector3 axis;
611  }; // struct JointModelRevoluteUnalignedTpl
612 
613 } //namespace pinocchio
614 
615 #include <boost/type_traits.hpp>
616 
617 namespace boost
618 {
619  template<typename Scalar, int Options>
620  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
621  : public integral_constant<bool,true> {};
622 
623  template<typename Scalar, int Options>
624  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
625  : public integral_constant<bool,true> {};
626 
627  template<typename Scalar, int Options>
628  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteUnalignedTpl<Scalar,Options> >
629  : public integral_constant<bool,true> {};
630 
631  template<typename Scalar, int Options>
632  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteUnalignedTpl<Scalar,Options> >
633  : public integral_constant<bool,true> {};
634 }
635 
636 
637 #endif // ifndef __pinocchio_joint_revolute_unaligned_hpp__
pinocchio::MatrixMatrixProduct
Definition: matrix.hpp:64
pinocchio::MultiplicationOp
Forward declaration of the multiplication operation return type. Should be overloaded,...
Definition: binary-op.hpp:15
pinocchio::JointDataRevoluteUnalignedTpl
Definition: fwd.hpp:31
pinocchio::isUnitary
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:140
pinocchio::JointModelRevoluteUnalignedTpl::axis
Vector3 axis
3d main axis of the joint.
Definition: joint-revolute-unaligned.hpp:610
pinocchio::idx_q
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
continuous.x
x
— Training
Definition: continuous.py:157
pinocchio::ConstraintForceOp
Return type of the Constraint::Transpose * Force operation.
Definition: constraint-base.hpp:35
pinocchio::idx_v
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...
pinocchio::SE3Tpl< Scalar, Options >
pinocchio::JointDataBase
Definition: joint-data-base.hpp:82
pinocchio::ConstraintRevoluteUnalignedTpl
Definition: joint-revolute-unaligned.hpp:195
pinocchio::MotionAlgebraAction
Return type of the ation of a Motion onto an object of type D.
Definition: motion.hpp:46
pinocchio::impl::LhsMultiplicationOp
Definition: binary-op.hpp:20
pinocchio::ConstraintRevoluteUnalignedTpl::TransposeConst
Definition: joint-revolute-unaligned.hpp:291
pinocchio::MotionBase
Definition: fwd.hpp:40
pinocchio::SE3GroupAction
Definition: se3.hpp:39
pinocchio::MotionZeroTpl
Definition: fwd.hpp:44
pinocchio::JointRevoluteUnalignedTpl
Definition: joint-revolute-unaligned.hpp:425
pinocchio::MotionRevoluteUnalignedTpl
Definition: joint-revolute-unaligned.hpp:18
pinocchio::toRotationMatrix
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:24
pinocchio::ForceSetTpl
Definition: force-set.hpp:14
pinocchio::ConstraintForceSetOp
Return type of the Constraint::Transpose * ForceSet operation.
Definition: constraint-base.hpp:42
pinocchio::JointModelRevoluteUnalignedTpl
Definition: fwd.hpp:28
pinocchio::SizeDepType::ColsReturn
Definition: matrix-block.hpp:43
pinocchio::ForceDense
Definition: force-dense.hpp:24
pinocchio::ConstraintBase
Definition: constraint-base.hpp:48
pinocchio::InertiaTpl
Definition: fwd.hpp:52
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
pinocchio::MotionTpl< Scalar, Options >
pinocchio::ForceDense::angular
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:35
pinocchio::JointModelRevoluteUnalignedTpl::cast
JointModelRevoluteUnalignedTpl< NewScalar, Options > cast() const
Definition: joint-revolute-unaligned.hpp:597
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11