pinocchio  2.2.1-dirty
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;
20 
21  template<typename Scalar, int Options>
22  struct SE3GroupAction< MotionRevoluteUnalignedTpl<Scalar,Options> >
23  {
25  };
26 
27  template<typename Scalar, int Options, typename MotionDerived>
28  struct MotionAlgebraAction< MotionRevoluteUnalignedTpl<Scalar,Options>, MotionDerived>
29  {
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,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  }; // traits MotionRevoluteUnalignedTpl
55 
56  template<typename _Scalar, int _Options>
58  : MotionBase< MotionRevoluteUnalignedTpl<_Scalar,_Options> >
59  {
60  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
61  MOTION_TYPEDEF_TPL(MotionRevoluteUnalignedTpl);
62 
64 
65  template<typename Vector3Like, typename OtherScalar>
66  MotionRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis,
67  const OtherScalar & w)
68  : axis(axis)
69  , w(w)
70  {}
71 
72  inline PlainReturnType plain() const
73  {
74  return PlainReturnType(PlainReturnType::Vector3::Zero(),
75  axis*w);
76  }
77 
78  template<typename OtherScalar>
79  MotionRevoluteUnalignedTpl __mult__(const OtherScalar & alpha) const
80  {
81  return MotionRevoluteUnalignedTpl(axis,alpha*w);
82  }
83 
84  template<typename MotionDerived>
85  inline void addTo(MotionDense<MotionDerived> & v) const
86  {
87  v.angular() += axis*w;
88  }
89 
90  template<typename Derived>
91  void setTo(MotionDense<Derived> & other) const
92  {
93  other.linear().setZero();
94  other.angular().noalias() = axis*w;
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  // Angular
101  v.angular().noalias() = w * m.rotation() * axis;
102 
103  // Linear
104  v.linear().noalias() = m.translation().cross(v.angular());
105  }
106 
107  template<typename S2, int O2>
108  MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
109  {
110  MotionPlain res;
111  se3Action_impl(m,res);
112  return res;
113  }
114 
115  template<typename S2, int O2, typename D2>
116  void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
117  {
118  // Linear
119  // TODO: use v.angular() as temporary variable
120  Vector3 v3_tmp;
121  v3_tmp.noalias() = axis.cross(m.translation());
122  v3_tmp *= w;
123  v.linear().noalias() = m.rotation().transpose() * v3_tmp;
124 
125  // Angular
126  v.angular().noalias() = m.rotation().transpose() * axis;
127  v.angular() *= w;
128  }
129 
130  template<typename S2, int O2>
131  MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
132  {
133  MotionPlain res;
134  se3ActionInverse_impl(m,res);
135  return res;
136  }
137 
138  template<typename M1, typename M2>
139  void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
140  {
141  // Linear
142  mout.linear().noalias() = v.linear().cross(axis);
143  mout.linear() *= w;
144 
145  // Angular
146  mout.angular().noalias() = v.angular().cross(axis);
147  mout.angular() *= w;
148  }
149 
150  template<typename M1>
151  MotionPlain motionAction(const MotionDense<M1> & v) const
152  {
153  MotionPlain res;
154  motionAction(v,res);
155  return res;
156  }
157 
158  // data
159  Vector3 axis;
160  Scalar w;
161 
162  }; // struct MotionRevoluteUnalignedTpl
163 
164  template<typename S1, int O1, typename MotionDerived>
165  inline typename MotionDerived::MotionPlain
166  operator+(const MotionRevoluteUnalignedTpl<S1,O1> & m1,
167  const MotionDense<MotionDerived> & m2)
168  {
169  typename MotionDerived::MotionPlain res(m2);
170  res += m1;
171  return res;
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 ConstraintRevoluteUnalignedTpl;
183 
184  template<typename _Scalar, int _Options>
185  struct traits< ConstraintRevoluteUnalignedTpl<_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 ConstraintRevoluteUnalignedTpl
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< ConstraintRevoluteUnalignedTpl<Scalar,Options>, MotionDerived >
208  { typedef Eigen::Matrix<Scalar,6,1,Options> ReturnType; };
209 
210  template<typename Scalar, int Options, typename ForceDerived>
211  struct ConstraintForceOp< ConstraintRevoluteUnalignedTpl<Scalar,Options>, ForceDerived>
212  {
213  typedef typename traits< ConstraintRevoluteUnalignedTpl<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< ConstraintRevoluteUnalignedTpl<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< ConstraintRevoluteUnalignedTpl<_Scalar,_Options> >
229  {
230  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
231  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintRevoluteUnalignedTpl)
232 
233  enum { NV = 1 };
234 
235  typedef typename traits<ConstraintRevoluteUnalignedTpl>::Vector3 Vector3;
236 
238 
239  template<typename Vector3Like>
240  ConstraintRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
241  : axis(axis)
242  {}
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(axis,v[0]);
249  }
250 
251  template<typename S1, int O1>
253  se3Action(const SE3Tpl<S1,O1> & m) const
254  {
256 
257  /* X*S = [ R pxR ; 0 R ] [ 0 ; a ] = [ px(Ra) ; Ra ] */
258  ReturnType res;
259  res.template segment<3>(ANGULAR).noalias() = m.rotation() * axis;
260  res.template segment<3>(LINEAR).noalias() = m.translation().cross(res.template segment<3>(ANGULAR));
261  return res;
262  }
263 
264  template<typename S1, int O1>
266  se3ActionInverse(const SE3Tpl<S1,O1> & m) const
267  {
269 
270  ReturnType res;
271  res.template segment<3>(ANGULAR).noalias() = m.rotation().transpose() * axis;
272  res.template segment<3>(LINEAR).noalias() = -m.rotation().transpose() * m.translation().cross(axis);
273  return res;
274  }
275 
276  int nv_impl() const { return NV; }
277 
279  {
280  const ConstraintRevoluteUnalignedTpl & ref;
281  TransposeConst(const ConstraintRevoluteUnalignedTpl & ref) : ref(ref) {}
282 
283  template<typename ForceDerived>
284  typename ConstraintForceOp<ConstraintRevoluteUnalignedTpl,ForceDerived>::ReturnType
285  operator*(const ForceDense<ForceDerived> & f) const
286  {
287  typedef typename ConstraintForceOp<ConstraintRevoluteUnalignedTpl,ForceDerived>::ReturnType ReturnType;
288  ReturnType res;
289  res[0] = ref.axis.dot(f.angular());
290  return res;
291  }
292 
293  /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
294  template<typename ForceSet>
295  typename ConstraintForceSetOp<ConstraintRevoluteUnalignedTpl,ForceSet>::ReturnType
296  operator*(const Eigen::MatrixBase<ForceSet> & F)
297  {
298  EIGEN_STATIC_ASSERT(ForceSet::RowsAtCompileTime==6,THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
299  /* Return ax.T * F[3:end,:] */
300  return ref.axis.transpose() * F.template middleRows<3>(ANGULAR);
301  }
302 
303  };
304 
305  TransposeConst transpose() const { return TransposeConst(*this); }
306 
307  /* CRBA joint operators
308  * - ForceSet::Block = ForceSet
309  * - ForceSet operator* (Inertia Y,Constraint S)
310  * - MatrixBase operator* (Constraint::Transpose S, ForceSet::Block)
311  * - SE3::act(ForceSet::Block)
312  */
313  DenseBase matrix_impl() const
314  {
315  DenseBase S;
316  S << Vector3::Zero(), axis;
317  return S;
318  }
319 
320  template<typename MotionDerived>
322  motionAction(const MotionDense<MotionDerived> & m) const
323  {
324  const typename MotionDerived::ConstLinearType v = m.linear();
325  const typename MotionDerived::ConstAngularType w = m.angular();
326 
327  DenseBase res;
328  res << v.cross(axis), w.cross(axis);
329 
330  return res;
331  }
332 
333  // data
334  Vector3 axis;
335 
336  }; // struct ConstraintRevoluteUnalignedTpl
337 
338  template<typename S1, int O1,typename S2, int O2>
340  {
341  typedef Eigen::Matrix<S2,6,1,O2> ReturnType;
342  };
343 
344  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
345  namespace impl
346  {
347  template<typename S1, int O1, typename S2, int O2>
349  {
350  typedef InertiaTpl<S1,O1> Inertia;
352  typedef typename MultiplicationOp<Inertia,Constraint>::ReturnType ReturnType;
353  static inline ReturnType run(const Inertia & Y,
354  const Constraint & cru)
355  {
356  ReturnType res;
357 
358  /* YS = [ m -mcx ; mcx I-mcxcx ] [ 0 ; w ] = [ mcxw ; Iw -mcxcxw ] */
359  const typename Inertia::Scalar & m = Y.mass();
360  const typename Inertia::Vector3 & c = Y.lever();
361  const typename Inertia::Symmetric3 & I = Y.inertia();
362 
363  res.template segment<3>(Inertia::LINEAR) = -m*c.cross(cru.axis);
364  res.template segment<3>(Inertia::ANGULAR).noalias() = I*cru.axis;
365  res.template segment<3>(Inertia::ANGULAR) += c.cross(res.template segment<3>(Inertia::LINEAR));
366 
367  return res;
368  }
369  };
370  } // namespace impl
371 
372  template<typename M6Like, typename Scalar, int Options>
373  struct MultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteUnalignedTpl<Scalar,Options> >
374  {
375  typedef typename SizeDepType<3>::ColsReturn<M6Like>::ConstType M6LikeCols;
376  typedef typename Eigen::internal::remove_const<M6LikeCols>::type M6LikeColsNonConst;
377 
379  typedef typename Constraint::Vector3 Vector3;
380  typedef const typename MatrixMatrixProduct<M6LikeColsNonConst,Vector3>::type ReturnType;
381  };
382 
383  /* [ABA] operator* (Inertia Y,Constraint S) */
384  namespace impl
385  {
386  template<typename M6Like, typename Scalar, int Options>
387  struct LhsMultiplicationOp<Eigen::MatrixBase<M6Like>, ConstraintRevoluteUnalignedTpl<Scalar,Options> >
388  {
390  typedef typename MultiplicationOp<Eigen::MatrixBase<M6Like>,Constraint>::ReturnType ReturnType;
391 
392  static inline ReturnType run(const Eigen::MatrixBase<M6Like> & Y,
393  const Constraint & cru)
394  {
395  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(M6Like,6,6);
396  return Y.derived().template middleCols<3>(Constraint::ANGULAR) * cru.axis;
397  }
398  };
399  } // namespace impl
400 
401  template<typename Scalar, int Options> struct JointRevoluteUnalignedTpl;
402 
403  template<typename _Scalar, int _Options>
404  struct traits< JointRevoluteUnalignedTpl<_Scalar,_Options> >
405  {
406  enum {
407  NQ = 1,
408  NV = 1
409  };
410  typedef _Scalar Scalar;
411  enum { Options = _Options };
418 
419  // [ABA]
420  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
421  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
422  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
423 
424  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
425 
426  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
427  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
428 
429  };
430 
431  template<typename Scalar, int Options>
432  struct traits< JointDataRevoluteUnalignedTpl<Scalar,Options> >
434 
435  template<typename Scalar, int Options>
436  struct traits <JointModelRevoluteUnalignedTpl<Scalar,Options> >
438 
439  template<typename _Scalar, int _Options>
441  : public JointDataBase< JointDataRevoluteUnalignedTpl<_Scalar,_Options> >
442  {
443  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
445  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
446  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
447 
448  Transformation_t M;
449  Constraint_t S;
450  Motion_t v;
451  Bias_t c;
452 
453  // [ABA] specific data
454  U_t U;
455  D_t Dinv;
456  UD_t UDinv;
457 
459 
460  template<typename Vector3Like>
461  JointDataRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
462  : M(1)
463  , S(axis)
464  , v(axis,(Scalar)NAN)
465  , U(), Dinv(), UDinv()
466  {}
467 
468  static std::string classname() { return std::string("JointDataRevoluteUnaligned"); }
469  std::string shortname() const { return classname(); }
470 
471  }; // struct JointDataRevoluteUnalignedTpl
472 
473  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelRevoluteUnalignedTpl);
474  template<typename _Scalar, int _Options>
476  : public JointModelBase< JointModelRevoluteUnalignedTpl<_Scalar,_Options> >
477  {
478  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
480  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
481  typedef Eigen::Matrix<Scalar,3,1,_Options> Vector3;
482 
484  using Base::id;
485  using Base::idx_q;
486  using Base::idx_v;
487  using Base::setIndexes;
488 
490 
491  JointModelRevoluteUnalignedTpl(const Scalar & x,
492  const Scalar & y,
493  const Scalar & z)
494  : axis(x,y,z)
495  {
496  axis.normalize();
497  assert(isUnitary(axis) && "Rotation axis is not unitary");
498  }
499 
500  template<typename Vector3Like>
501  JointModelRevoluteUnalignedTpl(const Eigen::MatrixBase<Vector3Like> & axis)
502  : axis(axis)
503  {
504  EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
505  assert(isUnitary(axis) && "Rotation axis is not unitary");
506  }
507 
508  JointDataDerived createData() const { return JointDataDerived(axis); }
509 
510  using Base::isEqual;
511  bool isEqual(const JointModelRevoluteUnalignedTpl & other) const
512  {
513  return Base::isEqual(other) && axis == other.axis;
514  }
515 
516  template<typename ConfigVector>
517  void calc(JointDataDerived & data,
518  const typename Eigen::MatrixBase<ConfigVector> & qs) const
519  {
520  typedef typename ConfigVector::Scalar OtherScalar;
521  typedef Eigen::AngleAxis<Scalar> AngleAxis;
522 
523  const OtherScalar & q = qs[idx_q()];
524 
525  data.M.rotation(AngleAxis(q,axis).toRotationMatrix());
526  }
527 
528  template<typename ConfigVector, typename TangentVector>
529  void calc(JointDataDerived & data,
530  const typename Eigen::MatrixBase<ConfigVector> & qs,
531  const typename Eigen::MatrixBase<TangentVector> & vs) const
532  {
533  calc(data,qs.derived());
534 
535  data.v.w = (Scalar)vs[idx_v()];
536  }
537 
538  template<typename Matrix6Like>
539  void calc_aba(JointDataDerived & data, const Eigen::MatrixBase<Matrix6Like> & I, const bool update_I) const
540  {
541  data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * axis;
542  data.Dinv[0] = (Scalar)(1)/axis.dot(data.U.template segment<3>(Motion::ANGULAR));
543  data.UDinv.noalias() = data.U * data.Dinv;
544 
545  if (update_I)
546  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I) -= data.UDinv * data.U.transpose();
547  }
548 
549  static std::string classname() { return std::string("JointModelRevoluteUnaligned"); }
550  std::string shortname() const { return classname(); }
551 
553  template<typename NewScalar>
555  {
557  ReturnType res(axis.template cast<NewScalar>());
558  res.setIndexes(id(),idx_q(),idx_v());
559  return res;
560  }
561 
562  // data
563 
567  Vector3 axis;
568  }; // struct JointModelRevoluteUnalignedTpl
569 
570 } //namespace pinocchio
571 
572 #include <boost/type_traits.hpp>
573 
574 namespace boost
575 {
576  template<typename Scalar, int Options>
577  struct has_nothrow_constructor< ::pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
578  : public integral_constant<bool,true> {};
579 
580  template<typename Scalar, int Options>
581  struct has_nothrow_copy< ::pinocchio::JointModelRevoluteUnalignedTpl<Scalar,Options> >
582  : public integral_constant<bool,true> {};
583 
584  template<typename Scalar, int Options>
585  struct has_nothrow_constructor< ::pinocchio::JointDataRevoluteUnalignedTpl<Scalar,Options> >
586  : public integral_constant<bool,true> {};
587 
588  template<typename Scalar, int Options>
589  struct has_nothrow_copy< ::pinocchio::JointDataRevoluteUnalignedTpl<Scalar,Options> >
590  : public integral_constant<bool,true> {};
591 }
592 
593 
594 #endif // ifndef __pinocchio_joint_revolute_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...
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>
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:40
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
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:21
JointModelRevoluteUnalignedTpl< NewScalar, Options > cast() const
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)