pinocchio  2.2.1-dirty
joint-spherical.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_spherical_hpp__
7 #define __pinocchio_joint_spherical_hpp__
8 
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/multibody/joint/joint-base.hpp"
11 #include "pinocchio/multibody/constraint.hpp"
12 #include "pinocchio/math/sincos.hpp"
13 #include "pinocchio/spatial/inertia.hpp"
14 #include "pinocchio/spatial/skew.hpp"
15 
16 namespace pinocchio
17 {
18 
19  template<typename Scalar, int Options = 0> struct MotionSphericalTpl;
21 
22  template<typename Scalar, int Options>
23  struct SE3GroupAction< MotionSphericalTpl<Scalar,Options> >
24  {
26  };
27 
28  template<typename Scalar, int Options, typename MotionDerived>
29  struct MotionAlgebraAction< MotionSphericalTpl<Scalar,Options>, MotionDerived>
30  {
32  };
33 
34  template<typename _Scalar, int _Options>
35  struct traits< MotionSphericalTpl<_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 MotionSphericalTpl
56 
57  template<typename _Scalar, int _Options>
58  struct MotionSphericalTpl
59  : MotionBase< MotionSphericalTpl<_Scalar,_Options> >
60  {
61  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
62 
63  MOTION_TYPEDEF_TPL(MotionSphericalTpl);
64 
66 
67  template<typename Vector3Like>
68  MotionSphericalTpl(const Eigen::MatrixBase<Vector3Like> & w)
69  : w(w)
70  {}
71 
72  Vector3 & operator() () { return w; }
73  const Vector3 & operator() () const { return w; }
74 
75  inline PlainReturnType plain() const
76  {
77  return PlainReturnType(PlainReturnType::Vector3::Zero(), w);
78  }
79 
80  template<typename MotionDerived>
81  void addTo(MotionDense<MotionDerived> & other) const
82  {
83  other.angular() += w;
84  }
85 
86  template<typename Derived>
87  void setTo(MotionDense<Derived> & other) const
88  {
89  other.linear().setZero();
90  other.angular() = w;
91  }
92 
93  MotionSphericalTpl __plus__(const MotionSphericalTpl & other) const
94  {
95  return MotionSphericalTpl(w + other.w);
96  }
97 
98  bool isEqual_impl(const MotionSphericalTpl & other) const
99  {
100  return w == other.w;
101  }
102 
103  template<typename MotionDerived>
104  bool isEqual_impl(const MotionDense<MotionDerived> & other) const
105  {
106  return other.angular() == w && other.linear().isZero(0);
107  }
108 
109  template<typename S2, int O2, typename D2>
110  void se3Action_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
111  {
112  // Angular
113  v.angular().noalias() = m.rotation() * w;
114 
115  // Linear
116  v.linear().noalias() = m.translation().cross(v.angular());
117  }
118 
119  template<typename S2, int O2>
120  MotionPlain se3Action_impl(const SE3Tpl<S2,O2> & m) const
121  {
122  MotionPlain res;
123  se3Action_impl(m,res);
124  return res;
125  }
126 
127  template<typename S2, int O2, typename D2>
128  void se3ActionInverse_impl(const SE3Tpl<S2,O2> & m, MotionDense<D2> & v) const
129  {
130  // Linear
131  // TODO: use v.angular() as temporary variable
132  Vector3 v3_tmp;
133  v3_tmp.noalias() = w.cross(m.translation());
134  v.linear().noalias() = m.rotation().transpose() * v3_tmp;
135 
136  // Angular
137  v.angular().noalias() = m.rotation().transpose() * w;
138  }
139 
140  template<typename S2, int O2>
141  MotionPlain se3ActionInverse_impl(const SE3Tpl<S2,O2> & m) const
142  {
143  MotionPlain res;
144  se3ActionInverse_impl(m,res);
145  return res;
146  }
147 
148  template<typename M1, typename M2>
149  void motionAction(const MotionDense<M1> & v, MotionDense<M2> & mout) const
150  {
151  // Linear
152  mout.linear().noalias() = v.linear().cross(w);
153 
154  // Angular
155  mout.angular().noalias() = v.angular().cross(w);
156  }
157 
158  template<typename M1>
159  MotionPlain motionAction(const MotionDense<M1> & v) const
160  {
161  MotionPlain res;
162  motionAction(v,res);
163  return res;
164  }
165 
166  // data
167  Vector3 w;
168  }; // struct MotionSphericalTpl
169 
170  template<typename S1, int O1, typename MotionDerived>
171  inline typename MotionDerived::MotionPlain
172  operator+(const MotionSphericalTpl<S1,O1> & m1, const MotionDense<MotionDerived> & m2)
173  {
174  return typename MotionDerived::MotionPlain(m2.linear(),m2.angular() + m1.w);
175  }
176 
177  template<typename Scalar, int Options> struct ConstraintSphericalTpl;
178 
179  template<typename _Scalar, int _Options>
180  struct traits< ConstraintSphericalTpl<_Scalar,_Options> >
181  {
182  typedef _Scalar Scalar;
183  enum { Options = _Options };
184  enum {
185  LINEAR = 0,
186  ANGULAR = 3
187  };
189  typedef Eigen::Matrix<Scalar,3,1,Options> JointForce;
190  typedef Eigen::Matrix<Scalar,6,3,Options> DenseBase;
191  typedef DenseBase MatrixReturnType;
192  typedef const DenseBase ConstMatrixReturnType;
193  }; // struct traits struct ConstraintSphericalTpl
194 
195  template<typename _Scalar, int _Options>
197  : public ConstraintBase< ConstraintSphericalTpl<_Scalar,_Options> >
198  {
199  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
200 
201  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintSphericalTpl)
202 
204 
205  enum { NV = 3 };
206 
207  int nv_impl() const { return NV; }
208 
209  template<typename Vector3Like>
210  JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & w) const
211  {
212  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like,3);
213  return JointMotion(w);
214  }
215 
217  {
218  template<typename Derived>
220  operator* (const ForceDense<Derived> & phi)
221  { return phi.angular(); }
222 
223  /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
224  template<typename MatrixDerived>
225  const typename SizeDepType<3>::RowsReturn<MatrixDerived>::ConstType
226  operator*(const Eigen::MatrixBase<MatrixDerived> & F) const
227  {
228  assert(F.rows()==6);
229  return F.derived().template middleRows<3>(Inertia::ANGULAR);
230  }
231  };
232 
233  TransposeConst transpose() const { return TransposeConst(); }
234 
235  DenseBase matrix_impl() const
236  {
237  DenseBase S;
238  S.template block <3,3>(LINEAR,0).setZero();
239  S.template block <3,3>(ANGULAR,0).setIdentity();
240  return S;
241  }
242 
243  template<typename S1, int O1>
244  Eigen::Matrix<S1,6,3,O1> se3Action(const SE3Tpl<S1,O1> & m) const
245  {
246  Eigen::Matrix<S1,6,3,O1> X_subspace;
247  cross(m.translation(),m.rotation(),X_subspace.template middleRows<3>(LINEAR));
248  X_subspace.template middleRows<3>(ANGULAR) = m.rotation();
249 
250  return X_subspace;
251  }
252 
253  template<typename S1, int O1>
254  Eigen::Matrix<S1,6,3,O1> se3ActionInverse(const SE3Tpl<S1,O1> & m) const
255  {
256  Eigen::Matrix<S1,6,3,O1> X_subspace;
257  AxisX::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(0));
258  AxisY::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(1));
259  AxisZ::cross(m.translation(),X_subspace.template middleRows<3>(ANGULAR).col(2));
260 
261  X_subspace.template middleRows<3>(LINEAR).noalias()
262  = m.rotation().transpose() * X_subspace.template middleRows<3>(ANGULAR);
263  X_subspace.template middleRows<3>(ANGULAR) = m.rotation().transpose();
264 
265  return X_subspace;
266  }
267 
268  template<typename MotionDerived>
269  DenseBase motionAction(const MotionDense<MotionDerived> & m) const
270  {
271  const typename MotionDerived::ConstLinearType v = m.linear();
272  const typename MotionDerived::ConstAngularType w = m.angular();
273 
274  DenseBase res;
275  skew(v,res.template middleRows<3>(LINEAR));
276  skew(w,res.template middleRows<3>(ANGULAR));
277 
278  return res;
279  }
280 
281  }; // struct ConstraintSphericalTpl
282 
283  template<typename MotionDerived, typename S2, int O2>
284  inline typename MotionDerived::MotionPlain
285  operator^(const MotionDense<MotionDerived> & m1,
286  const MotionSphericalTpl<S2,O2> & m2)
287  {
288  return m2.motionAction(m1);
289  }
290 
291  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
292  template<typename S1, int O1, typename S2, int O2>
293  inline Eigen::Matrix<S2,6,3,O2>
294  operator*(const InertiaTpl<S1,O1> & Y,
296  {
297  typedef InertiaTpl<S1,O1> Inertia;
298  typedef typename Inertia::Symmetric3 Symmetric3;
299  Eigen::Matrix<S2,6,3,O2> M;
300  // M.block <3,3> (Inertia::LINEAR, 0) = - Y.mass () * skew(Y.lever ());
301  M.template block<3,3>(Inertia::LINEAR,0) = alphaSkew(-Y.mass(), Y.lever());
302  M.template block<3,3>(Inertia::ANGULAR,0) = (Y.inertia() - typename Symmetric3::AlphaSkewSquare(Y.mass(), Y.lever())).matrix();
303  return M;
304  }
305 
306  /* [ABA] Y*S operator*/
307  template<typename M6Like, typename S2, int O2>
308  inline typename SizeDepType<3>::ColsReturn<M6Like>::ConstType
309  operator*(const Eigen::MatrixBase<M6Like> & Y,
311  {
312  typedef ConstraintSphericalTpl<S2,O2> Constraint;
313  return Y.derived().template middleCols<3>(Constraint::ANGULAR);
314  }
315 
316  template<typename S1, int O1>
318  { typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
319 
320  template<typename S1, int O1, typename MotionDerived>
321  struct MotionAlgebraAction< ConstraintSphericalTpl<S1,O1>,MotionDerived >
322  { typedef Eigen::Matrix<S1,6,3,O1> ReturnType; };
323 
324  template<typename Scalar, int Options> struct JointSphericalTpl;
325 
326  template<typename _Scalar, int _Options>
327  struct traits< JointSphericalTpl<_Scalar,_Options> >
328  {
329  enum {
330  NQ = 4,
331  NV = 3
332  };
333  typedef _Scalar Scalar;
334  enum { Options = _Options };
341 
342  // [ABA]
343  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
344  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
345  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
346 
347  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
348 
349  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
350  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
351  };
352 
353  template<typename Scalar, int Options>
354  struct traits< JointDataSphericalTpl<Scalar,Options> >
356 
357  template<typename Scalar, int Options>
358  struct traits< JointModelSphericalTpl<Scalar,Options> >
360 
361  template<typename _Scalar, int _Options>
362  struct JointDataSphericalTpl : public JointDataBase< JointDataSphericalTpl<_Scalar,_Options> >
363  {
364  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
365 
367  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
368  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
369 
370  Constraint_t S;
371  Transformation_t M;
372  Motion_t v;
373  Bias_t c;
374 
375  // [ABA] specific data
376  U_t U;
377  D_t Dinv;
378  UD_t UDinv;
379 
380  JointDataSphericalTpl () : M(1), U(), Dinv(), UDinv() {}
381 
382  static std::string classname() { return std::string("JointDataSpherical"); }
383  std::string shortname() const { return classname(); }
384 
385  }; // struct JointDataSphericalTpl
386 
387  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelSphericalTpl);
388  template<typename _Scalar, int _Options>
390  : public JointModelBase< JointModelSphericalTpl<_Scalar,_Options> >
391  {
392  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
393 
395  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
396 
398  using Base::id;
399  using Base::idx_q;
400  using Base::idx_v;
401  using Base::setIndexes;
402 
403  JointDataDerived createData() const { return JointDataDerived(); }
404 
405  template<typename ConfigVectorLike>
406  inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const
407  {
408  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike);
409  typedef typename Eigen::Quaternion<typename ConfigVectorLike::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;
410  typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
411 
412  ConstQuaternionMap quat(q_joint.derived().data());
413  //assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
414  assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= 1e-4);
415 
416  M.rotation(quat.matrix());
417  M.translation().setZero();
418  }
419 
420  template<typename QuaternionDerived>
421  void calc(JointDataDerived & data,
422  const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
423  {
424  data.M.rotation(quat.matrix());
425  }
426 
427  template<typename ConfigVector>
428  EIGEN_DONT_INLINE
429  void calc(JointDataDerived & data,
430  const typename Eigen::PlainObjectBase<ConfigVector> & qs) const
431  {
432  typedef typename Eigen::Quaternion<typename ConfigVector::Scalar,ConfigVector::Options> Quaternion;
433  typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
434 
435  ConstQuaternionMap quat(qs.template segment<NQ>(idx_q()).data());
436  calc(data,quat);
437  }
438 
439  template<typename ConfigVector>
440  EIGEN_DONT_INLINE
441  void calc(JointDataDerived & data,
442  const typename Eigen::MatrixBase<ConfigVector> & qs) const
443  {
444  typedef typename Eigen::Quaternion<Scalar,Options> Quaternion;
445 
446  const Quaternion quat(qs.template segment<NQ>(idx_q()));
447  calc(data,quat);
448  }
449 
450  template<typename ConfigVector, typename TangentVector>
451  void calc(JointDataDerived & data,
452  const typename Eigen::MatrixBase<ConfigVector> & qs,
453  const typename Eigen::MatrixBase<TangentVector> & vs) const
454  {
455  calc(data,qs.derived());
456 
457  data.v() = vs.template segment<NV>(idx_v());
458  }
459 
460  template<typename Matrix6Like>
461  void calc_aba(JointDataDerived & data,
462  const Eigen::MatrixBase<Matrix6Like> & I,
463  const bool update_I) const
464  {
465  data.U = I.template block<6,3>(0,Inertia::ANGULAR);
466 
467  // compute inverse
468 // data.Dinv.setIdentity();
469 // data.U.template middleRows<3>(Inertia::ANGULAR).llt().solveInPlace(data.Dinv);
470  internal::PerformStYSInversion<Scalar>::run(data.U.template middleRows<3>(Inertia::ANGULAR),data.Dinv);
471 
472  data.UDinv.template middleRows<3>(Inertia::ANGULAR).setIdentity(); // can be put in data constructor
473  data.UDinv.template middleRows<3>(Inertia::LINEAR).noalias() = data.U.template block<3,3>(Inertia::LINEAR, 0) * data.Dinv;
474 
475  if (update_I)
476  {
477  Matrix6Like & I_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I);
478  I_.template block<3,3>(Inertia::LINEAR,Inertia::LINEAR)
479  -= data.UDinv.template middleRows<3>(Inertia::LINEAR) * I_.template block<3,3> (Inertia::ANGULAR, Inertia::LINEAR);
480  I_.template block<6,3>(0,Inertia::ANGULAR).setZero();
481  I_.template block<3,3>(Inertia::ANGULAR,Inertia::LINEAR).setZero();
482  }
483  }
484 
485  static std::string classname() { return std::string("JointModelSpherical"); }
486  std::string shortname() const { return classname(); }
487 
489  template<typename NewScalar>
491  {
493  ReturnType res;
494  res.setIndexes(id(),idx_q(),idx_v());
495  return res;
496  }
497 
498 
499  }; // struct JointModelSphericalTpl
500 
501 } // namespace pinocchio
502 
503 #include <boost/type_traits.hpp>
504 
505 namespace boost
506 {
507  template<typename Scalar, int Options>
508  struct has_nothrow_constructor< ::pinocchio::JointModelSphericalTpl<Scalar,Options> >
509  : public integral_constant<bool,true> {};
510 
511  template<typename Scalar, int Options>
512  struct has_nothrow_copy< ::pinocchio::JointModelSphericalTpl<Scalar,Options> >
513  : public integral_constant<bool,true> {};
514 
515  template<typename Scalar, int Options>
516  struct has_nothrow_constructor< ::pinocchio::JointDataSphericalTpl<Scalar,Options> >
517  : public integral_constant<bool,true> {};
518 
519  template<typename Scalar, int Options>
520  struct has_nothrow_copy< ::pinocchio::JointDataSphericalTpl<Scalar,Options> >
521  : public integral_constant<bool,true> {};
522 }
523 
524 #endif // ifndef __pinocchio_joint_spherical_hpp__
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 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.
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
void cross(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3xIn > &Min, const Eigen::MatrixBase< Matrix3xOut > &Mout)
Applies the cross product onto the columns of M.
Definition: skew.hpp:211
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:40
JointModelSphericalTpl< NewScalar, Options > cast() const
Main pinocchio namespace.
Definition: treeview.dox:24
void alphaSkew(const Scalar alpha, const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector multiplied by a given scalar. i.e. the antisymmetric matrix representation of the cross product operator ( )
Definition: skew.hpp:124
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:32
void skew(const Eigen::MatrixBase< Vector3 > &v, const Eigen::MatrixBase< Matrix3 > &M)
Computes the skew representation of a given 3d vector, i.e. the antisymmetric matrix representation o...
Definition: skew.hpp:21
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)