pinocchio  2.2.1-dirty
joint-composite.hpp
1 //
2 // Copyright (c) 2016-2019 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 
46  PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE
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 
68  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
69  PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR
70 
71  typedef JointCollectionTpl<Scalar,Options> JointCollection;
72  typedef JointDataTpl<Scalar,Options,JointCollectionTpl> JointDataVariant;
73 
75 
76  // JointDataComposite() {} // can become necessary if we want a vector of JointDataComposite ?
77 
79  : joints()
80  , iMlast(0), pjMi(0)
81  , S(0)
82  , M(),v(),c()
83  , U(6,0), Dinv(0,0), UDinv(6,0)
84  , StU(0,0)
85  {}
86 
87 
88  JointDataCompositeTpl(const JointDataVector & joint_data, const int /*nq*/, const int nv)
89  : joints(joint_data), iMlast(joint_data.size()), pjMi(joint_data.size())
90  , S(nv)
91  , M(), v(), c()
92  , U(6,nv), Dinv(nv,nv), UDinv(6,nv)
93  , StU(nv,nv)
94  {}
95 
97  JointDataVector joints;
98 
101 
104 
105  Constraint_t S;
106  Transformation_t M;
107  Motion_t v;
108  Bias_t c;
109 
110  // // [ABA] specific data
111  U_t U;
112  D_t Dinv;
113  UD_t UDinv;
114 
115  D_t StU;
116 
117  static std::string classname() { return std::string("JointDataComposite"); }
118  std::string shortname() const { return classname(); }
119 
120  };
121 
122  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
123  inline std::ostream & operator <<(std::ostream & os, const JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> & jdata)
124  {
126 
127  os << "JointDataComposite containing following models:\n" ;
128  for (typename JointDataVector::const_iterator it = jdata.joints.begin();
129  it != jdata.joints.end(); ++it)
130  os << " " << shortname(*it) << std::endl;
131  return os;
132  }
133 
134 
135  template<typename NewScalar, typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
136  struct CastType< NewScalar, JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
137  {
139  };
140 
141  template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl>
143  : public JointModelBase< JointModelCompositeTpl<_Scalar,_Options,JointCollectionTpl> >
144  {
145  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
146 
149  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
150 
151  typedef JointCollectionTpl<Scalar,Options> JointCollection;
152  typedef JointModelTpl<Scalar,Options,JointCollectionTpl> JointModelVariant;
153 
154  typedef SE3Tpl<Scalar,Options> SE3;
157 
159 
160  using Base::id;
161  using Base::idx_q;
162  using Base::idx_v;
163  using Base::setIndexes;
164  using Base::nq;
165  using Base::nv;
166 
169  : joints()
170  , jointPlacements()
171  , m_nq(0)
172  , m_nv(0)
173  , njoints(0)
174  {}
175 
177  JointModelCompositeTpl(const size_t size)
178  : joints()
179  , jointPlacements()
180  , m_nq(0)
181  , m_nv(0)
182  , njoints(0)
183  {
184  joints.reserve(size); jointPlacements.reserve(size);
185  m_idx_q.reserve(size); m_idx_v.reserve(size);
186  m_nqs.reserve(size); m_nvs.reserve(size);
187  }
188 
195  template<typename JointModel>
197  const SE3 & placement = SE3::Identity())
198  : joints(1,(JointModelVariant)jmodel.derived())
199  , jointPlacements(1,placement)
200  , m_nq(jmodel.nq())
201  , m_nv(jmodel.nv())
202  , m_idx_q(1,0), m_nqs(1,jmodel.nq())
203  , m_idx_v(1,0), m_nvs(1,jmodel.nv())
204  , njoints(1)
205  {}
206 
213  : Base(other)
214  , joints(other.joints)
215  , jointPlacements(other.jointPlacements)
216  , m_nq(other.m_nq)
217  , m_nv(other.m_nv)
218  , m_idx_q(other.m_idx_q), m_nqs(other.m_nqs)
219  , m_idx_v(other.m_idx_v), m_nvs(other.m_nvs)
220  , njoints(other.njoints)
221  {}
222 
223 
232  template<typename JointModel>
233  JointModelDerived & addJoint(const JointModelBase<JointModel> & jmodel,
234  const SE3 & placement = SE3::Identity())
235  {
236  joints.push_back((JointModelVariant)jmodel.derived());
237  jointPlacements.push_back(placement);
238 
239  m_nq += jmodel.nq(); m_nv += jmodel.nv();
240 
241  updateJointIndexes();
242  njoints++;
243 
244  return *this;
245  }
246 
247  JointDataDerived createData() const
248  {
249  typename JointDataDerived::JointDataVector jdata(joints.size());
250  for (int i = 0; i < (int)joints.size(); ++i)
251  jdata[(size_t)i] = ::pinocchio::createData<Scalar,Options,JointCollectionTpl>(joints[(size_t)i]);
252  return JointDataDerived(jdata,nq(),nv());
253  }
254 
255  template<typename, int, template<typename S, int O> class, typename>
256  friend struct JointCompositeCalcZeroOrderStep;
257 
258  template<typename ConfigVectorType>
259  void calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVectorType> & qs) const;
260 
261  template<typename, int, template<typename S, int O> class, typename, typename>
262  friend struct JointCompositeCalcFirstOrderStep;
263 
264  template<typename ConfigVectorType, typename TangentVectorType>
265  void calc(JointDataDerived & data,
266  const Eigen::MatrixBase<ConfigVectorType> & qs,
267  const Eigen::MatrixBase<TangentVectorType> & vs) const;
268 
269  template<typename Matrix6Like>
270  void calc_aba(JointDataDerived & data,
271  const Eigen::MatrixBase<Matrix6Like> & I,
272  const bool update_I) const
273  {
274  data.U.noalias() = I * data.S.matrix();
275  data.StU.noalias() = data.S.matrix().transpose() * data.U;
276 
277  // compute inverse
278 // data.Dinv.setIdentity();
279 // data.StU.llt().solveInPlace(data.Dinv);
280  internal::PerformStYSInversion<Scalar>::run(data.StU,data.Dinv);
281  data.UDinv.noalias() = data.U * data.Dinv;
282 
283  if (update_I)
284  PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like,I).noalias() -= data.UDinv * data.U.transpose();
285  }
286 
287  int nv_impl() const { return m_nv; }
288  int nq_impl() const { return m_nq; }
289 
293  void setIndexes_impl(JointIndex id, int q, int v)
294  {
295  Base::setIndexes_impl(id, q, v);
296  updateJointIndexes();
297  }
298 
299  static std::string classname() { return std::string("JointModelComposite"); }
300  std::string shortname() const { return classname(); }
301 
302  JointModelCompositeTpl & operator=(const JointModelCompositeTpl & other)
303  {
304  Base::operator=(other);
305  m_nq = other.m_nq;
306  m_nv = other.m_nv;
307  m_idx_q = other.m_idx_q;
308  m_idx_v = other.m_idx_v;
309  m_nqs = other.m_nqs;
310  m_nvs = other.m_nvs;
311  joints = other.joints;
312  jointPlacements = other.jointPlacements;
313  njoints = other.njoints;
314 
315  return *this;
316  }
317 
318  using Base::isEqual;
319  bool isEqual(const JointModelCompositeTpl & other) const
320  {
321  return Base::isEqual(other)
322  && nq() == other.nq()
323  && nv() == other.nv()
324  && m_idx_q == other.m_idx_q
325  && m_idx_v == other.m_idx_v
326  && m_nqs == other.m_nqs
327  && m_nvs == other.m_nvs
328  && joints == other.joints
329  && jointPlacements == other.jointPlacements
330  && njoints == other.njoints;
331  }
332 
334  template<typename NewScalar>
336  {
338  ReturnType res((size_t)njoints);
339  res.setIndexes(id(),idx_q(),idx_v());
340  res.m_nq = m_nq;
341  res.m_nv = m_nv;
342  res.m_idx_q = m_idx_q;
343  res.m_idx_v = m_idx_v;
344  res.m_nqs = m_nqs;
345  res.m_nvs = m_nvs;
346  res.njoints = njoints;
347 
348  res.joints.resize(joints.size());
349  res.jointPlacements.resize(jointPlacements.size());
350  for(size_t k = 0; k < jointPlacements.size(); ++k)
351  {
352  res.joints[k] = joints[k].template cast<NewScalar>();
353  res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
354  }
355 
356  return res;
357  }
358 
360  JointModelVector joints;
363 
364  template<typename D>
365  typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
366  jointConfigSelector(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
367  template<typename D>
368  typename SizeDepType<NQ>::template SegmentReturn<D>::Type
369  jointConfigSelector( Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
370 
371  template<typename D>
372  typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
373  jointVelocitySelector(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
374  template<typename D>
375  typename SizeDepType<NV>::template SegmentReturn<D>::Type
376  jointVelocitySelector( Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
377 
378  template<typename D>
379  typename SizeDepType<NV>::template ColsReturn<D>::ConstType
380  jointCols(const Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
381  template<typename D>
382  typename SizeDepType<NV>::template ColsReturn<D>::Type
383  jointCols(Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
384 
385  template<typename D>
386  typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
387  jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
388  template<typename D>
389  typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
390  jointConfigSelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_q,nq()); }
391  template<typename D>
392  typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::ConstType
393  jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
394  template<typename D>
395  typename SizeDepType<Eigen::Dynamic>::template SegmentReturn<D>::Type
396  jointVelocitySelector_impl(Eigen::MatrixBase<D>& a) const { return a.segment(Base::i_v,nv()); }
397 
398  template<typename D>
399  typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::ConstType
400  jointCols_impl(const Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
401  template<typename D>
402  typename SizeDepType<Eigen::Dynamic>::template ColsReturn<D>::Type
403  jointCols_impl(Eigen::MatrixBase<D>& A) const { return A.middleCols(Base::i_v,nv()); }
404 
405 
406  protected:
407 
408  friend struct Serialize<JointModelCompositeTpl>;
409 
410  template<typename, int, template<typename,int> class>
411  friend struct JointModelCompositeTpl;
412 
416  {
417  int idx_q = this->idx_q();
418  int idx_v = this->idx_v();
419 
420  m_idx_q.resize(joints.size());
421  m_idx_v.resize(joints.size());
422  m_nqs.resize(joints.size());
423  m_nvs.resize(joints.size());
424 
425  for(size_t i = 0; i < joints.size(); ++i)
426  {
427  JointModelVariant & joint = joints[i];
428 
429  m_idx_q[i] = idx_q; m_idx_v[i] = idx_v;
430  ::pinocchio::setIndexes(joint,i,idx_q,idx_v);
431  m_nqs[i] = ::pinocchio::nq(joint);
432  m_nvs[i] = ::pinocchio::nv(joint);
433  idx_q += m_nqs[i]; idx_v += m_nvs[i];
434  }
435  }
436 
437 
439  int m_nq, m_nv;
440 
442 
444  std::vector<int> m_idx_q;
446  std::vector<int> m_nqs;
448  std::vector<int> m_idx_v;
450  std::vector<int> m_nvs;
451 
452  public:
454  int njoints;
455  };
456 
457 
458  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
459  inline std::ostream & operator <<(std::ostream & os, const JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> & jmodel)
460  {
462 
463  os << "JointModelComposite containing following models:\n" ;
464  for (typename JointModelVector::const_iterator it = jmodel.joints.begin();
465  it != jmodel.joints.end(); ++it)
466  os << " " << shortname(*it) << std::endl;
467 
468  return os;
469  }
470 
471 } // namespace pinocchio
472 
473 #include <boost/type_traits.hpp>
474 
475 namespace boost
476 {
477  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
478  struct has_nothrow_constructor< ::pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
479  : public integral_constant<bool,true> {};
480 
481  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
482  struct has_nothrow_copy< ::pinocchio::JointModelCompositeTpl<Scalar,Options,JointCollectionTpl> >
483  : public integral_constant<bool,true> {};
484 
485  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
486  struct has_nothrow_constructor< ::pinocchio::JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
487  : public integral_constant<bool,true> {};
488 
489  template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
490  struct has_nothrow_copy< ::pinocchio::JointDataCompositeTpl<Scalar,Options,JointCollectionTpl> >
491  : public integral_constant<bool,true> {};
492 }
493 
494 /* --- Details -------------------------------------------------------------- */
495 /* --- Details -------------------------------------------------------------- */
496 /* --- Details -------------------------------------------------------------- */
497 #include "pinocchio/multibody/joint/joint-composite.hxx"
498 
499 #endif // ifndef __pinocchio_joint_composite_hpp__
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
std::vector< int > m_nvs
Dimension of the segment in the tangent vector.
JointModelCompositeTpl< NewScalar, Options, JointCollectionTpl > cast() const
int idx_q(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdxQVisitor to get the index in the full model configuration space...
std::vector< int > m_idx_v
Index in the tangent vector.
void updateJointIndexes()
Update the indexes of the joints contained in the composition according to the position of the joint ...
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...
int nq(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNqVisitor to get the dimension of the joint configuration space...
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.
std::vector< int > m_idx_q
Keep information of both the dimension and the position of the joints in the composition.
JointModelCompositeTpl(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Constructor with one joint.
JointModelCompositeTpl()
Default contructor.
JointDataVector joints
Vector of joints.
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
void setIndexes_impl(JointIndex id, int q, int v)
Update the indexes of subjoints in the stack.
JointModelDerived & addJoint(const JointModelBase< JointModel > &jmodel, const SE3 &placement=SE3::Identity())
Add a joint to the vector of joints.
JointModelCompositeTpl(const JointModelCompositeTpl &other)
Copy constructor.
container::aligned_vector< Transformation_t > pjMi
Transforms from previous joint to joint i.
container::aligned_vector< Transformation_t > iMlast
Transforms from previous joint to last joint.
std::vector< int > m_nqs
Dimension of the segment in the config vector.
JointModelCompositeTpl(const size_t size)
Default contructor with a defined size.
Main pinocchio namespace.
Definition: treeview.dox:24
JointModelVector joints
Vector of joints contained in the joint composite.
container::aligned_vector< SE3 > jointPlacements
Vector of joint placements. Those placements correspond to the origin of the joint relatively to thei...
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:32
int m_nq
Dimensions of the config and tangent space of the composite joint.
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
Definition: spatial/fwd.hpp:43
void calc_aba(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel, JointDataTpl< Scalar, Options, JointCollectionTpl > &jdata, const Eigen::MatrixBase< Matrix6Type > &I, const bool update_I)
Visit a JointModelTpl and the corresponding JointDataTpl through JointCalcAbaVisitor to...
int njoints
Number of joints contained in the JointModelComposite.