Directory: | ./ |
---|---|
File: | include/pinocchio/multibody/liegroup/cartesian-product.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 20 | 122 | 16.4% |
Branches: | 14 | 194 | 7.2% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2016-2020 CNRS CNRS | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_multibody_liegroup_cartesian_product_operation_hpp__ | ||
6 | #define __pinocchio_multibody_liegroup_cartesian_product_operation_hpp__ | ||
7 | |||
8 | #include <pinocchio/multibody/liegroup/liegroup-base.hpp> | ||
9 | |||
10 | namespace pinocchio | ||
11 | { | ||
12 | template<int dim1, int dim2> | ||
13 | struct eval_set_dim | ||
14 | { | ||
15 | enum | ||
16 | { | ||
17 | value = dim1 + dim2 | ||
18 | }; | ||
19 | }; | ||
20 | |||
21 | template<int dim> | ||
22 | struct eval_set_dim<dim, Eigen::Dynamic> | ||
23 | { | ||
24 | enum | ||
25 | { | ||
26 | value = Eigen::Dynamic | ||
27 | }; | ||
28 | }; | ||
29 | |||
30 | template<int dim> | ||
31 | struct eval_set_dim<Eigen::Dynamic, dim> | ||
32 | { | ||
33 | enum | ||
34 | { | ||
35 | value = Eigen::Dynamic | ||
36 | }; | ||
37 | }; | ||
38 | |||
39 | template<typename LieGroup1, typename LieGroup2> | ||
40 | struct CartesianProductOperation; | ||
41 | |||
42 | template<typename LieGroup1, typename LieGroup2> | ||
43 | struct traits<CartesianProductOperation<LieGroup1, LieGroup2>> | ||
44 | { | ||
45 | typedef typename traits<LieGroup1>::Scalar Scalar; | ||
46 | enum | ||
47 | { | ||
48 | Options = traits<LieGroup1>::Options, | ||
49 | NQ = eval_set_dim < LieGroup1::NQ, | ||
50 | LieGroup2::NQ > ::value, | ||
51 | NV = eval_set_dim < LieGroup1::NV, | ||
52 | LieGroup2::NV > ::value | ||
53 | }; | ||
54 | }; | ||
55 | |||
56 | template<typename LieGroup1, typename LieGroup2> | ||
57 | struct CartesianProductOperation | ||
58 | : public LieGroupBase<CartesianProductOperation<LieGroup1, LieGroup2>> | ||
59 | { | ||
60 | PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(CartesianProductOperation); | ||
61 | |||
62 | 4652 | CartesianProductOperation() | |
63 | 4652 | : lg1() | |
64 | 9304 | , lg2() | |
65 | { | ||
66 | } | ||
67 | // Get dimension of Lie Group vector representation | ||
68 | // | ||
69 | // For instance, for SO(3), the dimension of the vector representation is | ||
70 | // 4 (quaternion) while the dimension of the tangent space is 3. | ||
71 | ✗ | Index nq() const | |
72 | { | ||
73 | ✗ | return lg1.nq() + lg2.nq(); | |
74 | } | ||
75 | |||
76 | // Get dimension of Lie Group tangent space | ||
77 | ✗ | Index nv() const | |
78 | { | ||
79 | ✗ | return lg1.nv() + lg2.nv(); | |
80 | } | ||
81 | |||
82 | ✗ | ConfigVector_t neutral() const | |
83 | { | ||
84 | ✗ | ConfigVector_t n; | |
85 | ✗ | n.resize(nq()); | |
86 | ✗ | Qo1(n) = lg1.neutral(); | |
87 | ✗ | Qo2(n) = lg2.neutral(); | |
88 | ✗ | return n; | |
89 | } | ||
90 | |||
91 | ✗ | std::string name() const | |
92 | { | ||
93 | ✗ | std::ostringstream oss; | |
94 | ✗ | oss << lg1.name() << "*" << lg2.name(); | |
95 | ✗ | return oss.str(); | |
96 | } | ||
97 | |||
98 | template<class ConfigL_t, class ConfigR_t, class Tangent_t> | ||
99 | ✗ | void difference_impl( | |
100 | const Eigen::MatrixBase<ConfigL_t> & q0, | ||
101 | const Eigen::MatrixBase<ConfigR_t> & q1, | ||
102 | const Eigen::MatrixBase<Tangent_t> & d) const | ||
103 | { | ||
104 | ✗ | lg1.difference(Q1(q0), Q1(q1), Vo1(d)); | |
105 | ✗ | lg2.difference(Q2(q0), Q2(q1), Vo2(d)); | |
106 | } | ||
107 | |||
108 | template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t> | ||
109 | ✗ | void dDifference_impl( | |
110 | const Eigen::MatrixBase<ConfigL_t> & q0, | ||
111 | const Eigen::MatrixBase<ConfigR_t> & q1, | ||
112 | const Eigen::MatrixBase<JacobianOut_t> & J) const | ||
113 | { | ||
114 | ✗ | J12(J).setZero(); | |
115 | ✗ | J21(J).setZero(); | |
116 | |||
117 | ✗ | lg1.template dDifference<arg>(Q1(q0), Q1(q1), J11(J)); | |
118 | ✗ | lg2.template dDifference<arg>(Q2(q0), Q2(q1), J22(J)); | |
119 | } | ||
120 | |||
121 | template<class ConfigIn_t, class Velocity_t, class ConfigOut_t> | ||
122 | ✗ | void integrate_impl( | |
123 | const Eigen::MatrixBase<ConfigIn_t> & q, | ||
124 | const Eigen::MatrixBase<Velocity_t> & v, | ||
125 | const Eigen::MatrixBase<ConfigOut_t> & qout) const | ||
126 | { | ||
127 | ✗ | lg1.integrate(Q1(q), V1(v), Qo1(qout)); | |
128 | ✗ | lg2.integrate(Q2(q), V2(v), Qo2(qout)); | |
129 | } | ||
130 | |||
131 | template<class Config_t, class Jacobian_t> | ||
132 | ✗ | void integrateCoeffWiseJacobian_impl( | |
133 | const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J) const | ||
134 | { | ||
135 | ✗ | assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension"); | |
136 | ✗ | Jacobian_t & J_ = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J); | |
137 | ✗ | J_.topRightCorner(lg1.nq(), lg2.nv()).setZero(); | |
138 | ✗ | J_.bottomLeftCorner(lg2.nq(), lg1.nv()).setZero(); | |
139 | |||
140 | ✗ | lg1.integrateCoeffWiseJacobian(Q1(q), J_.topLeftCorner(lg1.nq(), lg1.nv())); | |
141 | ✗ | lg2.integrateCoeffWiseJacobian(Q2(q), J_.bottomRightCorner(lg2.nq(), lg2.nv())); | |
142 | } | ||
143 | |||
144 | template<class Config_t, class Tangent_t, class JacobianOut_t> | ||
145 | ✗ | void dIntegrate_dq_impl( | |
146 | const Eigen::MatrixBase<Config_t> & q, | ||
147 | const Eigen::MatrixBase<Tangent_t> & v, | ||
148 | const Eigen::MatrixBase<JacobianOut_t> & J, | ||
149 | const AssignmentOperatorType op = SETTO) const | ||
150 | { | ||
151 | ✗ | switch (op) | |
152 | { | ||
153 | ✗ | case SETTO: | |
154 | ✗ | J12(J).setZero(); | |
155 | ✗ | J21(J).setZero(); | |
156 | // fallthrough | ||
157 | ✗ | case ADDTO: | |
158 | case RMTO: | ||
159 | ✗ | lg1.dIntegrate_dq(Q1(q), V1(v), J11(J), op); | |
160 | ✗ | lg2.dIntegrate_dq(Q2(q), V2(v), J22(J), op); | |
161 | ✗ | break; | |
162 | ✗ | default: | |
163 | ✗ | assert(false && "Wrong Op requesed value"); | |
164 | break; | ||
165 | } | ||
166 | } | ||
167 | |||
168 | template<class Config_t, class Tangent_t, class JacobianOut_t> | ||
169 | ✗ | void dIntegrate_dv_impl( | |
170 | const Eigen::MatrixBase<Config_t> & q, | ||
171 | const Eigen::MatrixBase<Tangent_t> & v, | ||
172 | const Eigen::MatrixBase<JacobianOut_t> & J, | ||
173 | const AssignmentOperatorType op = SETTO) const | ||
174 | { | ||
175 | ✗ | switch (op) | |
176 | { | ||
177 | ✗ | case SETTO: | |
178 | ✗ | J12(J).setZero(); | |
179 | ✗ | J21(J).setZero(); | |
180 | // fallthrough | ||
181 | ✗ | case ADDTO: | |
182 | case RMTO: | ||
183 | ✗ | lg1.dIntegrate_dv(Q1(q), V1(v), J11(J), op); | |
184 | ✗ | lg2.dIntegrate_dv(Q2(q), V2(v), J22(J), op); | |
185 | ✗ | break; | |
186 | ✗ | default: | |
187 | ✗ | assert(false && "Wrong Op requesed value"); | |
188 | break; | ||
189 | } | ||
190 | } | ||
191 | |||
192 | template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> | ||
193 | ✗ | void dIntegrateTransport_dq_impl( | |
194 | const Eigen::MatrixBase<Config_t> & q, | ||
195 | const Eigen::MatrixBase<Tangent_t> & v, | ||
196 | const Eigen::MatrixBase<JacobianIn_t> & J_in, | ||
197 | const Eigen::MatrixBase<JacobianOut_t> & J_out) const | ||
198 | { | ||
199 | ✗ | JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out); | |
200 | ✗ | JacobianOut_t & Jin = PINOCCHIO_EIGEN_CONST_CAST(JacobianIn_t, J_in); | |
201 | ✗ | lg1.dIntegrateTransport_dq( | |
202 | ✗ | Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(), | |
203 | ✗ | Jout.template topRows<LieGroup1::NV>()); | |
204 | ✗ | lg2.dIntegrateTransport_dq( | |
205 | ✗ | Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(), | |
206 | ✗ | Jout.template bottomRows<LieGroup2::NV>()); | |
207 | } | ||
208 | |||
209 | template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> | ||
210 | ✗ | void dIntegrateTransport_dv_impl( | |
211 | const Eigen::MatrixBase<Config_t> & q, | ||
212 | const Eigen::MatrixBase<Tangent_t> & v, | ||
213 | const Eigen::MatrixBase<JacobianIn_t> & J_in, | ||
214 | const Eigen::MatrixBase<JacobianOut_t> & J_out) const | ||
215 | { | ||
216 | ✗ | JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out); | |
217 | ✗ | JacobianOut_t & Jin = PINOCCHIO_EIGEN_CONST_CAST(JacobianIn_t, J_in); | |
218 | ✗ | lg1.dIntegrateTransport_dv( | |
219 | ✗ | Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(), | |
220 | ✗ | Jout.template topRows<LieGroup1::NV>()); | |
221 | ✗ | lg2.dIntegrateTransport_dv( | |
222 | ✗ | Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(), | |
223 | ✗ | Jout.template bottomRows<LieGroup2::NV>()); | |
224 | } | ||
225 | |||
226 | template<class Config_t, class Tangent_t, class Jacobian_t> | ||
227 | void dIntegrateTransport_dq_impl( | ||
228 | const Eigen::MatrixBase<Config_t> & q, | ||
229 | const Eigen::MatrixBase<Tangent_t> & v, | ||
230 | const Eigen::MatrixBase<Jacobian_t> & Jin) const | ||
231 | { | ||
232 | Jacobian_t & J = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, Jin); | ||
233 | lg1.dIntegrateTransport_dq(Q1(q), V1(v), J.template topRows<LieGroup1::NV>()); | ||
234 | lg2.dIntegrateTransport_dq(Q2(q), V2(v), J.template bottomRows<LieGroup2::NV>()); | ||
235 | } | ||
236 | |||
237 | template<class Config_t, class Tangent_t, class Jacobian_t> | ||
238 | void dIntegrateTransport_dv_impl( | ||
239 | const Eigen::MatrixBase<Config_t> & q, | ||
240 | const Eigen::MatrixBase<Tangent_t> & v, | ||
241 | const Eigen::MatrixBase<Jacobian_t> & Jin) const | ||
242 | { | ||
243 | Jacobian_t & J = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, Jin); | ||
244 | lg1.dIntegrateTransport_dv(Q1(q), V1(v), J.template topRows<LieGroup1::NV>()); | ||
245 | lg2.dIntegrateTransport_dv(Q2(q), V2(v), J.template bottomRows<LieGroup2::NV>()); | ||
246 | } | ||
247 | |||
248 | template<class ConfigL_t, class ConfigR_t> | ||
249 | ✗ | Scalar squaredDistance_impl( | |
250 | const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) const | ||
251 | { | ||
252 | ✗ | return lg1.squaredDistance(Q1(q0), Q1(q1)) + lg2.squaredDistance(Q2(q0), Q2(q1)); | |
253 | } | ||
254 | |||
255 | template<class Config_t> | ||
256 | ✗ | void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) const | |
257 | { | ||
258 | ✗ | lg1.normalize(Qo1(qout)); | |
259 | ✗ | lg2.normalize(Qo2(qout)); | |
260 | } | ||
261 | |||
262 | template<class Config_t> | ||
263 | bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin, const Scalar & prec) const | ||
264 | { | ||
265 | return lg1.isNormalized(Qo1(qin), prec) && lg2.isNormalized(Qo2(qin), prec); | ||
266 | } | ||
267 | |||
268 | template<class Config_t> | ||
269 | ✗ | void random_impl(const Eigen::MatrixBase<Config_t> & qout) const | |
270 | { | ||
271 | ✗ | lg1.random(Qo1(qout)); | |
272 | ✗ | lg2.random(Qo2(qout)); | |
273 | } | ||
274 | |||
275 | template<class ConfigL_t, class ConfigR_t, class ConfigOut_t> | ||
276 | 4686 | void randomConfiguration_impl( | |
277 | const Eigen::MatrixBase<ConfigL_t> & lower, | ||
278 | const Eigen::MatrixBase<ConfigR_t> & upper, | ||
279 | const Eigen::MatrixBase<ConfigOut_t> & qout) const | ||
280 | { | ||
281 |
3/6✓ Branch 2 taken 2343 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2343 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2343 times.
✗ Branch 9 not taken.
|
4686 | lg1.randomConfiguration(Q1(lower), Q1(upper), Qo1(qout)); |
282 |
3/6✓ Branch 2 taken 2343 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2343 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2343 times.
✗ Branch 9 not taken.
|
4686 | lg2.randomConfiguration(Q2(lower), Q2(upper), Qo2(qout)); |
283 | 4686 | } | |
284 | |||
285 | template<class ConfigL_t, class ConfigR_t> | ||
286 | 12 | bool isSameConfiguration_impl( | |
287 | const Eigen::MatrixBase<ConfigL_t> & q0, | ||
288 | const Eigen::MatrixBase<ConfigR_t> & q1, | ||
289 | const Scalar & prec) const | ||
290 | { | ||
291 |
3/6✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
|
12 | return lg1.isSameConfiguration(Q1(q0), Q1(q1), prec) |
292 |
5/10✓ Branch 0 taken 6 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 6 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 6 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 6 times.
✗ Branch 12 not taken.
|
12 | && lg2.isSameConfiguration(Q2(q0), Q2(q1), prec); |
293 | } | ||
294 | |||
295 | bool isEqual_impl(const CartesianProductOperation & other) const | ||
296 | { | ||
297 | return lg1 == other.lg1 && lg2 == other.lg2; | ||
298 | } | ||
299 | |||
300 | LieGroup1 lg1; | ||
301 | LieGroup2 lg2; | ||
302 | |||
303 | private: | ||
304 | // VectorSpaceOperationTpl<-1> within CartesianProductOperation will not work | ||
305 | // if Eigen version is lower than 3.2.1 | ||
306 | #if EIGEN_VERSION_AT_LEAST(3, 2, 1) | ||
307 | #define REMOVE_IF_EIGEN_TOO_LOW(x) x | ||
308 | #else | ||
309 | #define REMOVE_IF_EIGEN_TOO_LOW(x) | ||
310 | #endif | ||
311 | |||
312 | template<typename Config> | ||
313 | typename Config ::template ConstFixedSegmentReturnType<LieGroup1::NQ>::Type | ||
314 | 9396 | Q1(const Eigen::MatrixBase<Config> & q) const | |
315 | { | ||
316 | 9396 | return q.derived().template head<LieGroup1::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nq())); | |
317 | } | ||
318 | template<typename Config> | ||
319 | typename Config ::template ConstFixedSegmentReturnType<LieGroup2::NQ>::Type | ||
320 | 9396 | Q2(const Eigen::MatrixBase<Config> & q) const | |
321 | { | ||
322 | 9396 | return q.derived().template tail<LieGroup2::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nq())); | |
323 | } | ||
324 | template<typename Tangent> | ||
325 | typename Tangent::template ConstFixedSegmentReturnType<LieGroup1::NV>::Type | ||
326 | ✗ | V1(const Eigen::MatrixBase<Tangent> & v) const | |
327 | { | ||
328 | ✗ | return v.derived().template head<LieGroup1::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nv())); | |
329 | } | ||
330 | template<typename Tangent> | ||
331 | typename Tangent::template ConstFixedSegmentReturnType<LieGroup2::NV>::Type | ||
332 | ✗ | V2(const Eigen::MatrixBase<Tangent> & v) const | |
333 | { | ||
334 | ✗ | return v.derived().template tail<LieGroup2::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nv())); | |
335 | } | ||
336 | |||
337 | template<typename Config> | ||
338 | typename Config ::template FixedSegmentReturnType<LieGroup1::NQ>::Type | ||
339 | 4670 | Qo1(const Eigen::MatrixBase<Config> & q) const | |
340 | { | ||
341 | 9340 | return PINOCCHIO_EIGEN_CONST_CAST(Config, q).template head<LieGroup1::NQ>( | |
342 | 9340 | REMOVE_IF_EIGEN_TOO_LOW(lg1.nq())); | |
343 | } | ||
344 | template<typename Config> | ||
345 | typename Config ::template FixedSegmentReturnType<LieGroup2::NQ>::Type | ||
346 | 2335 | Qo2(const Eigen::MatrixBase<Config> & q) const | |
347 | { | ||
348 | 4670 | return PINOCCHIO_EIGEN_CONST_CAST(Config, q).template tail<LieGroup2::NQ>( | |
349 | 4670 | REMOVE_IF_EIGEN_TOO_LOW(lg2.nq())); | |
350 | } | ||
351 | template<typename Tangent> | ||
352 | typename Tangent::template FixedSegmentReturnType<LieGroup1::NV>::Type | ||
353 | ✗ | Vo1(const Eigen::MatrixBase<Tangent> & v) const | |
354 | { | ||
355 | ✗ | return PINOCCHIO_EIGEN_CONST_CAST(Tangent, v) | |
356 | ✗ | .template head<LieGroup1::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nv())); | |
357 | } | ||
358 | template<typename Tangent> | ||
359 | typename Tangent::template FixedSegmentReturnType<LieGroup2::NV>::Type | ||
360 | ✗ | Vo2(const Eigen::MatrixBase<Tangent> & v) const | |
361 | { | ||
362 | ✗ | return PINOCCHIO_EIGEN_CONST_CAST(Tangent, v) | |
363 | ✗ | .template tail<LieGroup2::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nv())); | |
364 | } | ||
365 | |||
366 | template<typename Jac> | ||
367 | ✗ | Eigen::Block<Jac, LieGroup1::NV, LieGroup1::NV> J11(const Eigen::MatrixBase<Jac> & J) const | |
368 | { | ||
369 | ✗ | return PINOCCHIO_EIGEN_CONST_CAST(Jac, J) | |
370 | ✗ | .template topLeftCorner<LieGroup1::NV, LieGroup1::NV>(lg1.nv(), lg1.nv()); | |
371 | } | ||
372 | template<typename Jac> | ||
373 | ✗ | Eigen::Block<Jac, LieGroup1::NV, LieGroup2::NV> J12(const Eigen::MatrixBase<Jac> & J) const | |
374 | { | ||
375 | ✗ | return PINOCCHIO_EIGEN_CONST_CAST(Jac, J) | |
376 | ✗ | .template topRightCorner<LieGroup1::NV, LieGroup2::NV>(lg1.nv(), lg2.nv()); | |
377 | } | ||
378 | template<typename Jac> | ||
379 | ✗ | Eigen::Block<Jac, LieGroup2::NV, LieGroup1::NV> J21(const Eigen::MatrixBase<Jac> & J) const | |
380 | { | ||
381 | ✗ | return PINOCCHIO_EIGEN_CONST_CAST(Jac, J) | |
382 | ✗ | .template bottomLeftCorner<LieGroup2::NV, LieGroup1::NV>(lg2.nv(), lg1.nv()); | |
383 | } | ||
384 | template<typename Jac> | ||
385 | ✗ | Eigen::Block<Jac, LieGroup2::NV, LieGroup2::NV> J22(const Eigen::MatrixBase<Jac> & J) const | |
386 | { | ||
387 | ✗ | return PINOCCHIO_EIGEN_CONST_CAST(Jac, J) | |
388 | ✗ | .template bottomRightCorner<LieGroup2::NV, LieGroup2::NV>(lg2.nv(), lg2.nv()); | |
389 | } | ||
390 | #undef REMOVE_IF_EIGEN_TOO_LOW | ||
391 | |||
392 | }; // struct CartesianProductOperation | ||
393 | |||
394 | } // namespace pinocchio | ||
395 | |||
396 | #endif // ifndef __pinocchio_multibody_liegroup_cartesian_product_operation_hpp__ | ||
397 |