pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
cartesian-product.hpp
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
10namespace 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 CartesianProductOperation()
63 : lg1()
64 , 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 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 lg1.randomConfiguration(Q1(lower), Q1(upper), Qo1(qout));
282 lg2.randomConfiguration(Q2(lower), Q2(upper), Qo2(qout));
283 }
284
285 template<class ConfigL_t, class ConfigR_t>
286 bool isSameConfiguration_impl(
287 const Eigen::MatrixBase<ConfigL_t> & q0,
288 const Eigen::MatrixBase<ConfigR_t> & q1,
289 const Scalar & prec) const
290 {
291 return lg1.isSameConfiguration(Q1(q0), Q1(q1), prec)
292 && 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 Q1(const Eigen::MatrixBase<Config> & q) const
315 {
316 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 Q2(const Eigen::MatrixBase<Config> & q) const
321 {
322 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 Qo1(const Eigen::MatrixBase<Config> & q) const
340 {
341 return PINOCCHIO_EIGEN_CONST_CAST(Config, q).template head<LieGroup1::NQ>(
342 REMOVE_IF_EIGEN_TOO_LOW(lg1.nq()));
343 }
344 template<typename Config>
345 typename Config ::template FixedSegmentReturnType<LieGroup2::NQ>::Type
346 Qo2(const Eigen::MatrixBase<Config> & q) const
347 {
348 return PINOCCHIO_EIGEN_CONST_CAST(Config, q).template tail<LieGroup2::NQ>(
349 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__
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11
pinocchio::Dynamic
const int Dynamic
Definition fwd.hpp:140
pinocchio::eval_set_dim
Definition cartesian-product.hpp:14
pinocchio::traits
Common traits structure to fully define base classes for CRTP.
Definition fwd.hpp:72