GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/liegroup/liegroup-base.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 188 216 87.0%
Branches: 76 191 39.8%
Line Branch Exec Source
1 //
2 // Copyright (c) 2016-2020 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_multibody_liegroup_liegroup_operation_base_hxx__
6 #define __pinocchio_multibody_liegroup_liegroup_operation_base_hxx__
7
8 #include "pinocchio/macros.hpp"
9
10 namespace pinocchio
11 {
12
13 // --------------- API with return value as argument ---------------------- //
14
15 template<class Derived>
16 template<class ConfigIn_t, class Tangent_t, class ConfigOut_t>
17 365553 void LieGroupBase<Derived>::integrate(
18 const Eigen::MatrixBase<ConfigIn_t> & q,
19 const Eigen::MatrixBase<Tangent_t> & v,
20 const Eigen::MatrixBase<ConfigOut_t> & qout) const
21 {
22 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigIn_t, ConfigVector_t);
23 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t, TangentVector_t);
24 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, ConfigVector_t);
25 731106 derived().integrate_impl(
26 365553 q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout));
27 365553 }
28
29 template<class Derived>
30 template<class Config_t, class Jacobian_t>
31 536 void LieGroupBase<Derived>::integrateCoeffWiseJacobian(
32 const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J) const
33 {
34 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
35
36 1072 derived().integrateCoeffWiseJacobian_impl(
37 536 q.derived(), PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J));
38 536 }
39
40 template<class Derived>
41 template<class Config_t, class Tangent_t, class JacobianOut_t>
42 1576 void LieGroupBase<Derived>::dIntegrate(
43 const Eigen::MatrixBase<Config_t> & q,
44 const Eigen::MatrixBase<Tangent_t> & v,
45 const Eigen::MatrixBase<JacobianOut_t> & J,
46 const ArgumentPosition arg,
47 const AssignmentOperatorType op) const
48 {
49
3/4
✓ Branch 0 taken 474 times.
✓ Branch 1 taken 314 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 474 times.
 1576 assert((arg == ARG0 || arg == ARG1) && "arg should be either ARG0 or ARG1");
50
51
2/3
✓ Branch 0 taken 314 times.
✓ Branch 1 taken 474 times.
✗ Branch 2 not taken.
 1576 switch (arg)
52 {
53 628 case ARG0:
54 628 dIntegrate_dq(q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J), op);
55 628 return;
56 948 case ARG1:
57 948 dIntegrate_dv(q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J), op);
58 948 return;
59 ✗ default:
60 ✗ return;
61 }
62 }
63
64 template<class Derived>
65 template<class Config_t, class Tangent_t, class JacobianOut_t>
66 1148 void LieGroupBase<Derived>::dIntegrate_dq(
67 const Eigen::MatrixBase<Config_t> & q,
68 const Eigen::MatrixBase<Tangent_t> & v,
69 const Eigen::MatrixBase<JacobianOut_t> & J,
70 const AssignmentOperatorType op) const
71 {
72 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
73 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t, TangentVector_t);
74 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t);
75 2296 derived().dIntegrate_dq_impl(
76 1148 q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J), op);
77 1148 }
78
79 template<class Derived>
80 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
81 26 void LieGroupBase<Derived>::dIntegrate_dq(
82 const Eigen::MatrixBase<Config_t> & q,
83 const Eigen::MatrixBase<Tangent_t> & v,
84 const Eigen::MatrixBase<JacobianIn_t> & Jin,
85 int self,
86 const Eigen::MatrixBase<JacobianOut_t> & Jout,
87 const AssignmentOperatorType op) const
88 {
89 PINOCCHIO_UNUSED_VARIABLE(self);
90 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
91 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t, TangentVector_t);
92
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
 26 assert(Jin.cols() == nv());
93
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
 26 assert(Jout.cols() == nv());
94
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
 26 assert(Jout.rows() == Jin.rows());
