GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/liegroup/liegroup-variant-visitors.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 142 149 95.3%
Branches: 72 145 49.7%
Line Branch Exec Source
1 //
2 // Copyright (c) 2018 CNRS
3 //
4
5 #ifndef __pinocchio_lie_group_variant_visitor_hxx__
6 #define __pinocchio_lie_group_variant_visitor_hxx__
7
8 #include "pinocchio/multibody/liegroup/liegroup-base.hpp"
9 #include "pinocchio/multibody/liegroup/cartesian-product-variant.hpp"
10 #include "pinocchio/multibody/visitor.hpp"
11
12 #include <string>
13
14 #define LIE_GROUP_VISITOR(VISITOR) \
15 VISITOR(ArgsType & args) \
16 : args(args) \
17 { \
18 } \
19 ArgsType & args
20
21 namespace pinocchio
22 {
23
24 namespace visitor
25 {
26 namespace bf = boost::fusion;
27
28 template<typename Visitor>
29 struct LieGroupVisitorBase : public boost::static_visitor<>
30 {
31 template<typename LieGroupDerived>
32 786 void operator()(const LieGroupBase<LieGroupDerived> & lg) const
33 {
34
1/2
✓ Branch 1 taken 393 times.
✗ Branch 2 not taken.
 786 bf::invoke(
35 &Visitor::template algo<LieGroupDerived>,
36
1/2
✓ Branch 2 taken 393 times.
✗ Branch 3 not taken.
 1572 bf::append(boost::ref(lg), static_cast<const Visitor *>(this)->args));
37 786 }
38
39 template<typename LieGroupCollection, typename ArgsTmp>
40 786 static void run(const LieGroupGenericTpl<LieGroupCollection> & lg, ArgsTmp args)
41 {
42
1/2
✓ Branch 2 taken 393 times.
✗ Branch 3 not taken.
 786 return boost::apply_visitor(Visitor(args), lg);
43 }
44 };
45
46 template<typename Scalar, int Options>
47 struct LieGroupEqual : public boost::static_visitor<bool>
48 {
49 template<typename T, typename U>
50 ✗ bool operator()(const T &, const U &) const
51 {
52 // Should we handle
53 // - dynamic size vector space versus static size vector space
54 // - dynamic versus static cartesian product
55 ✗ return false;
56 }
57
58 template<typename T>
59 18 bool operator()(const T & lhs, const T & rhs) const
60 {
61 18 return lhs == rhs;
62 }
63
64 /// \name Vector space comparison
65 /// \{
66 1 bool operator()(
67 const VectorSpaceOperationTpl<Eigen::Dynamic, Scalar, Options> & lhs,
68 const VectorSpaceOperationTpl<Eigen::Dynamic, Scalar, Options> & rhs) const
69 {
70 1 return lhs == rhs;
71 }
72
73 template<int Dim>
74 4 bool operator()(
75 const VectorSpaceOperationTpl<Dim, Scalar, Options> & lhs,
76 const VectorSpaceOperationTpl<Eigen::Dynamic, Scalar, Options> & rhs) const
77 {
78 4 return lhs.nq() == rhs.nq();
79 }
80
81 template<int Dim>
82 ✗ bool operator()(
83 const VectorSpaceOperationTpl<Eigen::Dynamic, Scalar, Options> & lhs,
84 const VectorSpaceOperationTpl<Dim, Scalar, Options> & rhs) const
85 {
86 ✗ return lhs.nq() == rhs.nq();
87 }
88 /// \}
89
90 template<
91 typename LieGroup1,
92 typename LieGroup2,
93 template<typename, int> class LieGroupCollectionTpl>
94 bool operator()(
95 const CartesianProductOperation<LieGroup1, LieGroup2> & lhs,
96 const CartesianProductOperationVariantTpl<Scalar, Options, LieGroupCollectionTpl> & rhs)
97 const
98 {
99 return rhs.isEqual(lhs);
100 }
101
102 template<
103 typename LieGroup1,
104 typename LieGroup2,
105 template<typename, int> class LieGroupCollectionTpl>
106 bool operator()(
107 const CartesianProductOperationVariantTpl<Scalar, Options, LieGroupCollectionTpl> & lhs,
108 const CartesianProductOperation<LieGroup1, LieGroup2> & rhs) const
109 {
110 return lhs.isEqual(rhs);
111 }
112 };
113 } // namespace visitor
114
115 #define PINOCCHIO_LG_CHECK_VECTOR_SIZE(type, var, exp_size) \
116 EIGEN_STATIC_ASSERT_VECTOR_ONLY(type); \
117 assert(var.size() == exp_size)
118 #define PINOCCHIO_LG_CHECK_MATRIX_SIZE(var, nr, nc) \
119 assert(var.rows() == nr); \
120 assert(var.cols() == nc)
121
122 #define PINOCCHIO_LG_VISITOR(Name, type, _method) \
123 /** @brief Lie Group visitor of the _method */ \
124 struct LieGroup##Name##Visitor : public boost::static_visitor<type> \
125 { \
126 template<typename LieGroupDerived> \
127 type operator()(const LieGroupBase<LieGroupDerived> & lg) const \
128 { \
129 return lg._method(); \
130 } \
131 \
132 template<typename LieGroupCollection> \
133 static type run(const LieGroupGenericTpl<LieGroupCollection> & lg) \
134 { \
135 return boost::apply_visitor(LieGroup##Name##Visitor(), lg); \
136 } \
137 }; \
138 \
139 template<typename LieGroupCollection> \
140 inline type _method(const LieGroupGenericTpl<LieGroupCollection> & lg) \
141 { \
142 return LieGroup##Name##Visitor::run(lg); \
143 }
144
145
1/2
✓ Branch 1 taken 666 times.
✗ Branch 2 not taken.
 3996 PINOCCHIO_LG_VISITOR(Nq, int, nq)
146
1/2
✓ Branch 1 taken 503 times.
✗ Branch 2 not taken.
 1509 PINOCCHIO_LG_VISITOR(Nv, int, nv)
147
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 236 PINOCCHIO_LG_VISITOR(Name, std::string, name)
148 #undef PINOCCHIO_LG_VISITOR
149
150 /**
151 * @brief Visitor of the Lie Group neutral element
152 */
153 template<typename Vector>
154 struct LieGroupNeutralVisitor : public boost::static_visitor<Vector>
155 {
156 template<typename LieGroupDerived>
157 118 Vector operator()(const LieGroupBase<LieGroupDerived> & lg) const
158 {
159
1/2
✓ Branch 2 taken 55 times.
✗ Branch 3 not taken.
 118 return lg.neutral();
160 }
161
162 template<typename LieGroupCollection>
163 59 static Vector run(const LieGroupGenericTpl<LieGroupCollection> & lg)
164 {
165
1/2
✓ Branch 1 taken 59 times.
✗ Branch 2 not taken.
 118 return boost::apply_visitor(LieGroupNeutralVisitor(), lg);
166 }
167 };
168
169 template<typename LieGroupCollection>
170 inline Eigen::
171 Matrix<typename LieGroupCollection::Scalar, Eigen::Dynamic, 1, LieGroupCollection::Options>
172 59 neutral(const LieGroupGenericTpl<LieGroupCollection> & lg)
173 {
174 typedef Eigen::Matrix<
175 typename LieGroupCollection::Scalar, Eigen::Dynamic, 1, LieGroupCollection::Options>
176 ReturnType;
177 59 return LieGroupNeutralVisitor<ReturnType>::run(lg);
178 }
179
180 #define PINOCCHIO_LG_VISITOR(Name, _method) \
181 /** @brief Visitor of the Lie Group _method method */ \
182 template<class M_t> \
183 struct LieGroup##Name##Visitor : visitor::LieGroupVisitorBase<LieGroup##Name##Visitor<M_t>> \
184 { \
185 typedef boost::fusion::vector<const Eigen::MatrixBase<M_t> &> ArgsType; \
186 LIE_GROUP_VISITOR(LieGroup##Name##Visitor); \
187 template<typename LieGroupDerived> \
188 static void algo(const LieGroupBase<LieGroupDerived> & lg, const Eigen::MatrixBase<M_t> & m1) \
189 { \
190 lg._method(m1); \
191 } \
192 }
193
194 244 PINOCCHIO_LG_VISITOR(Random, random);
195 36 PINOCCHIO_LG_VISITOR(Normalize, normalize);
196
197 template<typename LieGroupCollection, class Config_t>
198 71 inline void random(
199 const LieGroupGenericTpl<LieGroupCollection> & lg, const Eigen::MatrixBase<Config_t> & qout)
200 {
201
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 61 times.
 71 PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, qout, nq(lg));
202
203 typedef LieGroupRandomVisitor<Config_t> Operation;
204
1/2
✓ Branch 2 taken 61 times.
✗ Branch 3 not taken.
 71 Operation::run(lg, typename Operation::ArgsType(qout));
205 71 }
206
207 template<typename LieGroupCollection, class Config_t>
208 28 inline void normalize(
209 const LieGroupGenericTpl<LieGroupCollection> & lg, const Eigen::MatrixBase<Config_t> & qout)
210 {
211
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 18 times.
 28 PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, qout, nq(lg));
212
213 typedef LieGroupNormalizeVisitor<Config_t> Operation;
214
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 28 Operation::run(lg, typename Operation::ArgsType(qout));
215 28 }
216
217 #undef PINOCCHIO_LG_VISITOR
218
219 /** @brief Visitor of the Lie Group isNormalized method */
220 template<class Matrix_t>
221 struct LieGroupIsNormalizedVisitor
222 : visitor::LieGroupVisitorBase<LieGroupIsNormalizedVisitor<Matrix_t>>
223 {
224 typedef boost::fusion::
225 vector<const Eigen::MatrixBase<Matrix_t> &, const typename Matrix_t::Scalar &, bool &>
226 ArgsType;
227 8 LIE_GROUP_VISITOR(LieGroupIsNormalizedVisitor);
228 template<typename LieGroupDerived>
229 16 static void algo(
230 const LieGroupBase<LieGroupDerived> & lg,
231 const Eigen::MatrixBase<Matrix_t> & qin,
232 const typename Matrix_t::Scalar & prec,
233 bool & res)
234 {
235 16 res = lg.isNormalized(qin, prec);
236 16 }
237 };
238
239 template<typename LieGroupCollection, class Config_t>
240 8 inline bool isNormalized(
241 const LieGroupGenericTpl<LieGroupCollection> & lg,
242 const Eigen::MatrixBase<Config_t> & qin,
243 const typename Config_t::Scalar & prec)
244 {
245
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 8 times.
 8 PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, qin, nq(lg));
246
247 bool res;
248 typedef LieGroupIsNormalizedVisitor<Config_t> Operation;
249
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 8 Operation::run(lg, typename Operation::ArgsType(qin, prec, res));
250 8 return res;
251 }
252
253 /** @brief Visitor of the Lie Group isSameConfiguration method */
254 template<class Matrix1_t, class Matrix2_t>
255 struct LieGroupIsSameConfigurationVisitor
256 : visitor::LieGroupVisitorBase<LieGroupIsSameConfigurationVisitor<Matrix1_t, Matrix2_t>>
257 {
258 typedef boost::fusion::vector<
259 const Eigen::MatrixBase<Matrix1_t> &,
260 const Eigen::MatrixBase<Matrix2_t> &,
261 const typename Matrix1_t::Scalar &,
262 bool &>
263 ArgsType;
264 20 LIE_GROUP_VISITOR(LieGroupIsSameConfigurationVisitor);
265 template<typename LieGroupDerived>
266 20 static void algo(
267 const LieGroupBase<LieGroupDerived> & lg,
268 const Eigen::MatrixBase<Matrix1_t> & q0,
269 const Eigen::MatrixBase<Matrix2_t> & q1,
270 const typename Matrix1_t::Scalar & prec,
271 bool & res)
272 {
273 20 res = lg.isSameConfiguration(q0, q1, prec);
274 }
275 };
276
277 template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t>
278 20 inline bool isSameConfiguration(
279 const LieGroupGenericTpl<LieGroupCollection> & lg,
280 const Eigen::MatrixBase<ConfigL_t> & q0,
281 const Eigen::MatrixBase<ConfigR_t> & q1,
282 const typename ConfigL_t::Scalar & prec)
283 {
284
2/4
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 10 times.
 20 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
285
2/4
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 10 times.
 20 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
286
287 bool res;
288 typedef LieGroupIsSameConfigurationVisitor<ConfigL_t, ConfigR_t> Operation;
289
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 20 Operation::run(lg, typename Operation::ArgsType(q0, q1, prec, res));
290 20 return res;
291 }
292
293 #define PINOCCHIO_LG_VISITOR(Name, _method) \
294 /** @brief Visitor of the Lie Group _method method */ \
295 template<class Matrix1_t, class Matrix2_t> \
296 struct LieGroup##Name##Visitor \
297 : visitor::LieGroupVisitorBase<LieGroup##Name##Visitor<Matrix1_t, Matrix2_t>> \
298 { \
299 typedef boost::fusion::vector< \
300 const Eigen::MatrixBase<Matrix1_t> &, \
301 const Eigen::MatrixBase<Matrix2_t> &, \
302 typename Matrix1_t::Scalar &> \
303 ArgsType; \
304 LIE_GROUP_VISITOR(LieGroup##Name##Visitor); \
305 template<typename LieGroupDerived> \
306 static void algo( \
307 const LieGroupBase<LieGroupDerived> & lg, \
308 const Eigen::MatrixBase<Matrix1_t> & m1, \
309 const Eigen::MatrixBase<Matrix2_t> & m2, \
310 typename Matrix1_t::Scalar & res) \
311 { \
312 res = lg._method(m1, m2); \
313 } \
314 }
315
316 // PINOCCHIO_LG_VISITOR(Distance, distance);
317
0/2
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 112 PINOCCHIO_LG_VISITOR(SquaredDistance, squaredDistance);
318
319 template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t>
320 48 inline typename ConfigL_t::Scalar squaredDistance(
321 const LieGroupGenericTpl<LieGroupCollection> & lg,
322 const Eigen::MatrixBase<ConfigL_t> & q0,
323 const Eigen::MatrixBase<ConfigR_t> & q1)
324 {
325
2/4
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 28 times.
 48 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
326
2/4
✓ Branch 2 taken 28 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 28 times.
 48 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
327
328 typedef LieGroupSquaredDistanceVisitor<ConfigL_t, ConfigR_t> Operation;
329 ✗ typename ConfigL_t::Scalar d2;
330
2/4
✓ Branch 1 taken 28 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28 times.
✗ Branch 5 not taken.
 48 Operation::run(lg, typename Operation::ArgsType(q0, q1, d2));
331 48 return d2;
332 }
333
334 #undef PINOCCHIO_LG_VISITOR
335
336 #define PINOCCHIO_LG_VISITOR(Name, _method) \
337 /** @brief Visitor of the Lie Group _method method */ \
338 template<class Matrix1_t, class Matrix2_t, class Matrix3_t> \
339 struct LieGroup##Name##Visitor \
340 : visitor::LieGroupVisitorBase<LieGroup##Name##Visitor<Matrix1_t, Matrix2_t, Matrix3_t>> \
341 { \
342 typedef boost::fusion::vector< \
343 const Eigen::MatrixBase<Matrix1_t> &, \
344 const Eigen::MatrixBase<Matrix2_t> &, \
345 const Eigen::MatrixBase<Matrix3_t> &> \
346 ArgsType; \
347 LIE_GROUP_VISITOR(LieGroup##Name##Visitor); \
348 template<typename LieGroupDerived> \
349 static void algo( \
350 const LieGroupBase<LieGroupDerived> & lg, \
351 const Eigen::MatrixBase<Matrix1_t> & m1, \
352 const Eigen::MatrixBase<Matrix2_t> & m2, \
353 const Eigen::MatrixBase<Matrix3_t> & m3) \
354 { \
355 lg._method(m1, m2, m3); \
356 } \
357 }
358
359 184 PINOCCHIO_LG_VISITOR(Integrate, integrate);
360 64 PINOCCHIO_LG_VISITOR(Difference, difference);
361 20 PINOCCHIO_LG_VISITOR(RandomConfiguration, randomConfiguration);
362
363 template<typename LieGroupCollection, class ConfigIn_t, class Tangent_t, class ConfigOut_t>
364 56 inline void integrate(
365 const LieGroupGenericTpl<LieGroupCollection> & lg,
366 const Eigen::MatrixBase<ConfigIn_t> & q,
367 const Eigen::MatrixBase<Tangent_t> & v,
368 const Eigen::MatrixBase<ConfigOut_t> & qout)
369 {
370
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 46 times.
 56 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigIn_t, q, nq(lg));
371
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 46 times.
 56 PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg));
