pinocchio  2.4.4
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-model-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_multibody_joint_model_base_hpp__
7 #define __pinocchio_multibody_joint_model_base_hpp__
8 
9 #include "pinocchio/multibody/joint/joint-base.hpp"
10 #include "pinocchio/multibody/joint/joint-common-operations.hpp"
11 
12 #include "pinocchio/math/matrix-block.hpp"
13 
14 #include <limits>
15 
16 #define PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,TYPENAME) \
17  typedef Eigen::DenseIndex Index; \
18  typedef TYPENAME traits<Joint>::Scalar Scalar; \
19  typedef TYPENAME traits<Joint>::JointDataDerived JointDataDerived; \
20  typedef TYPENAME traits<Joint>::JointModelDerived JointModelDerived; \
21  typedef TYPENAME traits<Joint>::Constraint_t Constraint_t; \
22  typedef TYPENAME traits<Joint>::Transformation_t Transformation_t; \
23  typedef TYPENAME traits<Joint>::Motion_t Motion_t; \
24  typedef TYPENAME traits<Joint>::Bias_t Bias_t; \
25  typedef TYPENAME traits<Joint>::U_t U_t; \
26  typedef TYPENAME traits<Joint>::D_t D_t; \
27  typedef TYPENAME traits<Joint>::UD_t UD_t; \
28  enum { \
29  Options = traits<Joint>::Options, \
30  NQ = traits<Joint>::NQ, \
31  NV = traits<Joint>::NV \
32  }; \
33  typedef TYPENAME traits<Joint>::ConfigVector_t ConfigVector_t; \
34  typedef TYPENAME traits<Joint>::TangentVector_t TangentVector_t
35 
36 #ifdef __clang__
37 
38  #define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG)
39  #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,typename)
40 
41 #elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2)
42 
43  #define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,PINOCCHIO_EMPTY_ARG)
44  #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,typename)
45 
46 #else
47 
48  #define PINOCCHIO_JOINT_TYPEDEF(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,typename)
49  #define PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(Joint) PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint,typename)
50 
51 #endif
52 
53 #define PINOCCHIO_JOINT_USE_INDEXES(Joint) \
54  typedef JointModelBase<Joint> Base; \
55  using Base::idx_q; \
56  using Base::idx_v
57 
58 #define PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelTpl) \
59 template<typename Scalar, int Options, typename NewScalar> \
60 struct CastType< NewScalar, JointModelTpl<Scalar,Options> > \
61 { typedef JointModelTpl<NewScalar,Options> type; }
62 
63 namespace pinocchio
64 {
65 
66  template<typename Derived>
67  struct JointModelBase
68  {
69  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
70 
71  typedef typename traits<Derived>::JointDerived JointDerived;
72  PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived);
73 
74  JointModelDerived & derived() { return *static_cast<Derived*>(this); }
75  const JointModelDerived & derived() const { return *static_cast<const Derived*>(this); }
76 
77  JointDataDerived createData() const { return derived().createData(); }
78 
79  template<typename ConfigVectorType>
80  void calc(JointDataDerived & data,
81  const Eigen::MatrixBase<ConfigVectorType> & qs) const
82  {
83  derived().calc(data,qs.derived());
84  }
85 
86  template<typename ConfigVectorType, typename TangentVectorType>
87  void calc(JointDataDerived & data,
88  const Eigen::MatrixBase<ConfigVectorType> & qs,
89  const Eigen::MatrixBase<TangentVectorType> & vs) const
90  {
91  derived().calc(data,qs.derived(),vs.derived());
92  }
93 
94  template<typename Matrix6Type>
95  void calc_aba(JointDataDerived & data,
96  const Eigen::MatrixBase<Matrix6Type> & I,
97  const bool update_I = false) const
98  {
99  derived().calc_aba(data, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,I), update_I);
100  }
101 
102  int nv() const { return derived().nv_impl(); }
103  int nq() const { return derived().nq_impl(); }
104 
105  // Default _impl methods are reimplemented by dynamic-size joints.
106  int nv_impl() const { return NV; }
107  int nq_impl() const { return NQ; }
108 
109  int idx_q() const { return derived().idx_q_impl(); }
110  int idx_v() const { return derived().idx_v_impl(); }
111  JointIndex id() const { return derived().id_impl(); }
112 
113  int idx_q_impl() const { return i_q; }
114  int idx_v_impl() const { return i_v; }
115  JointIndex id_impl() const { return i_id; }
116 
117  void setIndexes(JointIndex id, int q, int v)
118  { derived().setIndexes_impl(id, q, v); }
119 
120  void setIndexes_impl(JointIndex id,int q,int v)
121  { i_id = id, i_q = q; i_v = v; }
122 
123  void disp(std::ostream & os) const
124  {
125  using namespace std;
126  os
127  << shortname() << endl
128  << " index: " << id() << endl
129  << " index q: " << idx_q() << endl
130  << " index v: " << idx_v() << endl
131  << " nq: " << nq() << endl
132  << " nv: " << nv() << endl
133  ;
134  }
135 
136  friend std::ostream & operator << (std::ostream & os, const JointModelBase<Derived> & joint)
137  {
138  joint.disp(os);
139  return os;
140  }
141 
142  std::string shortname() const { return derived().shortname(); }
143  static std::string classname() { return Derived::classname(); }
144 
145  template<typename NewScalar>
146  typename CastType<NewScalar,Derived>::type cast() const
147  { return derived().template cast<NewScalar>(); }
148 
149  template <class OtherDerived>
150  bool operator==(const JointModelBase<OtherDerived> & other) const
151  { return derived().isEqual(other.derived()); }
152 
153  template <class OtherDerived>
154  bool operator!=(const JointModelBase<OtherDerived> & other) const
155  { return !(derived() == other.derived()); }
156 
157  template <class OtherDerived>
158  bool isEqual(const JointModelBase<OtherDerived> &) const
159  { return false; }
160 
161  bool isEqual(const JointModelBase<Derived> & other) const
162  {
163  return other.id() == id()
164  && other.idx_q() == idx_q()
165  && other.idx_v() == idx_v();
166  }
167 
168  /* Acces to dedicated segment in robot config space. */
169  // Const access
170  template<typename D>
171  typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
172  jointConfigSelector(const Eigen::MatrixBase<D> & a) const
173  { return derived().jointConfigSelector_impl(a); }
174 
175  template<typename D>
176  typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
177  jointConfigSelector_impl(const Eigen::MatrixBase<D> & a) const
178  { return SizeDepType<NQ>::segment(a.derived(),idx_q(),nq()); }
179 
180  // Non-const access
181  template<typename D>
182  typename SizeDepType<NQ>::template SegmentReturn<D>::Type
183  jointConfigSelector(Eigen::MatrixBase<D> & a) const
184  { return derived().jointConfigSelector_impl(a); }
185 
186  template<typename D>
187  typename SizeDepType<NQ>::template SegmentReturn<D>::Type
188  jointConfigSelector_impl(Eigen::MatrixBase<D> & a) const
189  { return SizeDepType<NQ>::segment(a,idx_q(),nq()); }
190 
191  /* Acces to dedicated segment in robot config velocity space. */
192  // Const access
193  template<typename D>
194  typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
195  jointVelocitySelector(const Eigen::MatrixBase<D> & a) const
196  { return derived().jointVelocitySelector_impl(a.derived()); }
197 
198  template<typename D>
199  typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
200  jointVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const
201  { return SizeDepType<NV>::segment(a.derived(),idx_v(),nv()); }
202 
203  // Non-const access
204  template<typename D>
205  typename SizeDepType<NV>::template SegmentReturn<D>::Type
206  jointVelocitySelector(Eigen::MatrixBase<D> & a) const
207  { return derived().jointVelocitySelector_impl(a.derived()); }
208 
209  template<typename D>
210  typename SizeDepType<NV>::template SegmentReturn<D>::Type
211  jointVelocitySelector_impl(Eigen::MatrixBase<D> & a) const
212  { return SizeDepType<NV>::segment(a.derived(),idx_v(),nv()); }
213 
214  /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/
215  // Const access
216  template<typename D>
217  typename SizeDepType<NV>::template ColsReturn<D>::ConstType
218  jointCols(const Eigen::MatrixBase<D>& A) const
219  { return derived().jointCols_impl(A.derived()); }
220 
221  template<typename D>
222  typename SizeDepType<NV>::template ColsReturn<D>::ConstType
223  jointCols_impl(const Eigen::MatrixBase<D>& A) const
224  { return SizeDepType<NV>::middleCols(A.derived(),idx_v(),nv()); }
225 
226  // Non-const access
227  template<typename D>
228  typename SizeDepType<NV>::template ColsReturn<D>::Type
229  jointCols(Eigen::MatrixBase<D>& A) const
230  { return derived().jointCols_impl(A.derived()); }
231 
232  template<typename D>
233  typename SizeDepType<NV>::template ColsReturn<D>::Type
234  jointCols_impl(Eigen::MatrixBase<D> & A) const
235  { return SizeDepType<NV>::middleCols(A.derived(),idx_v(),nv()); }
236 
237  /* Acces to dedicated rows in a matrix.*/
238  // Const access
239  template<typename D>
240  typename SizeDepType<NV>::template RowsReturn<D>::ConstType
241  jointRows(const Eigen::MatrixBase<D> & A) const
242  { return derived().jointRows_impl(A.derived()); }
243 
244  template<typename D>
245  typename SizeDepType<NV>::template RowsReturn<D>::ConstType
246  jointRows_impl(const Eigen::MatrixBase<D> & A) const
247  { return SizeDepType<NV>::middleRows(A.derived(),idx_v(),nv()); }
248 
249  // Non-const access
250  template<typename D>
251  typename SizeDepType<NV>::template RowsReturn<D>::Type
252  jointRows(Eigen::MatrixBase<D> & A) const
253  { return derived().jointRows_impl(A.derived()); }
254 
255  template<typename D>
256  typename SizeDepType<NV>::template RowsReturn<D>::Type
257  jointRows_impl(Eigen::MatrixBase<D> & A) const
258  { return SizeDepType<NV>::middleRows(A.derived(),idx_v(),nv()); }
259 
261  // Const access
262  template<typename D>
263  typename SizeDepType<NV>::template BlockReturn<D>::ConstType
264  jointBlock(const Eigen::MatrixBase<D> & Mat) const
265  { return derived().jointBlock_impl(Mat.derived()); }
266 
267  template<typename D>
268  typename SizeDepType<NV>::template BlockReturn<D>::ConstType
269  jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const
270  { return SizeDepType<NV>::block(Mat.derived(),idx_v(),idx_v(),nv(),nv()); }
271 
272  // Non-const access
273  template<typename D>
274  typename SizeDepType<NV>::template BlockReturn<D>::Type
275  jointBlock(Eigen::MatrixBase<D> & Mat) const
276  { return derived().jointBlock_impl(Mat.derived()); }
277 
278  template<typename D>
279  typename SizeDepType<NV>::template BlockReturn<D>::Type
280  jointBlock_impl(Eigen::MatrixBase<D> & Mat) const
281  { return SizeDepType<NV>::block(Mat.derived(),idx_v(),idx_v(),nv(),nv()); }
282 
283  protected:
284 
288  inline JointModelBase()
289  : i_id(std::numeric_limits<JointIndex>::max()), i_q(-1), i_v(-1) {}
290 
295  inline JointModelBase(const JointModelBase & clone)
296  { *this = clone; }
297 
302  inline JointModelBase & operator=(const JointModelBase & clone)
303  {
304  i_id = clone.i_id;
305  i_q = clone.i_q;
306  i_v = clone.i_v;
307  return *this;
308  }
309 
310  // data
311  JointIndex i_id; // ID of the joint in the multibody list.
312  int i_q; // Index of the joint configuration in the joint configuration vector.
313  int i_v; // Index of the joint velocity in the joint velocity vector.
314 
315  }; // struct JointModelBase
316 
317 } // namespace pinocchio
318 
319 #endif // ifndef __pinocchio_multibody_joint_model_base_hpp__
320 
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 idx_v(),idx_v() in the matrix Mat...
Definition: joint-model-base.hpp:264
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::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-model-base.hpp:302
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24
pinocchio::JointModelBase::JointModelBase
JointModelBase(const JointModelBase &clone)
Definition: joint-model-base.hpp:295
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:35
pinocchio::CastType
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type...
Definition: fwd.hpp:46