Directory: | ./ |
---|---|
File: | include/pinocchio/multibody/joint/joint-generic.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 52 | 117 | 44.4% |
Branches: | 2 | 14 | 14.3% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2016-2021 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_multibody_joint_generic_hpp__ | ||
6 | #define __pinocchio_multibody_joint_generic_hpp__ | ||
7 | |||
8 | #include "pinocchio/multibody/joint/joint-collection.hpp" | ||
9 | #include "pinocchio/multibody/joint/joint-composite.hpp" | ||
10 | #include "pinocchio/multibody/joint/joint-basic-visitors.hxx" | ||
11 | #include "pinocchio/container/aligned-vector.hpp" | ||
12 | |||
13 | #include <boost/mpl/contains.hpp> | ||
14 | |||
15 | namespace pinocchio | ||
16 | { | ||
17 | |||
18 | template< | ||
19 | typename Scalar, | ||
20 | int Options = context::Options, | ||
21 | template<typename S, int O> class JointCollectionTpl = JointCollectionDefaultTpl> | ||
22 | struct JointTpl; | ||
23 | typedef JointTpl<context::Scalar> Joint; | ||
24 | |||
25 | template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> | ||
26 | struct traits<JointTpl<_Scalar, _Options, JointCollectionTpl>> | ||
27 | { | ||
28 | enum | ||
29 | { | ||
30 | Options = _Options, | ||
31 | NQ = Eigen::Dynamic, // Dynamic because unknown at compile time | ||
32 | NV = Eigen::Dynamic | ||
33 | }; | ||
34 | |||
35 | typedef _Scalar Scalar; | ||
36 | typedef JointCollectionTpl<Scalar, Options> JointCollection; | ||
37 | typedef JointDataTpl<Scalar, Options, JointCollectionTpl> JointDataDerived; | ||
38 | typedef JointModelTpl<Scalar, Options, JointCollectionTpl> JointModelDerived; | ||
39 | |||
40 | typedef JointMotionSubspaceTpl<Eigen::Dynamic, Scalar, Options> Constraint_t; | ||
41 | typedef SE3Tpl<Scalar, Options> Transformation_t; | ||
42 | typedef MotionTpl<Scalar, Options> Motion_t; | ||
43 | typedef MotionTpl<Scalar, Options> Bias_t; | ||
44 | |||
45 | // [ABA] | ||
46 | typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> U_t; | ||
47 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> D_t; | ||
48 | typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> UD_t; | ||
49 | |||
50 | typedef Constraint_t ConstraintTypeConstRef; | ||
51 | typedef Constraint_t ConstraintTypeRef; | ||
52 | typedef Transformation_t TansformTypeConstRef; | ||
53 | typedef Transformation_t TansformTypeRef; | ||
54 | typedef Motion_t MotionTypeConstRef; | ||
55 | typedef Motion_t MotionTypeRef; | ||
56 | typedef Bias_t BiasTypeConstRef; | ||
57 | typedef Bias_t BiasTypeRef; | ||
58 | typedef U_t UTypeConstRef; | ||
59 | typedef U_t UTypeRef; | ||
60 | typedef D_t DTypeConstRef; | ||
61 | typedef D_t DTypeRef; | ||
62 | typedef UD_t UDTypeConstRef; | ||
63 | typedef UD_t UDTypeRef; | ||
64 | |||
65 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> ConfigVector_t; | ||
66 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1, Options> TangentVector_t; | ||
67 | |||
68 | typedef ConfigVector_t ConfigVectorTypeConstRef; | ||
69 | typedef ConfigVector_t ConfigVectorTypeRef; | ||
70 | typedef TangentVector_t TangentVectorTypeConstRef; | ||
71 | typedef TangentVector_t TangentVectorTypeRef; | ||
72 | }; | ||
73 | |||
74 | template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> | ||
75 | struct traits<JointDataTpl<_Scalar, _Options, JointCollectionTpl>> | ||
76 | { | ||
77 | typedef JointTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; | ||
78 | typedef typename traits<JointDerived>::Scalar Scalar; | ||
79 | }; | ||
80 | |||
81 | template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> | ||
82 | struct traits<JointModelTpl<_Scalar, _Options, JointCollectionTpl>> | ||
83 | { | ||
84 | typedef JointTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; | ||
85 | typedef typename traits<JointDerived>::Scalar Scalar; | ||
86 | }; | ||
87 | |||
88 | template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> | ||
89 | struct JointDataTpl | ||
90 | : public JointDataBase<JointDataTpl<_Scalar, _Options, JointCollectionTpl>> | ||
91 | , JointCollectionTpl<_Scalar, _Options>::JointDataVariant | ||
92 | { | ||
93 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
94 | |||
95 | typedef JointTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; | ||
96 | typedef JointDataBase<JointDataTpl> Base; | ||
97 | |||
98 | PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); | ||
99 | |||
100 | typedef JointCollectionTpl<_Scalar, _Options> JointCollection; | ||
101 | typedef typename JointCollection::JointDataVariant JointDataVariant; | ||
102 | |||
103 | using Base::operator==; | ||
104 | using Base::operator!=; | ||
105 | |||
106 | JointDataVariant & toVariant() | ||
107 | { | ||
108 | return *static_cast<JointDataVariant *>(this); | ||
109 | } | ||
110 | ✗ | const JointDataVariant & toVariant() const | |
111 | { | ||
112 | ✗ | return *static_cast<const JointDataVariant *>(this); | |
113 | } | ||
114 | |||
115 | ✗ | ConfigVector_t joint_q() const | |
116 | { | ||
117 | ✗ | return pinocchio::joint_q(*this); | |
118 | } | ||
119 | ✗ | TangentVector_t joint_v() const | |
120 | { | ||
121 | ✗ | return pinocchio::joint_v(*this); | |
122 | } | ||
123 | 27 | Constraint_t S() const | |
124 | { | ||
125 | 27 | return joint_motin_subspace_xd(*this); | |
126 | } | ||
127 | 27 | Transformation_t M() const | |
128 | { | ||
129 | 27 | return joint_transform(*this); | |
130 | } | ||
131 | ✗ | Motion_t v() const | |
132 | { | ||
133 | ✗ | return motion(*this); | |
134 | } | ||
135 | ✗ | Bias_t c() const | |
136 | { | ||
137 | ✗ | return bias(*this); | |
138 | } | ||
139 | |||
140 | // [ABA CCRBA] | ||
141 | 83 | U_t U() const | |
142 | { | ||
143 | 83 | return u_inertia(*this); | |
144 | } | ||
145 | 2 | D_t Dinv() const | |
146 | { | ||
147 | 2 | return dinv_inertia(*this); | |
148 | } | ||
149 | 2 | UD_t UDinv() const | |
150 | { | ||
151 | 2 | return udinv_inertia(*this); | |
152 | } | ||
153 | 29 | D_t StU() const | |
154 | { | ||
155 | 29 | return stu_inertia(*this); | |
156 | } | ||
157 | |||
158 | 13 | JointDataTpl() | |
159 | 13 | : JointDataVariant() | |
160 | { | ||
161 | 13 | } | |
162 | |||
163 | ✗ | JointDataTpl(const JointDataVariant & jdata_variant) | |
164 | ✗ | : JointDataVariant(jdata_variant) | |
165 | { | ||
166 | } | ||
167 | |||
168 | template<typename JointDataDerived> | ||
169 | 36124 | JointDataTpl(const JointDataBase<JointDataDerived> & jdata) | |
170 | 36124 | : JointCollection::JointDataVariant((JointDataVariant)jdata.derived()) | |
171 | { | ||
172 | BOOST_MPL_ASSERT((boost::mpl::contains<typename JointDataVariant::types, JointDataDerived>)); | ||
173 | 36124 | } | |
174 | |||
175 | // Define all the standard accessors | ||
176 | ✗ | ConfigVector_t joint_q_accessor() const | |
177 | { | ||
178 | ✗ | return joint_q(); | |
179 | } | ||
180 | ✗ | TangentVector_t joint_v_accessor() const | |
181 | { | ||
182 | ✗ | return joint_v(); | |
183 | } | ||
184 | ✗ | Constraint_t S_accessor() const | |
185 | { | ||
186 | ✗ | return S(); | |
187 | } | ||
188 | ✗ | Transformation_t M_accessor() const | |
189 | { | ||
190 | ✗ | return M(); | |
191 | } | ||
192 | ✗ | Motion_t v_accessor() const | |
193 | { | ||
194 | ✗ | return v(); | |
195 | } | ||
196 | ✗ | Bias_t c_accessor() const | |
197 | { | ||
198 | ✗ | return c(); | |
199 | } | ||
200 | ✗ | U_t U_accessor() const | |
201 | { | ||
202 | ✗ | return U(); | |
203 | } | ||
204 | ✗ | D_t Dinv_accessor() const | |
205 | { | ||
206 | ✗ | return Dinv(); | |
207 | } | ||
208 | ✗ | UD_t UDinv_accessor() const | |
209 | { | ||
210 | ✗ | return UDinv(); | |
211 | } | ||
212 | D_t StU_accessor() const | ||
213 | { | ||
214 | return StU(); | ||
215 | } | ||
216 | |||
217 | ✗ | static std::string classname() | |
218 | { | ||
219 | ✗ | return "JointData"; | |
220 | } | ||
221 | ✗ | std::string shortname() const | |
222 | { | ||
223 | ✗ | return ::pinocchio::shortname(*this); | |
224 | } | ||
225 | |||
226 | template<typename JointDataDerived> | ||
227 | ✗ | bool isEqual(const JointDataBase<JointDataDerived> & other) const | |
228 | { | ||
229 | ✗ | return ::pinocchio::isEqual(*this, other.derived()); | |
230 | } | ||
231 | |||
232 | ✗ | bool isEqual(const JointDataTpl & other) const | |
233 | { | ||
234 | ✗ | return Base::isEqual(other) && toVariant() == other.toVariant(); | |
235 | } | ||
236 | |||
237 | ✗ | bool operator==(const JointDataTpl & other) const | |
238 | { | ||
239 | ✗ | return isEqual(other); | |
240 | } | ||
241 | |||
242 | ✗ | bool operator!=(const JointDataTpl & other) const | |
243 | { | ||
244 | ✗ | return !(*this == other); | |
245 | } | ||
246 | }; | ||
247 | |||
248 | template< | ||
249 | typename NewScalar, | ||
250 | typename Scalar, | ||
251 | int Options, | ||
252 | template<typename S, int O> | ||
253 | class JointCollectionTpl> | ||
254 | struct CastType<NewScalar, JointModelTpl<Scalar, Options, JointCollectionTpl>> | ||
255 | { | ||
256 | typedef JointModelTpl<NewScalar, Options, JointCollectionTpl> type; | ||
257 | }; | ||
258 | |||
259 | template<typename _Scalar, int _Options, template<typename S, int O> class JointCollectionTpl> | ||
260 | struct JointModelTpl | ||
261 | : JointModelBase<JointModelTpl<_Scalar, _Options, JointCollectionTpl>> | ||
262 | , JointCollectionTpl<_Scalar, _Options>::JointModelVariant | ||
263 | { | ||
264 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
265 | |||
266 | typedef JointTpl<_Scalar, _Options, JointCollectionTpl> JointDerived; | ||
267 | |||
268 | PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); | ||
269 | PINOCCHIO_JOINT_USE_INDEXES(JointModelTpl); | ||
270 | |||
271 | typedef JointCollectionTpl<Scalar, Options> JointCollection; | ||
272 | typedef typename JointCollection::JointDataVariant JointDataVariant; | ||
273 | typedef typename JointCollection::JointModelVariant JointModelVariant; | ||
274 | |||
275 | using Base::id; | ||
276 | using Base::setIndexes; | ||
277 | using Base::operator==; | ||
278 | using Base::operator!=; | ||
279 | |||
280 | 403 | JointModelTpl() | |
281 | 403 | : JointModelVariant() | |
282 | { | ||
283 | 403 | } | |
284 | |||
285 | JointModelTpl(const JointModelVariant & jmodel_variant) | ||
286 | : JointCollection::JointModelVariant(jmodel_variant) | ||
287 | { | ||
288 | } | ||
289 | |||
290 | ✗ | const std::vector<bool> hasConfigurationLimit() const | |
291 | { | ||
292 | ✗ | return ::pinocchio::hasConfigurationLimit(*this); | |
293 | } | ||
294 | |||
295 | ✗ | const std::vector<bool> hasConfigurationLimitInTangent() const | |
296 | { | ||
297 | ✗ | return ::pinocchio::hasConfigurationLimitInTangent(*this); | |
298 | } | ||
299 | |||
300 | template<typename JointModelDerived> | ||
301 | 9995 | JointModelTpl(const JointModelBase<JointModelDerived> & jmodel) | |
302 | 9995 | : JointModelVariant((JointModelVariant)jmodel.derived()) | |
303 | { | ||
304 | BOOST_MPL_ASSERT( | ||
305 | (boost::mpl::contains<typename JointModelVariant::types, JointModelDerived>)); | ||
306 | 9995 | } | |
307 | |||
308 | JointModelVariant & toVariant() | ||
309 | { | ||
310 | return *static_cast<JointModelVariant *>(this); | ||
311 | } | ||
312 | |||
313 | 119 | const JointModelVariant & toVariant() const | |
314 | { | ||
315 | 119 | return *static_cast<const JointModelVariant *>(this); | |
316 | } | ||
317 | |||
318 | 27 | JointDataDerived createData() const | |
319 | { | ||
320 | 27 | return ::pinocchio::createData<Scalar, Options, JointCollectionTpl>(*this); | |
321 | } | ||
322 | |||
323 | template<typename JointModelDerived> | ||
324 | ✗ | bool isEqual(const JointModelBase<JointModelDerived> & other) const | |
325 | { | ||
326 | ✗ | return ::pinocchio::isEqual(*this, other.derived()); | |
327 | } | ||
328 | |||
329 | template<typename JointModelDerived> | ||
330 | 59 | bool hasSameIndexes(const JointModelBase<JointModelDerived> & other) const | |
331 | { | ||
332 | 59 | return ::pinocchio::hasSameIndexes(*this, other.derived()); | |
333 | } | ||
334 | |||
335 | 59 | bool isEqual(const JointModelTpl & other) const | |
336 | { | ||
337 |
2/4✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 59 times.
✗ Branch 7 not taken.
|
59 | return Base::isEqual(other) && toVariant() == other.toVariant(); |
338 | } | ||
339 | |||
340 | 59 | bool operator==(const JointModelTpl & other) const | |
341 | { | ||
342 | 59 | return isEqual(other); | |
343 | } | ||
344 | |||
345 | ✗ | bool operator!=(const JointModelTpl & other) const | |
346 | { | ||
347 | ✗ | return !(*this == other); | |
348 | } | ||
349 | |||
350 | template<typename ConfigVector> | ||
351 | 27 | void calc(JointDataDerived & data, const Eigen::MatrixBase<ConfigVector> & q) const | |
352 | { | ||
353 | 27 | calc_zero_order(*this, data, q.derived()); | |
354 | 27 | } | |
355 | |||
356 | template<typename TangentVector> | ||
357 | void calc( | ||
358 | JointDataDerived & data, const Blank blank, const Eigen::MatrixBase<TangentVector> & v) const | ||
359 | { | ||
360 | calc_first_order(*this, data, blank, v.derived()); | ||
361 | } | ||
362 | |||
363 | template<typename ConfigVector, typename TangentVector> | ||
364 | ✗ | void calc( | |
365 | JointDataDerived & data, | ||
366 | const Eigen::MatrixBase<ConfigVector> & q, | ||
367 | const Eigen::MatrixBase<TangentVector> & v) const | ||
368 | { | ||
369 | ✗ | calc_first_order(*this, data, q.derived(), v.derived()); | |
370 | } | ||
371 | |||
372 | template<typename VectorLike, typename Matrix6Like> | ||
373 | ✗ | void calc_aba( | |
374 | JointDataDerived & data, | ||
375 | const Eigen::MatrixBase<VectorLike> & armature, | ||
376 | const Eigen::MatrixBase<Matrix6Like> & I, | ||
377 | const bool update_I) const | ||
378 | { | ||
379 | ✗ | ::pinocchio::calc_aba( | |
380 | ✗ | *this, data, armature.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I), update_I); | |
381 | } | ||
382 | |||
383 | 31 | std::string shortname() const | |
384 | { | ||
385 | 31 | return ::pinocchio::shortname(*this); | |
386 | } | ||
387 | ✗ | static std::string classname() | |
388 | { | ||
389 | ✗ | return "JointModel"; | |
390 | } | ||
391 | |||
392 | 819676 | int nq_impl() const | |
393 | { | ||
394 | 819676 | return ::pinocchio::nq(*this); | |
395 | } | ||
396 | 8387287 | int nv_impl() const | |
397 | { | ||
398 | 8387287 | return ::pinocchio::nv(*this); | |
399 | } | ||
400 | |||
401 | 1191839 | int idx_q_impl() const | |
402 | { | ||
403 | 1191839 | return ::pinocchio::idx_q(*this); | |
404 | } | ||
405 | 3577772 | int idx_v_impl() const | |
406 | { | ||
407 | 3577772 | return ::pinocchio::idx_v(*this); | |
408 | } | ||
409 | |||
410 | 95 | JointIndex id_impl() const | |
411 | { | ||
412 | 95 | return ::pinocchio::id(*this); | |
413 | } | ||
414 | |||
415 | 10642 | void setIndexes(JointIndex id, int nq, int nv) | |
416 | { | ||
417 | 10642 | ::pinocchio::setIndexes(*this, id, nq, nv); | |
418 | 10642 | } | |
419 | |||
420 | /// \returns An expression of *this with the Scalar type casted to NewScalar. | ||
421 | template<typename NewScalar> | ||
422 | ✗ | JointModelTpl<NewScalar, Options, JointCollectionTpl> cast() const | |
423 | { | ||
424 | ✗ | return cast_joint<NewScalar, Scalar, Options, JointCollectionTpl>(*this); | |
425 | } | ||
426 | }; | ||
427 | |||
428 | typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector; | ||
429 | typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointModel) JointModelVector; | ||
430 | |||
431 | template< | ||
432 | typename Scalar, | ||
433 | int Options, | ||
434 | template<typename S, int O> | ||
435 | class JointCollectionTpl, | ||
436 | typename JointDataDerived> | ||
437 | ✗ | bool operator==( | |
438 | const JointDataBase<JointDataDerived> & joint_data, | ||
439 | const JointDataTpl<Scalar, Options, JointCollectionTpl> & joint_data_generic) | ||
440 | { | ||
441 | ✗ | return joint_data_generic == joint_data.derived(); | |
442 | } | ||
443 | |||
444 | template< | ||
445 | typename Scalar, | ||
446 | int Options, | ||
447 | template<typename S, int O> | ||
448 | class JointCollectionTpl, | ||
449 | typename JointDataDerived> | ||
450 | ✗ | bool operator!=( | |
451 | const JointDataBase<JointDataDerived> & joint_data, | ||
452 | const JointDataTpl<Scalar, Options, JointCollectionTpl> & joint_data_generic) | ||
453 | { | ||
454 | ✗ | return joint_data_generic != joint_data.derived(); | |
455 | } | ||
456 | |||
457 | template< | ||
458 | typename Scalar, | ||
459 | int Options, | ||
460 | template<typename S, int O> | ||
461 | class JointCollectionTpl, | ||
462 | typename JointModelDerived> | ||
463 | ✗ | bool operator==( | |
464 | const JointModelBase<JointModelDerived> & joint_model, | ||
465 | const JointModelTpl<Scalar, Options, JointCollectionTpl> & joint_model_generic) | ||
466 | { | ||
467 | ✗ | return joint_model_generic == joint_model.derived(); | |
468 | } | ||
469 | |||
470 | template< | ||
471 | typename Scalar, | ||
472 | int Options, | ||
473 | template<typename S, int O> | ||
474 | class JointCollectionTpl, | ||
475 | typename JointModelDerived> | ||
476 | bool operator!=( | ||
477 | const JointModelBase<JointModelDerived> & joint_model, | ||
478 | const JointModelTpl<Scalar, Options, JointCollectionTpl> & joint_model_generic) | ||
479 | { | ||
480 | return joint_model_generic != joint_model.derived(); | ||
481 | } | ||
482 | |||
483 | } // namespace pinocchio | ||
484 | |||
485 | #endif // ifndef __pinocchio_multibody_joint_generic_hpp__ | ||
486 |