GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/liegroup/cartesian-product-variant.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 195 211 92.4%
Branches: 121 225 53.8%
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 51 void CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::append(
15 const LieGroupGeneric & lg)
16 {
17
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 liegroups.push_back(lg);
18
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 const Index lg_nq = ::pinocchio::nq(lg);
19
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 lg_nqs.push_back(lg_nq);
20 51 m_nq += lg_nq;
21
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 const Index lg_nv = ::pinocchio::nv(lg);
22
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 lg_nvs.push_back(lg_nv);
23 51 m_nv += lg_nv;
24
25
2/2
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 49 times.
 51 if (liegroups.size() > 1)
26
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 m_name += " x ";
27
2/4
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
 51 m_name += ::pinocchio::name(lg);
28
29
1/2
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
 51 m_neutral.conservativeResize(m_nq);
30
3/6
✓ Branch 1 taken 51 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 51 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 51 times.
✗ Branch 8 not taken.
 51 m_neutral.tail(lg_nq) = ::pinocchio::neutral(lg);
31 51 }
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 30 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 30 Index id_q = 0, id_v = 0;
42
2/2
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 21 times.
 64 for (size_t k = 0; k < liegroups.size(); ++k)
43 {
44 34 const Index & nq = lg_nqs[k];
45 34 const Index & nv = lg_nvs[k];
46
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 34 ::pinocchio::difference(
47
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
 34 liegroups[k], q0.segment(id_q, nq), q1.segment(id_q, nq),
48 34 PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d).segment(id_v, nv));
49
50 34 id_q += nq;
51 34 id_v += nv;
52 }
53 30 }
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 60 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 60 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).setZero();
64 60 Index id_q = 0, id_v = 0;
65
2/2
✓ Branch 1 taken 34 times.
✓ Branch 2 taken 30 times.
 128 for (size_t k = 0; k < liegroups.size(); ++k)
66 {
67 68 const Index & nq = lg_nqs[k];
68 68 const Index & nv = lg_nvs[k];
69
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 68 ::pinocchio::dDifference(
70
2/4
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 34 times.
✗ Branch 5 not taken.
 68 liegroups[k], q0.segment(id_q, nq), q1.segment(id_q, nq),
71 68 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).block(id_v, id_v, nv, nv), arg);
72
73 68 id_q += nq;
74 68 id_v += nv;
75 }
76 60 }
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 48 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 48 Index id_q = 0, id_v = 0;
95
2/2
✓ Branch 1 taken 28 times.
✓ Branch 2 taken 24 times.
 104 for (size_t k = 0; k < liegroups.size(); ++k)
96 {
97 56 const Index & nq = lg_nqs[k];
98 56 const Index & nv = lg_nvs[k];
99
2/2
✓ Branch 0 taken 14 times.
✓ Branch 1 taken 14 times.
 56 if (dDifferenceOnTheLeft)
100
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 ::pinocchio::dDifference<arg>(
101
3/6
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
 28 liegroups[k], q0.segment(id_q, nq), q1.segment(id_q, nq), SELF, Jin.middleRows(id_v, nv),
102 56 Jout.middleRows(id_v, nv), op);
103 else
104
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 28 ::pinocchio::dDifference<arg>(
105
3/6
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
 28 liegroups[k], q0.segment(id_q, nq), q1.segment(id_q, nq), Jin.middleCols(id_v, nv), SELF,
106 56 Jout.middleCols(id_v, nv), op);
107
108 56 id_q += nq;
109 56 id_v += nv;
110 }
111 48 }
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 42 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 42 ConfigOut_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout);
122 42 Index id_q = 0, id_v = 0;
123
2/2
✓ Branch 1 taken 38 times.
✓ Branch 2 taken 33 times.
 90 for (size_t k = 0; k < liegroups.size(); ++k)
124 {
125 48 const Index & nq = lg_nqs[k];
126 48 const Index & nv = lg_nvs[k];
127
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
 48 ::pinocchio::integrate(
128
2/4
✓ Branch 2 taken 38 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 38 times.
✗ Branch 6 not taken.
 48 liegroups[k], q.segment(id_q, nq), v.segment(id_v, nv), qout_.segment(id_q, nq));
129
130 48 id_q += nq;
131 48 id_v += nv;
132 }
133 42 }
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 30 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
1/2
✓ Branch 0 taken 21 times.
✗ Branch 1 not taken.
 30 if (op == SETTO)
163 30 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).setZero();
164 30 Index id_q = 0, id_v = 0;
165
2/2
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 21 times.
 64 for (size_t k = 0; k < liegroups.size(); ++k)