372
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 46 times.
 56 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigOut_t, qout, nq(lg));
373
374 typedef LieGroupIntegrateVisitor<ConfigIn_t, Tangent_t, ConfigOut_t> Operation;
375
1/2
✓ Branch 2 taken 46 times.
✗ Branch 3 not taken.
 56 Operation::run(lg, typename Operation::ArgsType(q, v, qout));
376 56 }
377
378 template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class Tangent_t>
379 42 inline void difference(
380 const LieGroupGenericTpl<LieGroupCollection> & lg,
381 const Eigen::MatrixBase<ConfigL_t> & q0,
382 const Eigen::MatrixBase<ConfigR_t> & q1,
383 const Eigen::MatrixBase<Tangent_t> & v)
384 {
385
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 32 times.
 42 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
386
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 32 times.
 42 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
387
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 32 times.
 42 PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg));
388
389 typedef LieGroupDifferenceVisitor<ConfigL_t, ConfigR_t, Tangent_t> Operation;
390
1/2
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
 42 Operation::run(lg, typename Operation::ArgsType(q0, q1, v));
391 42 }
392
393 template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class ConfigOut_t>
394 20 inline void randomConfiguration(
395 const LieGroupGenericTpl<LieGroupCollection> & lg,
396 const Eigen::MatrixBase<ConfigL_t> & q0,
397 const Eigen::MatrixBase<ConfigR_t> & q1,
398 const Eigen::MatrixBase<ConfigOut_t> & qout)
399 {
400
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
 20 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
401
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
 20 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
402
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 10 times.
 20 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigOut_t, qout, nq(lg));
