pinocchio  3.5.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
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 
16 namespace 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>
57  struct traits<JointModelCompositeTpl<_Scalar, _Options, JointCollectionTpl>>
58  {
59  typedef JointCompositeTpl<_Scalar, _Options, JointCollectionTpl> JointDerived;
60  typedef _Scalar Scalar;
61  };
62 
63  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
64  struct traits<JointDataCompositeTpl<_Scalar, _Options, JointCollectionTpl>>
65  {
66  typedef JointCompositeTpl<_Scalar, _Options, JointCollectionTpl> JointDerived;
67  typedef _Scalar Scalar;
68  };
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 
701 namespace 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::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::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::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::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::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::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::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::cast
JointModelCompositeTpl< NewScalar, Options, JointCollectionTpl > cast() const
Definition: joint-composite.hpp:440
pinocchio::JointModelCompositeTpl::m_idx_v
std::vector< int > m_idx_v
Index in the tangent vector.
Definition: joint-composite.hpp:684
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