pinocchio  3.9.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
Loading...
Searching...
No Matches
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
157namespace 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::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