pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
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 typedef TYPENAME traits<Joint>::is_mimicable_t is_mimicable_t; \
29 enum \
30 { \
31 Options = traits<Joint>::Options, \
32 NQ = traits<Joint>::NQ, \
33 NV = traits<Joint>::NV, \
34 NVExtended = traits<Joint>::NVExtended \
35 }; \
36 typedef TYPENAME traits<Joint>::ConfigVector_t ConfigVector_t; \
37 typedef TYPENAME traits<Joint>::TangentVector_t TangentVector_t
38
39#ifdef __clang__
40
41 #define PINOCCHIO_JOINT_TYPEDEF(Joint) \
42 PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, PINOCCHIO_EMPTY_ARG)
43 #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) \
44 PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename)
45
46#elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2)
47
48 #define PINOCCHIO_JOINT_TYPEDEF(Joint) \
49 PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, PINOCCHIO_EMPTY_ARG)
50 #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) \
51 PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename)
52
53#else
54
55 #define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename)
56 #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) \
57 PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, typename)
58
59#endif
60
61#define PINOCCHIO_JOINT_USE_INDEXES(Joint) \
62 typedef JointModelBase<Joint> Base; \
63 using Base::idx_q; \
64 using Base::idx_v; \
65 using Base::idx_vExtended
66
67#define PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTpl) \
68 template<typename Scalar, int Options, typename NewScalar> \
69 struct CastType<NewScalar, JointModelTpl<Scalar, Options>> \
70 { \
71 typedef JointModelTpl<NewScalar, Options> type; \
72 }
73
74namespace pinocchio
75{
76
77 template<typename Derived>
78 struct JointModelBase : NumericalBase<Derived>
79 {
80 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
81
82 typedef typename traits<Derived>::JointDerived JointDerived;
83 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
84
85 JointModelDerived & derived()
86 {
87 return *static_cast<Derived *>(this);
88 }
89 const JointModelDerived & derived() const
90 {
91 return *static_cast<const Derived *>(this);
92 }
93
94 JointDataDerived createData() const
95 {
96 return derived().createData();
97 }
98
99 const std::vector<bool> hasConfigurationLimit() const
100 {
101 return derived().hasConfigurationLimit();
102 }
103
104 const std::vector<bool> hasConfigurationLimitInTangent() const
105 {
106 return derived().hasConfigurationLimitInTangent();
107 }
108
109 template<typename ConfigVectorType>
110 void calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const
111 {
112 derived().calc(data, qs.derived());
113 }
114
115 template<typename TangentVectorType>
116 void calc(
117 JointDataDerived & data,
118 const Blank not_used,
119 const Eigen::MatrixBase<TangentVectorType> & vs) const
120 {
121 derived().calc(data, not_used, vs.derived());
122 }
123
124 template<typename ConfigVectorType, typename TangentVectorType>
125 void calc(
126 JointDataDerived & data,
127 const Eigen::MatrixBase<ConfigVectorType> & qs,
128 const Eigen::MatrixBase<TangentVectorType> & vs) const
129 {
130 derived().calc(data, qs.derived(), vs.derived());
131 }
132
133 template<typename VectorLike, typename Matrix6Like>
134 void calc_aba(
135 JointDataDerived & data,
136 const Eigen::MatrixBase<VectorLike> & armature,
137 const Eigen::MatrixBase<Matrix6Like> & I,
138 const bool update_I = false) const
139 {
140 derived().calc_aba(
141 data, armature.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I), update_I);
142 }
143
144 int nv() const
145 {
146 return derived().nv_impl();
147 }
148 int nq() const
149 {
150 return derived().nq_impl();
151 }
152 int nvExtended() const
153 {
154 return derived().nvExtended_impl();
155 }
156
157 // Default _impl methods are reimplemented by dynamic-size joints.
158 int nv_impl() const
159 {
160 return NV;
161 }
162 int nq_impl() const
163 {
164 return NQ;
165 }
166 int nvExtended_impl() const
167 {
168 return NVExtended;
169 }
170
171 int idx_q() const
172 {
173 return derived().idx_q_impl();
174 }
175 int idx_v() const
176 {
177 return derived().idx_v_impl();
178 }
179 int idx_vExtended() const
180 {
181 return derived().idx_vExtended_impl();
182 }
183 JointIndex id() const
184 {
185 return derived().id_impl();
186 }
187
188 int idx_q_impl() const
189 {
190 return i_q;
191 }
192 int idx_v_impl() const
193 {
194 return i_v;
195 }
196 int idx_vExtended_impl() const
197 {
198 return i_vExtended;
199 }
200 JointIndex id_impl() const
201 {
202 return i_id;
203 }
204
205 void setIndexes(JointIndex id, int q, int v)
206 {
207 derived().setIndexes_impl(id, q, v, v);
208 }
209
210 void setIndexes(JointIndex id, int q, int v, int vExtended)
211 {
212 derived().setIndexes_impl(id, q, v, vExtended);
213 }
214
215 void setIndexes_impl(JointIndex id, int q, int v, int vExtended)
216 {
217 i_id = id, i_q = q;
218 i_v = v;
219 i_vExtended = vExtended;
220 }
221
222 void disp(std::ostream & os) const
223 {
224 using namespace std;
225 os << shortname() << endl
226 << " index: " << id() << endl
227 << " index q: " << idx_q() << endl
228 << " index v: " << idx_v() << endl
229 << " index vExtended: " << idx_vExtended() << endl
230 << " nq: " << nq() << endl
231 << " nv: " << nv() << endl
232 << " nvExtended: " << nvExtended() << endl;
233 }
234
235 friend std::ostream & operator<<(std::ostream & os, const JointModelBase<Derived> & jmodel)
236 {
237 jmodel.derived().disp(os);
238 return os;
239 }
240
241 std::string shortname() const
242 {
243 return derived().shortname();
244 }
245 static std::string classname()
246 {
247 return Derived::classname();
248 }
249
250 template<typename NewScalar>
251 typename CastType<NewScalar, Derived>::type cast() const
252 {
253 return derived().template cast<NewScalar>();
254 }
255
256 template<class OtherDerived>
257 bool operator==(const JointModelBase<OtherDerived> & other) const
258 {
259 return derived().isEqual(other.derived());
260 }
261
262 template<class OtherDerived>
263 bool operator!=(const JointModelBase<OtherDerived> & other) const
264 {
265 return !(internal::comparison_eq(derived(), other.derived()));
266 }
267
268 template<class OtherDerived>
269 bool isEqual(const JointModelBase<OtherDerived> &) const
270 {
271 return false;
272 }
273
274 bool isEqual(const JointModelBase<Derived> & other) const
275 {
276 return derived().hasSameIndexes(other.derived());
277 }
278
279 template<class OtherDerived>
280 bool hasSameIndexes(const JointModelBase<OtherDerived> & other) const
281 {
282 return internal::comparison_eq(other.id(), id())
283 && internal::comparison_eq(other.idx_q(), idx_q())
284 && internal::comparison_eq(other.idx_v(), idx_v())
285 && internal::comparison_eq(other.idx_vExtended(), idx_vExtended());
286 }
287
288 /* Acces to dedicated segment in robot config space. */
289 // Const access
290 template<typename D>
291 typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
292 JointMappedConfigSelector(const Eigen::MatrixBase<D> & a) const
293 {
294 return derived().JointMappedConfigSelector_impl(a);
295 }
296
297 template<typename D>
298 typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
299 JointMappedConfigSelector_impl(const Eigen::MatrixBase<D> & a) const
300 {
301 return SizeDepType<NQ>::segment(a.derived(), idx_q(), nq());
302 }
303
304 // Non-const access
305 template<typename D>
306 typename SizeDepType<NQ>::template SegmentReturn<D>::Type
307 JointMappedConfigSelector(Eigen::MatrixBase<D> & a) const
308 {
309 return derived().JointMappedConfigSelector_impl(a);
310 }
311
312 template<typename D>
313 typename SizeDepType<NQ>::template SegmentReturn<D>::Type
314 JointMappedConfigSelector_impl(Eigen::MatrixBase<D> & a) const
315 {
316 return SizeDepType<NQ>::segment(a, idx_q(), nq());
317 }
318
319 // Const access
320 template<typename D>
321 typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
322 jointConfigSelector(const Eigen::MatrixBase<D> & a) const
323 {
324 return derived().jointConfigSelector_impl(a);
325 }
326
327 template<typename D>
328 typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
329 jointConfigSelector_impl(const Eigen::MatrixBase<D> & a) const
330 {
331 return SizeDepType<NQ>::segment(a.derived(), idx_q(), nq());
332 }
333
334 // Non-const access
335 template<typename D>
336 typename SizeDepType<NQ>::template SegmentReturn<D>::Type
337 jointConfigSelector(Eigen::MatrixBase<D> & a) const
338 {
339 return derived().jointConfigSelector_impl(a);
340 }
341
342 template<typename D>
343 typename SizeDepType<NQ>::template SegmentReturn<D>::Type
344 jointConfigSelector_impl(Eigen::MatrixBase<D> & a) const
345 {
346 return SizeDepType<NQ>::segment(a, idx_q(), nq());
347 }
348
349 /* Acces to dedicated segment in robot config velocity space. */
350 // Const access
351 template<typename D>
352 typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
353 JointMappedVelocitySelector(const Eigen::MatrixBase<D> & a) const
354 {
355 return derived().JointMappedVelocitySelector_impl(a.derived());
356 }
357
358 template<typename D>
359 typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
360 JointMappedVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const
361 {
362 return SizeDepType<NV>::segment(a.derived(), idx_v(), nvExtended());
363 }
364
365 // Non-const access
366 template<typename D>
367 typename SizeDepType<NV>::template SegmentReturn<D>::Type
368 JointMappedVelocitySelector(Eigen::MatrixBase<D> & a) const
369 {
370 return derived().JointMappedVelocitySelector_impl(a.derived());
371 }
372
373 template<typename D>
374 typename SizeDepType<NV>::template SegmentReturn<D>::Type
375 JointMappedVelocitySelector_impl(Eigen::MatrixBase<D> & a) const
376 {
377 return SizeDepType<NV>::segment(a.derived(), idx_v(), nvExtended());
378 }
379
380 // Const access
381 template<typename D>
382 typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
383 jointVelocitySelector(const Eigen::MatrixBase<D> & a) const
384 {
385 return derived().jointVelocitySelector_impl(a.derived());
386 }
387
388 template<typename D>
389 typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
390 jointVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const
391 {
392 return SizeDepType<NV>::segment(a.derived(), idx_v(), nv());
393 }
394
395 // Non-const access
396 template<typename D>
397 typename SizeDepType<NV>::template SegmentReturn<D>::Type
398 jointVelocitySelector(Eigen::MatrixBase<D> & a) const
399 {
400 return derived().jointVelocitySelector_impl(a.derived());
401 }
402
403 template<typename D>
404 typename SizeDepType<NV>::template SegmentReturn<D>::Type
405 jointVelocitySelector_impl(Eigen::MatrixBase<D> & a) const
406 {
407 return SizeDepType<NV>::segment(a.derived(), idx_v(), nv());
408 }
409
410 /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/
411 // Const access
412 template<typename D>
413 typename SizeDepType<NV>::template ColsReturn<D>::ConstType
414 jointCols(const Eigen::MatrixBase<D> & A) const
415 {
416 return derived().jointCols_impl(A.derived());
417 }
418
419 template<typename D>
420 typename SizeDepType<NVExtended>::template ColsReturn<D>::ConstType
421 jointExtendedModelCols(const Eigen::MatrixBase<D> & A) const
422 {
423 return derived().jointExtendedModelCols_impl(A.derived());
424 }
425
426 template<typename D>
427 typename SizeDepType<NV>::template ColsReturn<D>::ConstType
428 jointCols_impl(const Eigen::MatrixBase<D> & A) const
429 {
430 return SizeDepType<NV>::middleCols(A.derived(), idx_v(), nv());
431 }
432
433 template<typename D>
434 typename SizeDepType<NVExtended>::template ColsReturn<D>::ConstType
435 jointExtendedModelCols_impl(const Eigen::MatrixBase<D> & A) const
436 {
437 return SizeDepType<NVExtended>::middleCols(A.derived(), idx_vExtended(), nvExtended());
438 }
439
440 // Non-const access
441 // TODO rename Jac/Vel into Full/Red (for full system and reduced system ?)
442 template<typename D>
443 typename SizeDepType<NV>::template ColsReturn<D>::Type jointCols(Eigen::MatrixBase<D> & A) const
444 {
445 return derived().jointCols_impl(A.derived());
446 }
447
448 template<typename D>
449 typename SizeDepType<NVExtended>::template ColsReturn<D>::Type
450 jointExtendedModelCols(Eigen::MatrixBase<D> & A) const
451 {
452 return derived().jointExtendedModelCols_impl(A.derived());
453 }
454
455 template<typename D>
456 typename SizeDepType<NV>::template ColsReturn<D>::Type
457 jointCols_impl(Eigen::MatrixBase<D> & A) const
458 {
459 return SizeDepType<NV>::middleCols(A.derived(), idx_v(), nv());
460 }
461
462 template<typename D>
463 typename SizeDepType<NVExtended>::template ColsReturn<D>::Type
464 jointExtendedModelCols_impl(Eigen::MatrixBase<D> & A) const
465 {
466 return SizeDepType<NVExtended>::middleCols(A.derived(), idx_vExtended(), nvExtended());
467 }
468
469 /* Acces to dedicated rows in a matrix.*/
470 // Const access
471 template<typename D>
472 typename SizeDepType<NV>::template RowsReturn<D>::ConstType
473 jointRows(const Eigen::MatrixBase<D> & A) const
474 {
475 return derived().jointRows_impl(A.derived());
476 }
477
478 template<typename D>
479 typename SizeDepType<NVExtended>::template RowsReturn<D>::ConstType
480 jointExtendedModelRows(const Eigen::MatrixBase<D> & A) const
481 {
482 return derived().jointExtendedModelRows_impl(A.derived());
483 }
484
485 template<typename D>
486 typename SizeDepType<NV>::template RowsReturn<D>::ConstType
487 jointRows_impl(const Eigen::MatrixBase<D> & A) const
488 {
489 return SizeDepType<NV>::middleRows(A.derived(), idx_v(), nv());
490 }
491
492 template<typename D>
493 typename SizeDepType<NVExtended>::template RowsReturn<D>::ConstType
494 jointExtendedModelRows_impl(const Eigen::MatrixBase<D> & A) const
495 {
496 return SizeDepType<NVExtended>::middleRows(A.derived(), idx_vExtended(), nvExtended());
497 }
498
499 // Non-const access
500 template<typename D>
501 typename SizeDepType<NV>::template RowsReturn<D>::Type jointRows(Eigen::MatrixBase<D> & A) const
502 {
503 return derived().jointRows_impl(A.derived());
504 }
505
506 template<typename D>
507 typename SizeDepType<NVExtended>::template RowsReturn<D>::Type
508 jointExtendedModelRows(Eigen::MatrixBase<D> & A) const
509 {
510 return derived().jointExtendedModelRows_impl(A.derived());
511 }
512
513 template<typename D>
514 typename SizeDepType<NV>::template RowsReturn<D>::Type
515 jointRows_impl(Eigen::MatrixBase<D> & A) const
516 {
517 return SizeDepType<NV>::middleRows(A.derived(), idx_v(), nv());
518 }
519
520 template<typename D>
521 typename SizeDepType<NVExtended>::template RowsReturn<D>::Type
522 jointExtendedModelRows_impl(Eigen::MatrixBase<D> & A) const
523 {
524 return SizeDepType<NVExtended>::middleRows(A.derived(), idx_vExtended(), nvExtended());
525 }
526
529 // Const access
530 template<typename D>
531 typename SizeDepType<NV>::template BlockReturn<D>::ConstType
532 jointBlock(const Eigen::MatrixBase<D> & Mat) const
533 {
534 return derived().jointBlock_impl(Mat.derived());
535 }
536
537 template<typename D>
538 typename SizeDepType<NVExtended>::template BlockReturn<D>::ConstType
539 jointExtendedModelBlock(const Eigen::MatrixBase<D> & Mat) const
540 {
541 return derived().jointExtendedModelBlock_impl(Mat.derived());
542 }
543
544 template<typename D>
545 typename SizeDepType<NV>::template BlockReturn<D>::ConstType
546 jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const
547 {
548 return SizeDepType<NV>::block(Mat.derived(), idx_v(), idx_v(), nv(), nv());
549 }
550
551 template<typename D>
552 typename SizeDepType<NVExtended>::template BlockReturn<D>::ConstType
553 jointExtendedModelBlock_impl(const Eigen::MatrixBase<D> & Mat) const
554 {
555 return SizeDepType<NVExtended>::block(
556 Mat.derived(), idx_vExtended(), idx_vExtended(), nvExtended(), nvExtended());
557 }
558
559 // Non-const access
560 template<typename D>
561 typename SizeDepType<NV>::template BlockReturn<D>::Type
562 jointBlock(Eigen::MatrixBase<D> & Mat) const
563 {
564 return derived().jointBlock_impl(Mat.derived());
565 }
566
567 template<typename D>
568 typename SizeDepType<NVExtended>::template BlockReturn<D>::Type
569 jointExtendedModelBlock(Eigen::MatrixBase<D> & Mat) const
570 {
571 return derived().jointExtendedModelBlock_impl(Mat.derived());
572 }
573
574 template<typename D>
575 typename SizeDepType<NV>::template BlockReturn<D>::Type
576 jointBlock_impl(Eigen::MatrixBase<D> & Mat) const
577 {
578 return SizeDepType<NV>::block(Mat.derived(), idx_v(), idx_v(), nv(), nv());
579 }
580
581 template<typename D>
582 typename SizeDepType<NVExtended>::template BlockReturn<D>::Type
583 jointExtendedModelBlock_impl(Eigen::MatrixBase<D> & Mat) const
584 {
585 return SizeDepType<NVExtended>::block(
586 Mat.derived(), idx_vExtended(), idx_vExtended(), nvExtended(), nvExtended());
587 }
588
589 protected:
593 inline JointModelBase()
594 : i_id(std::numeric_limits<JointIndex>::max())
595 , i_q(-1)
596 , i_v(-1)
597 , i_vExtended(-1)
598 {
599 }
600
605 inline JointModelBase(const JointModelBase & clone)
606 {
607 *this = clone;
608 }
609
614 inline JointModelBase & operator=(const JointModelBase & clone)
615 {
616 i_id = clone.i_id;
617 i_q = clone.i_q;
618 i_v = clone.i_v;
619 i_vExtended = clone.i_vExtended;
620 return *this;
621 }
622
623 // data
624 JointIndex i_id; // ID of the joint in the multibody list.
625 int i_q; // Index of the joint configuration in the joint configuration vector.
626 int i_v; // Index of the joint velocity in the joint velocity vector.
627 int i_vExtended; // Index of the joint jacobian in the joint jacobian matrix.
628
629 }; // struct JointModelBase
630
631} // namespace pinocchio
632
633#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::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:605
pinocchio::JointModelBase::operator=
JointModelBase & operator=(const JointModelBase &clone)
Definition joint-model-base.hpp:614
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:532
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72