pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
joint-composite.hpp
1//
2// Copyright (c) 2016-2021 CNRS INRIA
3//
4
5#ifndef __pinocchio_multibody_joint_composite_hpp__
6#define __pinocchio_multibody_joint_composite_hpp__
7
8#include "pinocchio/multibody/joint/fwd.hpp"
9#include "pinocchio/multibody/joint/joint-collection.hpp"
10#include "pinocchio/multibody/joint/joint-basic-visitors.hpp"
11#include "pinocchio/container/aligned-vector.hpp"
12#include "pinocchio/spatial/act-on-set.hpp"
13
14#include "pinocchio/serialization/fwd.hpp"
15
16namespace pinocchio
17{
18
19 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
20 struct JointCompositeTpl;
21
22 template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
23 struct traits<JointCompositeTpl<_Scalar, _Options, JointCollectionTpl>>
24 {
25 typedef _Scalar Scalar;
26
27 enum
28 {
29 Options = _Options,
30 NQ = Eigen::Dynamic,
31 NV = Eigen::Dynamic,
32 NVExtended = Eigen::Dynamic
33 };
34
35 typedef JointCollectionTpl<Scalar, Options> JointCollection;
36 typedef JointDataCompositeTpl<Scalar, Options, JointCollectionTpl> JointDataDerived;
37 typedef JointModelCompositeTpl<Scalar, Options, JointCollectionTpl> JointModelDerived;
38 typedef JointMotionSubspaceTpl<Eigen::Dynamic, Scalar, Options> Constraint_t;
39 typedef SE3Tpl<Scalar, Options> Transformation_t;
40 typedef MotionTpl<Scalar, Options> Motion_t;
41 typedef MotionTpl<Scalar, Options> Bias_t;
42
43 // [ABA]
44 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> U_t;
45 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> D_t;
46 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> UD_t;
47
48 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> ConfigVector_t;
49 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> TangentVector_t;
50
51 typedef boost::mpl::false_ is_mimicable_t;
52
53 PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
54 };
55
56 template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
62
63 template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
69
70 template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
71 struct JointDataCompositeTpl
72 : public JointDataBase<JointDataCompositeTpl<_Scalar, _Options, JointCollectionTpl>>
73 {
74 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
75
76 typedef JointDataBase<JointDataCompositeTpl> Base;
77 typedef JointCompositeTpl<_Scalar, _Options, JointCollectionTpl> JointDerived;
78 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
79 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
80
81 typedef JointCollectionTpl<Scalar, Options> JointCollection;
82 typedef JointDataTpl<Scalar, Options, JointCollectionTpl> JointDataVariant;
83
84 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointDataVariant) JointDataVector;
85
86 // JointDataComposite() {} // can become necessary if we want a vector of JointDataComposite ?
87
88 JointDataCompositeTpl()
89 : joints()
90 , iMlast(0)
91 , pjMi(0)
92 , joint_q(ConfigVector_t::Zero(0))
93 , joint_v(TangentVector_t::Zero(0))
94 , S(0)
95 , M(Transformation_t::Identity())
96 , v(Motion_t::Zero())
97 , c(Motion_t::Zero())
98 , U(6, 0)
99 , Dinv(0, 0)
100 , UDinv(6, 0)
101 , StU(0, 0)
102 {
103 }
104
105 JointDataCompositeTpl(const JointDataVector & joint_data, const int nq, const int nv)
106 : joints(joint_data)
107 , iMlast(joint_data.size())
108 , pjMi(joint_data.size())
109 , joint_q(ConfigVector_t::Zero(nq))
110 , joint_v(TangentVector_t::Zero(nv))
111 , S(Constraint_t::Zero(nv))
112 , M(Transformation_t::Identity())
113 , v(Motion_t::Zero())
114 , c(Motion_t::Zero())
115 , U(U_t::Zero(6, nv))
116 , Dinv(D_t::Zero(nv, nv))
117 , UDinv(UD_t::Zero(6, nv))
118 , StU(D_t::Zero(nv, nv))
119 {
120 }
121
123 JointDataVector joints;
124
126 PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) iMlast;
127
129 PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) pjMi;
130
131 ConfigVector_t joint_q;
132 TangentVector_t joint_v;
133
134 Constraint_t S;
135 Transformation_t M;
136 Motion_t v;
137 Bias_t c;
138
139 // // [ABA] specific data
140 U_t U;
141 D_t Dinv;
142 UD_t UDinv;
143 D_t StU;
144
145 static std::string classname()
146 {
147 return std::string("JointDataComposite");
148 }
149 std::string shortname() const
150 {
151 return classname();
152 }
153
154 void disp(std::ostream & os) const
155 {
156 os << "JointDataComposite containing following models:\n";
157 for (typename JointDataVector::const_iterator it = joints.begin(); it != joints.end(); ++it)
158 os << " " << it->shortname() << std::endl;
159 }
160 };
161
162 template<
163 typename NewScalar,
164 typename Scalar,
165 int Options,
166 template<typename S, int O> class JointCollectionTpl>
167 struct CastType<NewScalar, JointModelCompositeTpl<Scalar, Options, JointCollectionTpl>>
168 {
169 typedef JointModelCompositeTpl<NewScalar, Options, JointCollectionTpl> type;
170 };
171
172 template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
173 struct JointModelCompositeTpl
174 : public JointModelBase<JointModelCompositeTpl<_Scalar, _Options, JointCollectionTpl>>
175 {
176 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
177
178 typedef JointModelBase<JointModelCompositeTpl> Base;
179 typedef JointCompositeTpl<_Scalar, _Options, JointCollectionTpl> JointDerived;
180 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
181
182 typedef JointCollectionTpl<Scalar, Options> JointCollection;
183 typedef JointModelTpl<Scalar, Options, JointCollectionTpl> JointModelVariant;
184
185 typedef SE3Tpl<Scalar, Options> SE3;
186 typedef MotionTpl<Scalar, Options> Motion;
187 typedef InertiaTpl<Scalar, Options> Inertia;
188
189 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModelVariant) JointModelVector;
190
191 using Base::id;
192 using Base::idx_q;
193 using Base::idx_v;
194 using Base::idx_vExtended;
195 using Base::nq;
196 using Base::nv;
197 using Base::nvExtended;
198 using Base::setIndexes;
199
201 JointModelCompositeTpl()
202 : joints()
203 , jointPlacements()
204 , m_nq(0)
205 , m_nv(0)
206 , m_nvExtended(0)
207 , njoints(0)
208 {
209 }
210
212 JointModelCompositeTpl(const size_t size)
213 : joints()
214 , jointPlacements()
215 , m_nq(0)
216 , m_nv(0)
217 , m_nvExtended(0)
218 , njoints(0)
219 {
220 joints.reserve(size);
221 jointPlacements.reserve(size);
222 m_idx_q.reserve(size);
223 m_idx_v.reserve(size);
224 m_idx_vExtended.reserve(size);
225 m_nqs.reserve(size);
226 m_nvs.reserve(size);
227 m_nvExtendeds.reserve(size);
228 }
229
236 template<typename JointModel>
237 JointModelCompositeTpl(
238 const JointModelBase<JointModel> & jmodel, const SE3 & placement = SE3::Identity())
239 : joints(1, (JointModelVariant)jmodel.derived())
240 , jointPlacements(1, placement)
241 , m_nq(jmodel.nq())
242 , m_nv(jmodel.nv())
243 , m_nvExtended(jmodel.nvExtended())
244 , m_idx_q(1, 0)
245 , m_nqs(1, jmodel.nq())
246 , m_idx_v(1, 0)
247 , m_nvs(1, jmodel.nv())
248 , m_idx_vExtended(1, 0)
249 , m_nvExtendeds(1, jmodel.nvExtended())
250 , njoints(1)
251 {
252 }
253
259 JointModelCompositeTpl(const JointModelCompositeTpl & other)
260 : Base(other)
261 , joints(other.joints)
262 , jointPlacements(other.jointPlacements)
263 , m_nq(other.m_nq)
264 , m_nv(other.m_nv)
265 , m_nvExtended(other.m_nvExtended)
266 , m_idx_q(other.m_idx_q)
267 , m_nqs(other.m_nqs)
268 , m_idx_v(other.m_idx_v)
269 , m_nvs(other.m_nvs)
270 , m_idx_vExtended(other.m_idx_vExtended)
271 , m_nvExtendeds(other.m_nvExtendeds)
272 , njoints(other.njoints)
273 {
274 }
275
284 template<typename JointModel>
285 JointModelDerived &
286 addJoint(const JointModelBase<JointModel> & jmodel, const SE3 & placement = SE3::Identity())
287 {
288 joints.push_back((JointModelVariant)jmodel.derived());
289 jointPlacements.push_back(placement);
290
291 m_nq += jmodel.nq();
292 m_nv += jmodel.nv();
293 m_nvExtended += jmodel.nvExtended();
294
295 updateJointIndexes();
296 njoints++;
297
298 return *this;
299 }
300
301 JointDataDerived createData() const
302 {
303 typename JointDataDerived::JointDataVector jdata(joints.size());
304 for (int i = 0; i < (int)joints.size(); ++i)
305 jdata[(size_t)i] =
306 ::pinocchio::createData<Scalar, Options, JointCollectionTpl>(joints[(size_t)i]);
307 return JointDataDerived(jdata, nq(), nv());
308 }
309
310 const std::vector<bool> hasConfigurationLimit() const
311 {
312 std::vector<bool> vec;
313 for (size_t i = 0; i < joints.size(); ++i)
314 {
315 const std::vector<bool> & joint_cf_limit = joints[i].hasConfigurationLimit();
316 vec.insert(vec.end(), joint_cf_limit.begin(), joint_cf_limit.end());
317 }
318 return vec;
319 }
320
321 const std::vector<bool> hasConfigurationLimitInTangent() const
322 {
323 std::vector<bool> vec;
324 for (size_t i = 0; i < joints.size(); ++i)
325 {
326 const std::vector<bool> & joint_cf_limit = joints[i].hasConfigurationLimitInTangent();
327 vec.insert(vec.end(), joint_cf_limit.begin(), joint_cf_limit.end());
328 }
329 return vec;
330 }
331
332 template<typename, int, template<typename S, int O> class, typename>
333 friend struct JointCompositeCalcZeroOrderStep;
334
335 template<typename ConfigVectorType>
336 void calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const;
337
338 template<typename, int, template<typename S, int O> class, typename, typename>
339 friend struct JointCompositeCalcFirstOrderStep;
340
341 template<typename ConfigVectorType, typename TangentVectorType>
342 void calc(
343 JointDataDerived & data,
344 const Eigen::MatrixBase<ConfigVectorType> & qs,
345 const Eigen::MatrixBase<TangentVectorType> & vs) const;
346
347 template<typename TangentVectorType>
348 void calc(
349 JointDataDerived & data,
350 const Blank blank,
351 const Eigen::MatrixBase<TangentVectorType> & vs) const;
352
353 template<typename VectorLike, typename Matrix6Like>
354 void calc_aba(
355 JointDataDerived & data,
356 const Eigen::MatrixBase<VectorLike> & armature,
357 const Eigen::MatrixBase<Matrix6Like> & I,
358 const bool update_I) const
359 {
360 data.U.noalias() = I * data.S.matrix();
361 data.StU.noalias() = data.S.matrix().transpose() * data.U;
362 data.StU.diagonal() += armature;
363
364 internal::PerformStYSInversion<Scalar>::run(data.StU, data.Dinv);
365 data.UDinv.noalias() = data.U * data.Dinv;
366
367 if (update_I)
368 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose();
369 }
370
371 int nv_impl() const
372 {
373 return m_nv;
374 }
375 int nq_impl() const
376 {
377 return m_nq;
378 }
379 int nvExtended_impl() const
380 {
381 return m_nvExtended;
382 }
383
387 void setIndexes_impl(JointIndex id, int q, int v, int vExtended)
388 {
389 Base::setIndexes_impl(id, q, v, vExtended);
390 updateJointIndexes();
391 }
392
393 static std::string classname()
394 {
395 return std::string("JointModelComposite");
396 }
397 std::string shortname() const
398 {
399 return classname();
400 }
401
402 JointModelCompositeTpl & operator=(const JointModelCompositeTpl & other)
403 {
404 Base::operator=(other);
405 m_nq = other.m_nq;
406 m_nv = other.m_nv;
407 m_nvExtended = other.m_nvExtended;
408 m_idx_q = other.m_idx_q;
409 m_idx_v = other.m_idx_v;
410 m_idx_vExtended = other.m_idx_vExtended;
411 m_nqs = other.m_nqs;
412 m_nvs = other.m_nvs;
413 m_nvExtendeds = other.m_nvExtendeds;
414 joints = other.joints;
415 jointPlacements = other.jointPlacements;
416 njoints = other.njoints;
417
418 return *this;
419 }
420
421 using Base::isEqual;
422 bool isEqual(const JointModelCompositeTpl & other) const
423 {
424 return Base::isEqual(other) && internal::comparison_eq(nq(), other.nq())
425 && internal::comparison_eq(nv(), other.nv())
426 && internal::comparison_eq(nvExtended(), other.nvExtended())
427 && internal::comparison_eq(m_idx_q, other.m_idx_q)
428 && internal::comparison_eq(m_idx_v, other.m_idx_v)
429 && internal::comparison_eq(m_idx_vExtended, other.m_idx_vExtended)
430 && internal::comparison_eq(m_nqs, other.m_nqs)
431 && internal::comparison_eq(m_nvs, other.m_nvs)
432 && internal::comparison_eq(m_nvExtendeds, other.m_nvExtendeds)
433 && internal::comparison_eq(joints, other.joints)
434 && internal::comparison_eq(jointPlacements, other.jointPlacements)
435 && internal::comparison_eq(njoints, other.njoints);
436 }
437
439 template<typename NewScalar>
440 JointModelCompositeTpl<NewScalar, Options, JointCollectionTpl> cast() const
441 {
442 typedef JointModelCompositeTpl<NewScalar, Options, JointCollectionTpl> ReturnType;
443 ReturnType res((size_t)njoints);
444 res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended());
445 res.m_nq = m_nq;
446 res.m_nv = m_nv;
447 res.m_nvExtended = m_nvExtended;
448 res.m_idx_q = m_idx_q;
449 res.m_idx_v = m_idx_v;
450 res.m_idx_vExtended = m_idx_vExtended;
451 res.m_nqs = m_nqs;
452 res.m_nvs = m_nvs;
453 res.m_nvExtendeds = m_nvExtendeds;
454 res.njoints = njoints;
455
456 res.joints.resize(joints.size());
457 res.jointPlacements.resize(jointPlacements.size());
458 for (size_t k = 0; k < jointPlacements.size(); ++k)
459 {
460 res.joints[k] = joints[k].template cast<NewScalar>();
461 res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
462 }
463
464 return res;
465 }
466
468 JointModelVector joints;
471 PINOCCHIO_ALIGNED_STD_VECTOR(SE3) jointPlacements;
472
473 template<typename D>
474 typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
475 JointMappedConfigSelector(const Eigen::MatrixBase<D> & a) const
476 {
477 return a.segment(Base::i_q, nq());
478 }
479 template<typename D>
480 typename SizeDepType<NQ>::template SegmentReturn<D>::Type
481 JointMappedConfigSelector(Eigen::MatrixBase<D> & a) const
482 {
483 return a.segment(Base::i_q, nq());
484 }
485
486 template<typename D>
487 typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
488 jointConfigSelector(const Eigen::MatrixBase<D> & a) const
489 {
490 return a.segment(Base::i_q, nq());
491 }
492 template<typename D>
493 typename SizeDepType<NQ>::template SegmentReturn<D>::Type
494 jointConfigSelector(Eigen::MatrixBase<D> & a) const
495 {
496 return a.segment(Base::i_q, nq());
497 }
498
499 template<typename D>
500 typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
501 JointMappedVelocitySelector(const Eigen::MatrixBase<D> & a) const
502 {
503 return a.segment(Base::i_v, nv());
504 }
505 template<typename D>
506 typename SizeDepType<NV>::template SegmentReturn<D>::Type
507 JointMappedVelocitySelector(Eigen::MatrixBase<D> & a) const
508 {
509 return a.segment(Base::i_v, nv());
510 }
511
512 template<typename D>
513 typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
514 jointVelocitySelector(const Eigen::MatrixBase<D> & a) const
515 {
516 return a.segment(Base::i_v, nv());
517 }
518 template<typename D>
519 typename SizeDepType<NV>::template SegmentReturn<D>::Type
520 jointVelocitySelector(Eigen::MatrixBase<D> & a) const
521 {
522 return a.segment(Base::i_v, nv());
523 }
524
525 template<typename D>
526 typename SizeDepType<NV>::template ColsReturn<D>::ConstType
527 jointCols(const Eigen::MatrixBase<D> & A) const
528 {
529 return A.middleCols(Base::i_v, nv());
530 }
531 template<typename D>
532 typename SizeDepType<NV>::template ColsReturn<D>::ConstType
533 jointExtendedModelCols(const Eigen::MatrixBase<D> & A) const
534 {
535 return A.middleCols(Base::i_vExtended, nvExtended());
536 }
537 template<typename D>
538 typename SizeDepType<NV>::template ColsReturn<D>::Type jointCols(Eigen::MatrixBase<D> & A) const
539 {
540 return A.middleCols(Base::i_v, nv());
541 }
542 template<typename D>
543 typename SizeDepType<NV>::template ColsReturn<D>::Type
544 jointExtendedModelCols(Eigen::MatrixBase<D> & A) const
545 {
546 return A.middleCols(Base::i_vExtended, nvExtended());
547 }
548
549 template<typename D>
550 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
551 jointConfigSelector_impl(const Eigen::MatrixBase<D> & a) const
552 {
553 return a.segment(Base::i_q, nq());
554 }
555 template<typename D>
556 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
557 jointConfigSelector_impl(Eigen::MatrixBase<D> & a) const
558 {
559 return a.segment(Base::i_q, nq());
560 }
561
562 template<typename D>
563 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
564 JointMappedConfigSelector_impl(const Eigen::MatrixBase<D> & a) const
565 {
566 return a.segment(Base::i_q, nq());
567 }
568 template<typename D>
569 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
570 JointMappedConfigSelector_impl(Eigen::MatrixBase<D> & a) const
571 {
572 return a.segment(Base::i_q, nq());
573 }
574
575 template<typename D>
576 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
577 JointMappedVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const
578 {
579 return a.segment(Base::i_v, nv());
580 }
581 template<typename D>
582 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
583 JointMappedVelocitySelector_impl(Eigen::MatrixBase<D> & a) const
584 {
585 return a.segment(Base::i_v, nv());
586 }
587
588 template<typename D>
589 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
590 jointVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const
591 {
592 return a.segment(Base::i_v, nv());
593 }
594 template<typename D>
595 typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
596 jointVelocitySelector_impl(Eigen::MatrixBase<D> & a) const
597 {
598 return a.segment(Base::i_v, nv());
599 }
600
601 template<typename D>
602 typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType
603 jointCols_impl(const Eigen::MatrixBase<D> & A) const
604 {
605 return A.middleCols(Base::i_v, nv());
606 }
607 template<typename D>
608 typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType
609 jointExtendedModelCols_impl(const Eigen::MatrixBase<D> & A) const
610 {
611 return A.middleCols(Base::i_vExtended, nvExtended());
612 }
613 template<typename D>
614 typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
615 jointCols_impl(Eigen::MatrixBase<D> & A) const
616 {
617 return A.middleCols(Base::i_v, nv());
618 }
619 template<typename D>
620 typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
621 jointExtendedModelCols_impl(Eigen::MatrixBase<D> & A) const
622 {
623 return A.middleCols(Base::i_vExtended, nvExtended());
624 }
625
626 void disp(std::ostream & os) const
627 {
628 typedef typename JointModelCompositeTpl<Scalar, Options, JointCollectionTpl>::JointModelVector
629 JointModelVector;
630
631 os << "JointModelComposite containing following models:\n";
632 for (typename JointModelVector::const_iterator it = joints.begin(); it != joints.end(); ++it)
633 os << " " << it->shortname() << std::endl;
634 }
635
636 protected:
637 friend struct Serialize<JointModelCompositeTpl>;
638
639 template<typename, int, template<typename, int> class>
640 friend struct JointModelCompositeTpl;
641
644 void updateJointIndexes()
645 {
646 int idx_q = this->idx_q();
647 int idx_v = this->idx_v();
648 int idx_vExtended = this->idx_vExtended();
649
650 m_idx_q.resize(joints.size());
651 m_idx_v.resize(joints.size());
652 m_idx_vExtended.resize(joints.size());
653 m_nqs.resize(joints.size());
654 m_nvs.resize(joints.size());
655 m_nvExtendeds.resize(joints.size());
656
657 for (size_t i = 0; i < joints.size(); ++i)
658 {
659 JointModelVariant & joint = joints[i];
660
661 m_idx_q[i] = idx_q;
662 m_idx_v[i] = idx_v;
663 m_idx_vExtended[i] = idx_vExtended;
664 ::pinocchio::setIndexes(joint, i, idx_q, idx_v, idx_vExtended);
665 m_nqs[i] = ::pinocchio::nq(joint);
666 m_nvs[i] = ::pinocchio::nv(joint);
667 m_nvExtendeds[i] = ::pinocchio::nvExtended(joint);
668 idx_q += m_nqs[i];
669 idx_v += m_nvs[i];
670 idx_vExtended += m_nvExtendeds[i];
671 }
672 }
673
675 int m_nq, m_nv, m_nvExtended;
676
678
680 std::vector<int> m_idx_q;
682 std::vector<int> m_nqs;
684 std::vector<int> m_idx_v;
686 std::vector<int> m_nvs;
688 std::vector<int> m_idx_vExtended;
690 std::vector<int> m_nvExtendeds;
691
692 public:
694 int njoints;
695 };
696
697} // namespace pinocchio
698
699#include <boost/type_traits.hpp>
700
701namespace boost
702{
703 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
704 struct has_nothrow_constructor<
705 ::pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollectionTpl>>
706 : public integral_constant<bool, true>
707 {
708 };
709
710 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
711 struct has_nothrow_copy<::pinocchio::JointModelCompositeTpl<Scalar, Options, JointCollectionTpl>>
712 : public integral_constant<bool, true>
713 {
714 };
715
716 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
717 struct has_nothrow_constructor<
718 ::pinocchio::JointDataCompositeTpl<Scalar, Options, JointCollectionTpl>>
719 : public integral_constant<bool, true>
720 {
721 };
722
723 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
724 struct has_nothrow_copy<::pinocchio::JointDataCompositeTpl<Scalar, Options, JointCollectionTpl>>
725 : public integral_constant<bool, true>
726 {
727 };
728} // namespace boost
729
730/* --- Details -------------------------------------------------------------- */
731/* --- Details -------------------------------------------------------------- */
732/* --- Details -------------------------------------------------------------- */
733#include "pinocchio/multibody/joint/joint-composite.hxx"
734
735#endif // ifndef __pinocchio_multibody_joint_composite_hpp__
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11
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::setIndexes
void setIndexes(JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointIndex id, int q, int v, int vExtended)
Visit a JointModelTpl through JointSetIndexesVisitor to set the indexes of the joint in the kinematic...
pinocchio::idx_v
int idx_v(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxVVisitor to get the index in the model tangent space correspond...
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::nvExtended
int nvExtended(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvExtendVisitor to get the dimension of the joint extended tangent...
pinocchio::idx_vExtended
int idx_vExtended(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdvExtendedVisitor to get the index in the model extended tangent ...
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::JointDataCompositeTpl::joints
JointDataVector joints
Vector of joints.
Definition joint-composite.hpp:123
pinocchio::JointDataCompositeTpl::PINOCCHIO_ALIGNED_STD_VECTOR
PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) pjMi
Transforms from previous joint to joint i.
pinocchio::JointDataCompositeTpl::PINOCCHIO_ALIGNED_STD_VECTOR
PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) iMlast
Transforms from previous joint to last joint.
pinocchio::JointModelBase::operator=
JointModelBase & operator=(const JointModelBase &clone)
Definition joint-model-base.hpp:614
pinocchio::JointModelCompositeTpl::JointModelCompositeTpl
JointModelCompositeTpl(const size_t size)
Default contructor with a defined size.
Definition joint-composite.hpp:212
pinocchio::JointModelCompositeTpl::setIndexes_impl
void setIndexes_impl(JointIndex id, int q, int v, int vExtended)
Update the indexes of subjoints in the stack.
Definition joint-composite.hpp:387
pinocchio::JointModelCompositeTpl::JointModelCompositeTpl
JointModelCompositeTpl()
Default contructor.
Definition joint-composite.hpp:201
pinocchio::JointModelCompositeTpl::JointModelCompositeTpl
JointModelCompositeTpl(const JointModelCompositeTpl &other)
Copy constructor.
Definition joint-composite.hpp:259
pinocchio::JointModelCompositeTpl::PINOCCHIO_ALIGNED_STD_VECTOR
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) jointPlacements
Vector of joint placements. Those placements correspond to the origin of the joint relatively to thei...
pinocchio::JointModelCompositeTpl::m_idx_q
std::vector< int > m_idx_q
Keep information of both the dimension and the position of the joints in the composition.
Definition joint-composite.hpp:680
pinocchio::JointModelCompositeTpl::m_nvExtendeds
std::vector< int > m_nvExtendeds
Dimension of the segment in the jacobian matrix.
Definition joint-composite.hpp:690
pinocchio::JointModelCompositeTpl::JointModelCompositeTpl
JointModelCompositeTpl(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Constructor with one joint.
Definition joint-composite.hpp:237
pinocchio::JointModelCompositeTpl::m_nvs
std::vector< int > m_nvs
Dimension of the segment in the tangent vector.
Definition joint-composite.hpp:686
pinocchio::JointModelCompositeTpl::joints
JointModelVector joints
Vector of joints contained in the joint composite.
Definition joint-composite.hpp:468
pinocchio::JointModelCompositeTpl::m_nqs
std::vector< int > m_nqs
Dimension of the segment in the config vector.
Definition joint-composite.hpp:682
pinocchio::JointModelCompositeTpl::m_idx_vExtended
std::vector< int > m_idx_vExtended
Index in the jacobian matrix.
Definition joint-composite.hpp:688
pinocchio::JointModelCompositeTpl::m_idx_v
std::vector< int > m_idx_v
Index in the tangent vector.
Definition joint-composite.hpp:684
pinocchio::JointModelCompositeTpl::cast
JointModelCompositeTpl< NewScalar, Options, JointCollectionTpl > cast() const
Definition joint-composite.hpp:440
pinocchio::JointModelCompositeTpl::addJoint
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
Definition joint-composite.hpp:286
pinocchio::JointModelCompositeTpl::njoints
int njoints
Number of joints contained in the JointModelComposite.
Definition joint-composite.hpp:694
pinocchio::JointModelCompositeTpl::updateJointIndexes
void updateJointIndexes()
Update the indexes of the joints contained in the composition according to the position of the joint ...
Definition joint-composite.hpp:644
pinocchio::JointModelCompositeTpl::m_nq
int m_nq
Dimensions of the config and tangent space of the composite joint.
Definition joint-composite.hpp:675
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72