GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/liegroup/liegroup-base.hxx
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 97 207 46.9%
Branches: 29 181 16.0%
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 130370 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 260740 derived().integrate_impl(
26 130370 q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout));
27 }
28
29 template<class Derived>
30 template<class Config_t, class Jacobian_t>
31 54 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 108 derived().integrateCoeffWiseJacobian_impl(
37 54 q.derived(), PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J));
38 }
39
40 template<class Derived>
41 template<class Config_t, class Tangent_t, class JacobianOut_t>
42 484 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 121 times.
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 121 times.
 484 assert((arg == ARG0 || arg == ARG1) && "arg should be either ARG0 or ARG1");
50
51
2/3
✓ Branch 0 taken 121 times.
✓ Branch 1 taken 121 times.
✗ Branch 2 not taken.
 484 switch (arg)
52 {
53 242 case ARG0:
54 242 dIntegrate_dq(q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J), op);
55 242 return;
56 242 case ARG1:
57 242 dIntegrate_dv(q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J), op);
58 242 return;
59 ✗ default:
60 ✗ return;
61 }
62 }
63
64 template<class Derived>
65 template<class Config_t, class Tangent_t, class JacobianOut_t>
66 242 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 484 derived().dIntegrate_dq_impl(
76 242 q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J), op);
77 }
78
79 template<class Derived>
80 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
81 ✗ 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 ✗ assert(Jin.cols() == nv());
93 ✗ assert(Jout.cols() == nv());
94 ✗ assert(Jout.rows() == Jin.rows());
95 ✗ derived().dIntegrate_product_impl(
96 ✗ 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 ✗ 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 ✗ assert(Jin.rows() == nv());
114 ✗ assert(Jout.rows() == nv());
115 ✗ assert(Jout.cols() == Jin.cols());
116 ✗ derived().dIntegrate_product_impl(
117 ✗ 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 242 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 484 derived().dIntegrate_dv_impl(
133 242 q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J), op);
134 }
135
136 template<class Derived>
137 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
138 ✗ 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 ✗ assert(Jin.cols() == nv());
150 ✗ assert(Jout.cols() == nv());
151 ✗ assert(Jout.rows() == Jin.rows());
152 ✗ derived().dIntegrate_product_impl(
153 ✗ 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 ✗ 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 ✗ assert(Jin.rows() == nv());
171 ✗ assert(Jout.rows() == nv());
172 ✗ assert(Jout.cols() == Jin.cols());
173 ✗ derived().dIntegrate_product_impl(
174 ✗ 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 44 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 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");
188
189
2/3
✓ Branch 0 taken 11 times.
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 44 switch (arg)
190 {
191 22 case ARG0:
192 22 dIntegrateTransport_dq(
193 22 q.derived(), v.derived(), Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout));
194 22 return;
195 22 case ARG1:
196 22 dIntegrateTransport_dv(
197 22 q.derived(), v.derived(), Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout));
198 22 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 22 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 44 derived().dIntegrateTransport_dq_impl(
216 22 q.derived(), v.derived(), Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout));
217 }
218
219 template<class Derived>
220 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
221 22 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 44 derived().dIntegrateTransport_dv_impl(
231 22 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 44 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 44 derived().interpolate_impl(q0, q1, u, PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout));
306 }
307
308 template<class Derived>
309 template<class Config_t>
310 338 void LieGroupBase<Derived>::normalize(const Eigen::MatrixBase<Config_t> & qout) const
311 {
312 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
313 338 return derived().normalize_impl(PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout));
314 }
315
316 template<class Derived>
317 template<class Config_t>
318 158 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 158 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 ✗ void LieGroupBase<Derived>::random(const Eigen::MatrixBase<Config_t> & qout) const
336 {
337 EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t);
338 ✗ 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 160438 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 320876 derived().randomConfiguration_impl(
352 160438 lower_pos_limit.derived(), upper_pos_limit.derived(),
353 160438 PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout));
354 160438 }
355
356 template<class Derived>
357 template<class ConfigL_t, class ConfigR_t, class Tangent_t>
358 2844 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 2844 derived().difference_impl(q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d));
367 }
368
369 template<class Derived>
370 template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
371 132 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 264 derived().template dDifference_impl<arg>(
381 132 q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J));
382 }
383
384 template<class Derived>
385 template<class ConfigL_t, class ConfigR_t, class JacobianOut_t>
386 132 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 33 times.
✓ Branch 1 taken 33 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 33 times.
 132 assert((arg == ARG0 || arg == ARG1) && "arg should be either ARG0 or ARG1");
393
394
2/3
✓ Branch 0 taken 33 times.
✓ Branch 1 taken 33 times.
✗ Branch 2 not taken.
 132 switch (arg)
