pinocchio  3.6.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
joint-data-base.hpp
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_multibody_joint_data_base_hpp__
7 #define __pinocchio_multibody_joint_data_base_hpp__
8 
9 #include "pinocchio/multibody/joint/joint-base.hpp"
10 #include "pinocchio/multibody/joint/joint-model-base.hpp"
11 
12 #define PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint, TYPENAME) \
13  PINOCCHIO_JOINT_MODEL_TYPEDEF_GENERIC(Joint, TYPENAME); \
14  typedef TYPENAME traits<Joint>::ConfigVectorTypeConstRef ConfigVectorTypeConstRef; \
15  typedef TYPENAME traits<Joint>::ConfigVectorTypeRef ConfigVectorTypeRef; \
16  typedef TYPENAME traits<Joint>::TangentVectorTypeConstRef TangentVectorTypeConstRef; \
17  typedef TYPENAME traits<Joint>::TangentVectorTypeRef TangentVectorTypeRef; \
18  typedef TYPENAME traits<Joint>::ConstraintTypeConstRef ConstraintTypeConstRef; \
19  typedef TYPENAME traits<Joint>::ConstraintTypeRef ConstraintTypeRef; \
20  typedef TYPENAME traits<Joint>::TansformTypeConstRef TansformTypeConstRef; \
21  typedef TYPENAME traits<Joint>::TansformTypeRef TansformTypeRef; \
22  typedef TYPENAME traits<Joint>::MotionTypeConstRef MotionTypeConstRef; \
23  typedef TYPENAME traits<Joint>::MotionTypeRef MotionTypeRef; \
24  typedef TYPENAME traits<Joint>::BiasTypeConstRef BiasTypeConstRef; \
25  typedef TYPENAME traits<Joint>::BiasTypeRef BiasTypeRef; \
26  typedef TYPENAME traits<Joint>::UTypeConstRef UTypeConstRef; \
27  typedef TYPENAME traits<Joint>::UTypeRef UTypeRef; \
28  typedef TYPENAME traits<Joint>::DTypeConstRef DTypeConstRef; \
29  typedef TYPENAME traits<Joint>::DTypeRef DTypeRef; \
30  typedef TYPENAME traits<Joint>::UDTypeConstRef UDTypeConstRef; \
31  typedef TYPENAME traits<Joint>::UDTypeRef UDTypeRef
32 
33 #ifdef __clang__
34 
35  #define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) \
36  PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint, PINOCCHIO_EMPTY_ARG)
37  #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) \
38  PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint, typename)
39 
40 #elif (__GNUC__ == 4) && (__GNUC_MINOR__ == 4) && (__GNUC_PATCHLEVEL__ == 2)
41 
42  #define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) \
43  PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint, PINOCCHIO_EMPTY_ARG)
44  #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) \
45  PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint, typename)
46 
47 #else
48 
49  #define PINOCCHIO_JOINT_DATA_TYPEDEF(Joint) PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint, typename)
50  #define PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(Joint) \
51  PINOCCHIO_JOINT_DATA_TYPEDEF_GENERIC(Joint, typename)
52 
53 #endif
54 
55 #define PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR \
56  ConfigVectorTypeConstRef joint_q_accessor() const \
57  { \
58  return joint_q; \
59  } \
60  ConfigVectorTypeRef joint_q_accessor() \
61  { \
62  return joint_q; \
63  } \
64  TangentVectorTypeConstRef joint_v_accessor() const \
65  { \
66  return joint_v; \
67  } \
68  TangentVectorTypeRef joint_v_accessor() \
69  { \
70  return joint_v; \
71  } \
72  ConstraintTypeConstRef S_accessor() const \
73  { \
74  return S; \
75  } \
76  ConstraintTypeRef S_accessor() \
77  { \
78  return S; \
79  } \
80  TansformTypeConstRef M_accessor() const \
81  { \
82  return M; \
83  } \
84  TansformTypeRef M_accessor() \
85  { \
86  return M; \
87  } \
88  MotionTypeConstRef v_accessor() const \
89  { \
90  return v; \
91  } \
92  MotionTypeRef v_accessor() \
93  { \
94  return v; \
95  } \
96  BiasTypeConstRef c_accessor() const \
97  { \
98  return c; \
99  } \
100  BiasTypeRef c_accessor() \
101  { \
102  return c; \
103  } \
104  UTypeConstRef U_accessor() const \
105  { \
106  return U; \
107  } \
108  UTypeRef U_accessor() \
109  { \
110  return U; \
111  } \
112  DTypeConstRef Dinv_accessor() const \
113  { \
114  return Dinv; \
115  } \
116  DTypeRef Dinv_accessor() \
117  { \
118  return Dinv; \
119  } \
120  UDTypeConstRef UDinv_accessor() const \
121  { \
122  return UDinv; \
123  } \
124  UDTypeRef UDinv_accessor() \
125  { \
126  return UDinv; \
127  } \
128  DTypeConstRef StU_accessor() const \
129  { \
130  return StU; \
131  } \
132  DTypeRef StU_accessor() \
133  { \
134  return StU; \
135  }
136 
137 #define PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE \
138  typedef const ConfigVector_t & ConfigVectorTypeConstRef; \
139  typedef ConfigVector_t & ConfigVectorTypeRef; \
140  typedef const TangentVector_t & TangentVectorTypeConstRef; \
141  typedef TangentVector_t & TangentVectorTypeRef; \
142  typedef const Constraint_t & ConstraintTypeConstRef; \
143  typedef Constraint_t & ConstraintTypeRef; \
144  typedef const Transformation_t & TansformTypeConstRef; \
145  typedef Transformation_t & TansformTypeRef; \
146  typedef const Motion_t & MotionTypeConstRef; \
147  typedef Motion_t & MotionTypeRef; \
148  typedef const Bias_t & BiasTypeConstRef; \
149  typedef Bias_t & BiasTypeRef; \
150  typedef const U_t & UTypeConstRef; \
151  typedef U_t & UTypeRef; \
152  typedef const D_t & DTypeConstRef; \
153  typedef D_t & DTypeRef; \
154  typedef const UD_t & UDTypeConstRef; \
155  typedef UD_t & UDTypeRef;
156 
157 namespace pinocchio
158 {
159 
160  template<typename Derived>
161  struct JointDataBase : NumericalBase<Derived>
162  {
163  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
164 
165  typedef typename traits<Derived>::JointDerived JointDerived;
166  PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived);
167 
168  Derived & derived()
169  {
170  return *static_cast<Derived *>(this);
171  }
172  const Derived & derived() const
173  {
174  return *static_cast<const Derived *>(this);
175  }
176 
177  ConfigVectorTypeConstRef joint_q() const
178  {
179  return derived().joint_q_accessor();
180  }
181  ConfigVectorTypeRef joint_q()
182  {
183  return derived().joint_q_accessor();
184  }
185 
186  TangentVectorTypeConstRef joint_v() const
187  {
188  return derived().joint_v_accessor();
189  }
190  TangentVectorTypeRef joint_v()
191  {
192  return derived().joint_v_accessor();
193  }
194 
195  ConstraintTypeConstRef S() const
196  {
197  return derived().S_accessor();
198  }
199  ConstraintTypeRef S()
200  {
201  return derived().S_accessor();
202  }
203  TansformTypeConstRef M() const
204  {
205  return derived().M_accessor();
206  }
207  TansformTypeRef M()
208  {
209  return derived().M_accessor();
210  }
211  MotionTypeConstRef v() const
212  {
213  return derived().v_accessor();
214  }
215  MotionTypeRef v()
216  {
217  return derived().v_accessor();
218  }
219  BiasTypeConstRef c() const
220  {
221  return derived().c_accessor();
222  }
223  BiasTypeRef c()
224  {
225  return derived().c_accessor();
226  }
227 
228  UTypeConstRef U() const
229  {
230  return derived().U_accessor();
231  }
232  UTypeRef U()
233  {
234  return derived().U_accessor();
235  }
236  DTypeConstRef Dinv() const
237  {
238  return derived().Dinv_accessor();
239  }
240  DTypeRef Dinv()
241  {
242  return derived().Dinv_accessor();
243  }
244  UDTypeConstRef UDinv() const
245  {
246  return derived().UDinv_accessor();
247  }
248  UDTypeRef UDinv()
249  {
250  return derived().UDinv_accessor();
251  }
252  DTypeConstRef StU() const
253  {
254  return derived().StU_accessor();
255  }
256  DTypeRef StU()
257  {
258  return derived().StU_accessor();
259  }
260 
261  std::string shortname() const
262  {
263  return derived().shortname();
264  }
265  static std::string classname()
266  {
267  return Derived::classname();
268  }
269 
270  void disp(std::ostream & os) const
271  {
272  using namespace std;
273  os << shortname() << endl;
274  }
275 
276  friend std::ostream & operator<<(std::ostream & os, const JointDataBase<Derived> & jdata)
277  {
278  jdata.derived().disp(os);
279  return os;
280  }
281 
282  template<typename OtherDerived>
283  bool operator==(const JointDataBase<OtherDerived> & other) const
284  {
285  return derived().isEqual(other.derived());
286  }
287 
289  bool isEqual(const JointDataBase<Derived> & other) const
290  {
291  return internal::comparison_eq(joint_q(), other.joint_q())
292  && internal::comparison_eq(joint_v(), other.joint_v())
293  && internal::comparison_eq(S(), other.S()) && internal::comparison_eq(M(), other.M())
294  && internal::comparison_eq(v(), other.v()) && internal::comparison_eq(c(), other.c())
295  && internal::comparison_eq(U(), other.U())
296  && internal::comparison_eq(Dinv(), other.Dinv())
297  && internal::comparison_eq(UDinv(), other.UDinv());
298  }
299 
301  template<typename OtherDerived>
302  bool isEqual(const JointDataBase<OtherDerived> & /*other*/) const
303  {
304  return false;
305  ;
306  }
307 
308  bool operator!=(const JointDataBase<Derived> & other) const
309  {
310  return derived().isNotEqual(other.derived());
311  }
312 
314  bool isNotEqual(const JointDataBase<Derived> & other) const
315  {
316  return !(internal::comparison_eq(derived(), other.derived()));
317  }
318 
319  protected:
321  inline JointDataBase()
322  {
323  }
324 
325  }; // struct JointDataBase
326 
327 } // namespace pinocchio
328 
329 #endif // ifndef __pinocchio_multibody_joint_data_base_hpp__
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
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::JointDataBase::isNotEqual
bool isNotEqual(const JointDataBase< Derived > &other) const
&#160;
Definition: joint-data-base.hpp:314
pinocchio::JointDataBase::isEqual
bool isEqual(const JointDataBase< OtherDerived > &) const
&#160;
Definition: joint-data-base.hpp:302
pinocchio::JointDataBase::JointDataBase
JointDataBase()
Default constructor: protected.
Definition: joint-data-base.hpp:321
pinocchio::JointDataBase::isEqual
bool isEqual(const JointDataBase< Derived > &other) const
&#160;
Definition: joint-data-base.hpp:289
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition: fwd.hpp:72