403
404 typedef LieGroupRandomConfigurationVisitor<ConfigL_t, ConfigR_t, ConfigOut_t> Operation;
405
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 20 Operation::run(lg, typename Operation::ArgsType(q0, q1, qout));
406 20 }
407
408 #undef PINOCCHIO_LG_VISITOR
409
410 /** @brief Visitor of the Lie Group interpolate method */
411 template<class Matrix1_t, class Matrix2_t, class Matrix3_t>
412 struct LieGroupInterpolateVisitor
413 : visitor::LieGroupVisitorBase<LieGroupInterpolateVisitor<Matrix1_t, Matrix2_t, Matrix3_t>>
414 {
415 typedef boost::fusion::vector<
416 const Eigen::MatrixBase<Matrix1_t> &,
417 const Eigen::MatrixBase<Matrix2_t> &,
418 const typename Matrix1_t::Scalar &,
419 const Eigen::MatrixBase<Matrix3_t> &>
420 ArgsType;
421 LIE_GROUP_VISITOR(LieGroupInterpolateVisitor);
422 template<typename LieGroupDerived>
423 static void algo(
424 const LieGroupBase<LieGroupDerived> & lg,
425 const Eigen::MatrixBase<Matrix1_t> & q0,
426 const Eigen::MatrixBase<Matrix2_t> & q1,
427 const typename Matrix1_t::Scalar & u,
428 const Eigen::MatrixBase<Matrix3_t> & qout)
429 {
430 lg.interpolate(q0, q1, u, qout);
431 }
432 };
433
434 template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class ConfigOut_t>
435 inline void interpolate(
436 const LieGroupGenericTpl<LieGroupCollection> & lg,
437 const Eigen::MatrixBase<ConfigL_t> & q0,
438 const Eigen::MatrixBase<ConfigR_t> & q1,
439 const typename ConfigL_t::Scalar & u,
440 const Eigen::MatrixBase<ConfigOut_t> & qout)
441 {
442 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
443 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
444 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigOut_t, qout, nq(lg));
445
446 typedef LieGroupInterpolateVisitor<ConfigL_t, ConfigR_t, ConfigOut_t> Operation;
447 Operation::run(lg, typename Operation::ArgsType(q0, q1, u, qout));
448 }
449
450 #define PINOCCHIO_LG_VISITOR(Name, _method, HasArgPos) \
451 /** @brief Visitor of the Lie Group _method method */ \
452 template<class Matrix1_t, class Matrix2_t, class Matrix3_t> \
453 struct LieGroup##Name##Visitor \
454 : visitor::LieGroupVisitorBase<LieGroup##Name##Visitor<Matrix1_t, Matrix2_t, Matrix3_t>> \
455 { \
456 typedef boost::fusion::vector< \
457 const Eigen::MatrixBase<Matrix1_t> &, \
458 const Eigen::MatrixBase<Matrix2_t> &, \
459 const Eigen::MatrixBase<Matrix3_t> &, \
460 const ArgumentPosition BOOST_PP_COMMA_IF(HasArgPos) \
461 BOOST_PP_IIF(HasArgPos, const AssignmentOperatorType, )> \
462 ArgsType; \
463 LIE_GROUP_VISITOR(LieGroup##Name##Visitor); \
464 template<typename LieGroupDerived> \
465 static void algo( \
466 const LieGroupBase<LieGroupDerived> & lg, \
467 const Eigen::MatrixBase<Matrix1_t> & m1, \
468 const Eigen::MatrixBase<Matrix2_t> & m2, \
469 const Eigen::MatrixBase<Matrix3_t> & m3, \
470 const ArgumentPosition arg BOOST_PP_COMMA_IF(HasArgPos) \
471 BOOST_PP_IF(HasArgPos, const AssignmentOperatorType op = SETTO, )) \
472 { \
473 lg._method(m1, m2, m3, arg BOOST_PP_COMMA_IF(HasArgPos) BOOST_PP_IF(HasArgPos, op, )); \
474 } \
475 }
476
477 192 PINOCCHIO_LG_VISITOR(DIntegrate, dIntegrate, 1);
478 68 PINOCCHIO_LG_VISITOR(DDifference, dDifference, 0);
479
480 template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianOut_t>
481 68 void dIntegrate(
482 const LieGroupGenericTpl<LieGroupCollection> & lg,
483 const Eigen::MatrixBase<Config_t> & q,
484 const Eigen::MatrixBase<Tangent_t> & v,
485 const Eigen::MatrixBase<JacobianOut_t> & J,
486 const ArgumentPosition arg,
487 const AssignmentOperatorType op)
488 {
489
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 48 times.
 68 PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, q, nq(lg));
490
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 48 times.
 68 PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg));
