Directory: | ./ |
---|---|
File: | include/pinocchio/multibody/joint/joint-basic-visitors.hxx |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 98 | 164 | 59.8% |
Branches: | 19 | 62 | 30.6% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2016-2020 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_multibody_joint_basic_visitors_hxx__ | ||
6 | #define __pinocchio_multibody_joint_basic_visitors_hxx__ | ||
7 | |||
8 | #include "pinocchio/multibody/joint/joint-basic-visitors.hpp" | ||
9 | #include "pinocchio/multibody/visitor.hpp" | ||
10 | |||
11 | namespace pinocchio | ||
12 | { | ||
13 | /// @cond DEV | ||
14 | |||
15 | /** | ||
16 | * @brief CreateJointData visitor | ||
17 | */ | ||
18 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
19 | struct CreateJointData : boost::static_visitor<JointDataTpl<Scalar, Options, JointCollectionTpl>> | ||
20 | { | ||
21 | typedef JointCollectionTpl<Scalar, Options> JointCollection; | ||
22 | typedef typename JointCollection::JointModelVariant JointModelVariant; | ||
23 | typedef JointDataTpl<Scalar, Options, JointCollectionTpl> JointDataVariant; | ||
24 | |||
25 | template<typename JointModelDerived> | ||
26 | 36124 | JointDataVariant operator()(const JointModelBase<JointModelDerived> & jmodel) const | |
27 | { | ||
28 |
1/2✓ Branch 2 taken 18062 times.
✗ Branch 3 not taken.
|
36124 | return JointDataVariant(jmodel.createData()); |
29 | } | ||
30 | |||
31 | 18062 | static JointDataVariant run(const JointModelVariant & jmodel) | |
32 | { | ||
33 |
1/2✓ Branch 1 taken 18062 times.
✗ Branch 2 not taken.
|
36124 | return boost::apply_visitor(CreateJointData(), jmodel); |
34 | } | ||
35 | }; | ||
36 | |||
37 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
38 | inline JointDataTpl<Scalar, Options, JointCollectionTpl> | ||
39 | 42 | createData(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel) | |
40 | { | ||
41 | 42 | return CreateJointData<Scalar, Options, JointCollectionTpl>::run(jmodel); | |
42 | } | ||
43 | |||
44 | /** | ||
45 | * @brief JointCalcZeroOrderVisitor fusion visitor | ||
46 | */ | ||
47 | template<typename ConfigVectorType> | ||
48 | struct JointCalcZeroOrderVisitor | ||
49 | : fusion::JointUnaryVisitorBase<JointCalcZeroOrderVisitor<ConfigVectorType>> | ||
50 | { | ||
51 | typedef boost::fusion::vector<const ConfigVectorType &> ArgsType; | ||
52 | |||
53 | template<typename JointModel> | ||
54 | 54 | static void algo( | |
55 | const pinocchio::JointModelBase<JointModel> & jmodel, | ||
56 | pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
57 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
58 | { | ||
59 | 54 | jmodel.calc(jdata.derived(), q.derived()); | |
60 | } | ||
61 | }; | ||
62 | |||
63 | template< | ||
64 | typename Scalar, | ||
65 | int Options, | ||
66 | template<typename S, int O> | ||
67 | class JointCollectionTpl, | ||
68 | typename ConfigVectorType> | ||
69 | 27 | inline void calc_zero_order( | |
70 | const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel, | ||
71 | JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata, | ||
72 | const Eigen::MatrixBase<ConfigVectorType> & q) | ||
73 | { | ||
74 | typedef JointCalcZeroOrderVisitor<ConfigVectorType> Algo; | ||
75 | |||
76 |
1/2✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
|
27 | Algo::run(jmodel, jdata, typename Algo::ArgsType(q.derived())); |
77 | 27 | } | |
78 | |||
79 | /** | ||
80 | * @brief JointCalcFirstOrderVisitor fusion visitor | ||
81 | */ | ||
82 | template<typename ConfigVectorType, typename TangentVectorType> | ||
83 | struct JointCalcFirstOrderVisitor | ||
84 | : fusion::JointUnaryVisitorBase<JointCalcFirstOrderVisitor<ConfigVectorType, TangentVectorType>> | ||
85 | { | ||
86 | typedef boost::fusion::vector<const ConfigVectorType &, const TangentVectorType &> ArgsType; | ||
87 | |||
88 | template<typename JointModel> | ||
89 | ✗ | static void algo( | |
90 | const pinocchio::JointModelBase<JointModel> & jmodel, | ||
91 | pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
92 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
93 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
94 | { | ||
95 | ✗ | jmodel.calc(jdata.derived(), q.derived(), v.derived()); | |
96 | } | ||
97 | }; | ||
98 | |||
99 | template< | ||
100 | typename Scalar, | ||
101 | int Options, | ||
102 | template<typename S, int O> | ||
103 | class JointCollectionTpl, | ||
104 | typename ConfigVectorType, | ||
105 | typename TangentVectorType> | ||
106 | ✗ | inline void calc_first_order( | |
107 | const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel, | ||
108 | JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata, | ||
109 | const Eigen::MatrixBase<ConfigVectorType> & q, | ||
110 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
111 | { | ||
112 | typedef JointCalcFirstOrderVisitor<ConfigVectorType, TangentVectorType> Algo; | ||
113 | |||
114 | ✗ | Algo::run(jmodel, jdata, typename Algo::ArgsType(q.derived(), v.derived())); | |
115 | } | ||
116 | |||
117 | /** | ||
118 | * @brief JointCalcFirstOrderVisitor fusion visitor | ||
119 | */ | ||
120 | template<typename TangentVectorType> | ||
121 | struct JointCalcFirstOrderVisitor<Blank, TangentVectorType> | ||
122 | : fusion::JointUnaryVisitorBase<JointCalcFirstOrderVisitor<Blank, TangentVectorType>> | ||
123 | { | ||
124 | typedef boost::fusion::vector<const Blank, const TangentVectorType &> ArgsType; | ||
125 | |||
126 | template<typename JointModel> | ||
127 | static void algo( | ||
128 | const pinocchio::JointModelBase<JointModel> & jmodel, | ||
129 | pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
130 | const Blank blank, | ||
131 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
132 | { | ||
133 | jmodel.calc(jdata.derived(), blank, v.derived()); | ||
134 | } | ||
135 | }; | ||
136 | |||
137 | template< | ||
138 | typename Scalar, | ||
139 | int Options, | ||
140 | template<typename S, int O> | ||
141 | class JointCollectionTpl, | ||
142 | typename TangentVectorType> | ||
143 | inline void calc_first_order( | ||
144 | const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel, | ||
145 | JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata, | ||
146 | const Blank blank, | ||
147 | const Eigen::MatrixBase<TangentVectorType> & v) | ||
148 | { | ||
149 | typedef JointCalcFirstOrderVisitor<Blank, TangentVectorType> Algo; | ||
150 | |||
151 | Algo::run(jmodel, jdata, typename Algo::ArgsType(blank, v.derived())); | ||
152 | } | ||
153 | |||
154 | /** | ||
155 | * @brief JointCalcAbaVisitor fusion visitor | ||
156 | */ | ||
157 | |||
158 | template<typename VectorLike, typename Matrix6Type> | ||
159 | struct JointCalcAbaVisitor | ||
160 | : fusion::JointUnaryVisitorBase<JointCalcAbaVisitor<VectorLike, Matrix6Type>> | ||
161 | { | ||
162 | |||
163 | typedef boost::fusion::vector<const VectorLike &, Matrix6Type &, bool> ArgsType; | ||
164 | |||
165 | template<typename JointModel> | ||
166 | ✗ | static void algo( | |
167 | const pinocchio::JointModelBase<JointModel> & jmodel, | ||
168 | pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata, | ||
169 | const Eigen::MatrixBase<VectorLike> & armature, | ||
170 | const Eigen::MatrixBase<Matrix6Type> & I, | ||
171 | bool update_I) | ||
172 | { | ||
173 | ✗ | jmodel.calc_aba( | |
174 | ✗ | jdata.derived(), armature.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type, I), update_I); | |
175 | } | ||
176 | }; | ||
177 | |||
178 | template< | ||
179 | typename Scalar, | ||
180 | int Options, | ||
181 | template<typename S, int O> | ||
182 | class JointCollectionTpl, | ||
183 | typename VectorLike, | ||
184 | typename Matrix6Type> | ||
185 | ✗ | inline void calc_aba( | |
186 | const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel, | ||
187 | JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata, | ||
188 | const Eigen::MatrixBase<VectorLike> & armature, | ||
189 | const Eigen::MatrixBase<Matrix6Type> & I, | ||
190 | const bool update_I) | ||
191 | { | ||
192 | typedef JointCalcAbaVisitor<VectorLike, Matrix6Type> Algo; | ||
193 | ✗ | Algo::run( | |
194 | jmodel, jdata, | ||
195 | ✗ | typename Algo::ArgsType( | |
196 | ✗ | PINOCCHIO_EIGEN_CONST_CAST(VectorLike, armature), | |
197 | ✗ | PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type, I), update_I)); | |
198 | } | ||
199 | |||
200 | /** | ||
201 | * @brief JointNvVisitor visitor | ||
202 | */ | ||
203 | struct JointNvVisitor : boost::static_visitor<int> | ||
204 | { | ||
205 | template<typename JointModelDerived> | ||
206 | 16789032 | int operator()(const JointModelBase<JointModelDerived> & jmodel) const | |
207 | { | ||
208 | 16789032 | return jmodel.nv(); | |
209 | } | ||
210 | |||
211 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
212 | 8394516 | static int run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel) | |
213 | { | ||
214 |
1/2✓ Branch 1 taken 8394516 times.
✗ Branch 2 not taken.
|
8394516 | return boost::apply_visitor(JointNvVisitor(), jmodel); |
215 | } | ||
216 | }; | ||
217 | |||
218 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
219 | 8394516 | inline int nv(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel) | |
220 | { | ||
221 | 8394516 | return JointNvVisitor::run(jmodel); | |
222 | } | ||
223 | |||
224 | /** | ||
225 | * @brief JointNqVisitor visitor | ||
226 | */ | ||
227 | struct JointNqVisitor : boost::static_visitor<int> | ||
228 | { | ||
229 | template<typename JointModelDerived> | ||
230 | 1636220 | int operator()(const JointModelBase<JointModelDerived> & jmodel) const | |
231 | { | ||
232 | 1636220 | return jmodel.nq(); | |
233 | } | ||
234 | |||
235 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
236 | 818110 | static int run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel) | |
237 | { | ||
238 |
1/2✓ Branch 1 taken 818110 times.
✗ Branch 2 not taken.
|
818110 | return boost::apply_visitor(JointNqVisitor(), jmodel); |
239 | } | ||
240 | }; | ||
241 | |||
242 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
243 | 818110 | inline int nq(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel) | |
244 | { | ||
245 | 818110 | return JointNqVisitor::run(jmodel); | |
246 | } | ||
247 | |||
248 | /** | ||
249 | * @brief JointConfigurationLimitVisitor visitor | ||
250 | */ | ||
251 | struct JointConfigurationLimitVisitor : boost::static_visitor<std::vector<bool>> | ||
252 | { | ||
253 | template<typename JointModelDerived> | ||
254 | ✗ | const std::vector<bool> operator()(const JointModelBase<JointModelDerived> & jmodel) const | |
255 | { | ||
256 | ✗ | return jmodel.hasConfigurationLimit(); | |
257 | } | ||
258 | |||
259 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
260 | static const std::vector<bool> | ||
261 | ✗ | run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel) | |
262 | { | ||
263 | ✗ | return boost::apply_visitor(JointConfigurationLimitVisitor(), jmodel); | |
264 | } | ||
265 | }; | ||
266 | |||
267 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
268 | inline const std::vector<bool> | ||
269 | ✗ | hasConfigurationLimit(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel) | |
270 | { | ||
271 | ✗ | return JointConfigurationLimitVisitor::run(jmodel); | |
272 | } | ||
273 | |||
274 | /** | ||
275 | * @brief JointConfigurationLimitInTangentVisitor visitor | ||
276 | */ | ||
277 | struct JointConfigurationLimitInTangentVisitor : boost::static_visitor<std::vector<bool>> | ||
278 | { | ||
279 | template<typename JointModelDerived> | ||
280 | ✗ | const std::vector<bool> operator()(const JointModelBase<JointModelDerived> & jmodel) const | |
281 | { | ||
282 | ✗ | return jmodel.hasConfigurationLimitInTangent(); | |
283 | } | ||
284 | |||
285 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
286 | static const std::vector<bool> | ||
287 | ✗ | run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel) | |
288 | { | ||
289 | ✗ | return boost::apply_visitor(JointConfigurationLimitInTangentVisitor(), jmodel); | |
290 | } | ||
291 | }; | ||
292 | |||
293 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
294 | inline const std::vector<bool> | ||
295 | ✗ | hasConfigurationLimitInTangent(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel) | |
296 | { | ||
297 | ✗ | return JointConfigurationLimitInTangentVisitor::run(jmodel); | |
298 | } | ||
299 | |||
300 | /** | ||
301 | * @brief JointIdxQVisitor visitor | ||
302 | */ | ||
303 | struct JointIdxQVisitor : boost::static_visitor<int> | ||
304 | { | ||
305 | template<typename JointModelDerived> | ||
306 | 2383678 | int operator()(const JointModelBase<JointModelDerived> & jmodel) const | |
307 | { | ||
308 | 2383678 | return jmodel.idx_q(); | |
309 | } | ||
310 | |||
311 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
312 | 1191839 | static int run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel) | |
313 | { | ||
314 |
1/2✓ Branch 1 taken 1191839 times.
✗ Branch 2 not taken.
|
1191839 | return boost::apply_visitor(JointIdxQVisitor(), jmodel); |
315 | } | ||
316 | }; | ||
317 | |||
318 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
319 | 1191839 | inline int idx_q(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel) | |
320 | { | ||
321 | 1191839 | return JointIdxQVisitor::run(jmodel); | |
322 | } | ||
323 | |||
324 | /** | ||
325 | * @brief JointIdxVVisitor visitor | ||
326 | */ | ||
327 | struct JointIdxVVisitor : boost::static_visitor<int> | ||
328 | { | ||
329 | template<typename JointModelDerived> | ||
330 | 7177912 | int operator()(const JointModelBase<JointModelDerived> & jmodel) const | |
331 | { | ||
332 | 7177912 | return jmodel.idx_v(); | |
333 | } | ||
334 | |||
335 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
336 | 3588956 | static int run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel) | |
337 | { | ||
338 |
1/2✓ Branch 1 taken 3588956 times.
✗ Branch 2 not taken.
|
3588956 | return boost::apply_visitor(JointIdxVVisitor(), jmodel); |
339 | } | ||
340 | }; | ||
341 | |||
342 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
343 | 3588956 | inline int idx_v(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel) | |
344 | { | ||
345 | 3588956 | return JointIdxVVisitor::run(jmodel); | |
346 | } | ||
347 | |||
348 | /** | ||
349 | * @brief JointIdVisitor visitor | ||
350 | */ | ||
351 | struct JointIdVisitor : boost::static_visitor<JointIndex> | ||
352 | { | ||
353 | template<typename JointModelDerived> | ||
354 | 190 | JointIndex operator()(const JointModelBase<JointModelDerived> & jmodel) const | |
355 | { | ||
356 | 190 | return jmodel.id(); | |
357 | } | ||
358 | |||
359 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
360 | 95 | static JointIndex run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel) | |
361 | { | ||
362 |
1/2✓ Branch 1 taken 95 times.
✗ Branch 2 not taken.
|
95 | return boost::apply_visitor(JointIdVisitor(), jmodel); |
363 | } | ||
364 | }; | ||
365 | |||
366 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
367 | 95 | inline JointIndex id(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel) | |
368 | { | ||
369 | 95 | return JointIdVisitor::run(jmodel); | |
370 | } | ||
371 | |||
372 | /** | ||
373 | * @brief JointSetIndexesVisitor visitor | ||
374 | */ | ||
375 | struct JointSetIndexesVisitor : boost::static_visitor<> | ||
376 | { | ||
377 | JointIndex id; | ||
378 | int q; | ||
379 | int v; | ||
380 | |||
381 | 6381 | JointSetIndexesVisitor(JointIndex id, int q, int v) | |
382 | 6381 | : id(id) | |
383 | 6381 | , q(q) | |
384 | 6381 | , v(v) | |
385 | { | ||
386 | 6381 | } | |
387 | |||
388 | template<typename JointModelDerived> | ||
389 | 12762 | void operator()(JointModelBase<JointModelDerived> & jmodel) const | |
390 | { | ||
391 | 12762 | jmodel.setIndexes(id, q, v); | |
392 | } | ||
393 | |||
394 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
395 | static void | ||
396 | 6381 | run(JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel, JointIndex id, int q, int v) | |
397 | { | ||
398 |
1/2✓ Branch 2 taken 6381 times.
✗ Branch 3 not taken.
|
6381 | return boost::apply_visitor(JointSetIndexesVisitor(id, q, v), jmodel); |
399 | } | ||
400 | }; | ||
401 | |||
402 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
403 | 6381 | inline void setIndexes( | |
404 | JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel, JointIndex id, int q, int v) | ||
405 | { | ||
406 | 6381 | return JointSetIndexesVisitor::run(jmodel, id, q, v); | |
407 | } | ||
408 | |||
409 | /** | ||
410 | * @brief JointModelShortnameVisitor visitor | ||
411 | */ | ||
412 | struct JointModelShortnameVisitor : boost::static_visitor<std::string> | ||
413 | { | ||
414 | template<typename JointModelDerived> | ||
415 | 62 | std::string operator()(const JointModelBase<JointModelDerived> & jmodel) const | |
416 | { | ||
417 | 62 | return jmodel.shortname(); | |
418 | } | ||
419 | |||
420 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
421 | 31 | static std::string run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel) | |
422 | { | ||
423 |
1/2✓ Branch 1 taken 31 times.
✗ Branch 2 not taken.
|
62 | return boost::apply_visitor(JointModelShortnameVisitor(), jmodel); |
424 | } | ||
425 | }; | ||
426 | |||
427 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
428 | 31 | inline std::string shortname(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel) | |
429 | { | ||
430 | 31 | return JointModelShortnameVisitor::run(jmodel); | |
431 | } | ||
432 | |||
433 | template< | ||
434 | typename NewScalar, | ||
435 | typename Scalar, | ||
436 | int Options, | ||
437 | template<typename S, int O> | ||
438 | class JointCollectionTpl> | ||
439 | struct JointCastVisitor | ||
440 | : fusion::JointUnaryVisitorBase< | ||
441 | JointCastVisitor<NewScalar, Scalar, Options, JointCollectionTpl>, | ||
442 | typename CastType<NewScalar, JointModelTpl<Scalar, Options, JointCollectionTpl>>::type> | ||
443 | { | ||
444 | typedef fusion::NoArg ArgsType; | ||
445 | |||
446 | typedef typename CastType<NewScalar, JointModelTpl<Scalar, Options, JointCollectionTpl>>::type | ||
447 | ReturnType; | ||
448 | |||
449 | template<typename JointModelDerived> | ||
450 | ✗ | static ReturnType algo(const JointModelBase<JointModelDerived> & jmodel) | |
451 | { | ||
452 | ✗ | return ReturnType(jmodel.template cast<NewScalar>()); | |
453 | } | ||
454 | }; | ||
455 | |||
456 | template< | ||
457 | typename NewScalar, | ||
458 | typename Scalar, | ||
459 | int Options, | ||
460 | template<typename S, int O> | ||
461 | class JointCollectionTpl> | ||
462 | typename CastType<NewScalar, JointModelTpl<Scalar, Options, JointCollectionTpl>>::type | ||
463 | ✗ | cast_joint(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel) | |
464 | { | ||
465 | typedef JointCastVisitor<NewScalar, Scalar, Options, JointCollectionTpl> Algo; | ||
466 | ✗ | return Algo::run(jmodel); | |
467 | } | ||
468 | |||
469 | template< | ||
470 | typename Scalar, | ||
471 | int Options, | ||
472 | template<typename S, int O> | ||
473 | class JointCollectionTpl, | ||
474 | typename JointModelDerived> | ||
475 | struct JointModelComparisonOperatorVisitor | ||
476 | : fusion::JointUnaryVisitorBase< | ||
477 | JointModelComparisonOperatorVisitor<Scalar, Options, JointCollectionTpl, JointModelDerived>, | ||
478 | bool> | ||
479 | { | ||
480 | typedef boost::fusion::vector<const JointModelDerived &> ArgsType; | ||
481 | |||
482 | template<typename JointModel> | ||
483 | static bool | ||
484 | ✗ | algo(const JointModelBase<JointModel> & jmodel_lhs, const JointModelDerived & jmodel_rhs) | |
485 | { | ||
486 | ✗ | return internal::comparison_eq(jmodel_lhs.derived(), jmodel_rhs); | |
487 | } | ||
488 | }; | ||
489 | |||
490 | template< | ||
491 | typename Scalar, | ||
492 | int Options, | ||
493 | template<typename S, int O> | ||
494 | class JointCollectionTpl, | ||
495 | typename JointModelDerived> | ||
496 | ✗ | bool isEqual( | |
497 | const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel_generic, | ||
498 | const JointModelBase<JointModelDerived> & jmodel) | ||
499 | { | ||
500 | typedef JointModelComparisonOperatorVisitor< | ||
501 | Scalar, Options, JointCollectionTpl, JointModelDerived> | ||
502 | Algo; | ||
503 | ✗ | return Algo::run(jmodel_generic, typename Algo::ArgsType(boost::ref(jmodel.derived()))); | |
504 | } | ||
505 | |||
506 | template< | ||
507 | typename Scalar, | ||
508 | int Options, | ||
509 | template<typename S, int O> | ||
510 | class JointCollectionTpl, | ||
511 | typename JointModelDerived> | ||
512 | struct JointModelHasSameIndexesVisitor | ||
513 | : fusion::JointUnaryVisitorBase< | ||
514 | JointModelHasSameIndexesVisitor<Scalar, Options, JointCollectionTpl, JointModelDerived>, | ||
515 | bool> | ||
516 | { | ||
517 | typedef boost::fusion::vector<const JointModelDerived &> ArgsType; | ||
518 | |||
519 | template<typename JointModel> | ||
520 | static bool | ||
521 | 118 | algo(const JointModelBase<JointModel> & jmodel_lhs, const JointModelDerived & jmodel_rhs) | |
522 | { | ||
523 | 118 | return jmodel_lhs.derived().hasSameIndexes(jmodel_rhs); | |
524 | } | ||
525 | }; | ||
526 | |||
527 | template< | ||
528 | typename Scalar, | ||
529 | int Options, | ||
530 | template<typename S, int O> | ||
531 | class JointCollectionTpl, | ||
532 | typename JointModelDerived> | ||
533 | 59 | bool hasSameIndexes( | |
534 | const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel_generic, | ||
535 | const JointModelBase<JointModelDerived> & jmodel) | ||
536 | { | ||
537 | typedef JointModelHasSameIndexesVisitor<Scalar, Options, JointCollectionTpl, JointModelDerived> | ||
538 | Algo; | ||
539 |
2/4✓ Branch 2 taken 59 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 59 times.
✗ Branch 6 not taken.
|
118 | return Algo::run(jmodel_generic, typename Algo::ArgsType(boost::ref(jmodel.derived()))); |
540 | } | ||
541 | |||
542 | // | ||
543 | // Visitors on JointDatas | ||
544 | // | ||
545 | |||
546 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
547 | struct JointQVisitor | ||
548 | : boost::static_visitor< | ||
549 | typename JointDataTpl<Scalar, Options, JointCollectionTpl>::ConfigVector_t> | ||
550 | { | ||
551 | typedef typename JointDataTpl<Scalar, Options, JointCollectionTpl>::ConfigVector_t ReturnType; | ||
552 | |||
553 | template<typename JointDataDerived> | ||
554 | ✗ | ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const | |
555 | { | ||
556 | ✗ | return jdata.joint_q(); | |
557 | } | ||
558 | |||
559 | ✗ | static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
560 | { | ||
561 | ✗ | return boost::apply_visitor(JointQVisitor(), jdata); | |
562 | } | ||
563 | }; | ||
564 | |||
565 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
566 | inline typename JointDataTpl<Scalar, Options, JointCollectionTpl>::ConfigVector_t | ||
567 | ✗ | joint_q(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
568 | { | ||
569 | ✗ | return JointQVisitor<Scalar, Options, JointCollectionTpl>::run(jdata); | |
570 | } | ||
571 | |||
572 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
573 | struct JointVVisitor | ||
574 | : boost::static_visitor< | ||
575 | typename JointDataTpl<Scalar, Options, JointCollectionTpl>::ConfigVector_t> | ||
576 | { | ||
577 | typedef typename JointDataTpl<Scalar, Options, JointCollectionTpl>::TangentVector_t ReturnType; | ||
578 | |||
579 | template<typename JointDataDerived> | ||
580 | ✗ | ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const | |
581 | { | ||
582 | ✗ | return jdata.joint_v(); | |
583 | } | ||
584 | |||
585 | ✗ | static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
586 | { | ||
587 | ✗ | return boost::apply_visitor(JointVVisitor(), jdata); | |
588 | } | ||
589 | }; | ||
590 | |||
591 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
592 | inline typename JointDataTpl<Scalar, Options, JointCollectionTpl>::TangentVector_t | ||
593 | ✗ | joint_v(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
594 | { | ||
595 | ✗ | return JointVVisitor<Scalar, Options, JointCollectionTpl>::run(jdata); | |
596 | } | ||
597 | |||
598 | /** | ||
599 | * @brief JointConstraintVisitor visitor | ||
600 | */ | ||
601 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
602 | struct JointConstraintVisitor | ||
603 | : boost::static_visitor<JointMotionSubspaceTpl<Eigen::Dynamic, Scalar, Options>> | ||
604 | { | ||
605 | typedef JointMotionSubspaceTpl<Eigen::Dynamic, Scalar, Options> ReturnType; | ||
606 | |||
607 | template<typename JointDataDerived> | ||
608 | 54 | ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const | |
609 | { | ||
610 |
1/2✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
|
54 | return ReturnType(jdata.S().matrix()); |
611 | } | ||
612 | |||
613 | 27 | static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
614 | { | ||
615 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
54 | return boost::apply_visitor(JointConstraintVisitor(), jdata); |
616 | } | ||
617 | }; | ||
618 | |||
619 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
620 | inline JointMotionSubspaceTpl<Eigen::Dynamic, Scalar, Options> | ||
621 | 27 | joint_motin_subspace_xd(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
622 | { | ||
623 | 27 | return JointConstraintVisitor<Scalar, Options, JointCollectionTpl>::run(jdata); | |
624 | } | ||
625 | |||
626 | /** | ||
627 | * @brief JointTransformVisitor visitor | ||
628 | */ | ||
629 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
630 | struct JointTransformVisitor : boost::static_visitor<SE3Tpl<Scalar, Options>> | ||
631 | { | ||
632 | typedef SE3Tpl<Scalar, Options> ReturnType; | ||
633 | |||
634 | template<typename JointDataDerived> | ||
635 | 54 | ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const | |
636 | { | ||
637 | 54 | return ReturnType(jdata.M()); | |
638 | } | ||
639 | |||
640 | 27 | static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
641 | { | ||
642 |
1/2✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
|
54 | return boost::apply_visitor(JointTransformVisitor(), jdata); |
643 | } | ||
644 | }; | ||
645 | |||
646 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
647 | inline SE3Tpl<Scalar, Options> | ||
648 | 27 | joint_transform(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
649 | { | ||
650 | 27 | return JointTransformVisitor<Scalar, Options, JointCollectionTpl>::run(jdata); | |
651 | } | ||
652 | |||
653 | /** | ||
654 | * @brief JointMotionVisitor visitor | ||
655 | */ | ||
656 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
657 | struct JointMotionVisitor : boost::static_visitor<MotionTpl<Scalar, Options>> | ||
658 | { | ||
659 | typedef MotionTpl<Scalar, Options> ReturnType; | ||
660 | |||
661 | template<typename JointDataDerived> | ||
662 | ✗ | ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const | |
663 | { | ||
664 | ✗ | return ReturnType(jdata.v()); | |
665 | } | ||
666 | |||
667 | ✗ | static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
668 | { | ||
669 | ✗ | return boost::apply_visitor(JointMotionVisitor(), jdata); | |
670 | } | ||
671 | }; | ||
672 | |||
673 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
674 | inline MotionTpl<Scalar, Options> | ||
675 | ✗ | motion(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
676 | { | ||
677 | ✗ | return JointMotionVisitor<Scalar, Options, JointCollectionTpl>::run(jdata); | |
678 | } | ||
679 | |||
680 | /** | ||
681 | * @brief JointBiasVisitor visitor | ||
682 | */ | ||
683 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
684 | struct JointBiasVisitor : boost::static_visitor<MotionTpl<Scalar, Options>> | ||
685 | { | ||
686 | typedef MotionTpl<Scalar, Options> ReturnType; | ||
687 | |||
688 | template<typename JointDataDerived> | ||
689 | ✗ | ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const | |
690 | { | ||
691 | ✗ | return ReturnType(jdata.c()); | |
692 | } | ||
693 | |||
694 | ✗ | static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
695 | { | ||
696 | ✗ | return boost::apply_visitor(JointBiasVisitor(), jdata); | |
697 | } | ||
698 | }; | ||
699 | |||
700 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
701 | inline MotionTpl<Scalar, Options> | ||
702 | ✗ | bias(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
703 | { | ||
704 | ✗ | return JointBiasVisitor<Scalar, Options, JointCollectionTpl>::run(jdata); | |
705 | } | ||
706 | |||
707 | /** | ||
708 | * @brief JointUInertiaVisitor visitor | ||
709 | */ | ||
710 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
711 | struct JointUInertiaVisitor | ||
712 | : boost::static_visitor<Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options>> | ||
713 | { | ||
714 | typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> ReturnType; | ||
715 | |||
716 | template<typename JointDataDerived> | ||
717 | 166 | ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const | |
718 | { | ||
719 | 166 | return ReturnType(jdata.U()); | |
720 | } | ||
721 | |||
722 | 83 | static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
723 | { | ||
724 |
1/2✓ Branch 1 taken 83 times.
✗ Branch 2 not taken.
|
166 | return boost::apply_visitor(JointUInertiaVisitor(), jdata); |
725 | } | ||
726 | }; | ||
727 | |||
728 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
729 | inline Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> | ||
730 | 83 | u_inertia(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
731 | { | ||
732 | 83 | return JointUInertiaVisitor<Scalar, Options, JointCollectionTpl>::run(jdata); | |
733 | } | ||
734 | |||
735 | /** | ||
736 | * @brief JointDInvInertiaVisitor visitor | ||
737 | */ | ||
738 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
739 | struct JointDInvInertiaVisitor | ||
740 | : boost::static_visitor<Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options>> | ||
741 | { | ||
742 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> ReturnType; | ||
743 | |||
744 | template<typename JointDataDerived> | ||
745 | 4 | ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const | |
746 | { | ||
747 | 4 | return ReturnType(jdata.Dinv()); | |
748 | } | ||
749 | |||
750 | 2 | static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
751 | { | ||
752 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | return boost::apply_visitor(JointDInvInertiaVisitor(), jdata); |
753 | } | ||
754 | }; | ||
755 | |||
756 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
757 | inline Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> | ||
758 | 2 | dinv_inertia(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
759 | { | ||
760 | 2 | return JointDInvInertiaVisitor<Scalar, Options, JointCollectionTpl>::run(jdata); | |
761 | } | ||
762 | |||
763 | /** | ||
764 | * @brief JointUDInvInertiaVisitor visitor | ||
765 | */ | ||
766 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
767 | struct JointUDInvInertiaVisitor | ||
768 | : boost::static_visitor<Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options>> | ||
769 | { | ||
770 | typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> ReturnType; | ||
771 | |||
772 | template<typename JointDataDerived> | ||
773 | 4 | ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const | |
774 | { | ||
775 | 4 | return ReturnType(jdata.UDinv()); | |
776 | } | ||
777 | |||
778 | 2 | static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
779 | { | ||
780 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
4 | return boost::apply_visitor(JointUDInvInertiaVisitor(), jdata); |
781 | } | ||
782 | }; | ||
783 | |||
784 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
785 | inline Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> | ||
786 | 2 | udinv_inertia(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
787 | { | ||
788 | 2 | return JointUDInvInertiaVisitor<Scalar, Options, JointCollectionTpl>::run(jdata); | |
789 | } | ||
790 | |||
791 | /** | ||
792 | * @brief JointStUInertiaVisitor visitor | ||
793 | */ | ||
794 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
795 | struct JointStUInertiaVisitor | ||
796 | : boost::static_visitor<Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options>> | ||
797 | { | ||
798 | typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> ReturnType; | ||
799 | |||
800 | template<typename JointDataDerived> | ||
801 | 58 | ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const | |
802 | { | ||
803 | 58 | return ReturnType(jdata.StU()); | |
804 | } | ||
805 | |||
806 | 29 | static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
807 | { | ||
808 |
1/2✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
|
58 | return boost::apply_visitor(JointStUInertiaVisitor(), jdata); |
809 | } | ||
810 | }; | ||
811 | |||
812 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
813 | inline Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> | ||
814 | 29 | stu_inertia(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
815 | { | ||
816 | 29 | return JointStUInertiaVisitor<Scalar, Options, JointCollectionTpl>::run(jdata); | |
817 | } | ||
818 | |||
819 | /** | ||
820 | * @brief JointDataShortnameVisitor visitor | ||
821 | */ | ||
822 | struct JointDataShortnameVisitor : boost::static_visitor<std::string> | ||
823 | { | ||
824 | template<typename JointDataDerived> | ||
825 | ✗ | std::string operator()(const JointDataBase<JointDataDerived> & jdata) const | |
826 | { | ||
827 | ✗ | return jdata.shortname(); | |
828 | } | ||
829 | |||
830 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
831 | ✗ | static std::string run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
832 | { | ||
833 | ✗ | return boost::apply_visitor(JointDataShortnameVisitor(), jdata); | |
834 | } | ||
835 | }; | ||
836 | |||
837 | template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl> | ||
838 | ✗ | inline std::string shortname(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata) | |
839 | { | ||
840 | ✗ | return JointDataShortnameVisitor::run(jdata); | |
841 | } | ||
842 | |||
843 | template< | ||
844 | typename Scalar, | ||
845 | int Options, | ||
846 | template<typename S, int O> | ||
847 | class JointCollectionTpl, | ||
848 | typename JointDataDerived> | ||
849 | struct JointDataComparisonOperatorVisitor | ||
850 | : fusion::JointUnaryVisitorBase< | ||
851 | JointDataComparisonOperatorVisitor<Scalar, Options, JointCollectionTpl, JointDataDerived>, | ||
852 | bool> | ||
853 | { | ||
854 | typedef boost::fusion::vector<const JointDataDerived &> ArgsType; | ||
855 | |||
856 | template<typename JointData> | ||
857 | ✗ | static bool algo(const JointDataBase<JointData> & jdata_lhs, const JointDataDerived & jdata_rhs) | |
858 | { | ||
859 | ✗ | return jdata_lhs.derived() == jdata_rhs; | |
860 | } | ||
861 | }; | ||
862 | |||
863 | template< | ||
864 | typename Scalar, | ||
865 | int Options, | ||
866 | template<typename S, int O> | ||
867 | class JointCollectionTpl, | ||
868 | typename JointDataDerived> | ||
869 | ✗ | bool isEqual( | |
870 | const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata_generic, | ||
871 | const JointDataBase<JointDataDerived> & jdata) | ||
872 | { | ||
873 | typedef JointDataComparisonOperatorVisitor< | ||
874 | Scalar, Options, JointCollectionTpl, JointDataDerived> | ||
875 | Algo; | ||
876 | ✗ | return Algo::run(jdata_generic, typename Algo::ArgsType(boost::ref(jdata.derived()))); | |
877 | } | ||
878 | |||
879 | /// @endcond | ||
880 | |||
881 | } // namespace pinocchio | ||
882 | |||
883 | #endif // ifndef __pinocchio_multibody_joint_basic_visitors_hxx__ | ||
884 |