95 52 derived().dIntegrate_product_impl(
96 26 q.derived(), v.derived(), Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout),
97 false, ARG0, op);
98 }
99
100 template<class Derived>
101 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
102 26 void LieGroupBase<Derived>::dIntegrate_dq(
103 const Eigen::MatrixBase<Config_t> & q,
104 const Eigen::MatrixBase<Tangent_t> & v,
105 int self,
106 const Eigen::MatrixBase<JacobianIn_t> & Jin,
107 const Eigen::MatrixBase<JacobianOut_t> & Jout,
108 const AssignmentOperatorType op) const
109 {
110 PINOCCHIO_UNUSED_VARIABLE(self);
111 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
112 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t, TangentVector_t);
113
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
 26 assert(Jin.rows() == nv());
114
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
 26 assert(Jout.rows() == nv());
115
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
 26 assert(Jout.cols() == Jin.cols());
116 52 derived().dIntegrate_product_impl(
117 26 q.derived(), v.derived(), Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout),
118 true, ARG0, op);
119 }
120
121 template<class Derived>
122 template<class Config_t, class Tangent_t, class JacobianOut_t>
123 1628 void LieGroupBase<Derived>::dIntegrate_dv(
124 const Eigen::MatrixBase<Config_t> & q,
125 const Eigen::MatrixBase<Tangent_t> & v,
126 const Eigen::MatrixBase<JacobianOut_t> & J,
127 const AssignmentOperatorType op) const
128 {
129 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
130 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t, TangentVector_t);
131 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t);
132 3256 derived().dIntegrate_dv_impl(
133 1628 q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J), op);
134 1628 }
135
136 template<class Derived>
137 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
138 26 void LieGroupBase<Derived>::dIntegrate_dv(
139 const Eigen::MatrixBase<Config_t> & q,
140 const Eigen::MatrixBase<Tangent_t> & v,
141 const Eigen::MatrixBase<JacobianIn_t> & Jin,
142 int self,
143 const Eigen::MatrixBase<JacobianOut_t> & Jout,
144 const AssignmentOperatorType op) const
145 {
146 PINOCCHIO_UNUSED_VARIABLE(self);
147 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
148 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t, TangentVector_t);
149
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
 26 assert(Jin.cols() == nv());
150
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
 26 assert(Jout.cols() == nv());
151
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
 26 assert(Jout.rows() == Jin.rows());
152 52 derived().dIntegrate_product_impl(
153 26 q.derived(), v.derived(), Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout),
154 false, ARG1, op);
155 }
156
157 template<class Derived>
158 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
159 26 void LieGroupBase<Derived>::dIntegrate_dv(
160 const Eigen::MatrixBase<Config_t> & q,
161 const Eigen::MatrixBase<Tangent_t> & v,
162 int self,
163 const Eigen::MatrixBase<JacobianIn_t> & Jin,
164 const Eigen::MatrixBase<JacobianOut_t> & Jout,
165 const AssignmentOperatorType op) const
166 {
167 PINOCCHIO_UNUSED_VARIABLE(self);
168 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
169 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t, TangentVector_t);
170
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
 26 assert(Jin.rows() == nv());
171
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
 26 assert(Jout.rows() == nv());
172
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
 26 assert(Jout.cols() == Jin.cols());