491
2/4
✗ Branch 2 not taken.
✓ Branch 3 taken 48 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 48 times.
 68 PINOCCHIO_LG_CHECK_MATRIX_SIZE(J, nv(lg), nv(lg));
492
493 typedef LieGroupDIntegrateVisitor<Config_t, Tangent_t, JacobianOut_t> Operation;
494
1/2
✓ Branch 2 taken 48 times.
✗ Branch 3 not taken.
 68 Operation::run(lg, typename Operation::ArgsType(q, v, J, arg, op));
495 68 }
496
497 template<typename LieGroupCollection, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
498 54 void dDifference(
499 const LieGroupGenericTpl<LieGroupCollection> & lg,
500 const Eigen::MatrixBase<ConfigL_t> & q0,
501 const Eigen::MatrixBase<ConfigR_t> & q1,
502 const Eigen::MatrixBase<JacobianOut_t> & J,
503 const ArgumentPosition arg)
504 {
505
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 34 times.
 54 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
506
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 34 times.
 54 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
507
2/4
✗ Branch 2 not taken.
✓ Branch 3 taken 34 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 34 times.
 54 PINOCCHIO_LG_CHECK_MATRIX_SIZE(J, nv(lg), nv(lg));
508
509 typedef LieGroupDDifferenceVisitor<ConfigL_t, ConfigR_t, JacobianOut_t> Operation;
510
1/2
✓ Branch 2 taken 34 times.
✗ Branch 3 not taken.
 54 Operation::run(lg, typename Operation::ArgsType(q0, q1, J, arg));
511 54 }
512
513 #undef PINOCCHIO_LG_VISITOR
514
515 template<class M1_t, class M2_t, class M3_t, class M4_t, bool dIntegrateOnTheLeft>
516 struct LieGroupDIntegrateProductVisitor
517 : visitor::LieGroupVisitorBase<
518 LieGroupDIntegrateProductVisitor<M1_t, M2_t, M3_t, M4_t, dIntegrateOnTheLeft>>
519 {
520 typedef boost::fusion::vector<
521 const M1_t &,
522 const M2_t &,
523 const M3_t &,
524 M4_t &,
525 const ArgumentPosition,
526 const AssignmentOperatorType>
527 ArgsType;
528
529 56 LIE_GROUP_VISITOR(LieGroupDIntegrateProductVisitor);
530
531 template<typename LieGroupDerived>
532 56 static void algo(
533 const LieGroupBase<LieGroupDerived> & lg,
534 const M1_t & q,
535 const M2_t & v,
536 const M3_t & J_in,
537 M4_t & J_out,
538 const ArgumentPosition arg,
539 const AssignmentOperatorType op)
540 {
541
3/6
✓ Branch 0 taken 14 times.
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 14 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 56 PINOCCHIO_CHECK_INPUT_ARGUMENT(
542 arg == ARG0 || arg == ARG1, "arg should be either ARG0 or ARG1");
543
2/3
✓ Branch 0 taken 14 times.
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 56 switch (arg)
544 {
545 28 case ARG0:
546 if (dIntegrateOnTheLeft)
547 14 lg.dIntegrate_dq(q, v, SELF, J_in, J_out, op);
548 else
549 14 lg.dIntegrate_dq(q, v, J_in, SELF, J_out, op);
550 28 return;
551 28 case ARG1:
552 if (dIntegrateOnTheLeft)
553 14 lg.dIntegrate_dv(q, v, SELF, J_in, J_out, op);
554 else
555 14 lg.dIntegrate_dv(q, v, J_in, SELF, J_out, op);
556 28 return;
557 ✗ default:
558 ✗ return;
559 }
560 }
561 };
562
563 template<
564 typename LieGroupCollection,
565 class Config_t,
566 class Tangent_t,
567 class JacobianIn_t,
568 class JacobianOut_t>
569 14 void dIntegrate(
570 const LieGroupGenericTpl<LieGroupCollection> & lg,
571 const Eigen::MatrixBase<Config_t> & q,
572 const Eigen::MatrixBase<Tangent_t> & v,
573 const Eigen::MatrixBase<JacobianIn_t> & J_in,
574 int self,
575 const Eigen::MatrixBase<JacobianOut_t> & J_out,
576 const ArgumentPosition arg,
577 const AssignmentOperatorType op)
578 {
579 PINOCCHIO_UNUSED_VARIABLE(self);
580
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 14 times.
 14 PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, q, nq(lg));
