| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| 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 { value = dim1 + dim2 }; |
||
16 |
}; |
||
17 |
|||
18 |
template<int dim> |
||
19 |
struct eval_set_dim<dim,Eigen::Dynamic> |
||
20 |
{ |
||
21 |
enum { value = Eigen::Dynamic }; |
||
22 |
}; |
||
23 |
|||
24 |
template<int dim> |
||
25 |
struct eval_set_dim<Eigen::Dynamic,dim> |
||
26 |
{ |
||
27 |
enum { value = Eigen::Dynamic }; |
||
28 |
}; |
||
29 |
|||
30 |
template<typename LieGroup1, typename LieGroup2> |
||
31 |
struct CartesianProductOperation; |
||
32 |
|||
33 |
template<typename LieGroup1, typename LieGroup2> |
||
34 |
struct traits<CartesianProductOperation<LieGroup1, LieGroup2> > { |
||
35 |
typedef typename traits<LieGroup1>::Scalar Scalar; |
||
36 |
enum { |
||
37 |
Options = traits<LieGroup1>::Options, |
||
38 |
NQ = eval_set_dim<LieGroup1::NQ,LieGroup2::NQ>::value, |
||
39 |
NV = eval_set_dim<LieGroup1::NV,LieGroup2::NV>::value |
||
40 |
}; |
||
41 |
}; |
||
42 |
|||
43 |
template<typename LieGroup1, typename LieGroup2> |
||
44 |
struct CartesianProductOperation : public LieGroupBase <CartesianProductOperation<LieGroup1, LieGroup2> > |
||
45 |
{ |
||
46 |
PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(CartesianProductOperation); |
||
47 |
|||
48 |
13690 |
CartesianProductOperation () : lg1 (), lg2 () |
|
49 |
{ |
||
50 |
13690 |
} |
|
51 |
// Get dimension of Lie Group vector representation |
||
52 |
// |
||
53 |
// For instance, for SO(3), the dimension of the vector representation is |
||
54 |
// 4 (quaternion) while the dimension of the tangent space is 3. |
||
55 |
2398 |
Index nq () const |
|
56 |
{ |
||
57 |
2398 |
return lg1.nq () + lg2.nq (); |
|
58 |
} |
||
59 |
|||
60 |
// Get dimension of Lie Group tangent space |
||
61 |
1608 |
Index nv () const |
|
62 |
{ |
||
63 |
1608 |
return lg1.nv () + lg2.nv (); |
|
64 |
} |
||
65 |
|||
66 |
5 |
ConfigVector_t neutral () const |
|
67 |
{ |
||
68 |
5 |
ConfigVector_t n; |
|
69 |
5 |
n.resize (nq ()); |
|
70 |
✓✗✓✗ |
5 |
Qo1(n) = lg1.neutral (); |
71 |
✓✗✓✗ |
5 |
Qo2(n) = lg2.neutral (); |
72 |
5 |
return n; |
|
73 |
} |
||
74 |
|||
75 |
244 |
std::string name () const |
|
76 |
{ |
||
77 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
488 |
std::ostringstream oss; oss << lg1.name () << "*" << lg2.name (); |
78 |
✓✗ | 488 |
return oss.str (); |
79 |
} |
||
80 |
|||
81 |
template <class ConfigL_t, class ConfigR_t, class Tangent_t> |
||
82 |
1557 |
void difference_impl(const Eigen::MatrixBase<ConfigL_t> & q0, |
|
83 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
84 |
const Eigen::MatrixBase<Tangent_t> & d) const |
||
85 |
{ |
||
86 |
✓✗✓✗ ✓✗ |
1557 |
lg1.difference(Q1(q0), Q1(q1), Vo1(d)); |
87 |
✓✗✓✗ ✓✗ |
1557 |
lg2.difference(Q2(q0), Q2(q1), Vo2(d)); |
88 |
1557 |
} |
|
89 |
|||
90 |
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t> |
||
91 |
324 |
void dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0, |
|
92 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
93 |
const Eigen::MatrixBase<JacobianOut_t> & J) const |
||
94 |
{ |
||
95 |
✓✗ | 324 |
J12(J).setZero(); |
96 |
✓✗ | 324 |
J21(J).setZero(); |
97 |
|||
98 |
✓✗✓✗ ✓✗ |
324 |
lg1.template dDifference<arg> (Q1(q0), Q1(q1), J11(J)); |
99 |
✓✗✓✗ ✓✗ |
324 |
lg2.template dDifference<arg> (Q2(q0), Q2(q1), J22(J)); |
100 |
324 |
} |
|
101 |
|||
102 |
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t> |
||
103 |
2379 |
void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q, |
|
104 |
const Eigen::MatrixBase<Velocity_t> & v, |
||
105 |
const Eigen::MatrixBase<ConfigOut_t> & qout) const |
||
106 |
{ |
||
107 |
✓✗✓✗ ✓✗ |
2379 |
lg1.integrate(Q1(q), V1(v), Qo1(qout)); |
108 |
✓✗✓✗ ✓✗ |
2379 |
lg2.integrate(Q2(q), V2(v), Qo2(qout)); |
109 |
2379 |
} |
|
110 |
|||
111 |
template <class Config_t, class Jacobian_t> |
||
112 |
80 |
void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q, |
|
113 |
const Eigen::MatrixBase<Jacobian_t> & J) const |
||
114 |
{ |
||
115 |
✓✗✓✗ |
80 |
assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension"); |
116 |
80 |
Jacobian_t & J_ = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J); |
|
117 |
✓✗ | 80 |
J_.topRightCorner(lg1.nq(),lg2.nv()).setZero(); |
118 |
✓✗ | 80 |
J_.bottomLeftCorner(lg2.nq(),lg1.nv()).setZero(); |
119 |
|||
120 |
✓✗✓✗ |
80 |
lg1.integrateCoeffWiseJacobian(Q1(q), |
121 |
J_.topLeftCorner(lg1.nq(),lg1.nv())); |
||
122 |
✓✗✓✗ |
80 |
lg2.integrateCoeffWiseJacobian(Q2(q), J_.bottomRightCorner(lg2.nq(),lg2.nv())); |
123 |
80 |
} |
|
124 |
|||
125 |
template <class Config_t, class Tangent_t, class JacobianOut_t> |
||
126 |
85 |
void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & q, |
|
127 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
128 |
const Eigen::MatrixBase<JacobianOut_t> & J, |
||
129 |
const AssignmentOperatorType op=SETTO) const |
||
130 |
{ |
||
131 |
✓✗✗ | 85 |
switch(op) |
132 |
{ |
||
133 |
85 |
case SETTO: |
|
134 |
✓✗ | 85 |
J12(J).setZero(); |
135 |
✓✗ | 85 |
J21(J).setZero(); |
136 |
// fallthrough |
||
137 |
85 |
case ADDTO: |
|
138 |
case RMTO: |
||
139 |
✓✗✓✗ ✓✗ |
85 |
lg1.dIntegrate_dq(Q1(q), V1(v), J11(J),op); |
140 |
✓✗✓✗ ✓✗ |
85 |
lg2.dIntegrate_dq(Q2(q), V2(v), J22(J),op); |
141 |
85 |
break; |
|
142 |
default: |
||
143 |
assert(false && "Wrong Op requesed value"); |
||
144 |
break; |
||
145 |
} |
||
146 |
85 |
} |
|
147 |
|||
148 |
template <class Config_t, class Tangent_t, class JacobianOut_t> |
||
149 |
165 |
void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & q, |
|
150 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
151 |
const Eigen::MatrixBase<JacobianOut_t> & J, |
||
152 |
const AssignmentOperatorType op=SETTO) const |
||
153 |
{ |
||
154 |
✓✗✗ | 165 |
switch(op) |
155 |
{ |
||
156 |
165 |
case SETTO: |
|
157 |
✓✗ | 165 |
J12(J).setZero(); |
158 |
✓✗ | 165 |
J21(J).setZero(); |
159 |
// fallthrough |
||
160 |
165 |
case ADDTO: |
|
161 |
case RMTO: |
||
162 |
✓✗✓✗ ✓✗ |
165 |
lg1.dIntegrate_dv(Q1(q), V1(v), J11(J),op); |
163 |
✓✗✓✗ ✓✗ |
165 |
lg2.dIntegrate_dv(Q2(q), V2(v), J22(J),op); |
164 |
165 |
break; |
|
165 |
default: |
||
166 |
assert(false && "Wrong Op requesed value"); |
||
167 |
break; |
||
168 |
} |
||
169 |
165 |
} |
|
170 |
|||
171 |
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
||
172 |
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q, |
||
173 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
174 |
const Eigen::MatrixBase<JacobianIn_t> & J_in, |
||
175 |
const Eigen::MatrixBase<JacobianOut_t> & J_out) const |
||
176 |
{ |
||
177 |
JacobianOut_t& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out); |
||
178 |
JacobianOut_t& Jin = PINOCCHIO_EIGEN_CONST_CAST(JacobianIn_t,J_in); |
||
179 |
lg1.dIntegrateTransport_dq(Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(),Jout.template topRows<LieGroup1::NV>()); |
||
180 |
lg2.dIntegrateTransport_dq(Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(),Jout.template bottomRows<LieGroup2::NV>()); |
||
181 |
} |
||
182 |
|||
183 |
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
||
184 |
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q, |
||
185 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
186 |
const Eigen::MatrixBase<JacobianIn_t> & J_in, |
||
187 |
const Eigen::MatrixBase<JacobianOut_t> & J_out) const |
||
188 |
{ |
||
189 |
JacobianOut_t& Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out); |
||
190 |
JacobianOut_t& Jin = PINOCCHIO_EIGEN_CONST_CAST(JacobianIn_t,J_in); |
||
191 |
lg1.dIntegrateTransport_dv(Q1(q), V1(v), Jin.template topRows<LieGroup1::NV>(),Jout.template topRows<LieGroup1::NV>()); |
||
192 |
lg2.dIntegrateTransport_dv(Q2(q), V2(v), Jin.template bottomRows<LieGroup2::NV>(),Jout.template bottomRows<LieGroup2::NV>()); |
||
193 |
} |
||
194 |
|||
195 |
|||
196 |
template <class Config_t, class Tangent_t, class Jacobian_t> |
||
197 |
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & q, |
||
198 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
199 |
const Eigen::MatrixBase<Jacobian_t> & Jin) const |
||
200 |
{ |
||
201 |
Jacobian_t& J = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,Jin); |
||
202 |
lg1.dIntegrateTransport_dq(Q1(q), V1(v), J.template topRows<LieGroup1::NV>()); |
||
203 |
lg2.dIntegrateTransport_dq(Q2(q), V2(v), J.template bottomRows<LieGroup2::NV>()); |
||
204 |
} |
||
205 |
|||
206 |
template <class Config_t, class Tangent_t, class Jacobian_t> |
||
207 |
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & q, |
||
208 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
209 |
const Eigen::MatrixBase<Jacobian_t> & Jin) const |
||
210 |
{ |
||
211 |
Jacobian_t& J = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,Jin); |
||
212 |
lg1.dIntegrateTransport_dv(Q1(q), V1(v), J.template topRows<LieGroup1::NV>()); |
||
213 |
lg2.dIntegrateTransport_dv(Q2(q), V2(v), J.template bottomRows<LieGroup2::NV>()); |
||
214 |
} |
||
215 |
|||
216 |
template <class ConfigL_t, class ConfigR_t> |
||
217 |
2 |
Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0, |
|
218 |
const Eigen::MatrixBase<ConfigR_t> & q1) const |
||
219 |
{ |
||
220 |
2 |
return lg1.squaredDistance(Q1(q0), Q1(q1)) |
|
221 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
2 |
+ lg2.squaredDistance(Q2(q0), Q2(q1)); |
222 |
} |
||
223 |
|||
224 |
template <class Config_t> |
||
225 |
1 |
void normalize_impl (const Eigen::MatrixBase<Config_t>& qout) const |
|
226 |
{ |
||
227 |
✓✗ | 1 |
lg1.normalize(Qo1(qout)); |
228 |
✓✗ | 1 |
lg2.normalize(Qo2(qout)); |
229 |
1 |
} |
|
230 |
|||
231 |
template <class Config_t> |
||
232 |
bool isNormalized_impl (const Eigen::MatrixBase<Config_t>& qin, const Scalar& prec) const |
||
233 |
{ |
||
234 |
return lg1.isNormalized(Qo1(qin), prec) && lg2.isNormalized(Qo2(qin), prec); |
||
235 |
} |
||
236 |
|||
237 |
template <class Config_t> |
||
238 |
1016 |
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const |
|
239 |
{ |
||
240 |
✓✗ | 1016 |
lg1.random(Qo1(qout)); |
241 |
✓✗ | 1016 |
lg2.random(Qo2(qout)); |
242 |
1016 |
} |
|
243 |
|||
244 |
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t> |
||
245 |
12450 |
void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & lower, |
|
246 |
const Eigen::MatrixBase<ConfigR_t> & upper, |
||
247 |
const Eigen::MatrixBase<ConfigOut_t> & qout) |
||
248 |
const |
||
249 |
{ |
||
250 |
✓✗✓✗ ✓✗ |
12450 |
lg1.randomConfiguration(Q1(lower), Q1(upper), Qo1(qout)); |
251 |
✓✗✓✗ ✓✗ |
12450 |
lg2.randomConfiguration(Q2(lower), Q2(upper), Qo2(qout)); |
252 |
12450 |
} |
|
253 |
|||
254 |
template <class ConfigL_t, class ConfigR_t> |
||
255 |
18 |
bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0, |
|
256 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
257 |
const Scalar & prec) const |
||
258 |
{ |
||
259 |
18 |
return lg1.isSameConfiguration(Q1(q0), Q1(q1), prec) |
|
260 |
✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ |
18 |
&& lg2.isSameConfiguration(Q2(q0), Q2(q1), prec); |
261 |
} |
||
262 |
|||
263 |
bool isEqual_impl (const CartesianProductOperation& other) const |
||
264 |
{ |
||
265 |
return lg1 == other.lg1 && lg2 == other.lg2; |
||
266 |
} |
||
267 |
|||
268 |
LieGroup1 lg1; |
||
269 |
LieGroup2 lg2; |
||
270 |
|||
271 |
private: |
||
272 |
// VectorSpaceOperationTpl<-1> within CartesianProductOperation will not work |
||
273 |
// if Eigen version is lower than 3.2.1 |
||
274 |
#if EIGEN_VERSION_AT_LEAST(3,2,1) |
||
275 |
# define REMOVE_IF_EIGEN_TOO_LOW(x) x |
||
276 |
#else |
||
277 |
# define REMOVE_IF_EIGEN_TOO_LOW(x) |
||
278 |
#endif |
||
279 |
|||
280 |
31420 |
template <typename Config > typename Config ::template ConstFixedSegmentReturnType<LieGroup1::NQ>::Type Q1 (const Eigen::MatrixBase<Config >& q) const { return q.derived().template head<LieGroup1::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nq())); } |
|
281 |
15710 |
template <typename Config > typename Config ::template ConstFixedSegmentReturnType<LieGroup2::NQ>::Type Q2 (const Eigen::MatrixBase<Config >& q) const { return q.derived().template tail<LieGroup2::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nq())); } |
|
282 |
1316 |
template <typename Tangent> typename Tangent::template ConstFixedSegmentReturnType<LieGroup1::NV>::Type V1 (const Eigen::MatrixBase<Tangent>& v) const { return v.derived().template head<LieGroup1::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nv())); } |
|
283 |
1316 |
template <typename Tangent> typename Tangent::template ConstFixedSegmentReturnType<LieGroup2::NV>::Type V2 (const Eigen::MatrixBase<Tangent>& v) const { return v.derived().template tail<LieGroup2::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nv())); } |
|
284 |
|||
285 |
15854 |
template <typename Config > typename Config ::template FixedSegmentReturnType<LieGroup1::NQ>::Type Qo1 (const Eigen::MatrixBase<Config >& q) const { return PINOCCHIO_EIGEN_CONST_CAST(Config,q).template head<LieGroup1::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nq())); } |
|
286 |
7927 |
template <typename Config > typename Config ::template FixedSegmentReturnType<LieGroup2::NQ>::Type Qo2 (const Eigen::MatrixBase<Config >& q) const { return PINOCCHIO_EIGEN_CONST_CAST(Config,q).template tail<LieGroup2::NQ>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nq())); } |
|
287 |
779 |
template <typename Tangent> typename Tangent::template FixedSegmentReturnType<LieGroup1::NV>::Type Vo1 (const Eigen::MatrixBase<Tangent>& v) const { return PINOCCHIO_EIGEN_CONST_CAST(Tangent,v).template head<LieGroup1::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg1.nv())); } |
|
288 |
779 |
template <typename Tangent> typename Tangent::template FixedSegmentReturnType<LieGroup2::NV>::Type Vo2 (const Eigen::MatrixBase<Tangent>& v) const { return PINOCCHIO_EIGEN_CONST_CAST(Tangent,v).template tail<LieGroup2::NV>(REMOVE_IF_EIGEN_TOO_LOW(lg2.nv())); } |
|
289 |
|||
290 |
572 |
template <typename Jac> Eigen::Block<Jac, LieGroup1::NV, LieGroup1::NV> J11 (const Eigen::MatrixBase<Jac>& J) const { return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template topLeftCorner<LieGroup1::NV, LieGroup1::NV>(lg1.nv(),lg1.nv()); } |
|
291 |
288 |
template <typename Jac> Eigen::Block<Jac, LieGroup1::NV, LieGroup2::NV> J12 (const Eigen::MatrixBase<Jac>& J) const { return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template topRightCorner<LieGroup1::NV, LieGroup2::NV>(lg1.nv(),lg2.nv()); } |
|
292 |
288 |
template <typename Jac> Eigen::Block<Jac, LieGroup2::NV, LieGroup1::NV> J21 (const Eigen::MatrixBase<Jac>& J) const { return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template bottomLeftCorner<LieGroup2::NV, LieGroup1::NV>(lg2.nv(),lg1.nv()); } |
|
293 |
288 |
template <typename Jac> Eigen::Block<Jac, LieGroup2::NV, LieGroup2::NV> J22 (const Eigen::MatrixBase<Jac>& J) const { return PINOCCHIO_EIGEN_CONST_CAST(Jac,J).template bottomRightCorner<LieGroup2::NV, LieGroup2::NV>(lg2.nv(),lg2.nv()); } |
|
294 |
#undef REMOVE_IF_EIGEN_TOO_LOW |
||
295 |
|||
296 |
}; // struct CartesianProductOperation |
||
297 |
|||
298 |
} // namespace pinocchio |
||
299 |
|||
300 |
#endif // ifndef __pinocchio_multibody_liegroup_cartesian_product_operation_hpp__ |
| Generated by: GCOVR (Version 4.2) |