pinocchio  2.2.1-dirty
joint-base.hpp
1 //
2 // Copyright (c) 2015-2019 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/math/matrix-block.hpp"
10 
11 #include "pinocchio/multibody/fwd.hpp"
12 #include "pinocchio/multibody/joint/fwd.hpp"
13 #include "pinocchio/multibody/joint/joint-common-operations.hpp"
14 
15 #include <limits>
16 
17 #define PINOCCHIO_JOINT_TYPEDEF_GENERIC(Joint,TYPENAME) \
18  typedef Eigen::DenseIndex Index; \
19  typedef TYPENAME traits<Joint>::Scalar Scalar; \
20  typedef TYPENAME traits<Joint>::JointDataDerived JointDataDerived; \
21  typedef TYPENAME traits<Joint>::JointModelDerived JointModelDerived; \
22  typedef TYPENAME traits<Joint>::Constraint_t Constraint_t; \
23  typedef TYPENAME traits<Joint>::Transformation_t Transformation_t; \
24  typedef TYPENAME traits<Joint>::Motion_t Motion_t; \
25  typedef TYPENAME traits<Joint>::Bias_t Bias_t; \
26  typedef TYPENAME traits<Joint>::U_t U_t; \
27  typedef TYPENAME traits<Joint>::D_t D_t; \
28  typedef TYPENAME traits<Joint>::UD_t UD_t; \
29  enum { \
30  Options = traits<Joint>::Options, \
31  NQ = traits<Joint>::NQ, \
32  NV = traits<Joint>::NV \
33  }; \
34  typedef TYPENAME traits<Joint>::ConfigVector_t ConfigVector_t; \
35  typedef TYPENAME traits<Joint>::TangentVector_t TangentVector_t
36 
37 #define PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,TYPENAME) \
38  PINOCCHIO_JOINT_TYPEDEF_GENERIC(Joint,TYPENAME); \
39  typedef TYPENAME traits<Joint>::ConstraintTypeConstRef ConstraintTypeConstRef; \
40  typedef TYPENAME traits<Joint>::TansformTypeConstRef TansformTypeConstRef; \
41  typedef TYPENAME traits<Joint>::MotionTypeConstRef MotionTypeConstRef; \
42  typedef TYPENAME traits<Joint>::BiasTypeConstRef BiasTypeConstRef; \
43  typedef TYPENAME traits<Joint>::UTypeConstRef UTypeConstRef; \
44  typedef TYPENAME traits<Joint>::UTypeRef UTypeRef; \
45  typedef TYPENAME traits<Joint>::DTypeConstRef DTypeConstRef; \
46  typedef TYPENAME traits<Joint>::UDTypeConstRef UDTypeConstRef
47 
48 #ifdef __clang__
49 
50  #define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG)
51  #define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG)
52 
53  #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_TYPEDEF_GENERIC(Joint,typename)
54  #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename)
55 
56 #elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2)
57 
58  #define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG)
59  #define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG)
60 
61  #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_TYPEDEF_GENERIC(Joint,typename)
62  #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename)
63 
64 #else
65 
66  #define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_TYPEDEF_GENERIC(Joint,typename)
67  #define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename)
68 
69  #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_TYPEDEF_GENERIC(Joint,typename)
70  #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint,typename)
71 
72 #endif
73 
74 #define PINOCCHIO_JOINT_USE_INDEXES \
75  typedef JointModelBase<JointModelDerived> Base; \
76  using Base::idx_q; \
77  using Base::idx_v
78 
79 #define PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTpl) \
80 template<typename Scalar, int Options, typename NewScalar> \
81 struct CastType< NewScalar, JointModelTpl<Scalar,Options> > \
82 { typedef JointModelTpl<NewScalar,Options> type; }
83 
84 
85 #define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR \
86  ConstraintTypeConstRef S_accessor() const { return S; } \
87  TansformTypeConstRef M_accessor() const { return M; } \
88  MotionTypeConstRef v_accessor() const { return v; } \
89  BiasTypeConstRef c_accessor() const { return c; } \
90  UTypeConstRef U_accessor() const { return U; } \
91  UTypeRef U_accessor() { return U; } \
92  DTypeConstRef Dinv_accessor() const { return Dinv; } \
93  UDTypeConstRef UDinv_accessor() const { return UDinv; }
94 
95 #define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE \
96  typedef const Constraint_t & ConstraintTypeConstRef; \
97  typedef const Transformation_t & TansformTypeConstRef; \
98  typedef const Motion_t & MotionTypeConstRef; \
99  typedef const Bias_t & BiasTypeConstRef; \
100  typedef const U_t & UTypeConstRef; \
101  typedef U_t & UTypeRef; \
102  typedef const D_t & DTypeConstRef; \
103  typedef const UD_t & UDTypeConstRef;
104 
105 namespace pinocchio
106 {
107 
108  template<typename Derived>
110  {
111  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
112 
113  typedef typename traits<Derived>::JointDerived JointDerived;
114  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
115 
116  Derived & derived() { return *static_cast<Derived*>(this); }
117  const Derived & derived() const { return *static_cast<const Derived*>(this); }
118 
119  ConstraintTypeConstRef S() const { return derived().S_accessor(); }
120  TansformTypeConstRef M() const { return derived().M_accessor(); }
121  MotionTypeConstRef v() const { return derived().v_accessor(); }
122  BiasTypeConstRef c() const { return derived().c_accessor(); }
123 
124  UTypeConstRef U() const { return derived().U_accessor(); }
125  UTypeRef U() { return derived().U_accessor(); }
126  DTypeConstRef Dinv() const { return derived().Dinv_accessor(); }
127  UDTypeConstRef UDinv() const { return derived().UDinv_accessor(); }
128 
129  std::string shortname() const { return derived().shortname(); }
130  static std::string classname() { return Derived::classname(); }
131 
132  void disp(std::ostream & os) const
133  {
134  using namespace std;
135  os << shortname() << endl;
136  }
137 
138  friend std::ostream & operator << (std::ostream & os, const JointDataBase<Derived> & joint)
139  {
140  joint.disp(os);
141  return os;
142  }
143 
144  protected:
145 
147  inline JointDataBase() {}
148 
149  }; // struct JointDataBase
150 
151  template<typename Derived>
153  {
154  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
155 
156  typedef typename traits<Derived>::JointDerived JointDerived;
157  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
158 
159  JointModelDerived & derived() { return *static_cast<Derived*>(this); }
160  const JointModelDerived & derived() const { return *static_cast<const Derived*>(this); }
161 
162  JointDataDerived createData() const { return derived().createData(); }
163 
164  template<typename ConfigVectorType>
165  void calc(JointDataDerived & data,
166  const Eigen::MatrixBase<ConfigVectorType> & qs) const
167  {
168  derived().calc(data,qs.derived());
169  }
170 
171  template<typename ConfigVectorType, typename TangentVectorType>
172  void calc(JointDataDerived & data,
173  const Eigen::MatrixBase<ConfigVectorType> & qs,
174  const Eigen::MatrixBase<TangentVectorType> & vs) const
175  {
176  derived().calc(data,qs.derived(),vs.derived());
177  }
178 
179  template<typename Matrix6Type>
180  void calc_aba(JointDataDerived & data,
181  const Eigen::MatrixBase<Matrix6Type> & I,
182  const bool update_I = false) const
183  {
184  derived().calc_aba(data, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,I), update_I);
185  }
186 
187  int nv() const { return derived().nv_impl(); }
188  int nq() const { return derived().nq_impl(); }
189 
190  // Default _impl methods are reimplemented by dynamic-size joints.
191  int nv_impl() const { return NV; }
192  int nq_impl() const { return NQ; }
193 
194  int idx_q() const { return derived().idx_q_impl(); }
195  int idx_v() const { return derived().idx_v_impl(); }
196  JointIndex id() const { return derived().id_impl(); }
197 
198  int idx_q_impl() const { return i_q; }
199  int idx_v_impl() const { return i_v; }
200  JointIndex id_impl() const { return i_id; }
201 
202  void setIndexes(JointIndex id, int q, int v)
203  { derived().setIndexes_impl(id, q, v); }
204 
205  void setIndexes_impl(JointIndex id,int q,int v)
206  { i_id = id, i_q = q; i_v = v; }
207 
208  void disp(std::ostream & os) const
209  {
210  using namespace std;
211  os
212  << shortname() << endl
213  << " index: " << id() << endl
214  << " index q: " << idx_q() << endl
215  << " index v: " << idx_v() << endl
216  << " nq: " << nq() << endl
217  << " nv: " << nv() << endl
218  ;
219  }
220 
221  friend std::ostream & operator << (std::ostream & os, const JointModelBase<Derived> & joint)
222  {
223  joint.disp(os);
224  return os;
225  }
226 
227  std::string shortname() const { return derived().shortname(); }
228  static std::string classname() { return Derived::classname(); }
229 
230  template<typename NewScalar>
231  typename CastType<NewScalar,Derived>::type cast() const
232  { return derived().template cast<NewScalar>(); }
233 
234  template <class OtherDerived>
235  bool operator==(const JointModelBase<OtherDerived> & other) const
236  { return derived().isEqual(other.derived()); }
237 
238  template <class OtherDerived>
239  bool operator!=(const JointModelBase<OtherDerived> & other) const
240  { return !(derived() == other.derived()); }
241 
242  template <class OtherDerived>
243  bool isEqual(const JointModelBase<OtherDerived> &) const
244  { return false; }
245 
246  bool isEqual(const JointModelBase<Derived> & other) const
247  {
248  return other.id() == id()
249  && other.idx_q() == idx_q()
250  && other.idx_v() == idx_v();
251  }
252 
253  /* Acces to dedicated segment in robot config space. */
254  // Const access
255  template<typename D>
256  typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
257  jointConfigSelector(const Eigen::MatrixBase<D> & a) const
258  { return derived().jointConfigSelector_impl(a); }
259 
260  template<typename D>
261  typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
262  jointConfigSelector_impl(const Eigen::MatrixBase<D> & a) const
263  { return SizeDepType<NQ>::segment(a.derived(),idx_q(),nq()); }
264 
265  // Non-const access
266  template<typename D>
267  typename SizeDepType<NQ>::template SegmentReturn<D>::Type
268  jointConfigSelector(Eigen::MatrixBase<D> & a) const
269  { return derived().jointConfigSelector_impl(a); }
270 
271  template<typename D>
272  typename SizeDepType<NQ>::template SegmentReturn<D>::Type
273  jointConfigSelector_impl(Eigen::MatrixBase<D> & a) const
274  { return SizeDepType<NQ>::segment(a,idx_q(),nq()); }
275 
276  /* Acces to dedicated segment in robot config velocity space. */
277  // Const access
278  template<typename D>
279  typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
280  jointVelocitySelector(const Eigen::MatrixBase<D> & a) const
281  { return derived().jointVelocitySelector_impl(a.derived()); }
282 
283  template<typename D>
284  typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
285  jointVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const
286  { return SizeDepType<NV>::segment(a.derived(),idx_v(),nv()); }
287 
288  // Non-const access
289  template<typename D>
290  typename SizeDepType<NV>::template SegmentReturn<D>::Type
291  jointVelocitySelector(Eigen::MatrixBase<D> & a) const
292  { return derived().jointVelocitySelector_impl(a.derived()); }
293 
294  template<typename D>
295  typename SizeDepType<NV>::template SegmentReturn<D>::Type
296  jointVelocitySelector_impl(Eigen::MatrixBase<D> & a) const
297  { return SizeDepType<NV>::segment(a.derived(),idx_v(),nv()); }
298 
299  /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/
300  // Const access
301  template<typename D>
302  typename SizeDepType<NV>::template ColsReturn<D>::ConstType
303  jointCols(const Eigen::MatrixBase<D>& A) const
304  { return derived().jointCols_impl(A.derived()); }
305 
306  template<typename D>
307  typename SizeDepType<NV>::template ColsReturn<D>::ConstType
308  jointCols_impl(const Eigen::MatrixBase<D>& A) const
309  { return SizeDepType<NV>::middleCols(A.derived(),idx_v(),nv()); }
310 
311  // Non-const access
312  template<typename D>
313  typename SizeDepType<NV>::template ColsReturn<D>::Type
314  jointCols(Eigen::MatrixBase<D>& A) const
315  { return derived().jointCols_impl(A.derived()); }
316 
317  template<typename D>
318  typename SizeDepType<NV>::template ColsReturn<D>::Type
319  jointCols_impl(Eigen::MatrixBase<D> & A) const
320  { return SizeDepType<NV>::middleCols(A.derived(),idx_v(),nv()); }
321 
322  /* Acces to dedicated rows in a matrix.*/
323  // Const access
324  template<typename D>
325  typename SizeDepType<NV>::template RowsReturn<D>::ConstType
326  jointRows(const Eigen::MatrixBase<D> & A) const
327  { return derived().jointRows_impl(A.derived()); }
328 
329  template<typename D>
330  typename SizeDepType<NV>::template RowsReturn<D>::ConstType
331  jointRows_impl(const Eigen::MatrixBase<D> & A) const
332  { return SizeDepType<NV>::middleRows(A.derived(),idx_v(),nv()); }
333 
334  // Non-const access
335  template<typename D>
336  typename SizeDepType<NV>::template RowsReturn<D>::Type
337  jointRows(Eigen::MatrixBase<D> & A) const
338  { return derived().jointRows_impl(A.derived()); }
339 
340  template<typename D>
341  typename SizeDepType<NV>::template RowsReturn<D>::Type
342  jointRows_impl(Eigen::MatrixBase<D> & A) const
343  { return SizeDepType<NV>::middleRows(A.derived(),idx_v(),nv()); }
344 
346  // Const access
347  template<typename D>
348  typename SizeDepType<NV>::template BlockReturn<D>::ConstType
349  jointBlock(const Eigen::MatrixBase<D> & Mat) const
350  { return derived().jointBlock_impl(Mat.derived()); }
351 
352  template<typename D>
353  typename SizeDepType<NV>::template BlockReturn<D>::ConstType
354  jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const
355  { return SizeDepType<NV>::block(Mat.derived(),idx_v(),idx_v(),nv(),nv()); }
356 
357  // Non-const access
358  template<typename D>
359  typename SizeDepType<NV>::template BlockReturn<D>::Type
360  jointBlock(Eigen::MatrixBase<D> & Mat) const
361  { return derived().jointBlock_impl(Mat.derived()); }
362 
363  template<typename D>
364  typename SizeDepType<NV>::template BlockReturn<D>::Type
365  jointBlock_impl(Eigen::MatrixBase<D> & Mat) const
366  { return SizeDepType<NV>::block(Mat.derived(),idx_v(),idx_v(),nv(),nv()); }
367 
368  protected:
369 
373  inline JointModelBase()
374  : i_id(std::numeric_limits<JointIndex>::max()), i_q(-1), i_v(-1) {}
375 
380  inline JointModelBase(const JointModelBase & clone)
381  { *this = clone; }
382 
387  inline JointModelBase & operator=(const JointModelBase & clone)
388  {
389  i_id = clone.i_id;
390  i_q = clone.i_q;
391  i_v = clone.i_v;
392  return *this;
393  }
394 
395  // data
396  JointIndex i_id; // ID of the joint in the multibody list.
397  int i_q; // Index of the joint configuration in the joint configuration vector.
398  int i_v; // Index of the joint velocity in the joint velocity vector.
399 
400  }; // struct JointModelBase
401 
402 } // namespace pinocchio
403 
404 #endif // ifndef __pinocchio_joint_base_hpp__
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space...
SizeDepType< NV >::template BlockReturn< D >::ConstType jointBlock(const Eigen::MatrixBase< D > &Mat) const
Returns a block of dimension nv()xnv() located at position idx_v(),idx_v() in the matrix Mat...
Definition: joint-base.hpp:349
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...
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.
JointIndex id(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointIdVisitor to get the index of the joint in the kinematic chain...
STL namespace.
JointDataBase()
Default constructor: protected.
Definition: joint-base.hpp:147
std::string shortname(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointShortnameVisitor to get the shortname of the derived joint model...
JointModelBase & operator=(const JointModelBase &clone)
Definition: joint-base.hpp:387
Main pinocchio namespace.
Definition: treeview.dox:24
JointModelBase(const JointModelBase &clone)
Definition: joint-base.hpp:380
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...
Common traits structure to fully define base classes for CRTP.
Definition: spatial/fwd.hpp:32
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...