581
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 14 times.
 14 PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg));
582
583 typedef LieGroupDIntegrateProductVisitor<
584 Config_t, Tangent_t, JacobianIn_t, JacobianOut_t, false>
585 Operation;
586
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 Operation::run(
587 14 lg, typename Operation::ArgsType(
588 q.derived(), v.derived(), J_in.derived(),
589 14 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), arg, op));
590 14 }
591
592 template<
593 typename LieGroupCollection,
594 class Config_t,
595 class Tangent_t,
596 class JacobianIn_t,
597 class JacobianOut_t>
598 14 void dIntegrate(
599 const LieGroupGenericTpl<LieGroupCollection> & lg,
600 const Eigen::MatrixBase<Config_t> & q,
601 const Eigen::MatrixBase<Tangent_t> & v,
602 int self,
603 const Eigen::MatrixBase<JacobianIn_t> & J_in,
604 const Eigen::MatrixBase<JacobianOut_t> & J_out,
605 const ArgumentPosition arg,
606 const AssignmentOperatorType op)
607 {
608 PINOCCHIO_UNUSED_VARIABLE(self);
609
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 14 times.
 14 PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, q, nq(lg));
610
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 14 times.
 14 PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg));
611
612 typedef LieGroupDIntegrateProductVisitor<Config_t, Tangent_t, JacobianIn_t, JacobianOut_t, true>
613 Operation;
614
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 Operation::run(
615 14 lg, typename Operation::ArgsType(
616 q.derived(), v.derived(), J_in.derived(),
617 14 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), arg, op));
618 14 }
619
620 template<
621 class M1_t,
622 class M2_t,
623 class M3_t,
624 class M4_t,
625 bool dDifferenceOnTheLeft,
626 ArgumentPosition arg>
627 struct LieGroupDDifferenceProductVisitor
628 : visitor::LieGroupVisitorBase<
629 LieGroupDDifferenceProductVisitor<M1_t, M2_t, M3_t, M4_t, dDifferenceOnTheLeft, arg>>
630 {
631 typedef boost::fusion::
632 vector<const M1_t &, const M2_t &, const M3_t &, M4_t &, const AssignmentOperatorType>
633 ArgsType;
634
635 56 LIE_GROUP_VISITOR(LieGroupDDifferenceProductVisitor);
636
637 template<typename LieGroupDerived>
638 56 static void algo(
639 const LieGroupBase<LieGroupDerived> & lg,
640 const M1_t & q0,
641 const M2_t & q1,
642 const M3_t & J_in,
643 M4_t & J_out,
644 const AssignmentOperatorType op)
645 {
646 if (dDifferenceOnTheLeft)
647 28 lg.template dDifference<arg>(q0, q1, SELF, J_in, J_out, op);
648 else
649 28 lg.template dDifference<arg>(q0, q1, J_in, SELF, J_out, op);
650 }
651 };
652
653 template<
654 ArgumentPosition arg,
655 typename LieGroupCollection,
656 class ConfigL_t,
657 class ConfigR_t,
658 class JacobianIn_t,
659 class JacobianOut_t>
660 28 void dDifference(
661 const LieGroupGenericTpl<LieGroupCollection> & lg,
662 const Eigen::MatrixBase<ConfigL_t> & q0,
663 const Eigen::MatrixBase<ConfigR_t> & q1,
664 const Eigen::MatrixBase<JacobianIn_t> & J_in,
665 int self,
666 const Eigen::MatrixBase<JacobianOut_t> & J_out,
667 const AssignmentOperatorType op)
668 {
669 PINOCCHIO_UNUSED_VARIABLE(self);
670
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 14 times.
 28 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
671
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 14 times.
 28 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
672
673 typedef LieGroupDDifferenceProductVisitor<
674 ConfigL_t, ConfigR_t, JacobianIn_t, JacobianOut_t, false, arg>
675 Operation;
676
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 Operation::run(
677 28 lg, typename Operation::ArgsType(
678 q0.derived(), q1.derived(), J_in.derived(),
679 28 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), op));
680 28 }
681
682 template<
683 ArgumentPosition arg,
684 typename LieGroupCollection,
685 class ConfigL_t,
686 class ConfigR_t,
687 class JacobianIn_t,
688 class JacobianOut_t>
689 28 void dDifference(
690 const LieGroupGenericTpl<LieGroupCollection> & lg,
691 const Eigen::MatrixBase<ConfigL_t> & q0,
692 const Eigen::MatrixBase<ConfigR_t> & q1,
693 int self,
694 const Eigen::MatrixBase<JacobianIn_t> & J_in,
695 const Eigen::MatrixBase<JacobianOut_t> & J_out,
696 const AssignmentOperatorType op)
697 {
698 PINOCCHIO_UNUSED_VARIABLE(self);
699
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 14 times.
 28 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigL_t, q0, nq(lg));
