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>
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
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>
72 : public JointDataBase<JointDataCompositeTpl<_Scalar, _Options, JointCollectionTpl>>
73 {
75
78 PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
79 PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
80
81 typedef JointCollectionTpl<Scalar, Options> JointCollection;
83
84 typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointDataVariant) JointDataVector;
85
86 // JointDataComposite() {} // can become necessary if we want a vector of JointDataComposite ?
87
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)
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
124
127
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 {
170 };
171
172 template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
174 : public JointModelBase<JointModelCompositeTpl<_Scalar, _Options, JointCollectionTpl>>
175 {
177
180 PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
181
184
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
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>
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
260 : Base(other)
262 , jointPlacements(other.jointPlacements)
263 , m_nq(other.m_nq)
264 , m_nv(other.m_nv)
265 , m_nvExtended(other.m_nvExtended)
267 , m_nqs(other.m_nqs)
269 , m_nvs(other.m_nvs)
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
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] =
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);
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>
441 {
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
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
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;
665 m_nqs[i] = ::pinocchio::nq(joint);
666 m_nvs[i] = ::pinocchio::nv(joint);
668 idx_q += m_nqs[i];
669 idx_v += m_nvs[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:
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__
Main pinocchio namespace.
Definition treeview.dox:11
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space.
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space.
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...
int idx_v(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxVVisitor to get the index in the model tangent space correspond...
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
int nvExtended(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvExtendVisitor to get the dimension of the joint extended tangent...
int idx_vExtended(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdvExtendedVisitor to get the index in the model extended tangent ...
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
Definition fwd.hpp:99
JointDataVector joints
Vector of joints.
PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) pjMi
Transforms from previous joint to joint i.
PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) iMlast
Transforms from previous joint to last joint.
JointModelBase & operator=(const JointModelBase &clone)
JointModelCompositeTpl(const size_t size)
Default contructor with a defined size.
void setIndexes_impl(JointIndex id, int q, int v, int vExtended)
Update the indexes of subjoints in the stack.
JointModelCompositeTpl()
Default contructor.
JointModelCompositeTpl(const JointModelCompositeTpl &other)
Copy constructor.
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) jointPlacements
Vector of joint placements. Those placements correspond to the origin of the joint relatively to thei...
std::vector< int > m_idx_q
Keep information of both the dimension and the position of the joints in the composition.
std::vector< int > m_nvExtendeds
Dimension of the segment in the jacobian matrix.
JointModelCompositeTpl(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Constructor with one joint.
std::vector< int > m_nvs
Dimension of the segment in the tangent vector.
JointModelVector joints
Vector of joints contained in the joint composite.
std::vector< int > m_nqs
Dimension of the segment in the config vector.
std::vector< int > m_idx_vExtended
Index in the jacobian matrix.
std::vector< int > m_idx_v
Index in the tangent vector.
JointModelCompositeTpl< NewScalar, Options, JointCollectionTpl > cast() const
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
int njoints
Number of joints contained in the JointModelComposite.
void updateJointIndexes()
Update the indexes of the joints contained in the composition according to the position of the joint ...
int m_nq
Dimensions of the config and tangent space of the composite joint.
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72