pinocchio  2.7.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-free-flyer.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_free_flyer_hpp__
7 #define __pinocchio_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/constraint.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> struct ConstraintIdentityTpl;
21 
22  template<typename _Scalar, int _Options>
23  struct traits< ConstraintIdentityTpl<_Scalar,_Options> >
24  {
25  typedef _Scalar Scalar;
26  enum { Options = _Options };
27  typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
28  enum {
29  LINEAR = 0,
30  ANGULAR = 3
31  };
32  typedef MotionTpl<Scalar,Options> JointMotion;
33  typedef Eigen::Matrix<Scalar,6,1,Options> JointForce;
34  typedef Eigen::Matrix<Scalar,6,6,Options> DenseBase;
35  typedef typename Matrix6::IdentityReturnType ConstMatrixReturnType;
36  typedef typename Matrix6::IdentityReturnType MatrixReturnType;
37  }; // traits ConstraintRevolute
38 
39 
40  template<typename _Scalar, int _Options>
41  struct ConstraintIdentityTpl
42  : ConstraintBase< ConstraintIdentityTpl<_Scalar,_Options> >
43  {
44  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
45  PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(ConstraintIdentityTpl)
46 
47  enum { NV = 6 };
48 
49  template<typename Vector6Like>
50  JointMotion __mult__(const Eigen::MatrixBase<Vector6Like> & vj) const
51  {
52  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6);
53  return JointMotion(vj);
54  }
55 
56  template<typename S1, int O1>
57  typename SE3Tpl<S1,O1>::ActionMatrixType
58  se3Action(const SE3Tpl<S1,O1> & m) const
59  {
60  return m.toActionMatrix();
61 
62  }
63 
64  template<typename S1, int O1>
65  typename SE3Tpl<S1,O1>::ActionMatrixType
66  se3ActionInverse(const SE3Tpl<S1,O1> & m) const
67  {
68  return m.toActionMatrixInverse();
69  }
70 
71  int nv_impl() const { return NV; }
72 
73  struct TransposeConst
74  {
75  template<typename Derived>
76  typename ForceDense<Derived>::ToVectorConstReturnType
77  operator*(const ForceDense<Derived> & phi)
78  { return phi.toVector(); }
79 
80  /* [CRBA] MatrixBase operator* (Constraint::Transpose S, ForceSet::Block) */
81  template<typename MatrixDerived>
82  typename PINOCCHIO_EIGEN_REF_CONST_TYPE(MatrixDerived)
83  operator*(const Eigen::MatrixBase<MatrixDerived> & F)
84  {
85  return F.derived();
86  }
87  };
88 
89  TransposeConst transpose() const { return TransposeConst(); }
90  MatrixReturnType matrix_impl() const { return DenseBase::Identity(); }
91 
92  template<typename MotionDerived>
93  typename MotionDerived::ActionMatrixType
94  motionAction(const MotionBase<MotionDerived> & v) const
95  { return v.toActionMatrix(); }
96 
97  bool isEqual(const ConstraintIdentityTpl &) const { return true; }
98 
99  }; // struct ConstraintIdentityTpl
100 
101  template<typename Scalar, int Options, typename Vector6Like>
102  MotionRef<const Vector6Like>
103  operator*(const ConstraintIdentityTpl<Scalar,Options> &,
104  const Eigen::MatrixBase<Vector6Like> & v)
105  {
106  EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Vector6Like,6);
107 // typedef typename ConstraintIdentityTpl<Scalar,Options>::Motion Motion;
108  typedef MotionRef<const Vector6Like> Motion;
109  return Motion(v.derived());
110  }
111 
112  /* [CRBA] ForceSet operator* (Inertia Y,Constraint S) */
113  template<typename S1, int O1, typename S2, int O2>
114  inline typename InertiaTpl<S1,O1>::Matrix6
115  operator*(const InertiaTpl<S1,O1> & Y, const ConstraintIdentityTpl<S2,O2> &)
116  {
117  return Y.matrix();
118  }
119 
120  /* [ABA] Y*S operator*/
121  template<typename Matrix6Like, typename S2, int O2>
122  inline typename PINOCCHIO_EIGEN_REF_CONST_TYPE(Matrix6Like)
123  operator*(const Eigen::MatrixBase<Matrix6Like> & Y, const ConstraintIdentityTpl<S2,O2> &)
124  {
125  return Y.derived();
126  }
127 
128  template<typename S1, int O1>
129  struct SE3GroupAction< ConstraintIdentityTpl<S1,O1> >
130  { typedef typename SE3Tpl<S1,O1>::ActionMatrixType ReturnType; };
131 
132  template<typename S1, int O1, typename MotionDerived>
133  struct MotionAlgebraAction< ConstraintIdentityTpl<S1,O1>,MotionDerived >
134  { typedef typename SE3Tpl<S1,O1>::ActionMatrixType ReturnType; };
135 
136  template<typename Scalar, int Options> struct JointFreeFlyerTpl;
137 
138  template<typename _Scalar, int _Options>
139  struct traits< JointFreeFlyerTpl<_Scalar,_Options> >
140  {
141  enum {
142  NQ = 7,
143  NV = 6
144  };
145  typedef _Scalar Scalar;
146  enum { Options = _Options };
147  typedef JointDataFreeFlyerTpl<Scalar,Options> JointDataDerived;
148  typedef JointModelFreeFlyerTpl<Scalar,Options> JointModelDerived;
149  typedef ConstraintIdentityTpl<Scalar,Options> Constraint_t;
150  typedef SE3Tpl<Scalar,Options> Transformation_t;
151  typedef MotionTpl<Scalar,Options> Motion_t;
152  typedef MotionZeroTpl<Scalar,Options> Bias_t;
153 
154  // [ABA]
155  typedef Eigen::Matrix<Scalar,6,NV,Options> U_t;
156  typedef Eigen::Matrix<Scalar,NV,NV,Options> D_t;
157  typedef Eigen::Matrix<Scalar,6,NV,Options> UD_t;
158 
159  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
160 
161  typedef Eigen::Matrix<Scalar,NQ,1,Options> ConfigVector_t;
162  typedef Eigen::Matrix<Scalar,NV,1,Options> TangentVector_t;
163  };
164 
165  template<typename Scalar, int Options>
166  struct traits< JointDataFreeFlyerTpl<Scalar,Options> >
167  { typedef JointFreeFlyerTpl<Scalar,Options> JointDerived; };
168 
169  template<typename Scalar, int Options>
170  struct traits< JointModelFreeFlyerTpl<Scalar,Options> >
171  { typedef JointFreeFlyerTpl<Scalar,Options> JointDerived; };
172 
173  template<typename _Scalar, int _Options>
174  struct JointDataFreeFlyerTpl : public JointDataBase< JointDataFreeFlyerTpl<_Scalar,_Options> >
175  {
176  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
177  typedef JointFreeFlyerTpl<_Scalar,_Options> JointDerived;
178  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
179  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
180 
181  Constraint_t S;
182  Transformation_t M;
183  Motion_t v;
184  Bias_t c;
185 
186  // [ABA] specific data
187  U_t U;
188  D_t Dinv;
189  UD_t UDinv;
190 
191  JointDataFreeFlyerTpl()
192  : M(Transformation_t::Identity())
193  , v(Motion_t::Zero())
194  , U(U_t::Zero())
195  , Dinv(D_t::Zero())
196  , UDinv(UD_t::Identity())
197  {}
198 
199  static std::string classname() { return std::string("JointDataFreeFlyer"); }
200  std::string shortname() const { return classname(); }
201 
202  }; // struct JointDataFreeFlyerTpl
203 
204  PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelFreeFlyerTpl);
205  template<typename _Scalar, int _Options>
206  struct JointModelFreeFlyerTpl
207  : public JointModelBase< JointModelFreeFlyerTpl<_Scalar,_Options> >
208  {
209  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
210  typedef JointFreeFlyerTpl<_Scalar,_Options> JointDerived;
211  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
212 
213  typedef JointModelBase<JointModelFreeFlyerTpl> Base;
214  using Base::id;
215  using Base::idx_q;
216  using Base::idx_v;
217  using Base::setIndexes;
218 
219  JointDataDerived createData() const { return JointDataDerived(); }
220 
221  const std::vector<bool> hasConfigurationLimit() const
222  {
223  return {true, true, true, false, false, false, false};
224  }
225 
226  const std::vector<bool> hasConfigurationLimitInTangent() const
227  {
228  return {true, true, true, false, false, false};
229  }
230 
231  template<typename ConfigVectorLike>
232  inline void forwardKinematics(Transformation_t & M, const Eigen::MatrixBase<ConfigVectorLike> & q_joint) const
233  {
234  EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigVector_t,ConfigVectorLike);
235  typedef typename Eigen::Quaternion<typename ConfigVectorLike::Scalar,PINOCCHIO_EIGEN_PLAIN_TYPE(ConfigVectorLike)::Options> Quaternion;
236  typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
237 
238  ConstQuaternionMap quat(q_joint.template tail<4>().data());
239  //assert(math::fabs(quat.coeffs().squaredNorm()-1.) <= sqrt(Eigen::NumTraits<typename V::Scalar>::epsilon())); TODO: check validity of the rhs precision
240  assert(math::fabs(static_cast<Scalar>(quat.coeffs().squaredNorm()-1)) <= 1e-4);
241 
242  M.rotation(quat.matrix());
243  M.translation(q_joint.template head<3>());
244  }
245 
246  template<typename Vector3Derived, typename QuaternionDerived>
247  EIGEN_DONT_INLINE
248  void calc(JointDataDerived & data,
249  const typename Eigen::MatrixBase<Vector3Derived> & trans,
250  const typename Eigen::QuaternionBase<QuaternionDerived> & quat) const
251  {
252  data.M.translation(trans);
253  data.M.rotation(quat.matrix());
254  }
255 
256  template<typename ConfigVector>
257  EIGEN_DONT_INLINE
258  void calc(JointDataDerived & data,
259  const typename Eigen::PlainObjectBase<ConfigVector> & qs) const
260  {
261  typedef typename Eigen::Quaternion<typename ConfigVector::Scalar,ConfigVector::Options> Quaternion;
262  typedef Eigen::Map<const Quaternion> ConstQuaternionMap;
263 
264  typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(idx_q());
265  ConstQuaternionMap quat(q.template tail<4>().data());
266 
267  calc(data,q.template head<3>(),quat);
268  }
269 
270  template<typename ConfigVector>
271  EIGEN_DONT_INLINE
272  void calc(JointDataDerived & data,
273  const typename Eigen::MatrixBase<ConfigVector> & qs) const
274  {
275  typedef typename Eigen::Quaternion<Scalar,Options> Quaternion;
276 
277  typename ConfigVector::template ConstFixedSegmentReturnType<NQ>::Type q = qs.template segment<NQ>(idx_q());
278  const Quaternion quat(q.template tail<4>());
279 
280  calc(data,q.template head<3>(),quat);
281  }
282 
283  template<typename ConfigVector, typename TangentVector>
284  EIGEN_DONT_INLINE
285  void calc(JointDataDerived & data,
286  const typename Eigen::MatrixBase<ConfigVector> & qs,
287  const typename Eigen::MatrixBase<TangentVector> & vs) const
288  {
289  calc(data,qs.derived());
290 
291  data.v = vs.template segment<NV>(idx_v());
292  }
293 
294  template<typename Matrix6Like>
295  void calc_aba(JointDataDerived & data,
296  const Eigen::MatrixBase<Matrix6Like> & I,
297  const bool update_I) const
298  {
299  data.U = I;
300 
301  // compute inverse
302 // data.Dinv.setIdentity();
303 // I.llt().solveInPlace(data.Dinv);
304  internal::PerformStYSInversion<Scalar>::run(I,data.Dinv);
305 
306  if (update_I)
307  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).setZero();
308  }
309 
310  static std::string classname() { return std::string("JointModelFreeFlyer"); }
311  std::string shortname() const { return classname(); }
312 
314  template<typename NewScalar>
315  JointModelFreeFlyerTpl<NewScalar,Options> cast() const
316  {
317  typedef JointModelFreeFlyerTpl<NewScalar,Options> ReturnType;
318  ReturnType res;
319  res.setIndexes(id(),idx_q(),idx_v());
320  return res;
321  }
322 
323  }; // struct JointModelFreeFlyerTpl
324 
325 } // namespace pinocchio
326 
327 #include <boost/type_traits.hpp>
328 
329 namespace boost
330 {
331  template<typename Scalar, int Options>
332  struct has_nothrow_constructor< ::pinocchio::JointModelFreeFlyerTpl<Scalar,Options> >
333  : public integral_constant<bool,true> {};
334 
335  template<typename Scalar, int Options>
336  struct has_nothrow_copy< ::pinocchio::JointModelFreeFlyerTpl<Scalar,Options> >
337  : public integral_constant<bool,true> {};
338 
339  template<typename Scalar, int Options>
340  struct has_nothrow_constructor< ::pinocchio::JointDataFreeFlyerTpl<Scalar,Options> >
341  : public integral_constant<bool,true> {};
342 
343  template<typename Scalar, int Options>
344  struct has_nothrow_copy< ::pinocchio::JointDataFreeFlyerTpl<Scalar,Options> >
345  : public integral_constant<bool,true> {};
346 }
347 
348 #endif // ifndef __pinocchio_joint_free_flyer_hpp__
pinocchio::JointDataFreeFlyerTpl
Definition: fwd.hpp:66
pinocchio::operator*
MultiplicationOp< InertiaTpl< Scalar, Options >, ConstraintDerived >::ReturnType operator*(const InertiaTpl< Scalar, Options > &Y, const ConstraintBase< ConstraintDerived > &constraint)
&#160;
Definition: constraint-base.hpp:112
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...
pinocchio::ConstraintIdentityTpl
Definition: joint-free-flyer.hpp:20
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
Definition: fwd.hpp:38
pinocchio::JointDataBase
Definition: joint-data-base.hpp:82
pinocchio::JointModelFreeFlyerTpl
Definition: fwd.hpp:63
pinocchio::MotionAlgebraAction
Return type of the ation of a Motion onto an object of type D.
Definition: motion.hpp:46
pinocchio::SE3GroupAction
Definition: se3.hpp:39
pinocchio::MotionZeroTpl
Definition: fwd.hpp:44
pinocchio::JointModelFreeFlyerTpl::cast
JointModelFreeFlyerTpl< NewScalar, Options > cast() const
Definition: joint-free-flyer.hpp:315
pinocchio::ConstraintIdentityTpl::TransposeConst
Definition: joint-free-flyer.hpp:73
pinocchio::JointFreeFlyerTpl
Definition: joint-free-flyer.hpp:136
pinocchio::ForceBase::toVector
ToVectorConstReturnType toVector() const
Return the force as an Eigen vector.
Definition: force-base.hpp:81
pinocchio::ForceDense
Definition: force-dense.hpp:24
pinocchio::ConstraintBase
Definition: constraint-base.hpp:48
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
pinocchio::MotionTpl< Scalar, Options >
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11