pinocchio  2.1.3
joint-base.hpp
1 //
2 // Copyright (c) 2015-2018 CNRS INRIA
3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_joint_base_hpp__
7 #define __pinocchio_joint_base_hpp__
8 
9 #include "pinocchio/multibody/fwd.hpp"
10 #include "pinocchio/multibody/joint/fwd.hpp"
11 
12 #include <limits>
13 
14 namespace pinocchio
15 {
16 
17 #define PINOCCHIO_JOINT_TYPEDEF_GENERIC(TYPENAME) \
18  typedef Eigen::DenseIndex Index; \
19  typedef TYPENAME traits<JointDerived>::Scalar Scalar; \
20  typedef TYPENAME traits<JointDerived>::JointDataDerived JointDataDerived; \
21  typedef TYPENAME traits<JointDerived>::JointModelDerived JointModelDerived; \
22  typedef TYPENAME traits<JointDerived>::Constraint_t Constraint_t; \
23  typedef TYPENAME traits<JointDerived>::Transformation_t Transformation_t; \
24  typedef TYPENAME traits<JointDerived>::Motion_t Motion_t; \
25  typedef TYPENAME traits<JointDerived>::Bias_t Bias_t; \
26  typedef TYPENAME traits<JointDerived>::F_t F_t; \
27  typedef TYPENAME traits<JointDerived>::U_t U_t; \
28  typedef TYPENAME traits<JointDerived>::D_t D_t; \
29  typedef TYPENAME traits<JointDerived>::UD_t UD_t; \
30  enum { \
31  Options = traits<JointDerived>::Options, \
32  NQ = traits<JointDerived>::NQ, \
33  NV = traits<JointDerived>::NV \
34  }; \
35  typedef TYPENAME traits<JointDerived>::ConfigVector_t ConfigVector_t; \
36  typedef TYPENAME traits<JointDerived>::TangentVector_t TangentVector_t
37 
38 #define PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(TYPENAME) \
39  PINOCCHIO_JOINT_TYPEDEF_GENERIC(TYPENAME); \
40  typedef TYPENAME traits<JointDerived>::ConstraintTypeConstRef ConstraintTypeConstRef; \
41  typedef TYPENAME traits<JointDerived>::TansformTypeConstRef TansformTypeConstRef; \
42  typedef TYPENAME traits<JointDerived>::MotionTypeConstRef MotionTypeConstRef; \
43  typedef TYPENAME traits<JointDerived>::BiasTypeConstRef BiasTypeConstRef; \
44  typedef TYPENAME traits<JointDerived>::UTypeConstRef UTypeConstRef; \
45  typedef TYPENAME traits<JointDerived>::UTypeRef UTypeRef; \
46  typedef TYPENAME traits<JointDerived>::DTypeConstRef DTypeConstRef; \
47  typedef TYPENAME traits<JointDerived>::UDTypeConstRef UDTypeConstRef
48 
49 #ifdef __clang__
50 
51  #define PINOCCHIO_JOINT_TYPEDEF PINOCCHIO_JOINT_TYPEDEF_GENERIC(PINOCCHIO_EMPTY_ARG)
52  #define PINOCCHIO_JOINT_DATA_TYPEDEF PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(PINOCCHIO_EMPTY_ARG)
53 
54  #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE PINOCCHIO_JOINT_TYPEDEF_GENERIC(typename)
55  #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(typename)
56 
57 #elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2)
58 
59  #define PINOCCHIO_JOINT_TYPEDEF PINOCCHIO_JOINT_TYPEDEF_GENERIC(PINOCCHIO_EMPTY_ARG)
60  #define PINOCCHIO_JOINT_DATA_TYPEDEF PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(PINOCCHIO_EMPTY_ARG)
61 
62  #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE PINOCCHIO_JOINT_TYPEDEF_GENERIC(typename)
63  #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(typename)
64 
65 #else
66 
67  #define PINOCCHIO_JOINT_TYPEDEF PINOCCHIO_JOINT_TYPEDEF_GENERIC(typename)
68  #define PINOCCHIO_JOINT_DATA_TYPEDEF PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(typename)
69 
70  #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE PINOCCHIO_JOINT_TYPEDEF_GENERIC(typename)
71  #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(typename)
72 
73 #endif
74 
75 #define PINOCCHIO_JOINT_USE_INDEXES \
76  typedef JointModelBase<JointModelDerived> Base; \
77  using Base::idx_q; \
78  using Base::idx_v
79 
80 #define PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTpl) \
81 template<typename Scalar, int Options, typename NewScalar> \
82 struct CastType< NewScalar, JointModelTpl<Scalar,Options> > \
83 { typedef JointModelTpl<NewScalar,Options> type; }
84 
85 
86 #define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR \
87  ConstraintTypeConstRef S_accessor() const { return S; } \
88  TansformTypeConstRef M_accessor() const { return M; } \
89  MotionTypeConstRef v_accessor() const { return v; } \
90  BiasTypeConstRef c_accessor() const { return c; } \
91  UTypeConstRef U_accessor() const { return U; } \
92  UTypeRef U_accessor() { return U; } \
93  DTypeConstRef Dinv_accessor() const { return Dinv; } \
94  UDTypeConstRef UDinv_accessor() const { return UDinv; }
95 
96 #define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE \
97  typedef const Constraint_t & ConstraintTypeConstRef; \
98  typedef const Transformation_t & TansformTypeConstRef; \
99  typedef const Motion_t & MotionTypeConstRef; \
100  typedef const Bias_t & BiasTypeConstRef; \
101  typedef const U_t & UTypeConstRef; \
102  typedef U_t & UTypeRef; \
103  typedef const D_t & DTypeConstRef; \
104  typedef const UD_t & UDTypeConstRef;
105 
106  template<typename Derived>
107  struct JointDataBase
108  {
109  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
110 
111  typedef typename traits<Derived>::JointDerived JointDerived;
112  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE;
113 
114  Derived & derived() { return *static_cast<Derived*>(this); }
115  const Derived & derived() const { return *static_cast<const Derived*>(this); }
116 
117  ConstraintTypeConstRef S() const { return derived().S_accessor(); }
118  TansformTypeConstRef M() const { return derived().M_accessor(); }
119  MotionTypeConstRef v() const { return derived().v_accessor(); }
120  BiasTypeConstRef c() const { return derived().c_accessor(); }
121  F_t & F() { return derived().F; }
122 
123  UTypeConstRef U() const { return derived().U_accessor(); }
124  UTypeRef U() { return derived().U_accessor(); }
125  DTypeConstRef Dinv() const { return derived().Dinv_accessor(); }
126  UDTypeConstRef UDinv() const { return derived().UDinv_accessor(); }
127 
128  std::string shortname() const { return derived().shortname(); }
129  static std::string classname() { return Derived::classname(); }
130 
131  void disp(std::ostream & os) const
132  {
133  using namespace std;
134  os << shortname() << endl;
135  }
136 
137  friend std::ostream & operator << (std::ostream & os, const JointDataBase<Derived> & joint)
138  {
139  joint.disp(os);
140  return os;
141  }
142 
143  protected:
144 
146  inline JointDataBase() {}
147 
148  }; // struct JointDataBase
149 
150  template<int NV>
151  struct SizeDepType
152  {
153  template<class Mat>
154  struct SegmentReturn
155  {
156  typedef typename Mat::template FixedSegmentReturnType<NV>::Type Type;
157  typedef typename Mat::template ConstFixedSegmentReturnType<NV>::Type ConstType;
158  };
159 
160  template<typename D>
161  static typename SegmentReturn<D>::ConstType
162  segment(const Eigen::MatrixBase<D> & mat,
163  typename Eigen::DenseBase<D>::Index start,
164  typename Eigen::DenseBase<D>::Index size = NV)
165  {
166  PINOCCHIO_UNUSED_VARIABLE(size);
167  return mat.template segment<NV>(start);
168  }
169 
170  template<typename D>
171  static typename SegmentReturn<D>::Type
172  segment(Eigen::MatrixBase<D> & mat,
173  typename Eigen::DenseBase<D>::Index start,
174  typename Eigen::DenseBase<D>::Index size = NV)
175  {
176  PINOCCHIO_UNUSED_VARIABLE(size);
177  return mat.template segment<NV>(start);
178  }
179 
180  template<class Mat>
181  struct ColsReturn
182  {
183  typedef typename Mat::template NColsBlockXpr<NV>::Type Type;
184  typedef typename Mat::template ConstNColsBlockXpr<NV>::Type ConstType;
185  };
186 
187  template<typename D>
188  static typename ColsReturn<D>::ConstType
189  middleCols(const Eigen::MatrixBase<D> & mat,
190  typename Eigen::DenseBase<D>::Index start,
191  typename Eigen::DenseBase<D>::Index size = NV)
192  {
193  PINOCCHIO_UNUSED_VARIABLE(size);
194  return mat.template middleCols<NV>(start);
195  }
196 
197  template<typename D>
198  static typename ColsReturn<D>::Type
199  middleCols(Eigen::MatrixBase<D> & mat,
200  typename Eigen::DenseBase<D>::Index start,
201  typename Eigen::DenseBase<D>::Index size = NV)
202  {
203  PINOCCHIO_UNUSED_VARIABLE(size);
204  return mat.template middleCols<NV>(start);
205  }
206 
207  template<class Mat>
208  struct RowsReturn
209  {
210  typedef typename Mat::template NRowsBlockXpr<NV>::Type Type;
211  typedef typename Mat::template ConstNRowsBlockXpr<NV>::Type ConstType;
212  };
213 
214  template<typename D>
215  static typename RowsReturn<D>::ConstType
216  middleRows(const Eigen::MatrixBase<D> & mat,
217  typename Eigen::DenseBase<D>::Index start,
218  typename Eigen::DenseBase<D>::Index size = NV)
219  {
220  PINOCCHIO_UNUSED_VARIABLE(size);
221  return mat.template middleRows<NV>(start);
222  }
223 
224  template<typename D>
225  static typename RowsReturn<D>::Type
226  middleRows(Eigen::MatrixBase<D> & mat,
227  typename Eigen::DenseBase<D>::Index start,
228  typename Eigen::DenseBase<D>::Index size = NV)
229  {
230  PINOCCHIO_UNUSED_VARIABLE(size);
231  return mat.template middleRows<NV>(start);
232  }
233 
234  template<class Mat>
235  struct BlockReturn
236  {
237  typedef Eigen::Block<Mat, NV, NV> Type;
238  typedef const Eigen::Block<const Mat, NV, NV> ConstType;
239  };
240 
241  template<typename D>
242  static typename BlockReturn<D>::ConstType
243  block(const Eigen::MatrixBase<D> & mat,
244  typename Eigen::DenseBase<D>::Index row_id,
245  typename Eigen::DenseBase<D>::Index col_id,
246  typename Eigen::DenseBase<D>::Index row_size_block = NV,
247  typename Eigen::DenseBase<D>::Index col_size_block = NV)
248  {
249  PINOCCHIO_UNUSED_VARIABLE(row_size_block);
250  PINOCCHIO_UNUSED_VARIABLE(col_size_block);
251  return mat.template block<NV,NV>(row_id,col_id);
252  }
253 
254  template<typename D>
255  static typename BlockReturn<D>::Type
256  block(Eigen::MatrixBase<D> & mat,
257  typename Eigen::DenseBase<D>::Index row_id,
258  typename Eigen::DenseBase<D>::Index col_id,
259  typename Eigen::DenseBase<D>::Index row_size_block = NV,
260  typename Eigen::DenseBase<D>::Index col_size_block = NV)
261  {
262  PINOCCHIO_UNUSED_VARIABLE(row_size_block);
263  PINOCCHIO_UNUSED_VARIABLE(col_size_block);
264  return mat.template block<NV,NV>(row_id,col_id);
265  }
266  };
267 
268  template<>
269  struct SizeDepType<Eigen::Dynamic>
270  {
271  template<class Mat>
272  struct SegmentReturn
273  {
274  typedef typename Mat::SegmentReturnType Type;
275  typedef typename Mat::ConstSegmentReturnType ConstType;
276  };
277 
278  template<typename D>
279  static typename SegmentReturn<D>::ConstType
280  segment(const Eigen::MatrixBase<D> & mat,
281  typename Eigen::DenseBase<D>::Index start,
282  typename Eigen::DenseBase<D>::Index size)
283  {
284  return mat.segment(start,size);
285  }
286 
287  template<typename D>
288  static typename SegmentReturn<D>::Type
289  segment(Eigen::MatrixBase<D> & mat,
290  typename Eigen::DenseBase<D>::Index start,
291  typename Eigen::DenseBase<D>::Index size)
292  {
293  return mat.segment(start,size);
294  }
295 
296  template<class Mat>
297  struct ColsReturn
298  {
299  typedef typename Mat::ColsBlockXpr Type;
300  typedef typename Mat::ConstColsBlockXpr ConstType;
301  };
302 
303  template<typename D>
304  static typename ColsReturn<D>::ConstType
305  middleCols(const Eigen::MatrixBase<D> & mat,
306  typename Eigen::DenseBase<D>::Index start,
307  typename Eigen::DenseBase<D>::Index size)
308  {
309  return mat.middleCols(start,size);
310  }
311 
312  template<typename D>
313  static typename ColsReturn<D>::Type
314  middleCols(Eigen::MatrixBase<D> & mat,
315  typename Eigen::DenseBase<D>::Index start,
316  typename Eigen::DenseBase<D>::Index size)
317  {
318  return mat.middleCols(start,size);
319  }
320 
321  template<class Mat>
322  struct RowsReturn
323  {
324  typedef typename Mat::RowsBlockXpr Type;
325  typedef typename Mat::ConstRowsBlockXpr ConstType;
326  };
327 
328  template<typename D>
329  static typename RowsReturn<D>::ConstType
330  middleRows(const Eigen::MatrixBase<D> & mat,
331  typename Eigen::DenseBase<D>::Index start,
332  typename Eigen::DenseBase<D>::Index size)
333  {
334  return mat.middleRows(start,size);
335  }
336 
337  template<typename D>
338  static typename RowsReturn<D>::Type
339  middleRows(Eigen::MatrixBase<D> & mat,
340  typename Eigen::DenseBase<D>::Index start,
341  typename Eigen::DenseBase<D>::Index size)
342  {
343  return mat.middleRows(start,size);
344  }
345 
346  template<class Mat>
347  struct BlockReturn
348  {
349  typedef Eigen::Block<Mat> Type;
350  typedef const Eigen::Block<const Mat> ConstType;
351  };
352 
353  template<typename D>
354  static typename BlockReturn<D>::ConstType
355  block(const Eigen::MatrixBase<D> & mat,
356  typename Eigen::DenseBase<D>::Index row_id,
357  typename Eigen::DenseBase<D>::Index col_id,
358  typename Eigen::DenseBase<D>::Index row_size_block,
359  typename Eigen::DenseBase<D>::Index col_size_block)
360  {
361  return mat.block(row_id,col_id,row_size_block,col_size_block);
362  }
363 
364  template<typename D>
365  static typename BlockReturn<D>::Type
366  block(Eigen::MatrixBase<D> & mat,
367  typename Eigen::DenseBase<D>::Index row_id,
368  typename Eigen::DenseBase<D>::Index col_id,
369  typename Eigen::DenseBase<D>::Index row_size_block,
370  typename Eigen::DenseBase<D>::Index col_size_block)
371  {
372  return mat.block(row_id,col_id,row_size_block,col_size_block);
373  }
374  };
375 
376  template<typename Derived>
377  struct JointModelBase
378  {
379  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
380 
381  typedef typename traits<Derived>::JointDerived JointDerived;
382  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE;
383 
384  JointModelDerived & derived() { return *static_cast<Derived*>(this); }
385  const JointModelDerived & derived() const { return *static_cast<const Derived*>(this); }
386 
387  JointDataDerived createData() const { return derived().createData(); }
388 
389  template<typename ConfigVectorType>
390  void calc(JointDataDerived & data,
391  const Eigen::MatrixBase<ConfigVectorType> & qs) const
392  {
393  derived().calc(data,qs.derived());
394  }
395 
396  template<typename ConfigVectorType, typename TangentVectorType>
397  void calc(JointDataDerived & data,
398  const Eigen::MatrixBase<ConfigVectorType> & qs,
399  const Eigen::MatrixBase<TangentVectorType> & vs) const
400  {
401  derived().calc(data,qs.derived(),vs.derived());
402 
403  }
404 
405  template<typename Matrix6Type>
406  void calc_aba(JointDataDerived & data,
407  const Eigen::MatrixBase<Matrix6Type> & I,
408  const bool update_I = false) const
409  {
410  derived().calc_aba(data, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,I), update_I);
411  }
412 
419  Scalar finiteDifferenceIncrement() const
420  { return derived().finiteDifferenceIncrement(); }
421 
422  JointIndex i_id; // ID of the joint in the multibody list.
423  int i_q; // Index of the joint configuration in the joint configuration vector.
424  int i_v; // Index of the joint velocity in the joint velocity vector.
425 
426  int nv() const { return derived().nv_impl(); }
427  int nq() const { return derived().nq_impl(); }
428 
429  // Default _impl methods are reimplemented by dynamic-size joints.
430  int nv_impl() const { return NV; }
431  int nq_impl() const { return NQ; }
432 
433  int idx_q() const { return i_q; }
434  int idx_v() const { return i_v; }
435  JointIndex id() const { return i_id; }
436 
437  void setIndexes(JointIndex id, int q, int v) { derived().setIndexes_impl(id, q, v); }
438 
439  void setIndexes_impl(JointIndex id,int q,int v) { i_id = id, i_q = q; i_v = v; }
440 
441  void disp(std::ostream & os) const
442  {
443  using namespace std;
444  os
445  << shortname() << endl
446  << " index: " << i_id << endl
447  << " index q: " << i_q << endl
448  << " index v: " << i_v << endl
449  << " nq: " << nq() << endl
450  << " nv: " << nv() << endl
451  ;
452  }
453 
454  friend std::ostream & operator << (std::ostream & os, const JointModelBase<Derived> & joint)
455  {
456  joint.disp(os);
457  return os;
458  }
459 
460  std::string shortname() const { return derived().shortname(); }
461  static std::string classname() { return Derived::classname(); }
462 
463  template<typename NewScalar>
464  typename CastType<NewScalar,Derived>::type cast() const
465  { return derived().template cast<NewScalar>(); }
466 
467  template <class OtherDerived>
468  bool operator==(const JointModelBase<OtherDerived> & other) const
469  { return derived().isEqual(other.derived()); }
470 
471  template <class OtherDerived>
472  bool operator!=(const JointModelBase<OtherDerived> & other) const
473  { return !(derived() == other.derived()); }
474 
475  template <class OtherDerived>
476  bool isEqual(const JointModelBase<OtherDerived> &) const { return false; }
477 
478  bool isEqual(const JointModelBase<Derived> & other) const
479  {
480  return other.id() == id()
481  && other.idx_q() == idx_q()
482  && other.idx_v() == idx_v();
483  }
484 
485  /* Acces to dedicated segment in robot config space. */
486  // Const access
487  template<typename D>
488  typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
489  jointConfigSelector(const Eigen::MatrixBase<D>& a) const
490  { return derived().jointConfigSelector_impl(a); }
491 
492  template<typename D>
493  typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
494  jointConfigSelector_impl(const Eigen::MatrixBase<D>& a) const
495  { return SizeDepType<NQ>::segment(a,i_q,nq()); }
496 
497  // Non-const access
498  template<typename D>
499  typename SizeDepType<NQ>::template SegmentReturn<D>::Type
500  jointConfigSelector( Eigen::MatrixBase<D>& a) const
501  { return derived().jointConfigSelector_impl(a); }
502 
503  template<typename D>
504  typename SizeDepType<NQ>::template SegmentReturn<D>::Type
505  jointConfigSelector_impl( Eigen::MatrixBase<D>& a) const
506  { return SizeDepType<NQ>::segment(a,i_q,nq()); }
507 
508  /* Acces to dedicated segment in robot config velocity space. */
509  // Const access
510  template<typename D>
511  typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
512  jointVelocitySelector(const Eigen::MatrixBase<D>& a) const
513  { return derived().jointVelocitySelector_impl(a); }
514 
515  template<typename D>
516  typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
517  jointVelocitySelector_impl(const Eigen::MatrixBase<D>& a) const
518  { return SizeDepType<NV>::segment(a,i_v,nv()); }
519 
520  // Non-const access
521  template<typename D>
522  typename SizeDepType<NV>::template SegmentReturn<D>::Type
523  jointVelocitySelector( Eigen::MatrixBase<D>& a) const
524  { return derived().jointVelocitySelector_impl(a); }
525 
526  template<typename D>
527  typename SizeDepType<NV>::template SegmentReturn<D>::Type
528  jointVelocitySelector_impl( Eigen::MatrixBase<D>& a) const
529  { return SizeDepType<NV>::segment(a,i_v,nv()); }
530 
531  template<typename D>
532  typename SizeDepType<NV>::template ColsReturn<D>::ConstType
533  jointCols(const Eigen::MatrixBase<D>& A) const
534  { return derived().jointCols_impl(A); }
535 
536  template<typename D>
537  typename SizeDepType<NV>::template ColsReturn<D>::ConstType
538  jointCols_impl(const Eigen::MatrixBase<D>& A) const
539  { return SizeDepType<NV>::middleCols(A,i_v,nv()); }
540 
541  template<typename D>
542  typename SizeDepType<NV>::template ColsReturn<D>::Type
543  jointCols(Eigen::MatrixBase<D>& A) const
544  { return derived().jointCols_impl(A); }
545 
546  template<typename D>
547  typename SizeDepType<NV>::template ColsReturn<D>::Type
548  jointCols_impl(Eigen::MatrixBase<D>& A) const
549  { return SizeDepType<NV>::middleCols(A,i_v,nv()); }
550 
551  template<typename D>
552  typename SizeDepType<NV>::template RowsReturn<D>::ConstType
553  jointRows(const Eigen::MatrixBase<D>& A) const
554  { return derived().jointRows_impl(A); }
555 
556  template<typename D>
557  typename SizeDepType<NV>::template RowsReturn<D>::ConstType
558  jointRows_impl(const Eigen::MatrixBase<D>& A) const
559  { return SizeDepType<NV>::middleRows(A,i_v,nv()); }
560 
561  template<typename D>
562  typename SizeDepType<NV>::template RowsReturn<D>::Type
563  jointRows(Eigen::MatrixBase<D>& A) const
564  { return derived().jointRows_impl(A); }
565 
566  template<typename D>
567  typename SizeDepType<NV>::template RowsReturn<D>::Type
568  jointRows_impl(Eigen::MatrixBase<D>& A) const
569  { return SizeDepType<NV>::middleRows(A,i_v,nv()); }
570 
572  template<typename D>
573  typename SizeDepType<NV>::template BlockReturn<D>::ConstType
574  jointBlock(const Eigen::MatrixBase<D> & Mat) const
575  { return derived().jointBlock_impl(Mat); }
576 
577  template<typename D>
578  typename SizeDepType<NV>::template BlockReturn<D>::ConstType
579  jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const
580  { return SizeDepType<NV>::block(Mat,i_v,i_v,nv(),nv()); }
581 
582  template<typename D>
583  typename SizeDepType<NV>::template BlockReturn<D>::Type
584  jointBlock(Eigen::MatrixBase<D> & Mat) const
585  { return derived().jointBlock_impl(Mat); }
586 
587  template<typename D>
588  typename SizeDepType<NV>::template BlockReturn<D>::Type
589  jointBlock_impl(Eigen::MatrixBase<D> & Mat) const
590  { return SizeDepType<NV>::block(Mat,i_v,i_v,nv(),nv()); }
591 
592  protected:
593 
597  inline JointModelBase()
598  : i_id(std::numeric_limits<JointIndex>::max()), i_q(-1), i_v(-1) {}
599 
604  inline JointModelBase(const JointModelBase & clone)
605  { *this = clone; }
606 
611  inline JointModelBase & operator=(const JointModelBase & clone)
612  {
613  i_id = clone.i_id;
614  i_q = clone.i_q;
615  i_v = clone.i_v;
616  return *this;
617  }
618 
619  }; // struct JointModelBase
620 
621 } // namespace pinocchio
622 
623 #endif // ifndef __pinocchio_joint_base_hpp__
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::JointModelBase::jointBlock
SizeDepType< NV >::template BlockReturn< D >::ConstType jointBlock(const Eigen::MatrixBase< D > &Mat) const
Returns a block of dimension nv()xnv() located at position i_v,i_v in the matrix Mat.
Definition: joint-base.hpp:574
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::idx_v
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...
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::createData
JointDataTpl< Scalar, Options, JointCollectionTpl > createData(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through CreateData visitor to create a JointDataTpl.
pinocchio::id
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain...
Eigen
Source from #include <cppad/example/cppad_eigen.hpp>
Definition: cppad.hpp:24
std
STL namespace.
pinocchio::JointDataBase::JointDataBase
JointDataBase()
Default constructor: protected.
Definition: joint-base.hpp:146
pinocchio::shortname
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
pinocchio::JointModelBase::operator=
JointModelBase & operator=(const JointModelBase &clone)
Definition: joint-base.hpp:611
pinocchio::JointModelBase::finiteDifferenceIncrement
Scalar finiteDifferenceIncrement() const
Return the resolution of the finite differerence increment according to the Scalar type...
Definition: joint-base.hpp:419
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24
pinocchio::JointModelBase::JointModelBase
JointModelBase(const JointModelBase &clone)
Definition: joint-base.hpp:604
pinocchio::setIndexes
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...
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:29
pinocchio::CastType
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
Definition: spatial/fwd.hpp:35
pinocchio::calc_aba
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...