166 {
167 34 const Index & nq = lg_nqs[k];
168 34 const Index & nv = lg_nvs[k];
169
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 34 ::pinocchio::dIntegrate(
170
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
 34 liegroups[k], q.segment(id_q, nq), v.segment(id_v, nv),
171 34 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).block(id_v, id_v, nv, nv), ARG0, op);
172
173 34 id_q += nq;
174 34 id_v += nv;
175 }
176 30 }
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 30 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
1/2
✓ Branch 0 taken 21 times.
✗ Branch 1 not taken.
 30 if (op == SETTO)
188 30 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).setZero();
189 30 Index id_q = 0, id_v = 0;
190
2/2
✓ Branch 1 taken 24 times.
✓ Branch 2 taken 21 times.
 64 for (size_t k = 0; k < liegroups.size(); ++k)
191 {
192 34 const Index & nq = lg_nqs[k];
193 34 const Index & nv = lg_nvs[k];
194
1/2
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
 34 ::pinocchio::dIntegrate(
195
2/4
✓ Branch 1 taken 24 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 24 times.
✗ Branch 5 not taken.
 34 liegroups[k], q.segment(id_q, nq), v.segment(id_v, nv),
196 34 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).block(id_v, id_v, nv, nv), ARG1, op);
197
198 34 id_q += nq;
199 34 id_v += nv;
200 }
201 30 }
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 24 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 24 Index id_q = 0, id_v = 0;
216
2/2
✓ Branch 1 taken 28 times.
✓ Branch 2 taken 24 times.
 52 for (size_t k = 0; k < liegroups.size(); ++k)
217 {
218 28 const Index & nq = lg_nqs[k];
219 28 const Index & nv = lg_nvs[k];
220
2/2
✓ Branch 0 taken 14 times.
✓ Branch 1 taken 14 times.
 28 if (dIntegrateOnTheLeft)
221
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 ::pinocchio::dIntegrate(
222
3/6
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
 14 liegroups[k], q.segment(id_q, nq), v.segment(id_v, nv), SELF, Jin.middleRows(id_v, nv),
223 28 Jout.middleRows(id_v, nv), arg, op);
224 else
225
1/2
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
 14 ::pinocchio::dIntegrate(
226
3/6
✓ Branch 1 taken 14 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14 times.
✗ Branch 8 not taken.
 14 liegroups[k], q.segment(id_q, nq), v.segment(id_v, nv), Jin.middleCols(id_v, nv), SELF,
227 28 Jout.middleCols(id_v, nv), arg, op);
228
229 28 id_q += nq;
230 28 id_v += nv;
231 }
232 24 }
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 30 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 30 Index id_q = 0, id_v = 0;
244
2/2
✓ Branch 1 taken 35 times.
✓ Branch 2 taken 30 times.
 65 for (size_t k = 0; k < liegroups.size(); ++k)
245 {
246 35 const Index & nq = lg_nqs[k];
247 35 const Index & nv = lg_nvs[k];
248
1/2
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
 35 ::pinocchio::dIntegrateTransport(
249
3/6
✓ Branch 1 taken 35 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 35 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 35 times.
✗ Branch 8 not taken.
 35 liegroups[k], q.segment(id_q, nq), v.segment(id_v, nv), J_in.middleRows(id_v, nv),
250 35 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out).middleRows(id_v, nv), ARG0);
251
252 35 id_q += nq;
253 35 id_v += nv;
254 }
255 30 }
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 6 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 6 Index id_q = 0, id_v = 0;
267
2/2
✓ Branch 1 taken 7 times.
✓ Branch 2 taken 6 times.
 13 for (size_t k = 0; k < liegroups.size(); ++k)
268 {
269 7 const Index & nq = lg_nqs[k];
270 7 const Index & nv = lg_nvs[k];
271
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 ::pinocchio::dIntegrateTransport(
272
3/6
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
 7 liegroups[k], q.segment(id_q, nq), v.segment(id_v, nv), J_in.middleRows(id_v, nv),
273 7 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out).middleRows(id_v, nv), ARG1);
274
275 7 id_q += nq;
276 7 id_v += nv;
277 }
278 6 }
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 36 CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::
328 squaredDistance_impl(
329 const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) const
330 {
331 36 Scalar d2 = Scalar(0);
332 36 Index id_q = 0;
333
2/2
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 18 times.
 76 for (size_t k = 0; k < liegroups.size(); ++k)
334 {
335 40 const Index & nq = lg_nqs[k];
336
2/11
✗ Branch 1 not taken.
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 20 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
 40 d2 += ::pinocchio::squaredDistance(liegroups[k], q0.segment(id_q, nq), q1.segment(id_q, nq));
337 40 id_q += nq;
338 }
339 36 return d2;
340 }
341
342 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
343 template<class Config_t>
344 void
345 18 CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::normalize_impl(
346 const Eigen::MatrixBase<Config_t> & qout) const
347 {
348 18 Index id_q = 0;
349
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 9 times.
 38 for (size_t k = 0; k < liegroups.size(); ++k)
350 {
351 20 const Index & nq = lg_nqs[k];
352
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 ::pinocchio::normalize(
353 20 liegroups[k], PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout).segment(id_q, nq));
354 20 id_q += nq;
355 }
356 18 }
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 48 void CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::random_impl(
380 const Eigen::MatrixBase<Config_t> & qout) const
381 {
382 48 Index id_q = 0;
383
2/2
✓ Branch 1 taken 45 times.
✓ Branch 2 taken 39 times.
 103 for (size_t k = 0; k < liegroups.size(); ++k)
384 {
385 55 const Index & nq = lg_nqs[k];
386
1/2
✓ Branch 1 taken 45 times.
✗ Branch 2 not taken.
 55 ::pinocchio::random(
387 55 liegroups[k], PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout).segment(id_q, nq));
388 55 id_q += nq;
389 }
390 48 }
391
392 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
393 template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
394 18 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 18 Index id_q = 0;
401
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 9 times.
 38 for (size_t k = 0; k < liegroups.size(); ++k)