173 52 derived().dIntegrate_product_impl(
174 26 q.derived(), v.derived(), Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout),
175 true, ARG1, op);
176 }
177
178 template<class Derived>
179 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
180 1588 void LieGroupBase<Derived>::dIntegrateTransport(
181 const Eigen::MatrixBase<Config_t> & q,
182 const Eigen::MatrixBase<Tangent_t> & v,
183 const Eigen::MatrixBase<JacobianIn_t> & Jin,
184 const Eigen::MatrixBase<JacobianOut_t> & Jout,
185 const ArgumentPosition arg) const
186 {
187
3/4
✓ Branch 0 taken 51 times.
✓ Branch 1 taken 743 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 51 times.
 1588 assert((arg == ARG0 || arg == ARG1) && "arg should be either ARG0 or ARG1");
188
189
2/3
✓ Branch 0 taken 743 times.
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 1588 switch (arg)
190 {
191 1486 case ARG0:
192 1486 dIntegrateTransport_dq(
193 1486 q.derived(), v.derived(), Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout));
194 1486 return;
195 102 case ARG1:
196 102 dIntegrateTransport_dv(
197 102 q.derived(), v.derived(), Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout));
198 102 return;
199 ✗ default:
200 ✗ return;
201 }
202 }
203
204 template<class Derived>
205 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
206 2126 void LieGroupBase<Derived>::dIntegrateTransport_dq(
207 const Eigen::MatrixBase<Config_t> & q,
208 const Eigen::MatrixBase<Tangent_t> & v,
209 const Eigen::MatrixBase<JacobianIn_t> & Jin,
210 const Eigen::MatrixBase<JacobianOut_t> & Jout) const
211 {
212 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
213 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t, TangentVector_t);
214 // EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t);
215 4252 derived().dIntegrateTransport_dq_impl(
216 2126 q.derived(), v.derived(), Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout));
217 2126 }
218
219 template<class Derived>
220 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
221 102 void LieGroupBase<Derived>::dIntegrateTransport_dv(
222 const Eigen::MatrixBase<Config_t> & q,
223 const Eigen::MatrixBase<Tangent_t> & v,
224 const Eigen::MatrixBase<JacobianIn_t> & Jin,
225 const Eigen::MatrixBase<JacobianOut_t> & Jout) const
226 {
227 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
228 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t, TangentVector_t);
229 // EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t);
230 204 derived().dIntegrateTransport_dv_impl(
231 102 q.derived(), v.derived(), Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout));
232 }
233
234 template<class Derived>
235 template<class Config_t, class Tangent_t, class Jacobian_t>
236 44 void LieGroupBase<Derived>::dIntegrateTransport(
237 const Eigen::MatrixBase<Config_t> & q,
238 const Eigen::MatrixBase<Tangent_t> & v,
239 const Eigen::MatrixBase<Jacobian_t> & J,
240 const ArgumentPosition arg) const
241 {
242
3/4
✓ Branch 0 taken 11 times.
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 11 times.
 44 assert((arg == ARG0 || arg == ARG1) && "arg should be either ARG0 or ARG1");
243
244
2/3
✓ Branch 0 taken 11 times.
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 44 switch (arg)
245 {
246 22 case ARG0:
247 22 dIntegrateTransport_dq(q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J));
248 22 return;
249 22 case ARG1:
250 22 dIntegrateTransport_dv(q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J));
251 22 return;
252 ✗ default:
253 ✗ return;
254 }
255 }
256
257 template<class Derived>
258 template<class Config_t, class Tangent_t, class Jacobian_t>
259 22 void LieGroupBase<Derived>::dIntegrateTransport_dq(
260 const Eigen::MatrixBase<Config_t> & q,
261 const Eigen::MatrixBase<Tangent_t> & v,
262 const Eigen::MatrixBase<Jacobian_t> & J) const
263 {
264 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
265 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t, TangentVector_t);
266 // EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t);
267 44 derived().dIntegrateTransport_dq_impl(
268 22 q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J));
269 }
270
271 template<class Derived>
272 template<class Config_t, class Tangent_t, class Jacobian_t>
273 22 void LieGroupBase<Derived>::dIntegrateTransport_dv(
274 const Eigen::MatrixBase<Config_t> & q,
275 const Eigen::MatrixBase<Tangent_t> & v,
276 const Eigen::MatrixBase<Jacobian_t> & J) const
277 {
278 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
279 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t, TangentVector_t);
280 // EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t);
281 44 derived().dIntegrateTransport_dv_impl(
282 22 q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J));
283 }
284
285 /**
286 * @brief Interpolation between two joint's configurations
287 *
288 * @param[in] q0 Initial configuration to interpolate
289 * @param[in] q1 Final configuration to interpolate
290 * @param[in] u u in [0;1] position along the interpolation.
291 *
292 * @return The interpolated configuration (q0 if u = 0, q1 if u = 1)
293 */
294 template<class Derived>
295 template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
296 96137 void LieGroupBase<Derived>::interpolate(
297 const Eigen::MatrixBase<ConfigL_t> & q0,
298 const Eigen::MatrixBase<ConfigR_t> & q1,
299 const Scalar & u,
300 const Eigen::MatrixBase<ConfigOut_t> & qout) const
301 {
302 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
303 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
304 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, ConfigVector_t);
305 96137 derived().interpolate_impl(q0, q1, u, PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout));
306 96137 }
307
308 template<class Derived>
309 template<class Config_t>
310 33170 void LieGroupBase<Derived>::normalize(const Eigen::MatrixBase<Config_t> & qout) const
311 {
312 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
313 33170 return derived().normalize_impl(PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout));
314 }
315
316 template<class Derived>
317 template<class Config_t>
318 143306 bool LieGroupBase<Derived>::isNormalized(
319 const Eigen::MatrixBase<Config_t> & qin, const Scalar & prec) const
320 {
321 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
322 143306 return derived().isNormalized_impl(qin, prec);
323 }
324
325 /**
326 * @brief Generate a random joint configuration, normalizing quaternions when necessary.
327 *
328 * \warning Do not take into account the joint limits. To shoot a configuration uniformingly
329 * depending on joint limits, see randomConfiguration
330 *
331 * @return The joint configuration
332 */
333 template<class Derived>
334 template<class Config_t>
335 5521 void LieGroupBase<Derived>::random(const Eigen::MatrixBase<Config_t> & qout) const
336 {
337 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
338 5521 return derived().random_impl(PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout));
339 }
340
341 template<class Derived>
342 template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
343 300926 void LieGroupBase<Derived>::randomConfiguration(
344 const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
345 const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit,
346 const Eigen::MatrixBase<ConfigOut_t> & qout) const
347 {
348 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
349 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
350 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, ConfigVector_t);
351 601852 derived().randomConfiguration_impl(
352 300926 lower_pos_limit.derived(), upper_pos_limit.derived(),
353 300926 PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout));
354 300924 }
355
356 template<class Derived>
357 template<class ConfigL_t, class ConfigR_t, class Tangent_t>
358 209585 void LieGroupBase<Derived>::difference(
359 const Eigen::MatrixBase<ConfigL_t> & q0,
360 const Eigen::MatrixBase<ConfigR_t> & q1,
361 const Eigen::MatrixBase<Tangent_t> & d) const
362 {
363 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
364 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
365 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t, TangentVector_t);
366 209585 derived().difference_impl(q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d));
367 209585 }
368
369 template<class Derived>
370 template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
371 2536 void LieGroupBase<Derived>::dDifference(
372 const Eigen::MatrixBase<ConfigL_t> & q0,
373 const Eigen::MatrixBase<ConfigR_t> & q1,
374 const Eigen::MatrixBase<JacobianOut_t> & J) const
375 {
376 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
377 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
378 EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(JacobianOut_t, JacobianMatrix_t);
379 PINOCCHIO_STATIC_ASSERT(arg == ARG0 || arg == ARG1, arg_SHOULD_BE_ARG0_OR_ARG1);
380 5072 derived().template dDifference_impl<arg>(
381 2536 q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J));
382 2536 }
383
384 template<class Derived>
385 template<class ConfigL_t, class ConfigR_t, class JacobianOut_t>
386 728 void LieGroupBase<Derived>::dDifference(
387 const Eigen::MatrixBase<ConfigL_t> & q0,
388 const Eigen::MatrixBase<ConfigR_t> & q1,
389 const Eigen::MatrixBase<JacobianOut_t> & J,
390 const ArgumentPosition arg) const
391 {
392
3/4
✓ Branch 0 taken 182 times.
✓ Branch 1 taken 182 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 182 times.
 728 assert((arg == ARG0 || arg == ARG1) && "arg should be either ARG0 or ARG1");
393
394
2/3
✓ Branch 0 taken 182 times.
✓ Branch 1 taken 182 times.
✗ Branch 2 not taken.
 728 switch (arg)
395 {
396 364 case ARG0:
397 364 dDifference<ARG0>(q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J));
398 364 return;
399 364 case ARG1:
400 364 dDifference<ARG1>(q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J));
401 364 return;
402 ✗ default:
403 ✗ return;
404 }
405 }
406
407 template<class Derived>
408 template<
409 ArgumentPosition arg,
410 class ConfigL_t,
411 class ConfigR_t,
412 class JacobianIn_t,
413 class JacobianOut_t>
414 52 void LieGroupBase<Derived>::dDifference(
415 const Eigen::MatrixBase<ConfigL_t> & q0,
416 const Eigen::MatrixBase<ConfigR_t> & q1,
417 const Eigen::MatrixBase<JacobianIn_t> & Jin,
418 int self,
419 const Eigen::MatrixBase<JacobianOut_t> & Jout,
420 const AssignmentOperatorType op) const
421 {
422 PINOCCHIO_UNUSED_VARIABLE(self);
423 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
424 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
425
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 26 times.
 52 assert(Jin.cols() == nv());
426
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 26 times.
 52 assert(Jout.cols() == nv());
427
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 26 times.
 52 assert(Jout.rows() == Jin.rows());
428 104 derived().template dDifference_product_impl<arg>(
429 52 q0.derived(), q1.derived(), Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout),
430 false, op);
431 }
432
433 template<class Derived>
434 template<
435 ArgumentPosition arg,
436 class ConfigL_t,
437 class ConfigR_t,
438 class JacobianIn_t,
439 class JacobianOut_t>
440 52 void LieGroupBase<Derived>::dDifference(
441 const Eigen::MatrixBase<ConfigL_t> & q0,
442 const Eigen::MatrixBase<ConfigR_t> & q1,
443 int self,
444 const Eigen::MatrixBase<JacobianIn_t> & Jin,
445 const Eigen::MatrixBase<JacobianOut_t> & Jout,
446 const AssignmentOperatorType op) const
447 {
448 PINOCCHIO_UNUSED_VARIABLE(self);
449 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
450 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
451
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 26 times.
 52 assert(Jin.rows() == nv());
452
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 26 times.
 52 assert(Jout.rows() == nv());
453
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 26 times.
 52 assert(Jout.cols() == Jin.cols());
454 104 derived().template dDifference_product_impl<arg>(
455 52 q0.derived(), q1.derived(), Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout),
456 true, op);
457 }
458
459 template<class Derived>
460 template<class ConfigL_t, class ConfigR_t>
461 33076 typename LieGroupBase<Derived>::Scalar LieGroupBase<Derived>::squaredDistance(
462 const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) const
463 {
464 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
465 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
466
0/2
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 33076 return derived().squaredDistance_impl(q0.derived(), q1.derived());
467 }
468
469 template<class Derived>
470 template<class ConfigL_t, class ConfigR_t>
471 32694 typename LieGroupBase<Derived>::Scalar LieGroupBase<Derived>::distance(
472 const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) const
473 {
474
0/2
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 32694 return sqrt(squaredDistance(q0.derived(), q1.derived()));
475 }
476
477 template<class Derived>
478 template<class ConfigL_t, class ConfigR_t>
479 290 bool LieGroupBase<Derived>::isSameConfiguration(
480 const Eigen::MatrixBase<ConfigL_t> & q0,
481 const Eigen::MatrixBase<ConfigR_t> & q1,
482 const Scalar & prec) const
483 {
484 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t);
485 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t);
486 290 return derived().isSameConfiguration_impl(q0.derived(), q1.derived(), prec);
487 }
488
489 // ----------------- API that allocates memory ---------------------------- //
490
491 template<class Derived>
492 template<class Config_t, class Tangent_t>
493 101822 typename LieGroupBase<Derived>::ConfigVector_t LieGroupBase<Derived>::integrate(
494 const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Tangent_t> & v) const
495 {
496
1/2
✓ Branch 2 taken 50920 times.
✗ Branch 3 not taken.
 101822 ConfigVector_t qout(nq());
497
1/2
✓ Branch 3 taken 18 times.
✗ Branch 4 not taken.
 101822 integrate(q.derived(), v.derived(), qout);
498 101822 return qout;
499 }
500
501 template<class Derived>
502 template<class ConfigL_t, class ConfigR_t>
503 95926 typename LieGroupBase<Derived>::ConfigVector_t LieGroupBase<Derived>::interpolate(
504 const Eigen::MatrixBase<ConfigL_t> & q0,
505 const Eigen::MatrixBase<ConfigR_t> & q1,
506 const Scalar & u) const
507 {
508
1/2
✓ Branch 2 taken 47966 times.
✗ Branch 3 not taken.
 95926 ConfigVector_t qout(nq());
509
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 95926 interpolate(q0.derived(), q1.derived(), u, qout);
510 95926 return qout;
511 }
512
513 template<class Derived>
514 2330 typename LieGroupBase<Derived>::ConfigVector_t LieGroupBase<Derived>::random() const
515 {
516
1/2
✓ Branch 2 taken 1180 times.
✗ Branch 3 not taken.
 2330 ConfigVector_t qout(nq());
517
1/2
✓ Branch 1 taken 33 times.
✗ Branch 2 not taken.
 2330 random(qout);
518 2330 return qout;
519 }
520
521 template<class Derived>
522 template<class ConfigL_t, class ConfigR_t>
523 98218 typename LieGroupBase<Derived>::ConfigVector_t LieGroupBase<Derived>::randomConfiguration(
524 const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit,
525 const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit) const
526 {
527
1/2
✓ Branch 2 taken 49109 times.
✗ Branch 3 not taken.
 98218 ConfigVector_t qout(nq());
528
0/2
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 98218 randomConfiguration(lower_pos_limit.derived(), upper_pos_limit.derived(), qout);
529 98218 return qout;
530 }
531
532 template<class Derived>
533 template<class ConfigL_t, class ConfigR_t>
534 171677 typename LieGroupBase<Derived>::TangentVector_t LieGroupBase<Derived>::difference(
535 const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) const
536 {
537
1/2
✓ Branch 2 taken 85845 times.
✗ Branch 3 not taken.
 171677 TangentVector_t diff(nv());
538
1/2
✓ Branch 3 taken 40 times.
✗ Branch 4 not taken.
 171677 difference(q0.derived(), q1.derived(), diff);
539 171677 return diff;
540 }
541
542 // ----------------- Default implementations ------------------------------ //
543 template<class Derived>
544 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
545 40 void LieGroupBase<Derived>::dIntegrate_product_impl(
546 const Config_t & q,
547 const Tangent_t & v,
548 const JacobianIn_t & Jin,
549 JacobianOut_t & Jout,
550 bool dIntegrateOnTheLeft,
551 const ArgumentPosition arg,
552 const AssignmentOperatorType op) const
553 {
554
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 Index nv_(nv());
555 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
556 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
557
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 JacobianMatrix_t J(nv_, nv_);
558
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 dIntegrate(q, v, J, arg);
559 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
560
1/4
✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 40 switch (op)
561 {
562 40 case SETTO:
563
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 10 times.
 40 if (dIntegrateOnTheLeft)
564
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 20 Jout = J * Jin;
565 else
566
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 20 Jout = Jin * J;
567 40 return;
568 ✗ case ADDTO:
569 ✗ if (dIntegrateOnTheLeft)
570 ✗ Jout += J * Jin;
571 else
572 ✗ Jout += Jin * J;
573 ✗ return;
574 ✗ case RMTO:
575 ✗ if (dIntegrateOnTheLeft)
576 ✗ Jout -= J * Jin;
577 else
578 ✗ Jout -= Jin * J;
579 ✗ return;
580 }
581 }
582
583 template<class Derived>
584 template<
585 ArgumentPosition arg,
586 class ConfigL_t,
587 class ConfigR_t,
588 class JacobianIn_t,
589 class JacobianOut_t>
590 40 void LieGroupBase<Derived>::dDifference_product_impl(
591 const ConfigL_t & q0,
592 const ConfigR_t & q1,
593 const JacobianIn_t & Jin,
594 JacobianOut_t & Jout,
595 bool dDifferenceOnTheLeft,
596 const AssignmentOperatorType op) const
597 {
598
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 Index nv_(nv());
599 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
600 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
601
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 JacobianMatrix_t J(nv_, nv_);
602
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 40 dDifference<arg>(q0, q1, J);
603 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
604
1/4
✓ Branch 0 taken 20 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 40 switch (op)
605 {
606 40 case SETTO:
607
2/2
✓ Branch 0 taken 10 times.
✓ Branch 1 taken 10 times.
 40 if (dDifferenceOnTheLeft)
608
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 20 Jout = J * Jin;
609 else
610
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 20 Jout = Jin * J;
611 40 return;
612 ✗ case ADDTO:
613 ✗ if (dDifferenceOnTheLeft)
614 ✗ Jout += J * Jin;
615 else
616 ✗ Jout += Jin * J;
617 ✗ return;
618 ✗ case RMTO:
619 ✗ if (dDifferenceOnTheLeft)
620 ✗ Jout -= J * Jin;
621 else
622 ✗ Jout -= Jin * J;
623 ✗ return;
624 }
625 }
626
627 template<class Derived>
628 template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
629 71651 void LieGroupBase<Derived>::interpolate_impl(
630 const Eigen::MatrixBase<ConfigL_t> & q0,
631 const Eigen::MatrixBase<ConfigR_t> & q1,
632 const Scalar & u,
633 const Eigen::MatrixBase<ConfigOut_t> & qout) const
634 {
635
3/6
✓ Branch 1 taken 35829 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35829 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35829 times.
✗ Branch 8 not taken.
 71707 TangentVector_t vdiff(u * difference(q0, q1));
636
1/2
✓ Branch 3 taken 35829 times.
✗ Branch 4 not taken.
 71651 integrate(q0.derived(), vdiff, PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout));