700
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 14 times.
 28 PINOCCHIO_LG_CHECK_VECTOR_SIZE(ConfigR_t, q1, nq(lg));
701
702 typedef LieGroupDDifferenceProductVisitor<
703 ConfigL_t, ConfigR_t, JacobianIn_t, JacobianOut_t, true, arg>
704 Operation;
705
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 Operation::run(
706 28 lg, typename Operation::ArgsType(
707 q0.derived(), q1.derived(), J_in.derived(),
708 28 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out), op));
709 28 }
710
711 template<class M1_t, class M2_t, class M3_t, class M4_t>
712 struct LieGroupDIntegrateTransportVisitor
713 : visitor::LieGroupVisitorBase<LieGroupDIntegrateTransportVisitor<M1_t, M2_t, M3_t, M4_t>>
714 {
715 typedef boost::fusion::vector<
716 const Eigen::MatrixBase<M1_t> &,
717 const Eigen::MatrixBase<M2_t> &,
718 const Eigen::MatrixBase<M3_t> &,
719 const Eigen::MatrixBase<M4_t> &,
720 const ArgumentPosition>
721 ArgsType;
722
723 42 LIE_GROUP_VISITOR(LieGroupDIntegrateTransportVisitor);
724
725 template<typename LieGroupDerived>
726 84 static void algo(
727 const LieGroupBase<LieGroupDerived> & lg,
728 const Eigen::MatrixBase<M1_t> & q,
729 const Eigen::MatrixBase<M2_t> & v,
730 const Eigen::MatrixBase<M3_t> & J_in,
731 const Eigen::MatrixBase<M4_t> & J_out,
732 const ArgumentPosition arg)
733 {
734 84 lg.dIntegrateTransport(q, v, J_in, J_out, arg);
735 }
736 };
737
738 template<class M1_t, class M2_t, class M3_t>
739 struct LieGroupDIntegrateTransportVisitor<M1_t, M2_t, M3_t, void>
740 : visitor::LieGroupVisitorBase<LieGroupDIntegrateTransportVisitor<M1_t, M2_t, M3_t, void>>
741 {
742 typedef boost::fusion::vector<
743 const Eigen::MatrixBase<M1_t> &,
744 const Eigen::MatrixBase<M2_t> &,
745 const Eigen::MatrixBase<M3_t> &,
746 const ArgumentPosition>
747 ArgsType;
748
749 LIE_GROUP_VISITOR(LieGroupDIntegrateTransportVisitor);
750 template<typename LieGroupDerived>
751 static void algo(
752 const LieGroupBase<LieGroupDerived> & lg,
753 const Eigen::MatrixBase<M1_t> & q,
754 const Eigen::MatrixBase<M2_t> & v,
755 const Eigen::MatrixBase<M3_t> & J,
756 const ArgumentPosition arg)
757 {
758 lg.dIntegrateTransport(q, v, J, arg);
759 }
760 };
761
762 template<
763 typename LieGroupCollection,
764 class Config_t,
765 class Tangent_t,
766 class JacobianIn_t,
767 class JacobianOut_t>
768 42 void dIntegrateTransport(
769 const LieGroupGenericTpl<LieGroupCollection> & lg,
770 const Eigen::MatrixBase<Config_t> & q,
771 const Eigen::MatrixBase<Tangent_t> & v,
772 const Eigen::MatrixBase<JacobianIn_t> & J_in,
773 const Eigen::MatrixBase<JacobianOut_t> & J_out,
774 const ArgumentPosition arg)
775 {
776
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 42 times.
 42 PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, q, nq(lg));
