pinocchio  3.3.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-model-base.hpp
1 //
2 // Copyright (c) 2015-2019 CNRS INRIA
3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_multibody_joint_model_base_hpp__
7 #define __pinocchio_multibody_joint_model_base_hpp__
8 
9 #include "pinocchio/multibody/joint/joint-base.hpp"
10 #include "pinocchio/multibody/joint/joint-common-operations.hpp"
11 
12 #include "pinocchio/math/matrix-block.hpp"
13 
14 #include <limits>
15 
16 #define PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, TYPENAME) \
17  typedef Eigen::DenseIndex Index; \
18  typedef TYPENAME traits<Joint>::Scalar Scalar; \
19  typedef TYPENAME traits<Joint>::JointDataDerived JointDataDerived; \
20  typedef TYPENAME traits<Joint>::JointModelDerived JointModelDerived; \
21  typedef TYPENAME traits<Joint>::Constraint_t Constraint_t; \
22  typedef TYPENAME traits<Joint>::Transformation_t Transformation_t; \
23  typedef TYPENAME traits<Joint>::Motion_t Motion_t; \
24  typedef TYPENAME traits<Joint>::Bias_t Bias_t; \
25  typedef TYPENAME traits<Joint>::U_t U_t; \
26  typedef TYPENAME traits<Joint>::D_t D_t; \
27  typedef TYPENAME traits<Joint>::UD_t UD_t; \
28  enum \
29  { \
30  Options = traits<Joint>::Options, \
31  NQ = traits<Joint>::NQ, \
32  NV = traits<Joint>::NV \
33  }; \
34  typedef TYPENAME traits<Joint>::ConfigVector_t ConfigVector_t; \
35  typedef TYPENAME traits<Joint>::TangentVector_t TangentVector_t
36 
37 #ifdef __clang__
38 
39  #define PINOCCHIO_JOINT_TYPEDEF(Joint) \
40  PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, PINOCCHIO_EMPTY_ARG)
41  #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) \
42  PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename)
43 
44 #elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2)
45 
46  #define PINOCCHIO_JOINT_TYPEDEF(Joint) \
47  PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, PINOCCHIO_EMPTY_ARG)
48  #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) \
49  PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename)
50 
51 #else
52 
53  #define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename)
54  #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) \
55  PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename)
56 
57 #endif
58 
59 #define PINOCCHIO_JOINT_USE_INDEXES(Joint) \
60  typedef JointModelBase<Joint> Base; \
61  using Base::idx_q; \
62  using Base::idx_v
63 
64 #define PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTpl) \
65  template<typename Scalar, int Options, typename NewScalar> \
66  struct CastType<NewScalar, JointModelTpl<Scalar, Options>> \
67  { \
68  typedef JointModelTpl<NewScalar, Options> type; \
69  }
70 
71 namespace pinocchio
72 {
73 
74  template<typename Derived>
75  struct JointModelBase : NumericalBase<Derived>
76  {
77  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
78 
79  typedef typename traits<Derived>::JointDerived JointDerived;
80  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
81 
82  JointModelDerived & derived()
83  {
84  return *static_cast<Derived *>(this);
85  }
86  const JointModelDerived & derived() const
87  {
88  return *static_cast<const Derived *>(this);
89  }
90 
91  JointDataDerived createData() const
92  {
93  return derived().createData();
94  }
95 
96  const std::vector<bool> hasConfigurationLimit() const
97  {
98  return derived().hasConfigurationLimit();
99  }
100 
101  const std::vector<bool> hasConfigurationLimitInTangent() const
102  {
103  return derived().hasConfigurationLimitInTangent();
104  }
105 
106  template<typename ConfigVectorType>
107  void calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const
108  {
109  derived().calc(data, qs.derived());
110  }
111 
112  template<typename TangentVectorType>
113  void calc(
114  JointDataDerived & data,
115  const Blank not_used,
116  const Eigen::MatrixBase<TangentVectorType> & vs) const
117  {
118  derived().calc(data, not_used, vs.derived());
119  }
120 
121  template<typename ConfigVectorType, typename TangentVectorType>
122  void calc(
123  JointDataDerived & data,
124  const Eigen::MatrixBase<ConfigVectorType> & qs,
125  const Eigen::MatrixBase<TangentVectorType> & vs) const
126  {
127  derived().calc(data, qs.derived(), vs.derived());
128  }
129 
130  template<typename VectorLike, typename Matrix6Like>
131  void calc_aba(
132  JointDataDerived & data,
133  const Eigen::MatrixBase<VectorLike> & armature,
134  const Eigen::MatrixBase<Matrix6Like> & I,
135  const bool update_I = false) const
136  {
137  derived().calc_aba(
138  data, armature.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I), update_I);
139  }
140 
141  int nv() const
142  {
143  return derived().nv_impl();
144  }
145  int nq() const
146  {
147  return derived().nq_impl();
148  }
149 
150  // Default _impl methods are reimplemented by dynamic-size joints.
151  int nv_impl() const
152  {
153  return NV;
154  }
155  int nq_impl() const
156  {
157  return NQ;
158  }
159 
160  int idx_q() const
161  {
162  return derived().idx_q_impl();
163  }
164  int idx_v() const
165  {
166  return derived().idx_v_impl();
167  }
168  JointIndex id() const
169  {
170  return derived().id_impl();
171  }
172 
173  int idx_q_impl() const
174  {
175  return i_q;
176  }
177  int idx_v_impl() const
178  {
179  return i_v;
180  }
181  JointIndex id_impl() const
182  {
183  return i_id;
184  }
185 
186  void setIndexes(JointIndex id, int q, int v)
187  {
188  derived().setIndexes_impl(id, q, v);
189  }
190 
191  void setIndexes_impl(JointIndex id, int q, int v)
192  {
193  i_id = id, i_q = q;
194  i_v = v;
195  }
196 
197  void disp(std::ostream & os) const
198  {
199  using namespace std;
200  os << shortname() << endl
201  << " index: " << id() << endl
202  << " index q: " << idx_q() << endl
203  << " index v: " << idx_v() << endl
204  << " nq: " << nq() << endl
205  << " nv: " << nv() << endl;
206  }
207 
208  friend std::ostream & operator<<(std::ostream & os, const JointModelBase<Derived> & joint)
209  {
210  joint.disp(os);
211  return os;
212  }
213 
214  std::string shortname() const
215  {
216  return derived().shortname();
217  }
218  static std::string classname()
219  {
220  return Derived::classname();
221  }
222 
223  template<typename NewScalar>
224  typename CastType<NewScalar, Derived>::type cast() const
225  {
226  return derived().template cast<NewScalar>();
227  }
228 
229  template<class OtherDerived>
230  bool operator==(const JointModelBase<OtherDerived> & other) const
231  {
232  return derived().isEqual(other.derived());
233  }
234 
235  template<class OtherDerived>
236  bool operator!=(const JointModelBase<OtherDerived> & other) const
237  {
238  return !(internal::comparison_eq(derived(), other.derived()));
239  }
240 
241  template<class OtherDerived>
242  bool isEqual(const JointModelBase<OtherDerived> &) const
243  {
244  return false;
245  }
246 
247  bool isEqual(const JointModelBase<Derived> & other) const
248  {
249  return derived().hasSameIndexes(other.derived());
250  }
251 
252  template<class OtherDerived>
253  bool hasSameIndexes(const JointModelBase<OtherDerived> & other) const
254  {
255  return internal::comparison_eq(other.id(), id())
256  && internal::comparison_eq(other.idx_q(), idx_q())
257  && internal::comparison_eq(other.idx_v(), idx_v());
258  }
259 
260  /* Acces to dedicated segment in robot config space. */
261  // Const access
262  template<typename D>
263  typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
264  jointConfigSelector(const Eigen::MatrixBase<D> & a) const
265  {
266  return derived().jointConfigSelector_impl(a);
267  }
268 
269  template<typename D>
270  typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
271  jointConfigSelector_impl(const Eigen::MatrixBase<D> & a) const
272  {
273  return SizeDepType<NQ>::segment(a.derived(), idx_q(), nq());
274  }
275 
276  // Non-const access
277  template<typename D>
278  typename SizeDepType<NQ>::template SegmentReturn<D>::Type
279  jointConfigSelector(Eigen::MatrixBase<D> & a) const
280  {
281  return derived().jointConfigSelector_impl(a);
282  }
283 
284  template<typename D>
285  typename SizeDepType<NQ>::template SegmentReturn<D>::Type
286  jointConfigSelector_impl(Eigen::MatrixBase<D> & a) const
287  {
288  return SizeDepType<NQ>::segment(a, idx_q(), nq());
289  }
290 
291  /* Acces to dedicated segment in robot config velocity space. */
292  // Const access
293  template<typename D>
294  typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
295  jointVelocitySelector(const Eigen::MatrixBase<D> & a) const
296  {
297  return derived().jointVelocitySelector_impl(a.derived());
298  }
299 
300  template<typename D>
301  typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
302  jointVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const
303  {
304  return SizeDepType<NV>::segment(a.derived(), idx_v(), nv());
305  }
306 
307  // Non-const access
308  template<typename D>
309  typename SizeDepType<NV>::template SegmentReturn<D>::Type
310  jointVelocitySelector(Eigen::MatrixBase<D> & a) const
311  {
312  return derived().jointVelocitySelector_impl(a.derived());
313  }
314 
315  template<typename D>
316  typename SizeDepType<NV>::template SegmentReturn<D>::Type
317  jointVelocitySelector_impl(Eigen::MatrixBase<D> & a) const
318  {
319  return SizeDepType<NV>::segment(a.derived(), idx_v(), nv());
320  }
321 
322  /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/
323  // Const access
324  template<typename D>
325  typename SizeDepType<NV>::template ColsReturn<D>::ConstType
326  jointCols(const Eigen::MatrixBase<D> & A) const
327  {
328  return derived().jointCols_impl(A.derived());
329  }
330 
331  template<typename D>
332  typename SizeDepType<NV>::template ColsReturn<D>::ConstType
333  jointCols_impl(const Eigen::MatrixBase<D> & A) const
334  {
335  return SizeDepType<NV>::middleCols(A.derived(), idx_v(), nv());
336  }
337 
338  // Non-const access
339  template<typename D>
340  typename SizeDepType<NV>::template ColsReturn<D>::Type jointCols(Eigen::MatrixBase<D> & A) const
341  {
342  return derived().jointCols_impl(A.derived());
343  }
344 
345  template<typename D>
346  typename SizeDepType<NV>::template ColsReturn<D>::Type
347  jointCols_impl(Eigen::MatrixBase<D> & A) const
348  {
349  return SizeDepType<NV>::middleCols(A.derived(), idx_v(), nv());
350  }
351 
352  /* Acces to dedicated rows in a matrix.*/
353  // Const access
354  template<typename D>
355  typename SizeDepType<NV>::template RowsReturn<D>::ConstType
356  jointRows(const Eigen::MatrixBase<D> & A) const
357  {
358  return derived().jointRows_impl(A.derived());
359  }
360 
361  template<typename D>
362  typename SizeDepType<NV>::template RowsReturn<D>::ConstType
363  jointRows_impl(const Eigen::MatrixBase<D> & A) const
364  {
365  return SizeDepType<NV>::middleRows(A.derived(), idx_v(), nv());
366  }
367 
368  // Non-const access
369  template<typename D>
370  typename SizeDepType<NV>::template RowsReturn<D>::Type jointRows(Eigen::MatrixBase<D> & A) const
371  {
372  return derived().jointRows_impl(A.derived());
373  }
374 
375  template<typename D>
376  typename SizeDepType<NV>::template RowsReturn<D>::Type
377  jointRows_impl(Eigen::MatrixBase<D> & A) const
378  {
379  return SizeDepType<NV>::middleRows(A.derived(), idx_v(), nv());
380  }
381 
384  // Const access
385  template<typename D>
386  typename SizeDepType<NV>::template BlockReturn<D>::ConstType
387  jointBlock(const Eigen::MatrixBase<D> & Mat) const
388  {
389  return derived().jointBlock_impl(Mat.derived());
390  }
391 
392  template<typename D>
393  typename SizeDepType<NV>::template BlockReturn<D>::ConstType
394  jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const
395  {
396  return SizeDepType<NV>::block(Mat.derived(), idx_v(), idx_v(), nv(), nv());
397  }
398 
399  // Non-const access
400  template<typename D>
401  typename SizeDepType<NV>::template BlockReturn<D>::Type
402  jointBlock(Eigen::MatrixBase<D> & Mat) const
403  {
404  return derived().jointBlock_impl(Mat.derived());
405  }
406 
407  template<typename D>
408  typename SizeDepType<NV>::template BlockReturn<D>::Type
409  jointBlock_impl(Eigen::MatrixBase<D> & Mat) const
410  {
411  return SizeDepType<NV>::block(Mat.derived(), idx_v(), idx_v(), nv(), nv());
412  }
413 
414  protected:
418  inline JointModelBase()
419  : i_id(std::numeric_limits<JointIndex>::max())
420  , i_q(-1)
421  , i_v(-1)
422  {
423  }
424 
429  inline JointModelBase(const JointModelBase & clone)
430  {
431  *this = clone;
432  }
433 
438  inline JointModelBase & operator=(const JointModelBase & clone)
439  {
440  i_id = clone.i_id;
441  i_q = clone.i_q;
442  i_v = clone.i_v;
443  return *this;
444  }
445 
446  // data
447  JointIndex i_id; // ID of the joint in the multibody list.
448  int i_q; // Index of the joint configuration in the joint configuration vector.
449  int i_v; // Index of the joint velocity in the joint velocity vector.
450 
451  }; // struct JointModelBase
452 
453 } // namespace pinocchio
454 
455 #endif // ifndef __pinocchio_multibody_joint_model_base_hpp__
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::id
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain.
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::nv
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space.
pinocchio::nq
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space.
pinocchio::shortname
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model.
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::Blank
Blank type.
Definition: fwd.hpp:77
pinocchio::CastType
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
Definition: fwd.hpp:99
pinocchio::JointModelBase::JointModelBase
JointModelBase(const JointModelBase &clone)
Definition: joint-model-base.hpp:429
pinocchio::JointModelBase::jointBlock
SizeDepType< NV >::template BlockReturn< D >::ConstType jointBlock(const Eigen::MatrixBase< D > &Mat) const
Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the matrix Mat.
Definition: joint-model-base.hpp:387
pinocchio::JointModelBase::operator=
JointModelBase & operator=(const JointModelBase &clone)
Definition: joint-model-base.hpp:438
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:72