637 71651 }
638
639 template<class Derived>
640 template<class ConfigL_t, class ConfigR_t>
641 28902 typename LieGroupBase<Derived>::Scalar LieGroupBase<Derived>::squaredDistance_impl(
642 const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) const
643 {
644 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
645 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
646
3/5
✓ Branch 1 taken 153 times.
✓ Branch 2 taken 14298 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 153 times.
✗ Branch 5 not taken.
 28902 TangentVector_t t(nv());
647
1/2
✓ Branch 3 taken 14451 times.
✗ Branch 4 not taken.
 28902 difference(q0.derived(), q1.derived(), t);
648 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
649
1/2
✓ Branch 1 taken 14451 times.
✗ Branch 2 not taken.
 57804 return t.squaredNorm();
650 }
651
652 template<class Derived>
653 template<class ConfigL_t, class ConfigR_t>
654 198 bool LieGroupBase<Derived>::isSameConfiguration_impl(
655 const Eigen::MatrixBase<ConfigL_t> & q0,
656 const Eigen::MatrixBase<ConfigR_t> & q1,
657 const Scalar & prec) const
658 {
659 198 return q0.isApprox(q1, prec);
660 }
661
662 template<class Derived>
663 299557 typename LieGroupBase<Derived>::Index LieGroupBase<Derived>::nq() const
664 {
665 299557 return derived().nq();
666 }
667
668 template<class Derived>
669 202036 typename LieGroupBase<Derived>::Index LieGroupBase<Derived>::nv() const
670 {
671 202036 return derived().nv();
672 }
673
674 template<class Derived>
675 152 typename LieGroupBase<Derived>::ConfigVector_t LieGroupBase<Derived>::neutral() const
676 {
677
0/2
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 152 return derived().neutral();
678 }
679
680 template<class Derived>
681 68 std::string LieGroupBase<Derived>::name() const
682 {
683 68 return derived().name();
684 }
685
686 } // namespace pinocchio
687
688 #endif // __pinocchio_multibody_liegroup_liegroup_operation_base_hxx__
689