GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/joint/joint-basic-visitors.hxx
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 218 220 99.1%
Branches: 44 120 36.7%
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 129054 JointDataVariant operator()(const JointModelBase<JointModelDerived> & jmodel) const
27 {
28
1/2
✓ Branch 2 taken 64527 times.
✗ Branch 3 not taken.
 129054 return JointDataVariant(jmodel.createData());
29 }
30
31 64533 static JointDataVariant run(const JointModelVariant & jmodel)
32 {
33
1/2
✓ Branch 1 taken 64527 times.
✗ Branch 2 not taken.
 129066 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 1970 createData(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
40 {
41 1970 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 1814 static void algo(
55 const pinocchio::JointModelBase<JointModel> & jmodel,
56 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
57 const Eigen::MatrixBase<ConfigVectorType> & q)
58 {
59 1814 jmodel.calc(jdata.derived(), q.derived());
60 }
61 };
62
63 template<
64 typename Scalar,
65 int Options,
66 template<typename S, int O> class JointCollectionTpl,
67 typename ConfigVectorType>
68 909 inline void calc_zero_order(
69 const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel,
70 JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata,
71 const Eigen::MatrixBase<ConfigVectorType> & q)
72 {
73 typedef JointCalcZeroOrderVisitor<ConfigVectorType> Algo;
74
75
1/2
✓ Branch 3 taken 907 times.
✗ Branch 4 not taken.
 909 Algo::run(jmodel, jdata, typename Algo::ArgsType(q.derived()));
76 909 }
77
78 /**
79 * @brief JointCalcFirstOrderVisitor fusion visitor
80 */
81 template<typename ConfigVectorType, typename TangentVectorType>
82 struct JointCalcFirstOrderVisitor
83 : fusion::JointUnaryVisitorBase<JointCalcFirstOrderVisitor<ConfigVectorType, TangentVectorType>>
84 {
85 typedef boost::fusion::vector<const ConfigVectorType &, const TangentVectorType &> ArgsType;
86
87 template<typename JointModel>
88 1952 static void algo(
89 const pinocchio::JointModelBase<JointModel> & jmodel,
90 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
91 const Eigen::MatrixBase<ConfigVectorType> & q,
92 const Eigen::MatrixBase<TangentVectorType> & v)
93 {
94 1952 jmodel.calc(jdata.derived(), q.derived(), v.derived());
95 }
96 };
97
98 template<
99 typename Scalar,
100 int Options,
101 template<typename S, int O> class JointCollectionTpl,
102 typename ConfigVectorType,
103 typename TangentVectorType>
104 978 inline void calc_first_order(
105 const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel,
106 JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata,
107 const Eigen::MatrixBase<ConfigVectorType> & q,
108 const Eigen::MatrixBase<TangentVectorType> & v)
109 {
110 typedef JointCalcFirstOrderVisitor<ConfigVectorType, TangentVectorType> Algo;
111
112
1/2
✓ Branch 4 taken 976 times.
✗ Branch 5 not taken.
 978 Algo::run(jmodel, jdata, typename Algo::ArgsType(q.derived(), v.derived()));
113 978 }
114
115 /**
116 * @brief JointCalcFirstOrderVisitor fusion visitor
117 */
118 template<typename TangentVectorType>
119 struct JointCalcFirstOrderVisitor<Blank, TangentVectorType>
120 : fusion::JointUnaryVisitorBase<JointCalcFirstOrderVisitor<Blank, TangentVectorType>>
121 {
122 typedef boost::fusion::vector<const Blank, const TangentVectorType &> ArgsType;
123
124 template<typename JointModel>
125 static void algo(
126 const pinocchio::JointModelBase<JointModel> & jmodel,
127 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
128 const Blank blank,
129 const Eigen::MatrixBase<TangentVectorType> & v)
130 {
131 jmodel.calc(jdata.derived(), blank, v.derived());
132 }
133 };
134
135 template<
136 typename Scalar,
137 int Options,
138 template<typename S, int O> class JointCollectionTpl,
139 typename TangentVectorType>
140 inline void calc_first_order(
141 const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel,
142 JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata,
143 const Blank blank,
144 const Eigen::MatrixBase<TangentVectorType> & v)
145 {
146 typedef JointCalcFirstOrderVisitor<Blank, TangentVectorType> Algo;
147
148 Algo::run(jmodel, jdata, typename Algo::ArgsType(blank, v.derived()));
149 }
150
151 /**
152 * @brief JointCalcAbaVisitor fusion visitor
153 */
154
155 template<typename VectorLike, typename Matrix6Type>
156 struct JointCalcAbaVisitor
157 : fusion::JointUnaryVisitorBase<JointCalcAbaVisitor<VectorLike, Matrix6Type>>
158 {
159
160 typedef boost::fusion::vector<const VectorLike &, Matrix6Type &, bool> ArgsType;
161
162 template<typename JointModel>
163 88 static void algo(
164 const pinocchio::JointModelBase<JointModel> & jmodel,
165 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
166 const Eigen::MatrixBase<VectorLike> & armature,
167 const Eigen::MatrixBase<Matrix6Type> & I,
168 bool update_I)
169 {
170 88 jmodel.calc_aba(
171 88 jdata.derived(), armature.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type, I), update_I);
172 }
173 };
174
175 template<
176 typename Scalar,
177 int Options,
178 template<typename S, int O> class JointCollectionTpl,
179 typename VectorLike,
180 typename Matrix6Type>
181 44 inline void calc_aba(
182 const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel,
183 JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata,
184 const Eigen::MatrixBase<VectorLike> & armature,
185 const Eigen::MatrixBase<Matrix6Type> & I,
186 const bool update_I)
187 {
188 typedef JointCalcAbaVisitor<VectorLike, Matrix6Type> Algo;
189
1/2
✓ Branch 1 taken 44 times.
✗ Branch 2 not taken.
 44 Algo::run(
190 jmodel, jdata,
191 44 typename Algo::ArgsType(
192 44 PINOCCHIO_EIGEN_CONST_CAST(VectorLike, armature),
193 44 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type, I), update_I));
194 44 }
195
196 template<typename InputType, typename ReturnType>
197 struct JointMappedConfigSelectorVisitor
198 : fusion::
199 JointUnaryVisitorBase<JointMappedConfigSelectorVisitor<InputType, ReturnType>, ReturnType>
200 {
201 typedef boost::fusion::vector<InputType> ArgsType;
202
203 template<typename JointModel>
204 3648 static ReturnType algo(const JointModelBase<JointModel> & jmodel, InputType a)
205 {
206 // Converting a VectorBlock of anysize (static or dynamic) to another vector block of anysize
207 // (static or dynamic) since there is no copy constructor.
208
1/2
✓ Branch 1 taken 1824 times.
✗ Branch 2 not taken.
 3648 auto vectorBlock = jmodel.JointMappedConfigSelector(a);
209
210 // VectorBlock does not implemet such getter, hack the Eigen::Block base class to retreive
211 // such values.
212 3648 const Eigen::DenseIndex start =
213 3648 vectorBlock.startRow()
214 3648 + vectorBlock.startCol(); // The other dimension is always 0 (for vectors)
215 3648 const Eigen::DenseIndex size =
216 88 vectorBlock.rows() * vectorBlock.cols(); // The other dimension is always 1 (for vectors)
217
218
1/2
✓ Branch 2 taken 1824 times.
✗ Branch 3 not taken.
 7296 return ReturnType(vectorBlock.nestedExpression(), start, size);
219 }
220 };
221
222 /**
223 * @brief JointNvVisitor visitor
224 */
225 struct JointNvVisitor : boost::static_visitor<int>
226 {
227 template<typename JointModelDerived>
228 20434152 int operator()(const JointModelBase<JointModelDerived> & jmodel) const
229 {
230 20434152 return jmodel.nv();
231 }
232
233 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
234 10221050 static int run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
235 {
236
1/2
✓ Branch 1 taken 10217076 times.
✗ Branch 2 not taken.
 10221050 return boost::apply_visitor(JointNvVisitor(), jmodel);
237 }
238 };
239
240 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
241 10221050 inline int nv(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
242 {
243 10221050 return JointNvVisitor::run(jmodel);
244 }
245
246 /**
247 * @brief JointNqVisitor visitor
248 */
249 struct JointNqVisitor : boost::static_visitor<int>
250 {
251 template<typename JointModelDerived>
252 3495646 int operator()(const JointModelBase<JointModelDerived> & jmodel) const
253 {
254 3495646 return jmodel.nq();
255 }
256
257 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
258 1749025 static int run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
259 {
260
1/2
✓ Branch 1 taken 1747823 times.
✗ Branch 2 not taken.
 1749025 return boost::apply_visitor(JointNqVisitor(), jmodel);
261 }
262 };
263
264 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
265 1749025 inline int nq(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
266 {
267 1749025 return JointNqVisitor::run(jmodel);
268 }
269
270 /**
271 * @brief JointNvExtendedVisitor visitor
272 */
273 struct JointNvExtendedVisitor : boost::static_visitor<int>
274 {
275 template<typename JointModelDerived>
276 6799460 int operator()(const JointModelBase<JointModelDerived> & jmodel) const
277 {
278 6799460 return jmodel.nvExtended();
279 }
280
281 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
282 3401558 static int run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
283 {
284
1/2
✓ Branch 1 taken 3399730 times.
✗ Branch 2 not taken.
 3401558 return boost::apply_visitor(JointNvExtendedVisitor(), jmodel);
285 }
286 };
287
288 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
289 3401558 inline int nvExtended(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
290 {
291 3401558 return JointNvExtendedVisitor::run(jmodel);
292 }
293
294 /**
295 * @brief JointConfigurationLimitVisitor visitor
296 */
297 struct JointConfigurationLimitVisitor : boost::static_visitor<std::vector<bool>>
298 {
299 template<typename JointModelDerived>
300 6 const std::vector<bool> operator()(const JointModelBase<JointModelDerived> & jmodel) const
301 {
302 6 return jmodel.hasConfigurationLimit();
303 }
304
305 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
306 static const std::vector<bool>
307 3 run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
308 {
309
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 return boost::apply_visitor(JointConfigurationLimitVisitor(), jmodel);
310 }
311 };
312
313 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
314 inline const std::vector<bool>
315 3 hasConfigurationLimit(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
316 {
317 3 return JointConfigurationLimitVisitor::run(jmodel);
318 }
319
320 /**
321 * @brief JointConfigurationLimitInTangentVisitor visitor
322 */
323 struct JointConfigurationLimitInTangentVisitor : boost::static_visitor<std::vector<bool>>
324 {
325 template<typename JointModelDerived>
326 6 const std::vector<bool> operator()(const JointModelBase<JointModelDerived> & jmodel) const
327 {
328 6 return jmodel.hasConfigurationLimitInTangent();
329 }
330
331 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
332 static const std::vector<bool>
333 3 run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
334 {
335
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 return boost::apply_visitor(JointConfigurationLimitInTangentVisitor(), jmodel);
336 }
337 };
338
339 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
340 inline const std::vector<bool>
341 3 hasConfigurationLimitInTangent(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
342 {
343 3 return JointConfigurationLimitInTangentVisitor::run(jmodel);
344 }
345
346 /**
347 * @brief JointIdxQVisitor visitor
348 */
349 struct JointIdxQVisitor : boost::static_visitor<int>
350 {
351 template<typename JointModelDerived>
352 3366150 int operator()(const JointModelBase<JointModelDerived> & jmodel) const
353 {
354 3366150 return jmodel.idx_q();
355 }
356
357 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
358 1685399 static int run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
359 {
360
1/2
✓ Branch 1 taken 1683075 times.
✗ Branch 2 not taken.
 1685399 return boost::apply_visitor(JointIdxQVisitor(), jmodel);
361 }
362 };
363
364 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
365 1685399 inline int idx_q(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
366 {
367 1685399 return JointIdxQVisitor::run(jmodel);
368 }
369
370 /**
371 * @brief JointIdxVVisitor visitor
372 */
373 struct JointIdxVVisitor : boost::static_visitor<int>
374 {
375 template<typename JointModelDerived>
376 4071684 int operator()(const JointModelBase<JointModelDerived> & jmodel) const
377 {
378 4071684 return jmodel.idx_v();
379 }
380
381 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
382 2038530 static int run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
383 {
384
1/2
✓ Branch 1 taken 2035842 times.
✗ Branch 2 not taken.
 2038530 return boost::apply_visitor(JointIdxVVisitor(), jmodel);
385 }
386 };
387
388 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
389 2038530 inline int idx_v(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
390 {
391 2038530 return JointIdxVVisitor::run(jmodel);
392 }
393
394 /**
395 * @brief JointIdxVExtendedVisitor visitor
396 */
397 struct JointIdxVExtendedVisitor : boost::static_visitor<int>
398 {
399 template<typename JointModelDerived>
400 13253638 int operator()(const JointModelBase<JointModelDerived> & jmodel) const
401 {
402 13253638 return jmodel.idx_vExtended();
403 }
404
405 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
406 6630907 static int run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
407 {
408
1/2
✓ Branch 1 taken 6626819 times.
✗ Branch 2 not taken.
 6630907 return boost::apply_visitor(JointIdxVExtendedVisitor(), jmodel);
409 }
410 };
411
412 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
413 6630907 inline int idx_vExtended(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
414 {
415 6630907 return JointIdxVExtendedVisitor::run(jmodel);
416 }
417
418 /**
419 * @brief JointIdVisitor visitor
420 */
421 struct JointIdVisitor : boost::static_visitor<JointIndex>
422 {
423 template<typename JointModelDerived>
424 7978 JointIndex operator()(const JointModelBase<JointModelDerived> & jmodel) const
425 {
426 7978 return jmodel.id();
427 }
428
429 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
430 5211 static JointIndex run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
431 {
432
1/2
✓ Branch 1 taken 3989 times.
✗ Branch 2 not taken.
 5211 return boost::apply_visitor(JointIdVisitor(), jmodel);
433 }
434 };
435
436 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
437 5211 inline JointIndex id(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
438 {
439 5211 return JointIdVisitor::run(jmodel);
440 }
441
442 /**
443 * @brief JointSetIndexesVisitor visitor
444 */
445 struct JointSetIndexesVisitor : boost::static_visitor<>
446 {
447 JointIndex id;
448 int q;
449 int v;
450 int vExtended;
451
452 18001 JointSetIndexesVisitor(JointIndex id, int q, int v, int vExtended)
453 18001 : id(id)
454 18001 , q(q)
455 18001 , v(v)
456 18001 , vExtended(vExtended)
457 {
458 18001 }
459
460 template<typename JointModelDerived>
461 36002 void operator()(JointModelBase<JointModelDerived> & jmodel) const
462 {
463 36002 jmodel.setIndexes(id, q, v, vExtended);
464 36002 }
465
466 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
467 18057 static void run(
468 JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel,
469 JointIndex id,
470 int q,
471 int v,
472 int vExtended)
473 {
474
1/2
✓ Branch 2 taken 18001 times.
✗ Branch 3 not taken.
 18057 return boost::apply_visitor(JointSetIndexesVisitor(id, q, v, vExtended), jmodel);
475 }
476 };
477
478 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
479 18057 inline void setIndexes(
480 JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel,
481 JointIndex id,
482 int q,
483 int v,
484 int vExtended)
485 {
486 18057 return JointSetIndexesVisitor::run(jmodel, id, q, v, vExtended);
487 }
488
489 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
490 inline void setIndexes(
491 JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel, JointIndex id, int q, int v)
492 {
493 return JointSetIndexesVisitor::run(jmodel, id, q, v, v);
494 }
495
496 /**
497 * @brief JointModelShortnameVisitor visitor
498 */
499 struct JointModelShortnameVisitor : boost::static_visitor<std::string>
500 {
501 template<typename JointModelDerived>
502 166 std::string operator()(const JointModelBase<JointModelDerived> & jmodel) const
503 {
504 166 return jmodel.shortname();
505 }
506
507 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
508 83 static std::string run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
509 {
510
1/2
✓ Branch 1 taken 83 times.
✗ Branch 2 not taken.
 166 return boost::apply_visitor(JointModelShortnameVisitor(), jmodel);
511 }
512 };
513
514 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
515 83 inline std::string shortname(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
516 {
517 83 return JointModelShortnameVisitor::run(jmodel);
518 }
519
520 template<
521 typename NewScalar,
522 typename Scalar,
523 int Options,
524 template<typename S, int O> class JointCollectionTpl>
525 struct JointCastVisitor
526 : fusion::JointUnaryVisitorBase<
527 JointCastVisitor<NewScalar, Scalar, Options, JointCollectionTpl>,
528 typename CastType<NewScalar, JointModelTpl<Scalar, Options, JointCollectionTpl>>::type>
529 {
530 typedef fusion::NoArg ArgsType;
531
532 typedef typename CastType<NewScalar, JointModelTpl<Scalar, Options, JointCollectionTpl>>::type
533 ReturnType;
534
535 template<typename JointModelDerived>
536 1476 static ReturnType algo(const JointModelBase<JointModelDerived> & jmodel)
537 {
538
1/2
✓ Branch 2 taken 738 times.
✗ Branch 3 not taken.
 1476 return ReturnType(jmodel.template cast<NewScalar>());
539 }
540 };
541
542 template<
543 typename NewScalar,
544 typename Scalar,
545 int Options,
546 template<typename S, int O> class JointCollectionTpl>
547 typename CastType<NewScalar, JointModelTpl<Scalar, Options, JointCollectionTpl>>::type
548 1132 cast_joint(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
549 {
550 typedef JointCastVisitor<NewScalar, Scalar, Options, JointCollectionTpl> Algo;
551 1132 return Algo::run(jmodel);
552 }
553
554 template<
555 typename Scalar,
556 int Options,
557 template<typename S, int O> class JointCollectionTpl,
558 typename JointModelDerived>
559 struct JointModelComparisonOperatorVisitor
560 : fusion::JointUnaryVisitorBase<
561 JointModelComparisonOperatorVisitor<Scalar, Options, JointCollectionTpl, JointModelDerived>,
562 bool>
563 {
564 typedef boost::fusion::vector<const JointModelDerived &> ArgsType;
565
566 template<typename JointModel>
567 static bool
568 92 algo(const JointModelBase<JointModel> & jmodel_lhs, const JointModelDerived & jmodel_rhs)
569 {
570 92 return internal::comparison_eq(jmodel_lhs.derived(), jmodel_rhs);
571 }
572 };
573
574 template<
575 typename Scalar,
576 int Options,
577 template<typename S, int O> class JointCollectionTpl,
578 typename JointModelDerived>
579 92 bool isEqual(
580 const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel_generic,
581 const JointModelBase<JointModelDerived> & jmodel)
582 {
583 typedef JointModelComparisonOperatorVisitor<
584 Scalar, Options, JointCollectionTpl, JointModelDerived>
585 Algo;
586
2/4
✓ Branch 2 taken 46 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 46 times.
✗ Branch 6 not taken.
 184 return Algo::run(jmodel_generic, typename Algo::ArgsType(boost::ref(jmodel.derived())));
587 }
588
589 template<
590 typename Scalar,
591 int Options,
592 template<typename S, int O> class JointCollectionTpl,
593 typename JointModelDerived>
594 struct JointModelHasSameIndexesVisitor
595 : fusion::JointUnaryVisitorBase<
596 JointModelHasSameIndexesVisitor<Scalar, Options, JointCollectionTpl, JointModelDerived>,
597 bool>
598 {
599 typedef boost::fusion::vector<const JointModelDerived &> ArgsType;
600
601 template<typename JointModel>
602 static bool
603 3888 algo(const JointModelBase<JointModel> & jmodel_lhs, const JointModelDerived & jmodel_rhs)
604 {
605 3888 return jmodel_lhs.derived().hasSameIndexes(jmodel_rhs);
606 }
607 };
608
609 template<
610 typename Scalar,
611 int Options,
612 template<typename S, int O> class JointCollectionTpl,
613 typename JointModelDerived>
614 3119 bool hasSameIndexes(
615 const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel_generic,
616 const JointModelBase<JointModelDerived> & jmodel)
617 {
618 typedef JointModelHasSameIndexesVisitor<Scalar, Options, JointCollectionTpl, JointModelDerived>
619 Algo;
620
2/4
✓ Branch 2 taken 1944 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1944 times.
✗ Branch 6 not taken.
 6238 return Algo::run(jmodel_generic, typename Algo::ArgsType(boost::ref(jmodel.derived())));
621 }
622
623 //
624 // Visitors on JointDatas
625 //
626
627 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
628 struct JointQVisitor
629 : boost::static_visitor<
630 typename JointDataTpl<Scalar, Options, JointCollectionTpl>::ConfigVector_t>
631 {
632 typedef typename JointDataTpl<Scalar, Options, JointCollectionTpl>::ConfigVector_t ReturnType;
633
634 template<typename JointDataDerived>
635 1708 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
636 {
637 1708 return jdata.joint_q();
638 }
639
640 854 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
641 {
642
1/2
✓ Branch 1 taken 854 times.
✗ Branch 2 not taken.
 1708 return boost::apply_visitor(JointQVisitor(), jdata);
643 }
644 };
645
646 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
647 inline typename JointDataTpl<Scalar, Options, JointCollectionTpl>::ConfigVector_t
648 854 joint_q(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
649 {
650 854 return JointQVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
651 }
652
653 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
654 struct JointVVisitor
655 : boost::static_visitor<
656 typename JointDataTpl<Scalar, Options, JointCollectionTpl>::ConfigVector_t>
657 {
658 typedef typename JointDataTpl<Scalar, Options, JointCollectionTpl>::TangentVector_t ReturnType;
659
660 template<typename JointDataDerived>
661 1356 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
662 {
663 1356 return jdata.joint_v();
664 }
665
666 678 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
667 {
668
1/2
✓ Branch 1 taken 678 times.
✗ Branch 2 not taken.
 1356 return boost::apply_visitor(JointVVisitor(), jdata);
669 }
670 };
671
672 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
673 inline typename JointDataTpl<Scalar, Options, JointCollectionTpl>::TangentVector_t
674 678 joint_v(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
675 {
676 678 return JointVVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
677 }
678
679 /**
680 * @brief JointConstraintVisitor visitor
681 */
682 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
683 struct JointConstraintVisitor
684 : boost::static_visitor<JointMotionSubspaceTpl<Eigen::Dynamic, Scalar, Options>>
685 {
686 typedef JointMotionSubspaceTpl<Eigen::Dynamic, Scalar, Options> ReturnType;
687
688 template<typename JointDataDerived>
689 4816 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
690 {
691
1/2
✓ Branch 3 taken 2404 times.
✗ Branch 4 not taken.
 4816 return ReturnType(jdata.S().matrix());
692 }
693
694 2409 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
695 {
696
1/2
✓ Branch 1 taken 2408 times.
✗ Branch 2 not taken.
 4818 return boost::apply_visitor(JointConstraintVisitor(), jdata);
697 }
698 };
699
700 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
701 inline JointMotionSubspaceTpl<Eigen::Dynamic, Scalar, Options>
702 2409 joint_motin_subspace_xd(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
703 {
704 2409 return JointConstraintVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
705 }
706
707 /**
708 * @brief JointTransformVisitor visitor
709 */
710 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
711 struct JointTransformVisitor : boost::static_visitor<SE3Tpl<Scalar, Options>>
712 {
713 typedef SE3Tpl<Scalar, Options> ReturnType;
714
715 template<typename JointDataDerived>
716 5116 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
717 {
718 5116 return ReturnType(jdata.M());
719 }
720
721 2560 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
722 {
723
1/2
✓ Branch 1 taken 2558 times.
✗ Branch 2 not taken.
 5120 return boost::apply_visitor(JointTransformVisitor(), jdata);
724 }
725 };
726
727 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
728 inline SE3Tpl<Scalar, Options>
729 2560 joint_transform(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
730 {
731 2560 return JointTransformVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
732 }
733
734 /**
735 * @brief JointMotionVisitor visitor
736 */
737 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
738 struct JointMotionVisitor : boost::static_visitor<MotionTpl<Scalar, Options>>
739 {
740 typedef MotionTpl<Scalar, Options> ReturnType;
741
742 template<typename JointDataDerived>
743 3918 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
744 {
745 3918 return ReturnType(jdata.v());
746 }
747
748 1961 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
749 {
750
1/2
✓ Branch 1 taken 1959 times.
✗ Branch 2 not taken.
 3922 return boost::apply_visitor(JointMotionVisitor(), jdata);
751 }
752 };
753
754 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
755 inline MotionTpl<Scalar, Options>
756 1961 motion(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
757 {
758 1961 return JointMotionVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
759 }
760
761 /**
762 * @brief JointBiasVisitor visitor
763 */
764 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
765 struct JointBiasVisitor : boost::static_visitor<MotionTpl<Scalar, Options>>
766 {
767 typedef MotionTpl<Scalar, Options> ReturnType;
768
769 template<typename JointDataDerived>
770 1968 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
771 {
772 1968 return ReturnType(jdata.c());
773 }
774
775 984 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
776 {
777
1/2
✓ Branch 1 taken 984 times.
✗ Branch 2 not taken.
 1968 return boost::apply_visitor(JointBiasVisitor(), jdata);
778 }
779 };
780
781 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
782 inline MotionTpl<Scalar, Options>
783 984 bias(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
784 {
785 984 return JointBiasVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
786 }
787
788 /**
789 * @brief JointUInertiaVisitor visitor
790 */
791 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
792 struct JointUInertiaVisitor
793 : boost::static_visitor<Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options>>
794 {
795 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> ReturnType;
796
797 template<typename JointDataDerived>
798 1710 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
799 {
800 1710 return ReturnType(jdata.U());
801 }
802
803 855 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
804 {
805
1/2
✓ Branch 1 taken 855 times.
✗ Branch 2 not taken.
 1710 return boost::apply_visitor(JointUInertiaVisitor(), jdata);
806 }
807 };
808
809 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
810 inline Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options>
811 855 u_inertia(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
812 {
813 855 return JointUInertiaVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
814 }
815
816 /**
817 * @brief JointDInvInertiaVisitor visitor
818 */
819 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
820 struct JointDInvInertiaVisitor
821 : boost::static_visitor<Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options>>
822 {
823 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> ReturnType;
824
825 template<typename JointDataDerived>
826 1548 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
827 {
828 1548 return ReturnType(jdata.Dinv());
829 }
830
831 774 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
832 {
833
1/2
✓ Branch 1 taken 774 times.
✗ Branch 2 not taken.
 1548 return boost::apply_visitor(JointDInvInertiaVisitor(), jdata);
834 }
835 };
836
837 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
838 inline Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options>
839 774 dinv_inertia(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
840 {
841 774 return JointDInvInertiaVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
842 }
843
844 /**
845 * @brief JointUDInvInertiaVisitor visitor
846 */
847 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
848 struct JointUDInvInertiaVisitor
849 : boost::static_visitor<Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options>>
850 {
851 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> ReturnType;
852
853 template<typename JointDataDerived>
854 1548 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
855 {
856 1548 return ReturnType(jdata.UDinv());
857 }
858
859 774 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
860 {
861
1/2
✓ Branch 1 taken 774 times.
✗ Branch 2 not taken.
 1548 return boost::apply_visitor(JointUDInvInertiaVisitor(), jdata);
862 }
863 };
864
865 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
866 inline Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options>
867 774 udinv_inertia(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
868 {
869 774 return JointUDInvInertiaVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
870 }
871
872 /**
873 * @brief JointStUInertiaVisitor visitor
874 */
875 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
876 struct JointStUInertiaVisitor
877 : boost::static_visitor<Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options>>
878 {
879 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> ReturnType;
880
881 template<typename JointDataDerived>
882 166 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
883 {
884 166 return ReturnType(jdata.StU());
885 }
886
887 83 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
888 {
889
1/2
✓ Branch 1 taken 83 times.
✗ Branch 2 not taken.
 166 return boost::apply_visitor(JointStUInertiaVisitor(), jdata);
890 }
891 };
892
893 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
894 inline Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options>
895 83 stu_inertia(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
896 {
897 83 return JointStUInertiaVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
898 }
899
900 /**
901 * @brief JointDataShortnameVisitor visitor
902 */
903 struct JointDataShortnameVisitor : boost::static_visitor<std::string>
904 {
905 template<typename JointDataDerived>
906 56 std::string operator()(const JointDataBase<JointDataDerived> & jdata) const
907 {
908 56 return jdata.shortname();
909 }
910
911 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
912 28 static std::string run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
913 {
914
1/2
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
 56 return boost::apply_visitor(JointDataShortnameVisitor(), jdata);
915 }
916 };
917
918 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
919 28 inline std::string shortname(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
920 {
921 28 return JointDataShortnameVisitor::run(jdata);
922 }
923
924 template<
925 typename Scalar,
926 int Options,
927 template<typename S, int O> class JointCollectionTpl,
928 typename JointDataDerived>
929 struct JointDataComparisonOperatorVisitor
930 : fusion::JointUnaryVisitorBase<
931 JointDataComparisonOperatorVisitor<Scalar, Options, JointCollectionTpl, JointDataDerived>,
932 bool>
933 {
934 typedef boost::fusion::vector<const JointDataDerived &> ArgsType;
935
936 template<typename JointData>
937 88 static bool algo(const JointDataBase<JointData> & jdata_lhs, const JointDataDerived & jdata_rhs)
938 {
939 88 return jdata_lhs.derived() == jdata_rhs;
940 }
941 };
942
943 template<
944 typename Scalar,
945 int Options,
946 template<typename S, int O> class JointCollectionTpl,
947 typename JointDataDerived>
948 88 bool isEqual(
949 const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata_generic,
950 const JointDataBase<JointDataDerived> & jdata)
951 {
952 typedef JointDataComparisonOperatorVisitor<
953 Scalar, Options, JointCollectionTpl, JointDataDerived>
954 Algo;
955
2/4
✓ Branch 2 taken 44 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 44 times.
✗ Branch 6 not taken.
 176 return Algo::run(jdata_generic, typename Algo::ArgsType(boost::ref(jdata.derived())));
956 }
957
958 // Meta-function to check is_mimicable_t trait
959 template<typename JointModel>
960 struct is_mimicable
961 {
962 static constexpr bool value = traits<typename JointModel::JointDerived>::is_mimicable_t::value;
963 };
964
965 template<typename JointModel>
966 struct CheckMimicVisitor : public boost::static_visitor<JointModel>
967 {
968 template<typename T>
969 typename boost::enable_if_c<is_mimicable<T>::value, JointModel>::type
970 3620 operator()(const T & value) const
971 {
972 3620 return value;
973 }
974
975 template<typename T>
976 typename boost::disable_if_c<is_mimicable<T>::value, JointModel>::type
977 ✗ operator()(const T & value) const
978 {
979 ✗ PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Type not supported in new variant");
980 return value;
981 }
982 };
983
984 template<typename JointModel>
985 3598 JointModel checkMimic(const JointModel & value)
986 {
987
1/2
✓ Branch 1 taken 1790 times.
✗ Branch 2 not taken.
 7196 return boost::apply_visitor(CheckMimicVisitor<JointModel>(), value);
988 }
989
990 template<typename ConfigVectorIn, typename Scalar, typename ConfigVectorOut>
991 struct ConfigVectorAffineTransformVisitor : public boost::static_visitor<void>
992 {
993 public:
994 const Eigen::MatrixBase<ConfigVectorIn> & qIn;
995 const Scalar & scaling;
996 const Scalar & offset;
997 const Eigen::MatrixBase<ConfigVectorOut> & qOut;
998
999 1807 ConfigVectorAffineTransformVisitor(
1000 const Eigen::MatrixBase<ConfigVectorIn> & qIn,
1001 const Scalar & scaling,
1002 const Scalar & offset,
1003 const Eigen::MatrixBase<ConfigVectorOut> & qOut)
1004 1807 : qIn(qIn)
1005 1807 , scaling(scaling)
1006 1807 , offset(offset)
1007 1807 , qOut(qOut)
1008 {
1009 1807 }
1010
1011 template<typename JointModel>
1012 3614 void operator()(const JointModel & /*jmodel*/) const
1013 {
1014 typedef typename ConfigVectorAffineTransform<typename JointModel::JointDerived>::Type
1015 AffineTransform;
1016 3614 AffineTransform::run(qIn, scaling, offset, qOut);
1017 }
1018 };
1019
1020 template<
1021 typename Scalar,
1022 int Options,
1023 template<typename S, int O> class JointCollectionTpl,
1024 typename ConfigVectorIn,
1025 typename ConfigVectorOut>
1026 1807 void configVectorAffineTransform(
1027 const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel,
1028 const Eigen::MatrixBase<ConfigVectorIn> & qIn,
1029 const Scalar & scaling,
1030 const Scalar & offset,
1031 const Eigen::MatrixBase<ConfigVectorOut> & qOut)
1032 {
1033
1/2
✓ Branch 1 taken 1807 times.
✗ Branch 2 not taken.
 1807 boost::apply_visitor(
1034 1807 ConfigVectorAffineTransformVisitor<ConfigVectorIn, Scalar, ConfigVectorOut>(
1035 qIn, scaling, offset, qOut),
1036 jmodel);
1037 1807 }
1038
1039 template<int Op, typename ForceType, typename ExpressionType>
1040 struct ApplyConstraintOnForceVisitor : public boost::static_visitor<void>
1041 {
1042 ForceType F;
1043 ExpressionType R;
1044
1045 104 ApplyConstraintOnForceVisitor(ForceType F_, ExpressionType R_)
1046 104 : F(F_)
1047 104 , R(R_)
1048 {
1049 104 }
1050
1051 template<typename JointDataDerived>
1052 104 void operator()(const JointDataBase<JointDataDerived> & jdata) const
1053 {
1054 // Since ExpressionType is often a temporary (Block, NoAlias) we need to const cast it
1055 switch (Op)
1056 {
1057 case SETTO:
1058
2/5
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
 26 const_cast<ExpressionType &>(R) = jdata.S().transpose() * F;
1059 26 break;
1060 case ADDTO:
1061
2/5
✓ Branch 3 taken 39 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 39 times.
✗ Branch 7 not taken.
 78 const_cast<ExpressionType &>(R) += jdata.S().transpose() * F;
1062 78 break;
1063 case RMTO:
1064 const_cast<ExpressionType &>(R) -= jdata.S().transpose() * F;
1065 break;
1066 default:
1067 assert(false && "Wrong Op requesed value");
1068 break;
1069 }
1070 }
1071
1072 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
1073 104 static void run(
1074 JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata,
1075 const ForceType F,
1076 ExpressionType R)
1077 {
1078
1/2
✓ Branch 2 taken 52 times.
✗ Branch 3 not taken.
 104 boost::apply_visitor(ApplyConstraintOnForceVisitor(F, R), jdata);
1079 104 }
1080 };
1081
1082 template<
1083 int Op,
1084 typename Scalar,
1085 int Options,
1086 template<typename S, int O> class JointCollectionTpl,
1087 typename ForceType,
1088 typename ExpressionType>
1089 52 void applyConstraintOnForceVisitor(
1090 JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata, ForceType F, ExpressionType R)
1091 {
1092 52 return ApplyConstraintOnForceVisitor<Op, ForceType, ExpressionType>::run(jdata, F, R);
1093 }
1094
1095 /// @endcond
1096
1097 } // namespace pinocchio
1098
1099 #endif // ifndef __pinocchio_multibody_joint_basic_visitors_hxx__
1100