402 {
403 20 const Index & nq = lg_nqs[k];
404
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 ::pinocchio::randomConfiguration(
405
2/4
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
 20 liegroups[k], lower.segment(id_q, nq), upper.segment(id_q, nq),
406 20 PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).segment(id_q, nq));
407
408 20 id_q += nq;
409 }
410 18 }
411
412 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
413 template<class ConfigL_t, class ConfigR_t>
414 18 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 18 Index id_q = 0;
421
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 9 times.
 38 for (size_t k = 0; k < liegroups.size(); ++k)
422 {
423 20 const Index & nq = lg_nqs[k];
424
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 20 if (!::pinocchio::isSameConfiguration(
425
2/4
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 10 times.
 40 liegroups[k], q0.segment(id_q, nq), q1.segment(id_q, nq), prec))
426 ✗ return false;
427
428 20 id_q += nq;
429 }
430 18 return true;
431 }
432
433 template<typename _Scalar, int _Options, template<typename, int> class LieGroupCollectionTpl>
434 CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>
435 5 CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::operator*(
436 const CartesianProductOperationVariantTpl & other) const
437 {
438 5 CartesianProductOperationVariantTpl res;
439
440
1/2
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
 5 res.liegroups.reserve(liegroups.size() + other.liegroups.size());
441
1/2
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 res.liegroups.insert(res.liegroups.end(), liegroups.begin(), liegroups.end());
442
1/2
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 res.liegroups.insert(res.liegroups.end(), other.liegroups.begin(), other.liegroups.end());
443
444
1/2
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
 5 res.lg_nqs.reserve(lg_nqs.size() + other.lg_nqs.size());
445
1/2
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 res.lg_nqs.insert(res.lg_nqs.end(), lg_nqs.begin(), lg_nqs.end());
446
1/2
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 res.lg_nqs.insert(res.lg_nqs.end(), other.lg_nqs.begin(), other.lg_nqs.end());
447
448
1/2
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
 5 res.lg_nvs.reserve(lg_nvs.size() + other.lg_nvs.size());
449
1/2
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 res.lg_nvs.insert(res.lg_nvs.end(), lg_nvs.begin(), lg_nvs.end());
450
1/2
✓ Branch 5 taken 5 times.
✗ Branch 6 not taken.
 5 res.lg_nvs.insert(res.lg_nvs.end(), other.lg_nvs.begin(), other.lg_nvs.end());
451
452 5 res.m_nq = m_nq + other.m_nq;
453 5 res.m_nv = m_nv + other.m_nv;
454
455
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 if (liegroups.size() > 0)
456
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 res.m_name = m_name;
457
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 if (other.liegroups.size() > 0)
458 {
459
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 if (liegroups.size() > 0)
460
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 res.m_name += " x ";
461
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 res.m_name += other.m_name;
462 }
463
464
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 res.m_neutral.resize(res.m_nq);
465
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 res.m_neutral.head(m_nq) = m_neutral;
466
2/4
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
 5 res.m_neutral.tail(other.m_nq) = other.m_neutral;
467
468 5 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 9 bool CartesianProductOperationVariantTpl<_Scalar, _Options, LieGroupCollectionTpl>::isEqual_impl(
499 const CartesianProductOperationVariantTpl & other) const
500 {
501
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 9 times.
 9 if (liegroups.size() != other.liegroups.size())
502 ✗ return false;
503
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 9 times.
 19 for (size_t k = 0; k < liegroups.size(); ++k)
504
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 10 times.
 10 if (liegroups[k].isDifferent_impl(other.liegroups[k]))
505 ✗ return false;
506 9 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