pinocchio  3.5.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-spherical-ZYX.hpp
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 // Copyright (c) 2015-2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_multibody_joint_spherical_ZYX_hpp__
7 #define __pinocchio_multibody_joint_spherical_ZYX_hpp__
8 
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/multibody/joint/joint-base.hpp"
11 #include "pinocchio/multibody/joint/joint-spherical.hpp"
12 #include "pinocchio/multibody/joint-motion-subspace.hpp"
13 #include "pinocchio/math/sincos.hpp"
14 #include "pinocchio/math/matrix.hpp"
15 #include "pinocchio/spatial/inertia.hpp"
16 #include "pinocchio/spatial/skew.hpp"
17 
18 namespace pinocchio
19 {
20  template<typename Scalar, int Options>
21  struct JointMotionSubspaceSphericalZYXTpl;
22 
23  template<typename _Scalar, int _Options>
24  struct traits<JointMotionSubspaceSphericalZYXTpl<_Scalar, _Options>>
25  {
26  typedef _Scalar Scalar;
27  enum
28  {
29  Options = _Options
30  };
31 
32  enum
33  {
34  LINEAR = 0,
35  ANGULAR = 3
36  };
37 
39  typedef Eigen::Matrix<Scalar, 3, 1, Options> JointForce;
40  typedef Eigen::Matrix<Scalar, 6, 3, Options> DenseBase;
41  typedef Eigen::Matrix<Scalar, 3, 3, Options> ReducedSquaredMatrix;
42 
43  typedef DenseBase MatrixReturnType;
44  typedef const DenseBase ConstMatrixReturnType;
45 
46  typedef ReducedSquaredMatrix StDiagonalMatrixSOperationReturnType;
47  }; // struct traits struct ConstraintRotationalSubspace
48 
49  template<typename _Scalar, int _Options>
51  : public JointMotionSubspaceBase<JointMotionSubspaceSphericalZYXTpl<_Scalar, _Options>>
52  {
53  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
54 
55  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceSphericalZYXTpl)
56 
57  enum
58  {
59  NV = 3
60  };
61  typedef Eigen::Matrix<Scalar, 3, 3, Options> Matrix3;
62 
64  {
65  }
66 
67  template<typename Matrix3Like>
68  JointMotionSubspaceSphericalZYXTpl(const Eigen::MatrixBase<Matrix3Like> & subspace)
69  : m_S(subspace)
70  {
71  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix3Like, 3, 3);
72  }
73 
74  template<typename Vector3Like>
75  JointMotion __mult__(const Eigen::MatrixBase<Vector3Like> & v) const
76  {
77  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector3Like, 3);
78  return JointMotion(m_S * v);
79  }
80 
81  Matrix3 & operator()()
82  {
83  return m_S;
84  }
85  const Matrix3 & operator()() const
86  {
87  return m_S;
88  }
89 
90  int nv_impl() const
91  {
92  return NV;
93  }
94 
96  : JointMotionSubspaceTransposeBase<JointMotionSubspaceSphericalZYXTpl>
97  {
100  : ref(ref)
101  {
102  }
103 
104  template<typename Derived>
105  const typename MatrixMatrixProduct<
106  Eigen::Transpose<const Matrix3>,
107  Eigen::Block<const typename ForceDense<Derived>::Vector6, 3, 1>>::type
108  operator*(const ForceDense<Derived> & phi) const
109  {
110  return ref.m_S.transpose() * phi.angular();
111  }
112 
113  /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
114  template<typename D>
115  const typename MatrixMatrixProduct<
116  typename Eigen::Transpose<const Matrix3>,
117  typename Eigen::MatrixBase<const D>::template NRowsBlockXpr<3>::Type>::type
118  operator*(const Eigen::MatrixBase<D> & F) const
119  {
120  EIGEN_STATIC_ASSERT(
121  D::RowsAtCompileTime == 6, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE)
122  return ref.m_S.transpose() * F.template middleRows<3>(ANGULAR);
123  }
124  }; // struct ConstraintTranspose
125 
126  ConstraintTranspose transpose() const
127  {
128  return ConstraintTranspose(*this);
129  }
130 
131  DenseBase matrix_impl() const
132  {
133  DenseBase S;
134  S.template middleRows<3>(LINEAR).setZero();
135  S.template middleRows<3>(ANGULAR) = m_S;
136  return S;
137  }
138 
139  // const typename Eigen::ProductReturnType<
140  // const ConstraintDense,
141  // const Matrix3
142  // >::Type
143  template<typename S1, int O1>
144  Eigen::Matrix<Scalar, 6, 3, Options> se3Action(const SE3Tpl<S1, O1> & m) const
145  {
146  // Eigen::Matrix <Scalar,6,3,Options> X_subspace;
147  // X_subspace.template block <3,3> (Motion::LINEAR, 0) = skew (m.translation ()) *
148  // m.rotation (); X_subspace.template block <3,3> (Motion::ANGULAR, 0) = m.rotation ();
149  //
150  // return (X_subspace * m_S).eval();
151 
152  Eigen::Matrix<Scalar, 6, 3, Options> result;
153 
154  // ANGULAR
155  result.template middleRows<3>(ANGULAR).noalias() = m.rotation() * m_S;
156 
157  // LINEAR
158  cross(
159  m.translation(), result.template middleRows<3>(Motion::ANGULAR),
160  result.template middleRows<3>(LINEAR));
161 
162  return result;
163  }
164 
165  template<typename S1, int O1>
166  Eigen::Matrix<Scalar, 6, 3, Options> se3ActionInverse(const SE3Tpl<S1, O1> & m) const
167  {
168  Eigen::Matrix<Scalar, 6, 3, Options> result;
169 
170  // LINEAR
171  cross(m.translation(), m_S, result.template middleRows<3>(ANGULAR));
172  result.template middleRows<3>(LINEAR).noalias() =
173  -m.rotation().transpose() * result.template middleRows<3>(ANGULAR);
174 
175  // ANGULAR
176  result.template middleRows<3>(ANGULAR).noalias() = m.rotation().transpose() * m_S;
177 
178  return result;
179  }
180 
181  template<typename MotionDerived>
182  DenseBase motionAction(const MotionDense<MotionDerived> & m) const
183  {
184  const typename MotionDerived::ConstLinearType v = m.linear();
185  const typename MotionDerived::ConstAngularType w = m.angular();
186 
187  DenseBase res;
188  cross(v, m_S, res.template middleRows<3>(LINEAR));
189  cross(w, m_S, res.template middleRows<3>(ANGULAR));
190 
191  return res;
192  }
193 
194  Matrix3 & angularSubspace()
195  {
196  return m_S;
197  }
198  const Matrix3 & angularSubspace() const
199  {
200  return m_S;
201  }
202 
203  bool isEqual(const JointMotionSubspaceSphericalZYXTpl & other) const
204  {
205  return internal::comparison_eq(m_S, other.m_S);
206  }
207 
208  protected:
209  Matrix3 m_S;
210 
211  }; // struct JointMotionSubspaceSphericalZYXTpl
212 
213  namespace details
214  {
215  template<typename Scalar, int Options>
217  {
220 
221  static ReturnType run(const JointMotionSubspaceBase<Constraint> & constraint)
222  {
223  return constraint.matrix().transpose() * constraint.matrix();
224  }
225  };
226  } // namespace details
227 
228  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
229  template<typename S1, int O1, typename S2, int O2>
230  Eigen::Matrix<S1, 6, 3, O1>
231  operator*(const InertiaTpl<S1, O1> & Y, const JointMotionSubspaceSphericalZYXTpl<S2, O2> & S)
232  {
235  Eigen::Matrix<S1, 6, 3, O1> M;
236  alphaSkew(-Y.mass(), Y.lever(), M.template middleRows<3>(Constraint::LINEAR));
237  M.template middleRows<3>(Constraint::ANGULAR) =
238  (Y.inertia() - typename Symmetric3::AlphaSkewSquare(Y.mass(), Y.lever())).matrix();
239 
240  return (M * S.angularSubspace()).eval();
241  }
242 
243  /* [ABA] Y*S operator (Inertia Y,Constraint S) */
244  // inline Eigen::Matrix<context::Scalar,6,3>
245  template<typename Matrix6Like, typename S2, int O2>
246  const typename MatrixMatrixProduct<
247  typename Eigen::internal::remove_const<
248  typename SizeDepType<3>::ColsReturn<Matrix6Like>::ConstType>::type,
249  typename JointMotionSubspaceSphericalZYXTpl<S2, O2>::Matrix3>::type
250  operator*(
251  const Eigen::MatrixBase<Matrix6Like> & Y, const JointMotionSubspaceSphericalZYXTpl<S2, O2> & S)
252  {
253  EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix6Like, 6, 6);
254  return Y.derived().template middleCols<3>(Inertia::ANGULAR) * S.angularSubspace();
255  }
256 
257  template<typename S1, int O1>
259  {
260  // typedef const typename Eigen::ProductReturnType<
261  // Eigen::Matrix <context::Scalar,6,3,0>,
262  // Eigen::Matrix <context::Scalar,3,3,0>
263  // >::Type Type;
264  typedef Eigen::Matrix<S1, 6, 3, O1> ReturnType;
265  };
266 
267  template<typename S1, int O1, typename MotionDerived>
269  {
270  typedef Eigen::Matrix<S1, 6, 3, O1> ReturnType;
271  };
272 
273  template<typename Scalar, int Options>
275 
276  template<typename _Scalar, int _Options>
277  struct traits<JointSphericalZYXTpl<_Scalar, _Options>>
278  {
279  enum
280  {
281  NQ = 3,
282  NV = 3,
283  NVExtended = 3
284  };
285  typedef _Scalar Scalar;
286  enum
287  {
288  Options = _Options
289  };
296 
297  // [ABA]
298  typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
299  typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
300  typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
301 
302  typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
303  typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
304 
305  typedef boost::mpl::false_ is_mimicable_t;
306 
307  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
308  };
309 
310  template<typename _Scalar, int _Options>
311  struct traits<JointDataSphericalZYXTpl<_Scalar, _Options>>
312  {
314  typedef _Scalar Scalar;
315  };
316 
317  template<typename _Scalar, int _Options>
318  struct traits<JointModelSphericalZYXTpl<_Scalar, _Options>>
319  {
321  typedef _Scalar Scalar;
322  };
323 
324  template<typename _Scalar, int _Options>
326  : public JointDataBase<JointDataSphericalZYXTpl<_Scalar, _Options>>
327  {
328  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
330  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
331  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
332 
333  ConfigVector_t joint_q;
334  TangentVector_t joint_v;
335 
336  Constraint_t S;
337  Transformation_t M;
338  Motion_t v;
339  Bias_t c;
340 
341  // [ABA] specific data
342  U_t U;
343  D_t Dinv;
344  UD_t UDinv;
345  D_t StU;
346 
348  : joint_q(ConfigVector_t::Zero())
349  , joint_v(TangentVector_t::Zero())
350  , S(Constraint_t::Matrix3::Zero())
351  , M(Transformation_t::Identity())
352  , v(Motion_t::Vector3::Zero())
353  , c(Bias_t::Vector3::Zero())
354  , U(U_t::Zero())
355  , Dinv(D_t::Zero())
356  , UDinv(UD_t::Zero())
357  , StU(D_t::Zero())
358  {
359  }
360 
361  static std::string classname()
362  {
363  return std::string("JointDataSphericalZYX");
364  }
365  std::string shortname() const
366  {
367  return classname();
368  }
369 
370  }; // strcut JointDataSphericalZYXTpl
371 
372  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelSphericalZYXTpl);
373  template<typename _Scalar, int _Options>
375  : public JointModelBase<JointModelSphericalZYXTpl<_Scalar, _Options>>
376  {
377  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
379  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
380 
382  using Base::id;
383  using Base::idx_q;
384  using Base::idx_v;
385  using Base::idx_vExtended;
386  using Base::setIndexes;
387 
388  JointDataDerived createData() const
389  {
390  return JointDataDerived();
391  }
392 
393  const std::vector<bool> hasConfigurationLimit() const
394  {
395  return {true, true, true};
396  }
397 
398  const std::vector<bool> hasConfigurationLimitInTangent() const
399  {
400  return {true, true, true};
401  }
402 
403  template<typename ConfigVector>
404  void calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
405  {
406  data.joint_q = qs.template segment<NQ>(idx_q());
407 
408  Scalar c0, s0;
409  SINCOS(data.joint_q(0), &s0, &c0);
410  Scalar c1, s1;
411  SINCOS(data.joint_q(1), &s1, &c1);
412  Scalar c2, s2;
413  SINCOS(data.joint_q(2), &s2, &c2);
414 
415  data.M.rotation() << c0 * c1, c0 * s1 * s2 - s0 * c2, c0 * s1 * c2 + s0 * s2, s0 * c1,
416  s0 * s1 * s2 + c0 * c2, s0 * s1 * c2 - c0 * s2, -s1, c1 * s2, c1 * c2;
417 
418  data.S.angularSubspace() << -s1, Scalar(0), Scalar(1), c1 * s2, c2, Scalar(0), c1 * c2, -s2,
419  Scalar(0);
420  }
421 
422  template<typename TangentVector>
423  void
424  calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
425  const
426  {
427  data.joint_v = vs.template segment<NV>(idx_v());
428  data.v().noalias() = data.S.angularSubspace() * data.joint_v;
429 
430  // TODO(jcarpent): to be done
431  // #define q_dot data.joint_v
432  // data.c()(0) = -c1 * q_dot(0) * q_dot(1);
433  // data.c()(1) = -s1 * s2 * q_dot(0) * q_dot(1) + c1 * c2 * q_dot(0) * q_dot(2) - s2 *
434  // q_dot(1) * q_dot(2); data.c()(2) = -s1 * c2 * q_dot(0) * q_dot(1) - c1 * s2 *
435  // q_dot(0) * q_dot(2) - c2 * q_dot(1) * q_dot(2);
436  // #undef q_dot
437  }
438 
439  template<typename ConfigVector, typename TangentVector>
440  void calc(
441  JointDataDerived & data,
442  const typename Eigen::MatrixBase<ConfigVector> & qs,
443  const typename Eigen::MatrixBase<TangentVector> & vs) const
444  {
445  data.joint_q = qs.template segment<NQ>(idx_q());
446 
447  Scalar c0, s0;
448  SINCOS(data.joint_q(0), &s0, &c0);
449  Scalar c1, s1;
450  SINCOS(data.joint_q(1), &s1, &c1);
451  Scalar c2, s2;
452  SINCOS(data.joint_q(2), &s2, &c2);
453 
454  data.M.rotation() << c0 * c1, c0 * s1 * s2 - s0 * c2, c0 * s1 * c2 + s0 * s2, s0 * c1,
455  s0 * s1 * s2 + c0 * c2, s0 * s1 * c2 - c0 * s2, -s1, c1 * s2, c1 * c2;
456 
457  data.S.angularSubspace() << -s1, Scalar(0), Scalar(1), c1 * s2, c2, Scalar(0), c1 * c2, -s2,
458  Scalar(0);
459 
460  data.joint_v = vs.template segment<NV>(idx_v());
461  data.v().noalias() = data.S.angularSubspace() * data.joint_v;
462 
463 #define q_dot data.joint_v
464  data.c()(0) = -c1 * q_dot(0) * q_dot(1);
465  data.c()(1) =
466  -s1 * s2 * q_dot(0) * q_dot(1) + c1 * c2 * q_dot(0) * q_dot(2) - s2 * q_dot(1) * q_dot(2);
467  data.c()(2) =
468  -s1 * c2 * q_dot(0) * q_dot(1) - c1 * s2 * q_dot(0) * q_dot(2) - c2 * q_dot(1) * q_dot(2);
469 #undef q_dot
470  }
471 
472  template<typename VectorLike, typename Matrix6Like>
473  void calc_aba(
474  JointDataDerived & data,
475  const Eigen::MatrixBase<VectorLike> & armature,
476  const Eigen::MatrixBase<Matrix6Like> & I,
477  const bool update_I) const
478  {
479  data.U.noalias() = I.template middleCols<3>(Motion::ANGULAR) * data.S.angularSubspace();
480  data.StU.noalias() =
481  data.S.angularSubspace().transpose() * data.U.template middleRows<3>(Motion::ANGULAR);
482  data.StU.diagonal() += armature;
483 
484  internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv);
485 
486  data.UDinv.noalias() = data.U * data.Dinv;
487 
488  if (update_I)
489  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
490  }
491 
492  static std::string classname()
493  {
494  return std::string("JointModelSphericalZYX");
495  }
496  std::string shortname() const
497  {
498  return classname();
499  }
500 
502  template<typename NewScalar>
504  {
506  ReturnType res;
507  res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended());
508  return res;
509  }
510 
511  }; // struct JointModelSphericalZYXTpl
512 
513 } // namespace pinocchio
514 
515 #include <boost/type_traits.hpp>
516 
517 namespace boost
518 {
519  template<typename Scalar, int Options>
520  struct has_nothrow_constructor<::pinocchio::JointModelSphericalZYXTpl<Scalar, Options>>
521  : public integral_constant<bool, true>
522  {
523  };
524 
525  template<typename Scalar, int Options>
526  struct has_nothrow_copy<::pinocchio::JointModelSphericalZYXTpl<Scalar, Options>>
527  : public integral_constant<bool, true>
528  {
529  };
530 
531  template<typename Scalar, int Options>
532  struct has_nothrow_constructor<::pinocchio::JointDataSphericalZYXTpl<Scalar, Options>>
533  : public integral_constant<bool, true>
534  {
535  };
536 
537  template<typename Scalar, int Options>
538  struct has_nothrow_copy<::pinocchio::JointDataSphericalZYXTpl<Scalar, Options>>
539  : public integral_constant<bool, true>
540  {
541  };
542 } // namespace boost
543 
544 #endif // ifndef __pinocchio_multibody_joint_spherical_ZYX_hpp__
ConstAngularType angular() const
Return the angular part of the force vector.
Definition: force-base.hpp:47
Main pinocchio namespace.
Definition: treeview.dox:11
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 SINCOS(const S1 &a, S2 *sa, S3 *ca)
Computes sin/cos values of a given input scalar.
Definition: sincos.hpp:27
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....
Definition: skew.hpp:134
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:228
Blank type.
Definition: fwd.hpp:77
JointModelSphericalZYXTpl< NewScalar, Options > cast() const
Return type of the ation of a Motion onto an object of type D.
Definition: motion.hpp:46
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:72