pinocchio  2.7.1
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  const std::vector<bool> hasConfigurationLimit() const
80  {
81  return derived().hasConfigurationLimit();
82  }
83 
84  const std::vector<bool> hasConfigurationLimitInTangent() const
85  {
86  return derived().hasConfigurationLimitInTangent();
87  }
88 
89  template<typename ConfigVectorType>
90  void calc(JointDataDerived & data,
91  const Eigen::MatrixBase<ConfigVectorType> & qs) const
92  {
93  derived().calc(data,qs.derived());
94  }
95 
96  template<typename ConfigVectorType, typename TangentVectorType>
97  void calc(JointDataDerived & data,
98  const Eigen::MatrixBase<ConfigVectorType> & qs,
99  const Eigen::MatrixBase<TangentVectorType> & vs) const
100  {
101  derived().calc(data,qs.derived(),vs.derived());
102  }
103 
104  template<typename Matrix6Type>
105  void calc_aba(JointDataDerived & data,
106  const Eigen::MatrixBase<Matrix6Type> & I,
107  const bool update_I = false) const
108  {
109  derived().calc_aba(data, PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type,I), update_I);
110  }
111 
112  int nv() const { return derived().nv_impl(); }
113  int nq() const { return derived().nq_impl(); }
114 
115  // Default _impl methods are reimplemented by dynamic-size joints.
116  int nv_impl() const { return NV; }
117  int nq_impl() const { return NQ; }
118 
119  int idx_q() const { return derived().idx_q_impl(); }
120  int idx_v() const { return derived().idx_v_impl(); }
121  JointIndex id() const { return derived().id_impl(); }
122 
123  int idx_q_impl() const { return i_q; }
124  int idx_v_impl() const { return i_v; }
125  JointIndex id_impl() const { return i_id; }
126 
127  void setIndexes(JointIndex id, int q, int v)
128  { derived().setIndexes_impl(id, q, v); }
129 
130  void setIndexes_impl(JointIndex id,int q,int v)
131  { i_id = id, i_q = q; i_v = v; }
132 
133  void disp(std::ostream & os) const
134  {
135  using namespace std;
136  os
137  << shortname() << endl
138  << " index: " << id() << endl
139  << " index q: " << idx_q() << endl
140  << " index v: " << idx_v() << endl
141  << " nq: " << nq() << endl
142  << " nv: " << nv() << endl
143  ;
144  }
145 
146  friend std::ostream & operator << (std::ostream & os, const JointModelBase<Derived> & joint)
147  {
148  joint.disp(os);
149  return os;
150  }
151 
152  std::string shortname() const { return derived().shortname(); }
153  static std::string classname() { return Derived::classname(); }
154 
155  template<typename NewScalar>
156  typename CastType<NewScalar,Derived>::type cast() const
157  { return derived().template cast<NewScalar>(); }
158 
159  template <class OtherDerived>
160  bool operator==(const JointModelBase<OtherDerived> & other) const
161  { return derived().isEqual(other.derived()); }
162 
163  template <class OtherDerived>
164  bool operator!=(const JointModelBase<OtherDerived> & other) const
165  { return !(derived() == other.derived()); }
166 
167  template <class OtherDerived>
168  bool isEqual(const JointModelBase<OtherDerived> &) const
169  { return false; }
170 
171  bool isEqual(const JointModelBase<Derived> & other) const
172  {
173  return derived().hasSameIndexes(other.derived());
174  }
175 
176  template <class OtherDerived>
177  bool hasSameIndexes(const JointModelBase<OtherDerived> & other) const
178  {
179  return other.id() == id()
180  && other.idx_q() == idx_q()
181  && other.idx_v() == idx_v();
182  }
183 
184  /* Acces to dedicated segment in robot config space. */
185  // Const access
186  template<typename D>
187  typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
188  jointConfigSelector(const Eigen::MatrixBase<D> & a) const
189  { return derived().jointConfigSelector_impl(a); }
190 
191  template<typename D>
192  typename SizeDepType<NQ>::template SegmentReturn<D>::ConstType
193  jointConfigSelector_impl(const Eigen::MatrixBase<D> & a) const
194  { return SizeDepType<NQ>::segment(a.derived(),idx_q(),nq()); }
195 
196  // Non-const access
197  template<typename D>
198  typename SizeDepType<NQ>::template SegmentReturn<D>::Type
199  jointConfigSelector(Eigen::MatrixBase<D> & a) const
200  { return derived().jointConfigSelector_impl(a); }
201 
202  template<typename D>
203  typename SizeDepType<NQ>::template SegmentReturn<D>::Type
204  jointConfigSelector_impl(Eigen::MatrixBase<D> & a) const
205  { return SizeDepType<NQ>::segment(a,idx_q(),nq()); }
206 
207  /* Acces to dedicated segment in robot config velocity space. */
208  // Const access
209  template<typename D>
210  typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
211  jointVelocitySelector(const Eigen::MatrixBase<D> & a) const
212  { return derived().jointVelocitySelector_impl(a.derived()); }
213 
214  template<typename D>
215  typename SizeDepType<NV>::template SegmentReturn<D>::ConstType
216  jointVelocitySelector_impl(const Eigen::MatrixBase<D> & a) const
217  { return SizeDepType<NV>::segment(a.derived(),idx_v(),nv()); }
218 
219  // Non-const access
220  template<typename D>
221  typename SizeDepType<NV>::template SegmentReturn<D>::Type
222  jointVelocitySelector(Eigen::MatrixBase<D> & a) const
223  { return derived().jointVelocitySelector_impl(a.derived()); }
224 
225  template<typename D>
226  typename SizeDepType<NV>::template SegmentReturn<D>::Type
227  jointVelocitySelector_impl(Eigen::MatrixBase<D> & a) const
228  { return SizeDepType<NV>::segment(a.derived(),idx_v(),nv()); }
229 
230  /* Acces to dedicated columns in a ForceSet or MotionSet matrix.*/
231  // Const access
232  template<typename D>
233  typename SizeDepType<NV>::template ColsReturn<D>::ConstType
234  jointCols(const Eigen::MatrixBase<D>& A) const
235  { return derived().jointCols_impl(A.derived()); }
236 
237  template<typename D>
238  typename SizeDepType<NV>::template ColsReturn<D>::ConstType
239  jointCols_impl(const Eigen::MatrixBase<D>& A) const
240  { return SizeDepType<NV>::middleCols(A.derived(),idx_v(),nv()); }
241 
242  // Non-const access
243  template<typename D>
244  typename SizeDepType<NV>::template ColsReturn<D>::Type
245  jointCols(Eigen::MatrixBase<D>& A) const
246  { return derived().jointCols_impl(A.derived()); }
247 
248  template<typename D>
249  typename SizeDepType<NV>::template ColsReturn<D>::Type
250  jointCols_impl(Eigen::MatrixBase<D> & A) const
251  { return SizeDepType<NV>::middleCols(A.derived(),idx_v(),nv()); }
252 
253  /* Acces to dedicated rows in a matrix.*/
254  // Const access
255  template<typename D>
256  typename SizeDepType<NV>::template RowsReturn<D>::ConstType
257  jointRows(const Eigen::MatrixBase<D> & A) const
258  { return derived().jointRows_impl(A.derived()); }
259 
260  template<typename D>
261  typename SizeDepType<NV>::template RowsReturn<D>::ConstType
262  jointRows_impl(const Eigen::MatrixBase<D> & A) const
263  { return SizeDepType<NV>::middleRows(A.derived(),idx_v(),nv()); }
264 
265  // Non-const access
266  template<typename D>
267  typename SizeDepType<NV>::template RowsReturn<D>::Type
268  jointRows(Eigen::MatrixBase<D> & A) const
269  { return derived().jointRows_impl(A.derived()); }
270 
271  template<typename D>
272  typename SizeDepType<NV>::template RowsReturn<D>::Type
273  jointRows_impl(Eigen::MatrixBase<D> & A) const
274  { return SizeDepType<NV>::middleRows(A.derived(),idx_v(),nv()); }
275 
277  // Const access
278  template<typename D>
279  typename SizeDepType<NV>::template BlockReturn<D>::ConstType
280  jointBlock(const Eigen::MatrixBase<D> & Mat) const
281  { return derived().jointBlock_impl(Mat.derived()); }
282 
283  template<typename D>
284  typename SizeDepType<NV>::template BlockReturn<D>::ConstType
285  jointBlock_impl(const Eigen::MatrixBase<D> & Mat) const
286  { return SizeDepType<NV>::block(Mat.derived(),idx_v(),idx_v(),nv(),nv()); }
287 
288  // Non-const access
289  template<typename D>
290  typename SizeDepType<NV>::template BlockReturn<D>::Type
291  jointBlock(Eigen::MatrixBase<D> & Mat) const
292  { return derived().jointBlock_impl(Mat.derived()); }
293 
294  template<typename D>
295  typename SizeDepType<NV>::template BlockReturn<D>::Type
296  jointBlock_impl(Eigen::MatrixBase<D> & Mat) const
297  { return SizeDepType<NV>::block(Mat.derived(),idx_v(),idx_v(),nv(),nv()); }
298 
299  protected:
300 
304  inline JointModelBase()
305  : i_id(std::numeric_limits<JointIndex>::max()), i_q(-1), i_v(-1) {}
306 
311  inline JointModelBase(const JointModelBase & clone)
312  { *this = clone; }
313 
318  inline JointModelBase & operator=(const JointModelBase & clone)
319  {
320  i_id = clone.i_id;
321  i_q = clone.i_q;
322  i_v = clone.i_v;
323  return *this;
324  }
325 
326  // data
327  JointIndex i_id; // ID of the joint in the multibody list.
328  int i_q; // Index of the joint configuration in the joint configuration vector.
329  int i_v; // Index of the joint velocity in the joint velocity vector.
330 
331  }; // struct JointModelBase
332 
333 } // namespace pinocchio
334 
335 #endif // ifndef __pinocchio_multibody_joint_model_base_hpp__
336 
pinocchio::SizeDepType
Definition: matrix-block.hpp:13
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::JointModelBase
Definition: joint-model-base.hpp:67
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::nv
int nv(const JointModelTpl< Scalar, Options, JointCollectionTpl > &jmodel)
Visit a JointModelTpl through JointNvVisitor to get the dimension of the joint tangent space.
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.
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::JointModelBase::JointModelBase
JointModelBase()
Definition: joint-model-base.hpp:304
pinocchio::JointModelBase::JointModelBase
JointModelBase(const JointModelBase &clone)
Definition: joint-model-base.hpp:311
pinocchio::JointModelBase::operator=
JointModelBase & operator=(const JointModelBase &clone)
Definition: joint-model-base.hpp:318
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:280
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::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:44
pinocchio::CastType
Type of the cast of a class C templated by Scalar and Options, to a new NewScalar type....
Definition: fwd.hpp:55
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11