777
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 42 times.
 42 PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg));
778
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 42 times.
 42 assert(J_in.rows() == nv(lg));
779
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 42 times.
 42 assert(J_out.rows() == nv(lg));
780
781 typedef LieGroupDIntegrateTransportVisitor<Config_t, Tangent_t, JacobianIn_t, JacobianOut_t>
782 Operation;
783
1/2
✓ Branch 2 taken 42 times.
✗ Branch 3 not taken.
 42 Operation::run(lg, typename Operation::ArgsType(q, v, J_in, J_out, arg));
784 42 }
785
786 template<typename LieGroupCollection, class Config_t, class Tangent_t, class JacobianOut_t>
787 void dIntegrateTransport(
788 const LieGroupGenericTpl<LieGroupCollection> & lg,
789 const Eigen::MatrixBase<Config_t> & q,
790 const Eigen::MatrixBase<Tangent_t> & v,
791 const Eigen::MatrixBase<JacobianOut_t> & J,
792 const ArgumentPosition arg)
793 {
794 PINOCCHIO_LG_CHECK_VECTOR_SIZE(Config_t, q, nq(lg));
795 PINOCCHIO_LG_CHECK_VECTOR_SIZE(Tangent_t, v, nv(lg));
796 assert(J.rows() == nv(lg));
797
798 typedef LieGroupDIntegrateTransportVisitor<Config_t, Tangent_t, JacobianOut_t, void> Operation;
799 Operation::run(lg, typename Operation::ArgsType(q, v, J, arg));
800 }
801
802 } // namespace pinocchio
803
804 #undef PINOCCHIO_LG_CHECK_VECTOR_SIZE
805
806 #endif // ifndef __pinocchio_lie_group_variant_visitor_hxx__
807