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 |
|
|
|