GCC Code Coverage Report

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