GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/liegroup/cartesian-product-variant.hxx
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 0 198 0.0%
Branches: 0 218 0.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2020 CNRS
3 //
4
5 #ifndef __pinocchio_cartesian_product_variant_hxx__
6 #define __pinocchio_cartesian_product_variant_hxx__
7
8 #include "pinocchio/multibody/liegroup/liegroup-variant-visitors.hpp"
9
10 namespace pinocchio
11 {
12
13 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
14 ✗ void CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::append(
15 const LieGroupGeneric & lg)
16 {
17 ✗ liegroups.push_back(lg);
18 ✗ const Index lg_nq = ::pinocchio::nq(lg);
19 ✗ lg_nqs.push_back(lg_nq);
20 ✗ m_nq += lg_nq;
21 ✗ const Index lg_nv = ::pinocchio::nv(lg);
22 ✗ lg_nvs.push_back(lg_nv);
23 ✗ m_nv += lg_nv;
24
25 ✗ if (liegroups.size() > 1)
26 ✗ m_name += " x ";
27 ✗ m_name += ::pinocchio::name(lg);
28
29 ✗ m_neutral.conservativeResize(m_nq);
30 ✗ m_neutral.tail(lg_nq) = ::pinocchio::neutral(lg);
31 }
32
33 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
34 template<class ConfigL_t, class ConfigR_t, class Tangent_t>
35 void
36 ✗ CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::difference_impl(
37 const Eigen::MatrixBase<ConfigL_t> & q0,
38 const Eigen::MatrixBase<ConfigR_t> & q1,
39 const Eigen::MatrixBase<Tangent_t> & d) const
40 {
41 ✗ Index id_q = 0, id_v = 0;
42 ✗ for (size_t k = 0; k < liegroups.size(); ++k)
43 {
44 ✗ const Index & nq = lg_nqs[k];
45 ✗ const Index & nv = lg_nvs[k];
46 ✗ ::pinocchio::difference(
47 ✗ liegroups[k], q0.segment(id_q, nq), q1.segment(id_q, nq),
48 ✗ PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d).segment(id_v, nv));
49
50 ✗ id_q += nq;
51 ✗ id_v += nv;
52 }
53 }
54
55 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
56 template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
57 void
58 ✗ CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::dDifference_impl(
59 const Eigen::MatrixBase<ConfigL_t> & q0,
60 const Eigen::MatrixBase<ConfigR_t> & q1,
61 const Eigen::MatrixBase<JacobianOut_t> & J) const
62 {
63 ✗ PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).setZero();
64 ✗ Index id_q = 0, id_v = 0;
65 ✗ for (size_t k = 0; k < liegroups.size(); ++k)
66 {
67 ✗ const Index & nq = lg_nqs[k];
68 ✗ const Index & nv = lg_nvs[k];
69 ✗ ::pinocchio::dDifference(
70 ✗ liegroups[k], q0.segment(id_q, nq), q1.segment(id_q, nq),
71 ✗ PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).block(id_v, id_v, nv, nv), arg);
72
73 ✗ id_q += nq;
74 ✗ id_v += nv;
75 }
76 }
77
78 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
79 template<
80 ArgumentPosition arg,
81 class ConfigL_t,
82 class ConfigR_t,
83 class JacobianIn_t,
84 class JacobianOut_t>
85 ✗ void CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::
86 dDifference_product_impl(
87 const ConfigL_t & q0,
88 const ConfigR_t & q1,
89 const JacobianIn_t & Jin,
90 JacobianOut_t & Jout,
91 bool dDifferenceOnTheLeft,
92 const AssignmentOperatorType op) const
93 {
94 ✗ Index id_q = 0, id_v = 0;
95 ✗ for (size_t k = 0; k < liegroups.size(); ++k)
96 {
97 ✗ const Index & nq = lg_nqs[k];
98 ✗ const Index & nv = lg_nvs[k];
99 ✗ if (dDifferenceOnTheLeft)
100 ✗ ::pinocchio::dDifference<arg>(
101 ✗ liegroups[k], q0.segment(id_q, nq), q1.segment(id_q, nq), SELF, Jin.middleRows(id_v, nv),
102 ✗ Jout.middleRows(id_v, nv), op);
103 else
104 ✗ ::pinocchio::dDifference<arg>(
105 ✗ liegroups[k], q0.segment(id_q, nq), q1.segment(id_q, nq), Jin.middleCols(id_v, nv), SELF,
106 ✗ Jout.middleCols(id_v, nv), op);
107
108 ✗ id_q += nq;
109 ✗ id_v += nv;
110 }
111 }
112
113 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
114 template<class ConfigIn_t, class Velocity_t, class ConfigOut_t>
115 void
116 ✗ CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::integrate_impl(
117 const Eigen::MatrixBase<ConfigIn_t> & q,
118 const Eigen::MatrixBase<Velocity_t> & v,
119 const Eigen::MatrixBase<ConfigOut_t> & qout) const
120 {
121 ✗ ConfigOut_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout);
122 ✗ Index id_q = 0, id_v = 0;
123 ✗ for (size_t k = 0; k < liegroups.size(); ++k)
124 {
125 ✗ const Index & nq = lg_nqs[k];
126 ✗ const Index & nv = lg_nvs[k];
127 ✗ ::pinocchio::integrate(
128 ✗ liegroups[k], q.segment(id_q, nq), v.segment(id_v, nv), qout_.segment(id_q, nq));
129
130 ✗ id_q += nq;
131 ✗ id_v += nv;
132 }
133 }
134
135 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
136 template<class Config_t, class Jacobian_t>
137 void CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::
138 integrateCoeffWiseJacobian_impl(
139 const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J) const
140 {
141 PINOCCHIO_UNUSED_VARIABLE(q);
142 PINOCCHIO_UNUSED_VARIABLE(J);
143 // not implemented yet assert(J.rows() == nq() && J.cols() == nv() && "J is not of the
144 // right dimension"); not implemented yet Jacobian_t & J_ =
145 // PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J); not implemented yet
146 // J_.topRightCorner(lg1.nq(),lg2.nv()).setZero(); not implemented yet
147 // J_.bottomLeftCorner(lg2.nq(),lg1.nv()).setZero(); not implemented yet not implemented yet
148 // lg1.integrateCoeffWiseJacobian(Q1(q), not implemented yet
149 // J_.topLeftCorner(lg1.nq(),lg1.nv())); not implemented yet
150 // lg2.integrateCoeffWiseJacobian(Q2(q), J_.bottomRightCorner(lg2.nq(),lg2.nv()));
151 }
152
153 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
154 template<class Config_t, class Tangent_t, class JacobianOut_t>
155 void
156 ✗ CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::dIntegrate_dq_impl(
157 const Eigen::MatrixBase<Config_t> & q,
158 const Eigen::MatrixBase<Tangent_t> & v,
159 const Eigen::MatrixBase<JacobianOut_t> & J,
160 const AssignmentOperatorType op) const
161 {
162 ✗ if (op == SETTO)
163 ✗ PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).setZero();
164 ✗ Index id_q = 0, id_v = 0;
165 ✗ for (size_t k = 0; k < liegroups.size(); ++k)
166 {
167 ✗ const Index & nq = lg_nqs[k];
168 ✗ const Index & nv = lg_nvs[k];
169 ✗ ::pinocchio::dIntegrate(
170 ✗ liegroups[k], q.segment(id_q, nq), v.segment(id_v, nv),
171 ✗ PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).block(id_v, id_v, nv, nv), ARG0, op);
172
173 ✗ id_q += nq;
174 ✗ id_v += nv;
175 }
176 }
177
178 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
179 template<class Config_t, class Tangent_t, class JacobianOut_t>
180 void
181 ✗ CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::dIntegrate_dv_impl(
182 const Eigen::MatrixBase<Config_t> & q,
183 const Eigen::MatrixBase<Tangent_t> & v,
184 const Eigen::MatrixBase<JacobianOut_t> & J,
185 const AssignmentOperatorType op) const
186 {
187 ✗ if (op == SETTO)
188 ✗ PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).setZero();
189 ✗ Index id_q = 0, id_v = 0;
190 ✗ for (size_t k = 0; k < liegroups.size(); ++k)
191 {
192 ✗ const Index & nq = lg_nqs[k];
193 ✗ const Index & nv = lg_nvs[k];
194 ✗ ::pinocchio::dIntegrate(
195 ✗ liegroups[k], q.segment(id_q, nq), v.segment(id_v, nv),
196 ✗ PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).block(id_v, id_v, nv, nv), ARG1, op);
197
198 ✗ id_q += nq;
199 ✗ id_v += nv;
200 }
201 }
202
203 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
204 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
205 ✗ void CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::
206 dIntegrate_product_impl(
207 const Config_t & q,
208 const Tangent_t & v,
209 const JacobianIn_t & Jin,
210 JacobianOut_t & Jout,
211 bool dIntegrateOnTheLeft,
212 const ArgumentPosition arg,
213 const AssignmentOperatorType op) const
214 {
215 ✗ Index id_q = 0, id_v = 0;
216 ✗ for (size_t k = 0; k < liegroups.size(); ++k)
217 {
218 ✗ const Index & nq = lg_nqs[k];
219 ✗ const Index & nv = lg_nvs[k];
220 ✗ if (dIntegrateOnTheLeft)
221 ✗ ::pinocchio::dIntegrate(
222 ✗ liegroups[k], q.segment(id_q, nq), v.segment(id_v, nv), SELF, Jin.middleRows(id_v, nv),
223 ✗ Jout.middleRows(id_v, nv), arg, op);
224 else
225 ✗ ::pinocchio::dIntegrate(
226 ✗ liegroups[k], q.segment(id_q, nq), v.segment(id_v, nv), Jin.middleCols(id_v, nv), SELF,
227 ✗ Jout.middleCols(id_v, nv), arg, op);
228
229 ✗ id_q += nq;
230 ✗ id_v += nv;
231 }
232 }
233
234 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
235 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
236 ✗ void CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::
237 dIntegrateTransport_dq_impl(
238 const Eigen::MatrixBase<Config_t> & q,
239 const Eigen::MatrixBase<Tangent_t> & v,
240 const Eigen::MatrixBase<JacobianIn_t> & J_in,
241 const Eigen::MatrixBase<JacobianOut_t> & J_out) const
242 {
243 ✗ Index id_q = 0, id_v = 0;
244 ✗ for (size_t k = 0; k < liegroups.size(); ++k)
245 {
246 ✗ const Index & nq = lg_nqs[k];
247 ✗ const Index & nv = lg_nvs[k];
248 ✗ ::pinocchio::dIntegrateTransport(
249 ✗ liegroups[k], q.segment(id_q, nq), v.segment(id_v, nv), J_in.middleRows(id_v, nv),
250 ✗ PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out).middleRows(id_v, nv), ARG0);
251
252 ✗ id_q += nq;
253 ✗ id_v += nv;
254 }
255 }
256
257 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
258 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
259 ✗ void CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::
260 dIntegrateTransport_dv_impl(
261 const Eigen::MatrixBase<Config_t> & q,
262 const Eigen::MatrixBase<Tangent_t> & v,
263 const Eigen::MatrixBase<JacobianIn_t> & J_in,
264 const Eigen::MatrixBase<JacobianOut_t> & J_out) const
265 {
266 ✗ Index id_q = 0, id_v = 0;
267 ✗ for (size_t k = 0; k < liegroups.size(); ++k)
268 {
269 ✗ const Index & nq = lg_nqs[k];
270 ✗ const Index & nv = lg_nvs[k];
271 ✗ ::pinocchio::dIntegrateTransport(
272 ✗ liegroups[k], q.segment(id_q, nq), v.segment(id_v, nv), J_in.middleRows(id_v, nv),
273 ✗ PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out).middleRows(id_v, nv), ARG1);
274
275 ✗ id_q += nq;
276 ✗ id_v += nv;
277 }
278 }
279
280 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
281 template<class Config_t, class Tangent_t, class JacobianOut_t>
282 void CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::
283 dIntegrateTransport_dq_impl(
284 const Eigen::MatrixBase<Config_t> & q,
285 const Eigen::MatrixBase<Tangent_t> & v,
286 const Eigen::MatrixBase<JacobianOut_t> & J) const
287 {
288 Index id_q = 0, id_v = 0;
289 for (size_t k = 0; k < liegroups.size(); ++k)
290 {
291 const Index & nq = lg_nqs[k];
292 const Index & nv = lg_nvs[k];
293 ::pinocchio::dIntegrateTransport(
294 liegroups[k], q.segment(id_q, nq), v.segment(id_v, nv),
295 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).middleRows(id_v, nv), ARG0);
296
297 id_q += nq;
298 id_v += nv;
299 }
300 }
301
302 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
303 template<class Config_t, class Tangent_t, class JacobianOut_t>
304 void CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::
305 dIntegrateTransport_dv_impl(
306 const Eigen::MatrixBase<Config_t> & q,
307 const Eigen::MatrixBase<Tangent_t> & v,
308 const Eigen::MatrixBase<JacobianOut_t> & J) const
309 {
310 Index id_q = 0, id_v = 0;
311 for (size_t k = 0; k < liegroups.size(); ++k)
312 {
313 const Index & nq = lg_nqs[k];
314 const Index & nv = lg_nvs[k];
315 ::pinocchio::dIntegrateTransport(
316 liegroups[k], q.segment(id_q, nq), v.segment(id_v, nv),
317 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).middleRows(id_v, nv), ARG1);
318
319 id_q += nq;
320 id_v += nv;
321 }
322 }
323
324 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
325 template<class ConfigL_t, class ConfigR_t>
326 typename CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::Scalar
327 ✗ CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::
328 squaredDistance_impl(
329 const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) const
330 {
331 ✗ Scalar d2 = Scalar(0);
332 ✗ Index id_q = 0;
333 ✗ for (size_t k = 0; k < liegroups.size(); ++k)
334 {
335 ✗ const Index & nq = lg_nqs[k];
336 ✗ d2 += ::pinocchio::squaredDistance(liegroups[k], q0.segment(id_q, nq), q1.segment(id_q, nq));
337 ✗ id_q += nq;
338 }
339 ✗ return d2;
340 }
341
342 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
343 template<class Config_t>
344 void
345 ✗ CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::normalize_impl(
346 const Eigen::MatrixBase<Config_t> & qout) const
347 {
348 ✗ Index id_q = 0;
349 ✗ for (size_t k = 0; k < liegroups.size(); ++k)
350 {
351 ✗ const Index & nq = lg_nqs[k];
352 ✗ ::pinocchio::normalize(
353 ✗ liegroups[k], PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout).segment(id_q, nq));
354 ✗ id_q += nq;
355 }
356 }
357
358 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
359 template<class Config_t>
360 bool
361 CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::isNormalized_impl(
362 const Eigen::MatrixBase<Config_t> & qin, const Scalar & prec) const
363 {
364 Index id_q = 0;
365 for (size_t k = 0; k < liegroups.size(); ++k)
366 {
367 const Index nq = lg_nqs[k];
368 const bool res_k = ::pinocchio::isNormalized(
369 liegroups[k], PINOCCHIO_EIGEN_CONST_CAST(Config_t, qin).segment(id_q, nq), prec);
370 if (!res_k)
371 return false;
372 id_q += nq;
373 }
374 return true;
375 }
376
377 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
378 template<class Config_t>
379 ✗ void CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::random_impl(
380 const Eigen::MatrixBase<Config_t> & qout) const
381 {
382 ✗ Index id_q = 0;
383 ✗ for (size_t k = 0; k < liegroups.size(); ++k)
384 {
385 ✗ const Index & nq = lg_nqs[k];
386 ✗ ::pinocchio::random(
387 ✗ liegroups[k], PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout).segment(id_q, nq));
388 ✗ id_q += nq;
389 }
390 }
391
392 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
393 template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
394 ✗ void CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::
395 randomConfiguration_impl(
396 const Eigen::MatrixBase<ConfigL_t> & lower,
397 const Eigen::MatrixBase<ConfigR_t> & upper,
398 const Eigen::MatrixBase<ConfigOut_t> & qout) const
399 {
400 ✗ Index id_q = 0;
401 ✗ for (size_t k = 0; k < liegroups.size(); ++k)
402 {
403 ✗ const Index & nq = lg_nqs[k];
404 ✗ ::pinocchio::randomConfiguration(
405 ✗ liegroups[k], lower.segment(id_q, nq), upper.segment(id_q, nq),
406 ✗ PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).segment(id_q, nq));
407
408 ✗ id_q += nq;
409 }
410 }
411
412 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
413 template<class ConfigL_t, class ConfigR_t>
414 ✗ bool CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::
415 isSameConfiguration_impl(
416 const Eigen::MatrixBase<ConfigL_t> & q0,
417 const Eigen::MatrixBase<ConfigR_t> & q1,
418 const Scalar & prec) const
419 {
420 ✗ Index id_q = 0;
421 ✗ for (size_t k = 0; k < liegroups.size(); ++k)
422 {
423 ✗ const Index & nq = lg_nqs[k];
424 ✗ if (!::pinocchio::isSameConfiguration(
425 ✗ liegroups[k], q0.segment(id_q, nq), q1.segment(id_q, nq), prec))
426 ✗ return false;
427
428 ✗ id_q += nq;
429 }
430 ✗ return true;
431 }
432
433 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
434 CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>
435 ✗ CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::operator*(
436 const CartesianProductOperationVariantTpl & other) const
437 {
438 ✗ CartesianProductOperationVariantTpl res;
439
440 ✗ res.liegroups.reserve(liegroups.size() + other.liegroups.size());
441 ✗ res.liegroups.insert(res.liegroups.end(), liegroups.begin(), liegroups.end());
442 ✗ res.liegroups.insert(res.liegroups.end(), other.liegroups.begin(), other.liegroups.end());
443
444 ✗ res.lg_nqs.reserve(lg_nqs.size() + other.lg_nqs.size());
445 ✗ res.lg_nqs.insert(res.lg_nqs.end(), lg_nqs.begin(), lg_nqs.end());
446 ✗ res.lg_nqs.insert(res.lg_nqs.end(), other.lg_nqs.begin(), other.lg_nqs.end());
447
448 ✗ res.lg_nvs.reserve(lg_nvs.size() + other.lg_nvs.size());
449 ✗ res.lg_nvs.insert(res.lg_nvs.end(), lg_nvs.begin(), lg_nvs.end());
450 ✗ res.lg_nvs.insert(res.lg_nvs.end(), other.lg_nvs.begin(), other.lg_nvs.end());
451
452 ✗ res.m_nq = m_nq + other.m_nq;
453 ✗ res.m_nv = m_nv + other.m_nv;
454
455 ✗ if (liegroups.size() > 0)
456 ✗ res.m_name = m_name;
457 ✗ if (other.liegroups.size() > 0)
458 {
459 ✗ if (liegroups.size() > 0)
460 ✗ res.m_name += " x ";
461 ✗ res.m_name += other.m_name;
462 }
463
464 ✗ res.m_neutral.resize(res.m_nq);
465 ✗ res.m_neutral.head(m_nq) = m_neutral;
466 ✗ res.m_neutral.tail(other.m_nq) = other.m_neutral;
467
468 ✗ return res;
469 }
470
471 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
472 CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl> &
473 ✗ CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::operator*=(
474 const CartesianProductOperationVariantTpl & other)
475 {
476 ✗ liegroups.insert(liegroups.end(), other.liegroups.begin(), other.liegroups.end());
477
478 ✗ lg_nqs.insert(lg_nqs.end(), other.lg_nqs.begin(), other.lg_nqs.end());
479 ✗ lg_nvs.insert(lg_nvs.end(), other.lg_nvs.begin(), other.lg_nvs.end());
480
481 ✗ m_nq += other.m_nq;
482 ✗ m_nv += other.m_nv;
483
484 ✗ if (other.liegroups.size() > 0)
485 {
486 ✗ if (liegroups.size())
487 ✗ m_name += " x ";
488 ✗ m_name += other.m_name;
489 }
490
491 ✗ m_neutral.conservativeResize(m_nq);
492 ✗ m_neutral.tail(other.m_nq) = other.m_neutral;
493
494 ✗ return *this;
495 }
496
497 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
498 ✗ bool CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::isEqual_impl(
499 const CartesianProductOperationVariantTpl & other) const
500 {
501 ✗ if (liegroups.size() != other.liegroups.size())
502 ✗ return false;
503 ✗ for (size_t k = 0; k < liegroups.size(); ++k)
504 ✗ if (liegroups[k].isDifferent_impl(other.liegroups[k]))
505 ✗ return false;
506 ✗ return true;
507 }
508
509 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
510 template<typename LieGroup1, typename LieGroup2>
511 bool CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::isEqual(
512 const CartesianProductOperation<LieGroup1, LieGroup2> & other) const
513 {
514 if (liegroups.size() != 2)
515 return false;
516 if (liegroups[0].isDifferent_impl(LieGroupGeneric(other.lg1)))
517 return false;
518 if (liegroups[1].isDifferent_impl(LieGroupGeneric(other.lg2)))
519 return false;
520 return true;
521 }
522
523 } // namespace pinocchio
524
525 #endif // ifndef __pinocchio_cartesian_product_variant_hxx__
526