395 {
396 66 case ARG0:
397 66 dDifference<ARG0>(q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J));
398 66 return;
399 66 case ARG1:
400 66 dDifference<ARG1>(q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J));
401 66 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 ✗ 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 ✗ assert(Jin.cols() == nv());
426 ✗ assert(Jout.cols() == nv());
427 ✗ assert(Jout.rows() == Jin.rows());
428 ✗ derived().template dDifference_product_impl<arg>(
429 ✗ 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 ✗ 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 ✗ assert(Jin.rows() == nv());
452 ✗ assert(Jout.rows() == nv());
453 ✗ assert(Jout.cols() == Jin.cols());
454 ✗ derived().template dDifference_product_impl<arg>(
455 ✗ 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 66 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 66 return derived().squaredDistance_impl(q0.derived(), q1.derived());
467 }
468
469 template<class Derived>
470 template<class ConfigL_t, class ConfigR_t>
471 ✗ typename LieGroupBase<Derived>::Scalar LieGroupBase<Derived>::distance(
472 const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) const
473 {
474 ✗ return sqrt(squaredDistance(q0.derived(), q1.derived()));
475 }
476
477 template<class Derived>
478 template<class ConfigL_t, class ConfigR_t>
479 102 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 102 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 ✗ typename LieGroupBase<Derived>::ConfigVector_t LieGroupBase<Derived>::integrate(
494 const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Tangent_t> & v) const
495 {
496 ✗ ConfigVector_t qout(nq());
497 ✗ integrate(q.derived(), v.derived(), qout);
498 ✗ return qout;
499 }
500
501 template<class Derived>
502 template<class ConfigL_t, class ConfigR_t>
503 ✗ 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 ✗ ConfigVector_t qout(nq());
509 ✗ interpolate(q0.derived(), q1.derived(), u, qout);
510 ✗ return qout;
511 }
512
513 template<class Derived>
514 ✗ typename LieGroupBase<Derived>::ConfigVector_t LieGroupBase<Derived>::random() const
515 {
516 ✗ ConfigVector_t qout(nq());
517 ✗ random(qout);
518 ✗ return qout;
519 }
520
521 template<class Derived>
522 template<class ConfigL_t, class ConfigR_t>
523 ✗ 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 ✗ ConfigVector_t qout(nq());
528 ✗ randomConfiguration(lower_pos_limit.derived(), upper_pos_limit.derived(), qout);
529 ✗ return qout;
530 }
531
532 template<class Derived>
533 template<class ConfigL_t, class ConfigR_t>
534 40 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 20 times.
✗ Branch 3 not taken.
 40 TangentVector_t diff(nv());
538
0/2
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 40 difference(q0.derived(), q1.derived(), diff);
539 40 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 ✗ 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 ✗ Index nv_(nv());
555 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
556 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
557 ✗ JacobianMatrix_t J(nv_, nv_);
558 ✗ dIntegrate(q, v, J, arg);
559 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
560 ✗ switch (op)
561 {
562 ✗ case SETTO:
563 ✗ if (dIntegrateOnTheLeft)
564 ✗ Jout = J * Jin;
565 else
566 ✗ Jout = Jin * J;
567 ✗ 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 ✗ 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 ✗ Index nv_(nv());
599 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
600 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
601 ✗ JacobianMatrix_t J(nv_, nv_);
602 ✗ dDifference<arg>(q0, q1, J);
603 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
604 ✗ switch (op)
605 {
606 ✗ case SETTO:
607 ✗ if (dDifferenceOnTheLeft)
608 ✗ Jout = J * Jin;
609 else
610 ✗ Jout = Jin * J;
611 ✗ 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 40 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 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 40 TangentVector_t vdiff(u * difference(q0, q1));
636
1/2
✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
 40 integrate(q0.derived(), vdiff, PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout));
637 }
638
639 template<class Derived>
640 template<class ConfigL_t, class ConfigR_t>
641 54 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
2/5
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 27 times.
✗ Branch 5 not taken.
 54 TangentVector_t t(nv());
647
1/2
✓ Branch 3 taken 27 times.
✗ Branch 4 not taken.
 54 difference(q0.derived(), q1.derived(), t);
648 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
649
1/2
✓ Branch 1 taken 27 times.
✗ Branch 2 not taken.
 108 return t.squaredNorm();
650 }
651
652 template<class Derived>
653 template<class ConfigL_t, class ConfigR_t>
654 66 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 66 return q0.isApprox(q1, prec);
660 }
661
662 template<class Derived>
663 ✗ typename LieGroupBase<Derived>::Index LieGroupBase<Derived>::nq() const
664 {
665 ✗ return derived().nq();
666 }
667
668 template<class Derived>
669 47 typename LieGroupBase<Derived>::Index LieGroupBase<Derived>::nv() const
670 {
671 47 return derived().nv();
672 }
673
674 template<class Derived>
675 ✗ typename LieGroupBase<Derived>::ConfigVector_t LieGroupBase<Derived>::neutral() const
676 {
677 ✗ return derived().neutral();
678 }
679
680 template<class Derived>
681 ✗ std::string LieGroupBase<Derived>::name() const
682 {
683 ✗ return derived().name();
684 }
685
686 } // namespace pinocchio
687
688 #endif // __pinocchio_multibody_liegroup_liegroup_operation_base_hxx__
689