pinocchio  3.5.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-free-flyer.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_free_flyer_hpp__
7 #define __pinocchio_multibody_joint_free_flyer_hpp__
8 
9 #include "pinocchio/macros.hpp"
10 #include "pinocchio/spatial/inertia.hpp"
11 #include "pinocchio/spatial/explog.hpp"
12 #include "pinocchio/multibody/joint/joint-base.hpp"
13 #include "pinocchio/multibody/joint-motion-subspace.hpp"
14 #include "pinocchio/math/fwd.hpp"
15 #include "pinocchio/math/quaternion.hpp"
16 
17 namespace pinocchio
18 {
19 
20  template<typename Scalar, int Options>
21  struct JointMotionSubspaceIdentityTpl;
22 
23  template<typename _Scalar, int _Options>
24  struct traits<JointMotionSubspaceIdentityTpl<_Scalar, _Options>>
25  {
26  typedef _Scalar Scalar;
27  enum
28  {
29  Options = _Options
30  };
31  typedef Eigen::Matrix<Scalar, 6, 6, Options> Matrix6;
32  enum
33  {
34  LINEAR = 0,
35  ANGULAR = 3
36  };
38  typedef Eigen::Matrix<Scalar, 6, 1, Options> JointForce;
39  typedef Eigen::Matrix<Scalar, 6, 6, Options> DenseBase;
40  typedef Eigen::Matrix<Scalar, 6, 6, Options> ReducedSquaredMatrix;
41 
42  typedef typename Matrix6::IdentityReturnType ConstMatrixReturnType;
43  typedef typename Matrix6::IdentityReturnType MatrixReturnType;
44  typedef typename Matrix6::IdentityReturnType StDiagonalMatrixSOperationReturnType;
45  }; // traits ConstraintRevolute
46 
47  template<typename _Scalar, int _Options>
49  : JointMotionSubspaceBase<JointMotionSubspaceIdentityTpl<_Scalar, _Options>>
50  {
51  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
52  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceIdentityTpl)
53 
54  enum
55  {
56  NV = 6
57  };
58 
59  template<typename Vector6Like>
60  JointMotion __mult__(const Eigen::MatrixBase<Vector6Like> & vj) const
61  {
62  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like, 6);
63  return JointMotion(vj);
64  }
65 
66  template<typename S1, int O1>
67  typename SE3Tpl<S1, O1>::ActionMatrixType se3Action(const SE3Tpl<S1, O1> & m) const
68  {
69  return m.toActionMatrix();
70  }
71 
72  template<typename S1, int O1>
73  typename SE3Tpl<S1, O1>::ActionMatrixType se3ActionInverse(const SE3Tpl<S1, O1> & m) const
74  {
75  return m.toActionMatrixInverse();
76  }
77 
78  int nv_impl() const
79  {
80  return NV;
81  }
82 
83  struct TransposeConst : JointMotionSubspaceTransposeBase<JointMotionSubspaceIdentityTpl>
84  {
85  template<typename Derived>
87  operator*(const ForceDense<Derived> & phi)
88  {
89  return phi.toVector();
90  }
91 
92  /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
93  template<typename MatrixDerived>
94  typename PINOCCHIO_EIGEN_REF_CONST_TYPE(MatrixDerived)
95  operator*(const Eigen::MatrixBase<MatrixDerived> & F)
96  {
97  return F.derived();
98  }
99  };
100 
101  TransposeConst transpose() const
102  {
103  return TransposeConst();
104  }
105  MatrixReturnType matrix_impl() const
106  {
107  return DenseBase::Identity();
108  }
109 
110  template<typename MotionDerived>
111  typename MotionDerived::ActionMatrixType motionAction(const MotionBase<MotionDerived> & v) const
112  {
113  return v.toActionMatrix();
114  }
115 
116  bool isEqual(const JointMotionSubspaceIdentityTpl &) const
117  {
118  return true;
119  }
120 
121  }; // struct JointMotionSubspaceIdentityTpl
122 
123  template<typename Scalar, int Options, typename Vector6Like>
124  MotionRef<const Vector6Like> operator*(
125  const JointMotionSubspaceIdentityTpl<Scalar, Options> &,
126  const Eigen::MatrixBase<Vector6Like> & v)
127  {
128  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like, 6);
129  // typedef typename JointMotionSubspaceIdentityTpl<Scalar,Options>::Motion Motion;
130  typedef MotionRef<const Vector6Like> Motion;
131  return Motion(v.derived());
132  }
133 
134  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
135  template<typename S1, int O1, typename S2, int O2>
136  inline typename InertiaTpl<S1, O1>::Matrix6
137  operator*(const InertiaTpl<S1, O1> & Y, const JointMotionSubspaceIdentityTpl<S2, O2> &)
138  {
139  return Y.matrix();
140  }
141 
142  /* [ABA] Y*S operator*/
143  template<typename Matrix6Like, typename S2, int O2>
144  inline typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like) operator*(
145  const Eigen::MatrixBase<Matrix6Like> & Y, const JointMotionSubspaceIdentityTpl<S2, O2> &)
146  {
147  return Y.derived();
148  }
149 
150  template<typename S1, int O1>
152  {
153  typedef typename SE3Tpl<S1, O1>::ActionMatrixType ReturnType;
154  };
155 
156  template<typename S1, int O1, typename MotionDerived>
158  {
159  typedef typename SE3Tpl<S1, O1>::ActionMatrixType ReturnType;
160  };
161 
162  template<typename Scalar, int Options>
164 
165  template<typename _Scalar, int _Options>
166  struct traits<JointFreeFlyerTpl<_Scalar, _Options>>
167  {
168  enum
169  {
170  NQ = 7,
171  NV = 6,
172  NVExtended = 6
173  };
174  typedef _Scalar Scalar;
175  enum
176  {
177  Options = _Options
178  };
185 
186  // [ABA]
187  typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
188  typedef Eigen::Matrix<Scalar, NV, NV, Options> D_t;
189  typedef Eigen::Matrix<Scalar, 6, NV, Options> UD_t;
190 
191  typedef Eigen::Matrix<Scalar, NQ, 1, Options> ConfigVector_t;
192  typedef Eigen::Matrix<Scalar, NV, 1, Options> TangentVector_t;
193 
194  typedef boost::mpl::false_ is_mimicable_t;
195 
196  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
197  };
198 
199  template<typename _Scalar, int _Options>
200  struct traits<JointDataFreeFlyerTpl<_Scalar, _Options>>
201  {
203  typedef _Scalar Scalar;
204  };
205 
206  template<typename _Scalar, int _Options>
207  struct traits<JointModelFreeFlyerTpl<_Scalar, _Options>>
208  {
210  typedef _Scalar Scalar;
211  };
212 
213  template<typename _Scalar, int _Options>
214  struct JointDataFreeFlyerTpl : public JointDataBase<JointDataFreeFlyerTpl<_Scalar, _Options>>
215  {
216  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
218  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
219  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
220 
221  ConfigVector_t joint_q;
222  TangentVector_t joint_v;
223 
224  Constraint_t S;
225  Transformation_t M;
226  Motion_t v;
227  Bias_t c;
228 
229  // [ABA] specific data
230  U_t U;
231  D_t Dinv;
232  UD_t UDinv;
233  D_t StU;
234 
236  : joint_q(ConfigVector_t::Zero())
237  , joint_v(TangentVector_t::Zero())
238  , M(Transformation_t::Identity())
239  , v(Motion_t::Zero())
240  , U(U_t::Zero())
241  , Dinv(D_t::Zero())
242  , UDinv(UD_t::Identity())
243  , StU(D_t::Zero())
244  {
245  joint_q[6] = Scalar(1);
246  }
247 
248  static std::string classname()
249  {
250  return std::string("JointDataFreeFlyer");
251  }
252  std::string shortname() const
253  {
254  return classname();
255  }
256 
257  }; // struct JointDataFreeFlyerTpl
258 
283  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl);
284  template<typename _Scalar, int _Options>
285  struct JointModelFreeFlyerTpl : public JointModelBase<JointModelFreeFlyerTpl<_Scalar, _Options>>
286  {
287  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
289  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
290 
292  using Base::id;
293  using Base::idx_q;
294  using Base::idx_v;
295  using Base::idx_vExtended;
296  using Base::setIndexes;
297 
298  JointDataDerived createData() const
299  {
300  return JointDataDerived();
301  }
302 
303  const std::vector<bool> hasConfigurationLimit() const
304  {
305  return {true, true, true, false, false, false, false};
306  }
307 
308  const std::vector<bool> hasConfigurationLimitInTangent() const
309  {
310  return {true, true, true, false, false, false};
311  }
312 
313  template<typename ConfigVectorLike>
314  inline void forwardKinematics(
315  Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const
316  {
317  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t, ConfigVectorLike);
318  typedef typename Eigen::Quaternion<
319  typename ConfigVectorLike::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options>
320  Quaternion;
321  typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
322 
323  ConstQuaternionMap quat(q_joint.template tail<4>().data());
324  // assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename
325  // V::Scalar>::epsilon())); TODO: check validity of the rhs precision
326  assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm() - 1)) <= 1e-4);
327 
328  M.rotation(quat.matrix());
329  M.translation(q_joint.template head<3>());
330  }
331 
332  template<typename Vector3Derived, typename QuaternionDerived>
333  PINOCCHIO_DONT_INLINE void calc(
334  JointDataDerived & data,
335  const typename Eigen::MatrixBase<Vector3Derived> & trans,
336  const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
337  {
338  data.M.translation(trans);
339  data.M.rotation(quat.matrix());
340  }
341 
342  template<typename ConfigVector>
343  PINOCCHIO_DONT_INLINE void
344  calc(JointDataDerived & data, const typename Eigen::MatrixBase<ConfigVector> & qs) const
345  {
346  typedef typename Eigen::Quaternion<Scalar, Options> Quaternion;
347  typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
348 
349  data.joint_q = qs.template segment<NQ>(idx_q());
350  ConstQuaternionMap quat(data.joint_q.template tail<4>().data());
351 
352  calc(data, data.joint_q.template head<3>(), quat);
353  }
354 
355  template<typename TangentVector>
356  PINOCCHIO_DONT_INLINE void
357  calc(JointDataDerived & data, const Blank, const typename Eigen::MatrixBase<TangentVector> & vs)
358  const
359  {
360  data.joint_v = vs.template segment<NV>(idx_v());
361  data.v = data.joint_v;
362  }
363 
364  template<typename ConfigVector, typename TangentVector>
365  PINOCCHIO_DONT_INLINE void calc(
366  JointDataDerived & data,
367  const typename Eigen::MatrixBase<ConfigVector> & qs,
368  const typename Eigen::MatrixBase<TangentVector> & vs) const
369  {
370  calc(data, qs.derived());
371 
372  data.joint_v = vs.template segment<NV>(idx_v());
373  data.v = data.joint_v;
374  }
375 
376  template<typename VectorLike, typename Matrix6Like>
377  void calc_aba(
378  JointDataDerived & data,
379  const Eigen::MatrixBase<VectorLike> & armature,
380  const Eigen::MatrixBase<Matrix6Like> & I,
381  const bool update_I) const
382  {
383  data.U = I;
384  data.StU = I;
385  data.StU.diagonal() += armature;
386 
387  internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv);
388  data.UDinv.noalias() = I * data.Dinv;
389 
390  if (update_I)
391  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
392  }
393 
394  static std::string classname()
395  {
396  return std::string("JointModelFreeFlyer");
397  }
398  std::string shortname() const
399  {
400  return classname();
401  }
402 
404  template<typename NewScalar>
406  {
408  ReturnType res;
409  res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended());
410  return res;
411  }
412 
413  }; // struct JointModelFreeFlyerTpl
414 
415 } // namespace pinocchio
416 
417 #include <boost/type_traits.hpp>
418 
419 namespace boost
420 {
421  template<typename Scalar, int Options>
422  struct has_nothrow_constructor<::pinocchio::JointModelFreeFlyerTpl<Scalar, Options>>
423  : public integral_constant<bool, true>
424  {
425  };
426 
427  template<typename Scalar, int Options>
428  struct has_nothrow_copy<::pinocchio::JointModelFreeFlyerTpl<Scalar, Options>>
429  : public integral_constant<bool, true>
430  {
431  };
432 
433  template<typename Scalar, int Options>
434  struct has_nothrow_constructor<::pinocchio::JointDataFreeFlyerTpl<Scalar, Options>>
435  : public integral_constant<bool, true>
436  {
437  };
438 
439  template<typename Scalar, int Options>
440  struct has_nothrow_copy<::pinocchio::JointDataFreeFlyerTpl<Scalar, Options>>
441  : public integral_constant<bool, true>
442  {
443  };
444 } // namespace boost
445 
446 #endif // ifndef __pinocchio_multibody_joint_free_flyer_hpp__
ToVectorConstReturnType toVector() const
Return the force as an Eigen vector.
Definition: force-base.hpp:108
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.
Blank type.
Definition: fwd.hpp:77
JointModelFreeFlyerTpl< NewScalar, Options > cast() const
Return type of the ation of a Motion onto an object of type D.
Definition: motion.hpp:46
ActionMatrixType toActionMatrixInverse() const
The action matrix of .
Definition: se3-base.hpp:111
ActionMatrixType toActionMatrix() const
The action matrix of .
Definition: se3-base.hpp:92
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:72