pinocchio  3.5.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-generic.hpp
1 //
2 // Copyright (c) 2016-2021 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_multibody_joint_generic_hpp__
6 #define __pinocchio_multibody_joint_generic_hpp__
7 
8 #include "pinocchio/multibody/joint/joint-collection.hpp"
9 #include "pinocchio/multibody/joint/joint-composite.hpp"
10 #include "pinocchio/multibody/joint/joint-basic-visitors.hxx"
11 #include "pinocchio/container/aligned-vector.hpp"
12 
13 #include <boost/mpl/contains.hpp>
14 
15 namespace pinocchio
16 {
17 
18  template<
19  typename Scalar,
20  int Options = context::Options,
21  template<typename S, int O> class JointCollectionTpl = JointCollectionDefaultTpl>
22  struct JointTpl;
24 
25  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
26  struct traits<JointTpl<_Scalar, _Options, JointCollectionTpl>>
27  {
28  enum
29  {
30  Options = _Options,
31  NQ = Eigen::Dynamic, // Dynamic because unknown at compile time
32  NV = Eigen::Dynamic,
33  NVExtended = Eigen::Dynamic
34  };
35 
36  typedef _Scalar Scalar;
37  typedef JointCollectionTpl<Scalar, Options> JointCollection;
40 
45 
46  // [ABA]
47  typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> U_t;
48  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> D_t;
49  typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> UD_t;
50 
56  typedef Motion_t MotionTypeRef;
57  typedef Bias_t BiasTypeConstRef;
58  typedef Bias_t BiasTypeRef;
59  typedef U_t UTypeConstRef;
60  typedef U_t UTypeRef;
61  typedef D_t DTypeConstRef;
62  typedef D_t DTypeRef;
63  typedef UD_t UDTypeConstRef;
64  typedef UD_t UDTypeRef;
65 
66  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> ConfigVector_t;
67  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> TangentVector_t;
68 
69  typedef ConfigVector_t ConfigVectorTypeConstRef;
70  typedef ConfigVector_t ConfigVectorTypeRef;
71  typedef TangentVector_t TangentVectorTypeConstRef;
72  typedef TangentVector_t TangentVectorTypeRef;
73 
74  typedef boost::mpl::false_ is_mimicable_t;
75  };
76 
77  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
78  struct traits<JointDataTpl<_Scalar, _Options, JointCollectionTpl>>
79  {
81  typedef typename traits<JointDerived>::Scalar Scalar;
82  };
83 
84  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
85  struct traits<JointModelTpl<_Scalar, _Options, JointCollectionTpl>>
86  {
88  typedef typename traits<JointDerived>::Scalar Scalar;
89  };
90 
91  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
92  struct JointDataTpl
93  : public JointDataBase<JointDataTpl<_Scalar, _Options, JointCollectionTpl>>
94  , JointCollectionTpl<_Scalar, _Options>::JointDataVariant
95  {
96  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
97 
100 
101  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
102 
103  typedef JointCollectionTpl<_Scalar, _Options> JointCollection;
104  typedef typename JointCollection::JointDataVariant JointDataVariant;
105 
106  using Base::operator==;
107  using Base::operator!=;
108 
109  JointDataVariant & toVariant()
110  {
111  return *static_cast<JointDataVariant *>(this);
112  }
113  const JointDataVariant & toVariant() const
114  {
115  return *static_cast<const JointDataVariant *>(this);
116  }
117 
118  ConfigVector_t joint_q() const
119  {
120  return pinocchio::joint_q(*this);
121  }
122  TangentVector_t joint_v() const
123  {
124  return pinocchio::joint_v(*this);
125  }
126  Constraint_t S() const
127  {
128  return joint_motin_subspace_xd(*this);
129  }
130  Transformation_t M() const
131  {
132  return joint_transform(*this);
133  }
134  Motion_t v() const
135  {
136  return motion(*this);
137  }
138  Bias_t c() const
139  {
140  return bias(*this);
141  }
142 
143  // [ABA CCRBA]
144  U_t U() const
145  {
146  return u_inertia(*this);
147  }
148  D_t Dinv() const
149  {
150  return dinv_inertia(*this);
151  }
152  UD_t UDinv() const
153  {
154  return udinv_inertia(*this);
155  }
156  D_t StU() const
157  {
158  return stu_inertia(*this);
159  }
160 
161  JointDataTpl()
162  : JointDataVariant()
163  {
164  }
165 
166  JointDataTpl(const JointDataVariant & jdata_variant)
167  : JointDataVariant(jdata_variant)
168  {
169  }
170 
171  template<typename JointDataDerived>
173  : JointCollection::JointDataVariant((JointDataVariant)jdata.derived())
174  {
175  BOOST_MPL_ASSERT((boost::mpl::contains<typename JointDataVariant::types, JointDataDerived>));
176  }
177 
178  // Define all the standard accessors
179  ConfigVector_t joint_q_accessor() const
180  {
181  return joint_q();
182  }
183  TangentVector_t joint_v_accessor() const
184  {
185  return joint_v();
186  }
187  Constraint_t S_accessor() const
188  {
189  return S();
190  }
191  Transformation_t M_accessor() const
192  {
193  return M();
194  }
195  Motion_t v_accessor() const
196  {
197  return v();
198  }
199  Bias_t c_accessor() const
200  {
201  return c();
202  }
203  U_t U_accessor() const
204  {
205  return U();
206  }
207  D_t Dinv_accessor() const
208  {
209  return Dinv();
210  }
211  UD_t UDinv_accessor() const
212  {
213  return UDinv();
214  }
215  D_t StU_accessor() const
216  {
217  return StU();
218  }
219 
220  static std::string classname()
221  {
222  return "JointData";
223  }
224  std::string shortname() const
225  {
226  return ::pinocchio::shortname(*this);
227  }
228 
229  template<typename JointDataDerived>
230  bool isEqual(const JointDataBase<JointDataDerived> & other) const
231  {
232  return ::pinocchio::isEqual(*this, other.derived());
233  }
234 
235  bool isEqual(const JointDataTpl & other) const
236  {
237  return Base::isEqual(other) && toVariant() == other.toVariant();
238  }
239 
240  bool operator==(const JointDataTpl & other) const
241  {
242  return isEqual(other);
243  }
244 
245  bool operator!=(const JointDataTpl & other) const
246  {
247  return !(*this == other);
248  }
249  };
250 
251  template<
252  typename NewScalar,
253  typename Scalar,
254  int Options,
255  template<typename S, int O> class JointCollectionTpl>
256  struct CastType<NewScalar, JointModelTpl<Scalar, Options, JointCollectionTpl>>
257  {
259  };
260 
261  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
263  : JointModelBase<JointModelTpl<_Scalar, _Options, JointCollectionTpl>>
264  , JointCollectionTpl<_Scalar, _Options>::JointModelVariant
265  {
266  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
267 
269 
270  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
271  PINOCCHIO_JOINT_USE_INDEXES(JointModelTpl);
272 
273  typedef JointCollectionTpl<Scalar, Options> JointCollection;
274  typedef typename JointCollection::JointDataVariant JointDataVariant;
275  typedef typename JointCollection::JointModelVariant JointModelVariant;
276 
277  using Base::id;
278  using Base::setIndexes;
279  using Base::operator==;
280  using Base::operator!=;
281 
282  JointModelTpl()
283  : JointModelVariant()
284  {
285  }
286 
287  JointModelTpl(const JointModelVariant & jmodel_variant)
288  : JointCollection::JointModelVariant(jmodel_variant)
289  {
290  }
291 
292  const std::vector<bool> hasConfigurationLimit() const
293  {
294  return ::pinocchio::hasConfigurationLimit(*this);
295  }
296 
297  const std::vector<bool> hasConfigurationLimitInTangent() const
298  {
299  return ::pinocchio::hasConfigurationLimitInTangent(*this);
300  }
301 
302  template<typename JointModelDerived>
304  : JointModelVariant((JointModelVariant)jmodel.derived())
305  {
306  BOOST_MPL_ASSERT(
307  (boost::mpl::contains<typename JointModelVariant::types, JointModelDerived>));
308  }
309 
310  JointModelVariant & toVariant()
311  {
312  return *static_cast<JointModelVariant *>(this);
313  }
314 
315  const JointModelVariant & toVariant() const
316  {
317  return *static_cast<const JointModelVariant *>(this);
318  }
319 
320  JointDataDerived createData() const
321  {
322  return ::pinocchio::createData<Scalar, Options, JointCollectionTpl>(*this);
323  }
324 
325  template<typename JointModelDerived>
326  bool isEqual(const JointModelBase<JointModelDerived> & other) const
327  {
328  return ::pinocchio::isEqual(*this, other.derived());
329  }
330 
331  template<typename JointModelDerived>
332  bool hasSameIndexes(const JointModelBase<JointModelDerived> & other) const
333  {
334  return ::pinocchio::hasSameIndexes(*this, other.derived());
335  }
336 
337  bool isEqual(const JointModelTpl & other) const
338  {
339  return Base::isEqual(other) && toVariant() == other.toVariant();
340  }
341 
342  bool operator==(const JointModelTpl & other) const
343  {
344  return isEqual(other);
345  }
346 
347  bool operator!=(const JointModelTpl & other) const
348  {
349  return !(*this == other);
350  }
351 
352  template<typename ConfigVector>
353  void calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVector> & q) const
354  {
355  calc_zero_order(*this, data, q.derived());
356  }
357 
358  template<typename TangentVector>
359  void calc(
360  JointDataDerived & data, const Blank blank, const Eigen::MatrixBase<TangentVector> & v) const
361  {
362  calc_first_order(*this, data, blank, v.derived());
363  }
364 
365  template<typename ConfigVector, typename TangentVector>
366  void calc(
367  JointDataDerived & data,
368  const Eigen::MatrixBase<ConfigVector> & q,
369  const Eigen::MatrixBase<TangentVector> & v) const
370  {
371  calc_first_order(*this, data, q.derived(), v.derived());
372  }
373 
374  template<typename VectorLike, typename Matrix6Like>
375  void calc_aba(
376  JointDataDerived & data,
377  const Eigen::MatrixBase<VectorLike> & armature,
378  const Eigen::MatrixBase<Matrix6Like> & I,
379  const bool update_I) const
380  {
381  ::pinocchio::calc_aba(
382  *this, data, armature.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I), update_I);
383  }
384 
385  /* Acces to dedicated segment in robot config space. */
386  // Const access
387  template<typename D>
388  typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
389  JointMappedConfigSelector_impl(const Eigen::MatrixBase<D> & a) const
390  {
391  typedef const Eigen::MatrixBase<D> & InputType;
392  typedef typename SizeDepType<NV>::template SegmentReturn<D>::ConstType ReturnType;
393  typedef JointMappedConfigSelectorVisitor<InputType, ReturnType> Visitor;
394  typename Visitor::ArgsType arg(a);
395  return Visitor::run(*this, arg);
396  }
397 
398  // Non-const access
399  template<typename D>
400  typename SizeDepType<NV>::template SegmentReturn<D>::Type
401  JointMappedConfigSelector_impl(Eigen::MatrixBase<D> & a) const
402  {
403  typedef Eigen::MatrixBase<D> & InputType;
404  typedef typename SizeDepType<NV>::template SegmentReturn<D>::Type ReturnType;
405  typedef JointMappedConfigSelectorVisitor<InputType, ReturnType> Visitor;
406  typename Visitor::ArgsType arg(a);
407  return Visitor::run(*this, arg);
408  }
409 
410  std::string shortname() const
411  {
412  return ::pinocchio::shortname(*this);
413  }
414  static std::string classname()
415  {
416  return "JointModel";
417  }
418 
419  int nq_impl() const
420  {
421  return ::pinocchio::nq(*this);
422  }
423  int nv_impl() const
424  {
425  return ::pinocchio::nv(*this);
426  }
427  int nvExtended_impl() const
428  {
429  return ::pinocchio::nvExtended(*this);
430  }
431 
432  int idx_q_impl() const
433  {
434  return ::pinocchio::idx_q(*this);
435  }
436  int idx_v_impl() const
437  {
438  return ::pinocchio::idx_v(*this);
439  }
440  int idx_vExtended_impl() const
441  {
442  return ::pinocchio::idx_vExtended(*this);
443  }
444 
445  JointIndex id_impl() const
446  {
447  return ::pinocchio::id(*this);
448  }
449 
450  void setIndexes(JointIndex id, int nq, int nv)
451  {
452  ::pinocchio::setIndexes(*this, id, nq, nv, nv);
453  }
454 
455  void setIndexes(JointIndex id, int nq, int nv, int nvExtended)
456  {
457  ::pinocchio::setIndexes(*this, id, nq, nv, nvExtended);
458  }
459 
461  template<typename NewScalar>
463  {
464  return cast_joint<NewScalar, Scalar, Options, JointCollectionTpl>(*this);
465  }
466  };
467 
468  typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector;
469  typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
470 
471  template<
472  typename Scalar,
473  int Options,
474  template<typename S, int O> class JointCollectionTpl,
475  typename JointDataDerived>
476  bool operator==(
477  const JointDataBase<JointDataDerived> & joint_data,
478  const JointDataTpl<Scalar, Options, JointCollectionTpl> & joint_data_generic)
479  {
480  return joint_data_generic == joint_data.derived();
481  }
482 
483  template<
484  typename Scalar,
485  int Options,
486  template<typename S, int O> class JointCollectionTpl,
487  typename JointDataDerived>
488  bool operator!=(
489  const JointDataBase<JointDataDerived> & joint_data,
490  const JointDataTpl<Scalar, Options, JointCollectionTpl> & joint_data_generic)
491  {
492  return joint_data_generic != joint_data.derived();
493  }
494 
495  template<
496  typename Scalar,
497  int Options,
498  template<typename S, int O> class JointCollectionTpl,
499  typename JointModelDerived>
500  bool operator==(
501  const JointModelBase<JointModelDerived> & joint_model,
502  const JointModelTpl<Scalar, Options, JointCollectionTpl> & joint_model_generic)
503  {
504  return joint_model_generic == joint_model.derived();
505  }
506 
507  template<
508  typename Scalar,
509  int Options,
510  template<typename S, int O> class JointCollectionTpl,
511  typename JointModelDerived>
512  bool operator!=(
513  const JointModelBase<JointModelDerived> & joint_model,
514  const JointModelTpl<Scalar, Options, JointCollectionTpl> & joint_model_generic)
515  {
516  return joint_model_generic != joint_model.derived();
517  }
518 
519 } // namespace pinocchio
520 
521 #endif // ifndef __pinocchio_multibody_joint_generic_hpp__
Main pinocchio namespace.
Definition: treeview.dox:11
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > u_inertia(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointUInertiaVisitor to get the U matrix of the inertia matrix decomposi...
MotionTpl< Scalar, Options > motion(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointMotionVisitor to get the joint internal motion as a dense motion.
MotionTpl< Scalar, Options > bias(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointBiasVisitor to get the joint bias as a dense motion.
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > stu_inertia(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointStUInertiaVisitor to get St*I*S matrix of the inertia matrix decomp...
JointMotionSubspaceTpl< Eigen::Dynamic, Scalar, Options > joint_motin_subspace_xd(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataVariant through JointConstraintVisitor to get the joint constraint as a dense constr...
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > dinv_inertia(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointDInvInertiaVisitor to get the D^{-1} matrix of the inertia matrix d...
SE3Tpl< Scalar, Options > joint_transform(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointTransformVisitor to get the joint internal transform (transform bet...
void calc_first_order(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Visit a JointModelTpl and the corresponding JointDataTpl through JointCalcFirstOrderVisitor to comput...
int nvExtended(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvExtendVisitor to get the dimension of the joint extended tangent...
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > udinv_inertia(const JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata)
Visit a JointDataTpl through JointUDInvInertiaVisitor to get U*D^{-1} matrix of the inertia matrix de...
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.
void calc_zero_order(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, const Eigen::MatrixBase< ConfigVectorType > &q)
Visit a JointModelTpl and the corresponding JointDataTpl through JointCalcZeroOrderVisitor to compute...
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space.
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.
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space.
Blank type.
Definition: fwd.hpp:77
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
Definition: fwd.hpp:99
bool isEqual(const JointDataBase< Derived > &other) const
&#160;
JointModelTpl< NewScalar, Options, JointCollectionTpl > cast() const
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:72