joint-composite.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2016-2021 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_joint_composite_hpp__
6 #define __pinocchio_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>
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  Options = _Options,
29  NQ = Eigen::Dynamic,
30  NV = Eigen::Dynamic
31  };
32 
33  typedef JointCollectionTpl<Scalar,Options> JointCollection;
40 
41  // [ABA]
42  typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> U_t;
43  typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Options> D_t;
44  typedef Eigen::Matrix<Scalar,6,Eigen::Dynamic,Options> UD_t;
45 
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 
52  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
53  struct traits< JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
55 
56  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
57  struct traits< JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
59 
60  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
62  : public JointDataBase< JointDataCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
63  {
64  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
65 
70 
71  typedef JointCollectionTpl<Scalar,Options> JointCollection;
73 
74  typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointDataVariant) JointDataVector;
75 
76  // JointDataComposite() {} // can become necessary if we want a vector of JointDataComposite ?
77 
79  : joints()
80  , iMlast(0)
81  , pjMi(0)
82  , S(0)
83  , M(Transformation_t::Identity())
84  , v(Motion_t::Zero())
85  , c(Motion_t::Zero())
86  , U(6,0), Dinv(0,0), UDinv(6,0)
87  , StU(0,0)
88  {}
89 
90 
91  JointDataCompositeTpl(const JointDataVector & joint_data, const int /*nq*/, const int nv)
92  : joints(joint_data), iMlast(joint_data.size()), pjMi(joint_data.size())
93  , S(Constraint_t::Zero(nv))
94  , M(Transformation_t::Identity())
95  , v(Motion_t::Zero())
96  , c(Motion_t::Zero())
97  , U(U_t::Zero(6,nv))
98  , Dinv(D_t::Zero(nv,nv))
99  , UDinv(UD_t::Zero(6,nv))
100  , StU(D_t::Zero(nv,nv))
101  {}
102 
104  JointDataVector joints;
105 
107  PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) iMlast;
108 
110  PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) pjMi;
111 
112  Constraint_t S;
113  Transformation_t M;
114  Motion_t v;
115  Bias_t c;
116 
117  // // [ABA] specific data
118  U_t U;
119  D_t Dinv;
120  UD_t UDinv;
121 
122  D_t StU;
123 
124  static std::string classname() { return std::string("JointDataComposite"); }
125  std::string shortname() const { return classname(); }
126 
127  };
128 
129  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
130  inline std::ostream & operator <<(std::ostream & os, const JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> & jdata)
131  {
133 
134  os << "JointDataComposite containing following models:\n" ;
135  for (typename JointDataVector::const_iterator it = jdata.joints.begin();
136  it != jdata.joints.end(); ++it)
137  os << " " << shortname(*it) << std::endl;
138  return os;
139  }
140 
141 
142  template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
143  struct CastType< NewScalar, JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
144  {
146  };
147 
148  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
150  : public JointModelBase< JointModelCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
151  {
152  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
153 
156  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
157 
158  typedef JointCollectionTpl<Scalar,Options> JointCollection;
160  typedef JointModel JointModelVariant;
161 
165 
166  typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector;
167 
168  using Base::id;
169  using Base::idx_q;
170  using Base::idx_v;
171  using Base::setIndexes;
172  using Base::nq;
173  using Base::nv;
174 
177  : joints()
178  , jointPlacements()
179  , m_nq(0)
180  , m_nv(0)
181  , njoints(0)
182  {}
183 
186  : joints()
187  , jointPlacements()
188  , m_nq(0)
189  , m_nv(0)
190  , njoints(0)
191  {
192  joints.reserve(size); jointPlacements.reserve(size);
193  m_idx_q.reserve(size); m_idx_v.reserve(size);
194  m_nqs.reserve(size); m_nvs.reserve(size);
195  }
196 
203  template<typename JointModel>
205  const SE3 & placement = SE3::Identity())
206  : joints(1,(JointModelVariant)jmodel.derived())
207  , jointPlacements(1,placement)
208  , m_nq(jmodel.nq())
209  , m_nv(jmodel.nv())
210  , m_idx_q(1,0), m_nqs(1,jmodel.nq())
211  , m_idx_v(1,0), m_nvs(1,jmodel.nv())
212  , njoints(1)
213  {}
214 
221  : Base(other)
222  , joints(other.joints)
223  , jointPlacements(other.jointPlacements)
224  , m_nq(other.m_nq)
225  , m_nv(other.m_nv)
226  , m_idx_q(other.m_idx_q), m_nqs(other.m_nqs)
227  , m_idx_v(other.m_idx_v), m_nvs(other.m_nvs)
228  , njoints(other.njoints)
229  {}
230 
231 
240  template<typename JointModel>
241  JointModelDerived & addJoint(const JointModelBase<JointModel> & jmodel,
242  const SE3 & placement = SE3::Identity())
243  {
244  joints.push_back((JointModelVariant)jmodel.derived());
245  jointPlacements.push_back(placement);
246 
247  m_nq += jmodel.nq(); m_nv += jmodel.nv();
248 
249  updateJointIndexes();
250  njoints++;
251 
252  return *this;
253  }
254 
255  JointDataDerived createData() const
256  {
257  typename JointDataDerived::JointDataVector jdata(joints.size());
258  for (int i = 0; i < (int)joints.size(); ++i)
259  jdata[(size_t)i] = ::pinocchio::createData<Scalar,Options,JointCollectionTpl>(joints[(size_t)i]);
260  return JointDataDerived(jdata,nq(),nv());
261  }
262 
263  const std::vector<bool> hasConfigurationLimit() const
264  {
265  std::vector<bool> vec;
266  for (size_t i = 0; i < joints.size(); ++i)
267  {
268  const std::vector<bool> & joint_cf_limit = joints[i].hasConfigurationLimit();
269  vec.insert(vec.end(),
270  joint_cf_limit.begin(),
271  joint_cf_limit.end());
272  }
273  return vec;
274  }
275 
276  const std::vector<bool> hasConfigurationLimitInTangent() const
277  {
278  std::vector<bool> vec;
279  for (size_t i = 0; i < joints.size(); ++i)
280  {
281  const std::vector<bool> & joint_cf_limit = joints[i].hasConfigurationLimitInTangent();
282  vec.insert(vec.end(),
283  joint_cf_limit.begin(),
284  joint_cf_limit.end());
285  }
286  return vec;
287  }
288 
289  template<typename, int, template<typename S, int O> class, typename>
290  friend struct JointCompositeCalcZeroOrderStep;
291 
292  template<typename ConfigVectorType>
293  void calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const;
294 
295  template<typename, int, template<typename S, int O> class, typename, typename>
296  friend struct JointCompositeCalcFirstOrderStep;
297 
298  template<typename ConfigVectorType, typename TangentVectorType>
299  void calc(JointDataDerived & data,
300  const Eigen::MatrixBase<ConfigVectorType> & qs,
301  const Eigen::MatrixBase<TangentVectorType> & vs) const;
302 
303  template<typename Matrix6Like>
304  void calc_aba(JointDataDerived & data,
305  const Eigen::MatrixBase<Matrix6Like> & I,
306  const bool update_I) const
307  {
308  data.U.noalias() = I * data.S.matrix();
309  data.StU.noalias() = data.S.matrix().transpose() * data.U;
310 
311  // compute inverse
312 // data.Dinv.setIdentity();
313 // data.StU.llt().solveInPlace(data.Dinv);
315  data.UDinv.noalias() = data.U * data.Dinv;
316 
317  if (update_I)
318  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).noalias() -= data.UDinv * data.U.transpose();
319  }
320 
321  int nv_impl() const { return m_nv; }
322  int nq_impl() const { return m_nq; }
323 
327  void setIndexes_impl(JointIndex id, int q, int v)
328  {
329  Base::setIndexes_impl(id, q, v);
330  updateJointIndexes();
331  }
332 
333  static std::string classname() { return std::string("JointModelComposite"); }
334  std::string shortname() const { return classname(); }
335 
337  {
338  Base::operator=(other);
339  m_nq = other.m_nq;
340  m_nv = other.m_nv;
341  m_idx_q = other.m_idx_q;
342  m_idx_v = other.m_idx_v;
343  m_nqs = other.m_nqs;
344  m_nvs = other.m_nvs;
345  joints = other.joints;
346  jointPlacements = other.jointPlacements;
347  njoints = other.njoints;
348 
349  return *this;
350  }
351 
352  using Base::isEqual;
353  bool isEqual(const JointModelCompositeTpl & other) const
354  {
355  return Base::isEqual(other)
356  && nq() == other.nq()
357  && nv() == other.nv()
358  && m_idx_q == other.m_idx_q
359  && m_idx_v == other.m_idx_v
360  && m_nqs == other.m_nqs
361  && m_nvs == other.m_nvs
362  && joints == other.joints
363  && jointPlacements == other.jointPlacements
364  && njoints == other.njoints;
365  }
366 
368  template<typename NewScalar>
370  {
372  ReturnType res((size_t)njoints);
373  res.setIndexes(id(),idx_q(),idx_v());
374  res.m_nq = m_nq;
375  res.m_nv = m_nv;
376  res.m_idx_q = m_idx_q;
377  res.m_idx_v = m_idx_v;
378  res.m_nqs = m_nqs;
379  res.m_nvs = m_nvs;
380  res.njoints = njoints;
381 
382  res.joints.resize(joints.size());
383  res.jointPlacements.resize(jointPlacements.size());
384  for(size_t k = 0; k < jointPlacements.size(); ++k)
385  {
386  res.joints[k] = joints[k].template cast<NewScalar>();
387  res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
388  }
389 
390  return res;
391  }
392 
394  JointModelVector joints;
396  PINOCCHIO_ALIGNED_STD_VECTOR(SE3) jointPlacements;
397 
398  template<typename D>
399  typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
400  jointConfigSelector(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
401  template<typename D>
402  typename SizeDepType<NQ>::template SegmentReturn<D>::Type
403  jointConfigSelector( Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
404 
405  template<typename D>
406  typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
407  jointVelocitySelector(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
408  template<typename D>
409  typename SizeDepType<NV>::template SegmentReturn<D>::Type
410  jointVelocitySelector( Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
411 
412  template<typename D>
413  typename SizeDepType<NV>::template ColsReturn<D>::ConstType
414  jointCols(const Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
415  template<typename D>
416  typename SizeDepType<NV>::template ColsReturn<D>::Type
417  jointCols(Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
418 
419  template<typename D>
420  typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
421  jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
422  template<typename D>
423  typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
424  jointConfigSelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
425  template<typename D>
426  typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
427  jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
428  template<typename D>
429  typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
430  jointVelocitySelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
431 
432  template<typename D>
433  typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType
434  jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
435  template<typename D>
436  typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
437  jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
438 
439 
440  protected:
441 
443 
444  template<typename, int, template<typename,int> class>
445  friend struct JointModelCompositeTpl;
446 
450  {
451  int idx_q = this->idx_q();
452  int idx_v = this->idx_v();
453 
454  m_idx_q.resize(joints.size());
455  m_idx_v.resize(joints.size());
456  m_nqs.resize(joints.size());
457  m_nvs.resize(joints.size());
458 
459  for(size_t i = 0; i < joints.size(); ++i)
460  {
461  JointModelVariant & joint = joints[i];
462 
463  m_idx_q[i] = idx_q; m_idx_v[i] = idx_v;
464  ::pinocchio::setIndexes(joint,i,idx_q,idx_v);
465  m_nqs[i] = ::pinocchio::nq(joint);
466  m_nvs[i] = ::pinocchio::nv(joint);
467  idx_q += m_nqs[i]; idx_v += m_nvs[i];
468  }
469  }
470 
471 
473  int m_nq, m_nv;
474 
476 
478  std::vector<int> m_idx_q;
480  std::vector<int> m_nqs;
482  std::vector<int> m_idx_v;
484  std::vector<int> m_nvs;
485 
486  public:
488  int njoints;
489  };
490 
491 
492  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
493  inline std::ostream & operator <<(std::ostream & os, const JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> & jmodel)
494  {
496 
497  os << "JointModelComposite containing following models:\n" ;
498  for (typename JointModelVector::const_iterator it = jmodel.joints.begin();
499  it != jmodel.joints.end(); ++it)
500  os << " " << shortname(*it) << std::endl;
501 
502  return os;
503  }
504 
505 } // namespace pinocchio
506 
507 #include <boost/type_traits.hpp>
508 
509 namespace boost
510 {
511  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
512  struct has_nothrow_constructor< ::pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
513  : public integral_constant<bool,true> {};
514 
515  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
516  struct has_nothrow_copy< ::pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
517  : public integral_constant<bool,true> {};
518 
519  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
520  struct has_nothrow_constructor< ::pinocchio::JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
521  : public integral_constant<bool,true> {};
522 
523  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
524  struct has_nothrow_copy< ::pinocchio::JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
525  : public integral_constant<bool,true> {};
526 }
527 
528 /* --- Details -------------------------------------------------------------- */
529 /* --- Details -------------------------------------------------------------- */
530 /* --- Details -------------------------------------------------------------- */
531 #include "pinocchio/multibody/joint/joint-composite.hxx"
532 
533 #endif // ifndef __pinocchio_joint_composite_hpp__
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > const Eigen::MatrixBase< TangentVectorType > & v
std::vector< int > m_nvs
Dimension of the segment in the tangent vector.
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
int NQ
Definition: dpendulum.py:8
int m_nq
Dimensions of the config and tangent space of the composite joint.
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector
SizeDepType< NV >::template SegmentReturn< D >::ConstType jointVelocitySelector(const Eigen::MatrixBase< D > &a) const
PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE typedef Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > ConfigVector_t
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 idx_v(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxVVisitor to get the index in the full model tangent space corre...
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > TangentVector_t
SizeDepType< NV >::template SegmentReturn< D >::Type jointVelocitySelector(Eigen::MatrixBase< D > &a) const
JointCompositeTpl< Scalar, Options, JointCollectionTpl > JointDerived
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space...
JointDataDerived createData() const
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > D_t
bool isEqual(const JointModelCompositeTpl &other) const
std::vector< int > m_idx_q
Keep information of both the dimension and the position of the joints in the composition.
InertiaTpl< Scalar, Options > Inertia
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain...
#define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
JointModelCompositeTpl & operator=(const JointModelCompositeTpl &other)
SizeDepType< Eigen::Dynamic >::template SegmentReturn< D >::Type jointConfigSelector_impl(Eigen::MatrixBase< D > &a) const
JointDataVector joints
Vector of joints.
void calc_aba(JointDataDerived &data, const Eigen::MatrixBase< Matrix6Like > &I, const bool update_I) const
static ColsReturn< D >::ConstType middleCols(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index start, typename Eigen::DenseBase< D >::Index size)
void updateJointIndexes()
Update the indexes of the joints contained in the composition according to the position of the joint ...
SizeDepType< Eigen::Dynamic >::template SegmentReturn< D >::Type jointVelocitySelector_impl(Eigen::MatrixBase< D > &a) const
JointDataCompositeTpl< Scalar, Options, JointCollectionTpl > JointDataDerived
PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR typedef JointCollectionTpl< Scalar, Options > JointCollection
SizeDepType< NV >::template ColsReturn< D >::ConstType jointCols(const Eigen::MatrixBase< D > &A) const
Vec3f c
JointDataCompositeTpl(const JointDataVector &joint_data, const int, const int nv)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
#define PINOCCHIO_EIGEN_CONST_CAST(TYPE, OBJ)
Macro for an automatic const_cast.
const std::vector< bool > hasConfigurationLimit() const
SE3::Scalar Scalar
Definition: conversions.cpp:13
vec
virtual bool isEqual(const CollisionGeometry &other) const=0
#define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
JointCompositeTpl< _Scalar, _Options, JointCollectionTpl > JointDerived
JointCollectionTpl< Scalar, Options > JointCollection
SizeDepType< Eigen::Dynamic >::template SegmentReturn< D >::ConstType jointConfigSelector_impl(const Eigen::MatrixBase< D > &a) const
void setIndexes_impl(JointIndex id, int q, int v)
Update the indexes of subjoints in the stack.
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
JointDataTpl< Scalar, Options, JointCollectionTpl > JointDataVariant
static SegmentReturn< D >::ConstType segment(const Eigen::MatrixBase< D > &mat, typename Eigen::DenseBase< D >::Index start, typename Eigen::DenseBase< D >::Index size)
JointModelVector joints
Vector of joints contained in the joint composite.
#define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint)
SizeDepType< Eigen::Dynamic >::template ColsReturn< D >::ConstType jointCols_impl(const Eigen::MatrixBase< D > &A) const
SizeDepType< Eigen::Dynamic >::template ColsReturn< D >::Type jointCols_impl(Eigen::MatrixBase< D > &A) const
static EIGEN_STRONG_INLINE void run(const Eigen::MatrixBase< M1 > &StYS, const Eigen::MatrixBase< M2 > &Dinv)
const std::vector< bool > hasConfigurationLimitInTangent() const
U
Definition: ocp.py:61
JointCompositeTpl< Scalar, Options, JointCollectionTpl > JointDerived
JointModelCompositeTpl< NewScalar, Options, JointCollectionTpl > cast() const
Vec3f a
JointModelDerived & derived()
JointModelCompositeTpl(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Constructor with one joint.
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointDataBase< JointDataCompositeTpl > Base
SizeDepType< NV >::template ColsReturn< D >::Type jointCols(Eigen::MatrixBase< D > &A) const
std::vector< int > m_nqs
Dimension of the segment in the config vector.
int njoints
Number of joints contained in the JointModelComposite.
FCL_REAL size() const
Main pinocchio namespace.
Definition: timings.cpp:28
JointCompositeTpl< _Scalar, _Options, JointCollectionTpl > JointDerived
MotionTpl< Scalar, Options > Motion
res
void setIndexes(JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointIndex id, int q, int v)
Visit a JointModelTpl through JointSetIndexesVisitor to set the indexes of the joint in the kinematic...
JointModelTpl< Scalar, Options, JointCollectionTpl > JointModel
EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef JointModelBase< JointModelCompositeTpl > Base
SizeDepType< NQ >::template SegmentReturn< D >::ConstType jointConfigSelector(const Eigen::MatrixBase< D > &a) const
Common traits structure to fully define base classes for CRTP.
Definition: src/fwd.hpp:44
JointModelCompositeTpl()
Default contructor.
JointModelCompositeTpl(const size_t size)
Default contructor with a defined size.
AABB & operator=(const AABB &other)=default
SizeDepType< Eigen::Dynamic >::template SegmentReturn< D >::ConstType jointVelocitySelector_impl(const Eigen::MatrixBase< D > &a) const
JointModelCompositeTpl< Scalar, Options, JointCollectionTpl > JointModelDerived
M
JointModelCompositeTpl(const JointModelCompositeTpl &other)
Copy constructor.
std::vector< int > m_idx_v
Index in the tangent vector.
NV
Definition: dcrba.py:444
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
Definition: src/fwd.hpp:55
SizeDepType< NQ >::template SegmentReturn< D >::Type jointConfigSelector(Eigen::MatrixBase< D > &a) const
SE3Tpl< Scalar, Options > SE3
#define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint)


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:30