GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/joint/joint-basic-visitors.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 167 167 100.0%
Branches: 31 62 50.0%
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 44030 JointDataVariant operator()(const JointModelBase<JointModelDerived> & jmodel) const
27 {
28
1/2
✓ Branch 2 taken 22015 times.
✗ Branch 3 not taken.
 44030 return JointDataVariant(jmodel.createData());
29 }
30
31 22021 static JointDataVariant run(const JointModelVariant & jmodel)
32 {
33
1/2
✓ Branch 1 taken 22015 times.
✗ Branch 2 not taken.
 44042 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 125 createData(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
40 {
41 125 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 58 static void algo(
55 const pinocchio::JointModelBase<JointModel> & jmodel,
56 pinocchio::JointDataBase<typename JointModel::JointDataDerived> & jdata,
57 const Eigen::MatrixBase<ConfigVectorType> & q)
58 {
59 58 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 31 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 29 times.
✗ Branch 4 not taken.
 31 Algo::run(jmodel, jdata, typename Algo::ArgsType(q.derived()));
76 31 }
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 104 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 104 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 54 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 52 times.
✗ Branch 5 not taken.
 54 Algo::run(jmodel, jdata, typename Algo::ArgsType(q.derived(), v.derived()));
113 54 }
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 100 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 100 jmodel.calc_aba(
171 100 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 50 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 50 times.
✗ Branch 2 not taken.
 50 Algo::run(
190 jmodel, jdata,
191 50 typename Algo::ArgsType(
192 50 PINOCCHIO_EIGEN_CONST_CAST(VectorLike, armature),
193 50 PINOCCHIO_EIGEN_CONST_CAST(Matrix6Type, I), update_I));
194 50 }
195
196 /**
197 * @brief JointNvVisitor visitor
198 */
199 struct JointNvVisitor : boost::static_visitor<int>
200 {
201 template<typename JointModelDerived>
202 20931118 int operator()(const JointModelBase<JointModelDerived> & jmodel) const
203 {
204 20931118 return jmodel.nv();
205 }
206
207 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
208 10469084 static int run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
209 {
210
1/2
✓ Branch 1 taken 10465559 times.
✗ Branch 2 not taken.
 10469084 return boost::apply_visitor(JointNvVisitor(), jmodel);
211 }
212 };
213
214 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
215 10469084 inline int nv(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
216 {
217 10469084 return JointNvVisitor::run(jmodel);
218 }
219
220 /**
221 * @brief JointNqVisitor visitor
222 */
223 struct JointNqVisitor : boost::static_visitor<int>
224 {
225 template<typename JointModelDerived>
226 3094716 int operator()(const JointModelBase<JointModelDerived> & jmodel) const
227 {
228 3094716 return jmodel.nq();
229 }
230
231 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
232 1548369 static int run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
233 {
234
1/2
✓ Branch 1 taken 1547358 times.
✗ Branch 2 not taken.
 1548369 return boost::apply_visitor(JointNqVisitor(), jmodel);
235 }
236 };
237
238 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
239 1548369 inline int nq(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
240 {
241 1548369 return JointNqVisitor::run(jmodel);
242 }
243
244 /**
245 * @brief JointConfigurationLimitVisitor visitor
246 */
247 struct JointConfigurationLimitVisitor : boost::static_visitor<std::vector<bool>>
248 {
249 template<typename JointModelDerived>
250 6 const std::vector<bool> operator()(const JointModelBase<JointModelDerived> & jmodel) const
251 {
252 6 return jmodel.hasConfigurationLimit();
253 }
254
255 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
256 static const std::vector<bool>
257 3 run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
258 {
259
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 return boost::apply_visitor(JointConfigurationLimitVisitor(), jmodel);
260 }
261 };
262
263 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
264 inline const std::vector<bool>
265 3 hasConfigurationLimit(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
266 {
267 3 return JointConfigurationLimitVisitor::run(jmodel);
268 }
269
270 /**
271 * @brief JointConfigurationLimitInTangentVisitor visitor
272 */
273 struct JointConfigurationLimitInTangentVisitor : boost::static_visitor<std::vector<bool>>
274 {
275 template<typename JointModelDerived>
276 6 const std::vector<bool> operator()(const JointModelBase<JointModelDerived> & jmodel) const
277 {
278 6 return jmodel.hasConfigurationLimitInTangent();
279 }
280
281 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
282 static const std::vector<bool>
283 3 run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
284 {
285
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 return boost::apply_visitor(JointConfigurationLimitInTangentVisitor(), jmodel);
286 }
287 };
288
289 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
290 inline const std::vector<bool>
291 3 hasConfigurationLimitInTangent(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
292 {
293 3 return JointConfigurationLimitInTangentVisitor::run(jmodel);
294 }
295
296 /**
297 * @brief JointIdxQVisitor visitor
298 */
299 struct JointIdxQVisitor : boost::static_visitor<int>
300 {
301 template<typename JointModelDerived>
302 2983072 int operator()(const JointModelBase<JointModelDerived> & jmodel) const
303 {
304 2983072 return jmodel.idx_q();
305 }
306
307 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
308 1493623 static int run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
309 {
310
1/2
✓ Branch 1 taken 1491536 times.
✗ Branch 2 not taken.
 1493623 return boost::apply_visitor(JointIdxQVisitor(), jmodel);
311 }
312 };
313
314 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
315 1493623 inline int idx_q(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
316 {
317 1493623 return JointIdxQVisitor::run(jmodel);
318 }
319
320 /**
321 * @brief JointIdxVVisitor visitor
322 */
323 struct JointIdxVVisitor : boost::static_visitor<int>
324 {
325 template<typename JointModelDerived>
326 9024122 int operator()(const JointModelBase<JointModelDerived> & jmodel) const
327 {
328 9024122 return jmodel.idx_v();
329 }
330
331 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
332 4515218 static int run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
333 {
334
1/2
✓ Branch 1 taken 4512061 times.
✗ Branch 2 not taken.
 4515218 return boost::apply_visitor(JointIdxVVisitor(), jmodel);
335 }
336 };
337
338 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
339 4515218 inline int idx_v(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
340 {
341 4515218 return JointIdxVVisitor::run(jmodel);
342 }
343
344 /**
345 * @brief JointIdVisitor visitor
346 */
347 struct JointIdVisitor : boost::static_visitor<JointIndex>
348 {
349 template<typename JointModelDerived>
350 4772 JointIndex operator()(const JointModelBase<JointModelDerived> & jmodel) const
351 {
352 4772 return jmodel.id();
353 }
354
355 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
356 3522 static JointIndex run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
357 {
358
1/2
✓ Branch 1 taken 2386 times.
✗ Branch 2 not taken.
 3522 return boost::apply_visitor(JointIdVisitor(), jmodel);
359 }
360 };
361
362 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
363 3522 inline JointIndex id(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
364 {
365 3522 return JointIdVisitor::run(jmodel);
366 }
367
368 /**
369 * @brief JointSetIndexesVisitor visitor
370 */
371 struct JointSetIndexesVisitor : boost::static_visitor<>
372 {
373 JointIndex id;
374 int q;
375 int v;
376
377 14594 JointSetIndexesVisitor(JointIndex id, int q, int v)
378 14594 : id(id)
379 14594 , q(q)
380 14594 , v(v)
381 {
382 14594 }
383
384 template<typename JointModelDerived>
385 29188 void operator()(JointModelBase<JointModelDerived> & jmodel) const
386 {
387 29188 jmodel.setIndexes(id, q, v);
388 29188 }
389
390 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
391 static void
392 14626 run(JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel, JointIndex id, int q, int v)
393 {
394
1/2
✓ Branch 2 taken 14594 times.
✗ Branch 3 not taken.
 14626 return boost::apply_visitor(JointSetIndexesVisitor(id, q, v), jmodel);
395 }
396 };
397
398 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
399 14626 inline void setIndexes(
400 JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel, JointIndex id, int q, int v)
401 {
402 14626 return JointSetIndexesVisitor::run(jmodel, id, q, v);
403 }
404
405 /**
406 * @brief JointModelShortnameVisitor visitor
407 */
408 struct JointModelShortnameVisitor : boost::static_visitor<std::string>
409 {
410 template<typename JointModelDerived>
411 166 std::string operator()(const JointModelBase<JointModelDerived> & jmodel) const
412 {
413 166 return jmodel.shortname();
414 }
415
416 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
417 83 static std::string run(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
418 {
419
1/2
✓ Branch 1 taken 83 times.
✗ Branch 2 not taken.
 166 return boost::apply_visitor(JointModelShortnameVisitor(), jmodel);
420 }
421 };
422
423 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
424 83 inline std::string shortname(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
425 {
426 83 return JointModelShortnameVisitor::run(jmodel);
427 }
428
429 template<
430 typename NewScalar,
431 typename Scalar,
432 int Options,
433 template<typename S, int O> class JointCollectionTpl>
434 struct JointCastVisitor
435 : fusion::JointUnaryVisitorBase<
436 JointCastVisitor<NewScalar, Scalar, Options, JointCollectionTpl>,
437 typename CastType<NewScalar, JointModelTpl<Scalar, Options, JointCollectionTpl>>::type>
438 {
439 typedef fusion::NoArg ArgsType;
440
441 typedef typename CastType<NewScalar, JointModelTpl<Scalar, Options, JointCollectionTpl>>::type
442 ReturnType;
443
444 template<typename JointModelDerived>
445 1288 static ReturnType algo(const JointModelBase<JointModelDerived> & jmodel)
446 {
447
1/2
✓ Branch 2 taken 644 times.
✗ Branch 3 not taken.
 1288 return ReturnType(jmodel.template cast<NewScalar>());
448 }
449 };
450
451 template<
452 typename NewScalar,
453 typename Scalar,
454 int Options,
455 template<typename S, int O> class JointCollectionTpl>
456 typename CastType<NewScalar, JointModelTpl<Scalar, Options, JointCollectionTpl>>::type
457 855 cast_joint(const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel)
458 {
459 typedef JointCastVisitor<NewScalar, Scalar, Options, JointCollectionTpl> Algo;
460 855 return Algo::run(jmodel);
461 }
462
463 template<
464 typename Scalar,
465 int Options,
466 template<typename S, int O> class JointCollectionTpl,
467 typename JointModelDerived>
468 struct JointModelComparisonOperatorVisitor
469 : fusion::JointUnaryVisitorBase<
470 JointModelComparisonOperatorVisitor<Scalar, Options, JointCollectionTpl, JointModelDerived>,
471 bool>
472 {
473 typedef boost::fusion::vector<const JointModelDerived &> ArgsType;
474
475 template<typename JointModel>
476 static bool
477 104 algo(const JointModelBase<JointModel> & jmodel_lhs, const JointModelDerived & jmodel_rhs)
478 {
479 104 return internal::comparison_eq(jmodel_lhs.derived(), jmodel_rhs);
480 }
481 };
482
483 template<
484 typename Scalar,
485 int Options,
486 template<typename S, int O> class JointCollectionTpl,
487 typename JointModelDerived>
488 104 bool isEqual(
489 const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel_generic,
490 const JointModelBase<JointModelDerived> & jmodel)
491 {
492 typedef JointModelComparisonOperatorVisitor<
493 Scalar, Options, JointCollectionTpl, JointModelDerived>
494 Algo;
495
2/4
✓ Branch 2 taken 52 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 52 times.
✗ Branch 6 not taken.
 208 return Algo::run(jmodel_generic, typename Algo::ArgsType(boost::ref(jmodel.derived())));
496 }
497
498 template<
499 typename Scalar,
500 int Options,
501 template<typename S, int O> class JointCollectionTpl,
502 typename JointModelDerived>
503 struct JointModelHasSameIndexesVisitor
504 : fusion::JointUnaryVisitorBase<
505 JointModelHasSameIndexesVisitor<Scalar, Options, JointCollectionTpl, JointModelDerived>,
506 bool>
507 {
508 typedef boost::fusion::vector<const JointModelDerived &> ArgsType;
509
510 template<typename JointModel>
511 static bool
512 3868 algo(const JointModelBase<JointModel> & jmodel_lhs, const JointModelDerived & jmodel_rhs)
513 {
514 3868 return jmodel_lhs.derived().hasSameIndexes(jmodel_rhs);
515 }
516 };
517
518 template<
519 typename Scalar,
520 int Options,
521 template<typename S, int O> class JointCollectionTpl,
522 typename JointModelDerived>
523 1964 bool hasSameIndexes(
524 const JointModelTpl<Scalar, Options, JointCollectionTpl> & jmodel_generic,
525 const JointModelBase<JointModelDerived> & jmodel)
526 {
527 typedef JointModelHasSameIndexesVisitor<Scalar, Options, JointCollectionTpl, JointModelDerived>
528 Algo;
529
2/4
✓ Branch 2 taken 798 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 798 times.
✗ Branch 6 not taken.
 3928 return Algo::run(jmodel_generic, typename Algo::ArgsType(boost::ref(jmodel.derived())));
530 }
531
532 //
533 // Visitors on JointDatas
534 //
535
536 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
537 struct JointQVisitor
538 : boost::static_visitor<
539 typename JointDataTpl<Scalar, Options, JointCollectionTpl>::ConfigVector_t>
540 {
541 typedef typename JointDataTpl<Scalar, Options, JointCollectionTpl>::ConfigVector_t ReturnType;
542
543 template<typename JointDataDerived>
544 1724 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
545 {
546 1724 return jdata.joint_q();
547 }
548
549 862 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
550 {
551
1/2
✓ Branch 1 taken 862 times.
✗ Branch 2 not taken.
 1724 return boost::apply_visitor(JointQVisitor(), jdata);
552 }
553 };
554
555 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
556 inline typename JointDataTpl<Scalar, Options, JointCollectionTpl>::ConfigVector_t
557 862 joint_q(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
558 {
559 862 return JointQVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
560 }
561
562 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
563 struct JointVVisitor
564 : boost::static_visitor<
565 typename JointDataTpl<Scalar, Options, JointCollectionTpl>::ConfigVector_t>
566 {
567 typedef typename JointDataTpl<Scalar, Options, JointCollectionTpl>::TangentVector_t ReturnType;
568
569 template<typename JointDataDerived>
570 1324 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
571 {
572 1324 return jdata.joint_v();
573 }
574
575 662 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
576 {
577
1/2
✓ Branch 1 taken 662 times.
✗ Branch 2 not taken.
 1324 return boost::apply_visitor(JointVVisitor(), jdata);
578 }
579 };
580
581 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
582 inline typename JointDataTpl<Scalar, Options, JointCollectionTpl>::TangentVector_t
583 662 joint_v(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
584 {
585 662 return JointVVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
586 }
587
588 /**
589 * @brief JointConstraintVisitor visitor
590 */
591 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
592 struct JointConstraintVisitor
593 : boost::static_visitor<JointMotionSubspaceTpl<Eigen::Dynamic, Scalar, Options>>
594 {
595 typedef JointMotionSubspaceTpl<Eigen::Dynamic, Scalar, Options> ReturnType;
596
597 template<typename JointDataDerived>
598 1430 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
599 {
600
1/2
✓ Branch 3 taken 713 times.
✗ Branch 4 not taken.
 1430 return ReturnType(jdata.S().matrix());
601 }
602
603 715 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
604 {
605
1/2
✓ Branch 1 taken 715 times.
✗ Branch 2 not taken.
 1430 return boost::apply_visitor(JointConstraintVisitor(), jdata);
606 }
607 };
608
609 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
610 inline JointMotionSubspaceTpl<Eigen::Dynamic, Scalar, Options>
611 715 joint_motin_subspace_xd(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
612 {
613 715 return JointConstraintVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
614 }
615
616 /**
617 * @brief JointTransformVisitor visitor
618 */
619 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
620 struct JointTransformVisitor : boost::static_visitor<SE3Tpl<Scalar, Options>>
621 {
622 typedef SE3Tpl<Scalar, Options> ReturnType;
623
624 template<typename JointDataDerived>
625 1436 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
626 {
627 1436 return ReturnType(jdata.M());
628 }
629
630 720 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
631 {
632
1/2
✓ Branch 1 taken 718 times.
✗ Branch 2 not taken.
 1440 return boost::apply_visitor(JointTransformVisitor(), jdata);
633 }
634 };
635
636 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
637 inline SE3Tpl<Scalar, Options>
638 720 joint_transform(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
639 {
640 720 return JointTransformVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
641 }
642
643 /**
644 * @brief JointMotionVisitor visitor
645 */
646 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
647 struct JointMotionVisitor : boost::static_visitor<MotionTpl<Scalar, Options>>
648 {
649 typedef MotionTpl<Scalar, Options> ReturnType;
650
651 template<typename JointDataDerived>
652 1478 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
653 {
654 1478 return ReturnType(jdata.v());
655 }
656
657 741 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
658 {
659
1/2
✓ Branch 1 taken 739 times.
✗ Branch 2 not taken.
 1482 return boost::apply_visitor(JointMotionVisitor(), jdata);
660 }
661 };
662
663 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
664 inline MotionTpl<Scalar, Options>
665 739 motion(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
666 {
667 739 return JointMotionVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
668 }
669
670 /**
671 * @brief JointBiasVisitor visitor
672 */
673 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
674 struct JointBiasVisitor : boost::static_visitor<MotionTpl<Scalar, Options>>
675 {
676 typedef MotionTpl<Scalar, Options> ReturnType;
677
678 template<typename JointDataDerived>
679 1374 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
680 {
681 1374 return ReturnType(jdata.c());
682 }
683
684 687 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
685 {
686
1/2
✓ Branch 1 taken 687 times.
✗ Branch 2 not taken.
 1374 return boost::apply_visitor(JointBiasVisitor(), jdata);
687 }
688 };
689
690 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
691 inline MotionTpl<Scalar, Options>
692 687 bias(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
693 {
694 687 return JointBiasVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
695 }
696
697 /**
698 * @brief JointUInertiaVisitor visitor
699 */
700 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
701 struct JointUInertiaVisitor
702 : boost::static_visitor<Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options>>
703 {
704 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> ReturnType;
705
706 template<typename JointDataDerived>
707 1648 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
708 {
709 1648 return ReturnType(jdata.U());
710 }
711
712 824 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
713 {
714
1/2
✓ Branch 1 taken 824 times.
✗ Branch 2 not taken.
 1648 return boost::apply_visitor(JointUInertiaVisitor(), jdata);
715 }
716 };
717
718 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
719 inline Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options>
720 824 u_inertia(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
721 {
722 824 return JointUInertiaVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
723 }
724
725 /**
726 * @brief JointDInvInertiaVisitor visitor
727 */
728 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
729 struct JointDInvInertiaVisitor
730 : boost::static_visitor<Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options>>
731 {
732 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> ReturnType;
733
734 template<typename JointDataDerived>
735 1486 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
736 {
737 1486 return ReturnType(jdata.Dinv());
738 }
739
740 743 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
741 {
742
1/2
✓ Branch 1 taken 743 times.
✗ Branch 2 not taken.
 1486 return boost::apply_visitor(JointDInvInertiaVisitor(), jdata);
743 }
744 };
745
746 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
747 inline Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options>
748 743 dinv_inertia(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
749 {
750 743 return JointDInvInertiaVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
751 }
752
753 /**
754 * @brief JointUDInvInertiaVisitor visitor
755 */
756 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
757 struct JointUDInvInertiaVisitor
758 : boost::static_visitor<Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options>>
759 {
760 typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> ReturnType;
761
762 template<typename JointDataDerived>
763 1486 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
764 {
765 1486 return ReturnType(jdata.UDinv());
766 }
767
768 743 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
769 {
770
1/2
✓ Branch 1 taken 743 times.
✗ Branch 2 not taken.
 1486 return boost::apply_visitor(JointUDInvInertiaVisitor(), jdata);
771 }
772 };
773
774 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
775 inline Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options>
776 743 udinv_inertia(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
777 {
778 743 return JointUDInvInertiaVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
779 }
780
781 /**
782 * @brief JointStUInertiaVisitor visitor
783 */
784 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
785 struct JointStUInertiaVisitor
786 : boost::static_visitor<Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options>>
787 {
788 typedef Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options> ReturnType;
789
790 template<typename JointDataDerived>
791 166 ReturnType operator()(const JointDataBase<JointDataDerived> & jdata) const
792 {
793 166 return ReturnType(jdata.StU());
794 }
795
796 83 static ReturnType run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
797 {
798
1/2
✓ Branch 1 taken 83 times.
✗ Branch 2 not taken.
 166 return boost::apply_visitor(JointStUInertiaVisitor(), jdata);
799 }
800 };
801
802 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
803 inline Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic, Options>
804 83 stu_inertia(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
805 {
806 83 return JointStUInertiaVisitor<Scalar, Options, JointCollectionTpl>::run(jdata);
807 }
808
809 /**
810 * @brief JointDataShortnameVisitor visitor
811 */
812 struct JointDataShortnameVisitor : boost::static_visitor<std::string>
813 {
814 template<typename JointDataDerived>
815 54 std::string operator()(const JointDataBase<JointDataDerived> & jdata) const
816 {
817 54 return jdata.shortname();
818 }
819
820 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
821 27 static std::string run(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
822 {
823
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 54 return boost::apply_visitor(JointDataShortnameVisitor(), jdata);
824 }
825 };
826
827 template<typename Scalar, int Options, template<typename S, int O> class JointCollectionTpl>
828 27 inline std::string shortname(const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata)
829 {
830 27 return JointDataShortnameVisitor::run(jdata);
831 }
832
833 template<
834 typename Scalar,
835 int Options,
836 template<typename S, int O> class JointCollectionTpl,
837 typename JointDataDerived>
838 struct JointDataComparisonOperatorVisitor
839 : fusion::JointUnaryVisitorBase<
840 JointDataComparisonOperatorVisitor<Scalar, Options, JointCollectionTpl, JointDataDerived>,
841 bool>
842 {
843 typedef boost::fusion::vector<const JointDataDerived &> ArgsType;
844
845 template<typename JointData>
846 100 static bool algo(const JointDataBase<JointData> & jdata_lhs, const JointDataDerived & jdata_rhs)
847 {
848 100 return jdata_lhs.derived() == jdata_rhs;
849 }
850 };
851
852 template<
853 typename Scalar,
854 int Options,
855 template<typename S, int O> class JointCollectionTpl,
856 typename JointDataDerived>
857 50 bool isEqual(
858 const JointDataTpl<Scalar, Options, JointCollectionTpl> & jdata_generic,
859 const JointDataBase<JointDataDerived> & jdata)
860 {
861 typedef JointDataComparisonOperatorVisitor<
862 Scalar, Options, JointCollectionTpl, JointDataDerived>
863 Algo;
864 100 return Algo::run(jdata_generic, typename Algo::ArgsType(boost::ref(jdata.derived())));
865 }
866
867 /// @endcond
868
869 } // namespace pinocchio
870
871 #endif // ifndef __pinocchio_multibody_joint_basic_visitors_hxx__
872