1 |
|
|
// |
2 |
|
|
// Copyright (c) 2016-2020 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#ifndef __pinocchio_multibody_liegroup_liegroup_operation_base_hxx__ |
6 |
|
|
#define __pinocchio_multibody_liegroup_liegroup_operation_base_hxx__ |
7 |
|
|
|
8 |
|
|
#include "pinocchio/macros.hpp" |
9 |
|
|
|
10 |
|
|
namespace pinocchio { |
11 |
|
|
|
12 |
|
|
// --------------- API with return value as argument ---------------------- // |
13 |
|
|
|
14 |
|
|
template <class Derived> |
15 |
|
|
template <class ConfigIn_t, class Tangent_t, class ConfigOut_t> |
16 |
|
243853 |
void LieGroupBase<Derived> |
17 |
|
|
::integrate(const Eigen::MatrixBase<ConfigIn_t> & q, |
18 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
19 |
|
|
const Eigen::MatrixBase<ConfigOut_t> & qout) const |
20 |
|
|
{ |
21 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigIn_t , ConfigVector_t); |
22 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t); |
23 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, ConfigVector_t); |
24 |
|
243853 |
derived().integrate_impl(q.derived(), v.derived(), PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout)); |
25 |
|
243853 |
} |
26 |
|
|
|
27 |
|
|
template <class Derived> |
28 |
|
|
template<class Config_t, class Jacobian_t> |
29 |
|
536 |
void LieGroupBase<Derived>:: |
30 |
|
|
integrateCoeffWiseJacobian(const Eigen::MatrixBase<Config_t > & q, |
31 |
|
|
const Eigen::MatrixBase<Jacobian_t> & J) const |
32 |
|
|
{ |
33 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t); |
34 |
|
|
|
35 |
|
536 |
derived().integrateCoeffWiseJacobian_impl(q.derived(),PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J)); |
36 |
|
|
|
37 |
|
536 |
} |
38 |
|
|
|
39 |
|
|
template <class Derived> |
40 |
|
|
template<class Config_t, class Tangent_t, class JacobianOut_t> |
41 |
|
828 |
void LieGroupBase<Derived>::dIntegrate(const Eigen::MatrixBase<Config_t > & q, |
42 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
43 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & J, |
44 |
|
|
const ArgumentPosition arg, |
45 |
|
|
const AssignmentOperatorType op) const |
46 |
|
|
{ |
47 |
✓✓✗✓
|
828 |
assert((arg==ARG0||arg==ARG1) && "arg should be either ARG0 or ARG1"); |
48 |
|
|
|
49 |
✓✓✗ |
828 |
switch (arg) { |
50 |
|
254 |
case ARG0: |
51 |
|
254 |
dIntegrate_dq(q.derived(),v.derived(), |
52 |
|
254 |
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),op); |
53 |
|
254 |
return; |
54 |
|
574 |
case ARG1: |
55 |
|
574 |
dIntegrate_dv(q.derived(),v.derived(), |
56 |
|
574 |
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),op); |
57 |
|
574 |
return; |
58 |
|
|
default: return; |
59 |
|
|
} |
60 |
|
|
} |
61 |
|
|
|
62 |
|
|
template <class Derived> |
63 |
|
|
template <class Config_t, class Tangent_t, class JacobianOut_t> |
64 |
|
762 |
void LieGroupBase<Derived>::dIntegrate_dq( |
65 |
|
|
const Eigen::MatrixBase<Config_t > & q, |
66 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
67 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & J, |
68 |
|
|
const AssignmentOperatorType op) const |
69 |
|
|
{ |
70 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t); |
71 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t); |
72 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t); |
73 |
|
1524 |
derived().dIntegrate_dq_impl(q.derived(), |
74 |
|
|
v.derived(), |
75 |
|
762 |
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J),op); |
76 |
|
762 |
} |
77 |
|
|
|
78 |
|
|
template <class Derived> |
79 |
|
|
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
80 |
|
|
void LieGroupBase<Derived>::dIntegrate_dq( |
81 |
|
|
const Eigen::MatrixBase<Config_t > & q, |
82 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
83 |
|
|
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
84 |
|
|
int self, |
85 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & Jout, |
86 |
|
|
const AssignmentOperatorType op) const |
87 |
|
|
{ |
88 |
|
|
PINOCCHIO_UNUSED_VARIABLE(self); |
89 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t); |
90 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t); |
91 |
|
|
assert(Jin.cols() == nv()); |
92 |
|
|
assert(Jout.cols() == nv()); |
93 |
|
|
assert(Jout.rows() == Jin.rows()); |
94 |
|
|
derived().dIntegrate_product_impl( |
95 |
|
|
q.derived(), v.derived(), |
96 |
|
|
Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout), |
97 |
|
|
false, ARG0, op); |
98 |
|
|
} |
99 |
|
|
|
100 |
|
|
template <class Derived> |
101 |
|
|
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
102 |
|
|
void LieGroupBase<Derived>::dIntegrate_dq( |
103 |
|
|
const Eigen::MatrixBase<Config_t > & q, |
104 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
105 |
|
|
int self, |
106 |
|
|
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
107 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & Jout, |
108 |
|
|
const AssignmentOperatorType op) const |
109 |
|
|
{ |
110 |
|
|
PINOCCHIO_UNUSED_VARIABLE(self); |
111 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t); |
112 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t); |
113 |
|
|
assert(Jin.rows() == nv()); |
114 |
|
|
assert(Jout.rows() == nv()); |
115 |
|
|
assert(Jout.cols() == Jin.cols()); |
116 |
|
|
derived().dIntegrate_product_impl( |
117 |
|
|
q.derived(), v.derived(), |
118 |
|
|
Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout), |
119 |
|
|
true, ARG0, op); |
120 |
|
|
} |
121 |
|
|
|
122 |
|
|
template <class Derived> |
123 |
|
|
template <class Config_t, class Tangent_t, class JacobianOut_t> |
124 |
|
1242 |
void LieGroupBase<Derived>::dIntegrate_dv( |
125 |
|
|
const Eigen::MatrixBase<Config_t > & q, |
126 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
127 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & J, |
128 |
|
|
const AssignmentOperatorType op) const |
129 |
|
|
{ |
130 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t); |
131 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t); |
132 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t); |
133 |
|
2484 |
derived().dIntegrate_dv_impl(q.derived(), |
134 |
|
|
v.derived(), |
135 |
|
1242 |
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J), |
136 |
|
|
op); |
137 |
|
1242 |
} |
138 |
|
|
|
139 |
|
|
template <class Derived> |
140 |
|
|
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
141 |
|
|
void LieGroupBase<Derived>::dIntegrate_dv( |
142 |
|
|
const Eigen::MatrixBase<Config_t > & q, |
143 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
144 |
|
|
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
145 |
|
|
int self, |
146 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & Jout, |
147 |
|
|
const AssignmentOperatorType op) const |
148 |
|
|
{ |
149 |
|
|
PINOCCHIO_UNUSED_VARIABLE(self); |
150 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t); |
151 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t); |
152 |
|
|
assert(Jin.cols() == nv()); |
153 |
|
|
assert(Jout.cols() == nv()); |
154 |
|
|
assert(Jout.rows() == Jin.rows()); |
155 |
|
|
derived().dIntegrate_product_impl( |
156 |
|
|
q.derived(), v.derived(), |
157 |
|
|
Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout), |
158 |
|
|
false, ARG1, op); |
159 |
|
|
} |
160 |
|
|
|
161 |
|
|
template <class Derived> |
162 |
|
|
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
163 |
|
|
void LieGroupBase<Derived>::dIntegrate_dv( |
164 |
|
|
const Eigen::MatrixBase<Config_t > & q, |
165 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
166 |
|
|
int self, |
167 |
|
|
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
168 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & Jout, |
169 |
|
|
const AssignmentOperatorType op) const |
170 |
|
|
{ |
171 |
|
|
PINOCCHIO_UNUSED_VARIABLE(self); |
172 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t); |
173 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t); |
174 |
|
|
assert(Jin.rows() == nv()); |
175 |
|
|
assert(Jout.rows() == nv()); |
176 |
|
|
assert(Jout.cols() == Jin.cols()); |
177 |
|
|
derived().dIntegrate_product_impl( |
178 |
|
|
q.derived(), v.derived(), |
179 |
|
|
Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout), |
180 |
|
|
true, ARG1, op); |
181 |
|
|
} |
182 |
|
|
|
183 |
|
|
template <class Derived> |
184 |
|
|
template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
185 |
|
36 |
void LieGroupBase<Derived>::dIntegrateTransport(const Eigen::MatrixBase<Config_t > & q, |
186 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
187 |
|
|
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
188 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & Jout, |
189 |
|
|
const ArgumentPosition arg) const |
190 |
|
|
{ |
191 |
✓✓✗✓
|
36 |
assert((arg==ARG0||arg==ARG1) && "arg should be either ARG0 or ARG1"); |
192 |
|
|
|
193 |
✓✓✗ |
36 |
switch (arg) { |
194 |
|
18 |
case ARG0: |
195 |
|
18 |
dIntegrateTransport_dq(q.derived(),v.derived(),Jin.derived(), |
196 |
|
18 |
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout)); |
197 |
|
18 |
return; |
198 |
|
18 |
case ARG1: |
199 |
|
18 |
dIntegrateTransport_dv(q.derived(),v.derived(),Jin.derived(), |
200 |
|
18 |
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout)); |
201 |
|
18 |
return; |
202 |
|
|
default: |
203 |
|
|
return; |
204 |
|
|
} |
205 |
|
|
} |
206 |
|
|
|
207 |
|
|
template <class Derived> |
208 |
|
|
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
209 |
|
18 |
void LieGroupBase<Derived>::dIntegrateTransport_dq(const Eigen::MatrixBase<Config_t > & q, |
210 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
211 |
|
|
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
212 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & Jout) const |
213 |
|
|
{ |
214 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t); |
215 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t); |
216 |
|
|
//EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t); |
217 |
|
36 |
derived().dIntegrateTransport_dq_impl(q.derived(), |
218 |
|
|
v.derived(), |
219 |
|
|
Jin.derived(), |
220 |
|
18 |
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout)); |
221 |
|
|
} |
222 |
|
|
|
223 |
|
|
template <class Derived> |
224 |
|
|
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
225 |
|
18 |
void LieGroupBase<Derived>::dIntegrateTransport_dv(const Eigen::MatrixBase<Config_t > & q, |
226 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
227 |
|
|
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
228 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & Jout) const |
229 |
|
|
{ |
230 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t); |
231 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t); |
232 |
|
|
//EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t); |
233 |
|
36 |
derived().dIntegrateTransport_dv_impl(q.derived(), |
234 |
|
|
v.derived(), |
235 |
|
|
Jin.derived(), |
236 |
|
18 |
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout)); |
237 |
|
|
} |
238 |
|
|
|
239 |
|
|
|
240 |
|
|
template <class Derived> |
241 |
|
|
template<class Config_t, class Tangent_t, class Jacobian_t> |
242 |
|
36 |
void LieGroupBase<Derived>::dIntegrateTransport(const Eigen::MatrixBase<Config_t > & q, |
243 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
244 |
|
|
const Eigen::MatrixBase<Jacobian_t> & J, |
245 |
|
|
const ArgumentPosition arg) const |
246 |
|
|
{ |
247 |
✓✓✗✓
|
36 |
assert((arg==ARG0||arg==ARG1) && "arg should be either ARG0 or ARG1"); |
248 |
|
|
|
249 |
✓✓✗ |
36 |
switch (arg) { |
250 |
|
18 |
case ARG0: |
251 |
|
18 |
dIntegrateTransport_dq(q.derived(),v.derived(), |
252 |
|
18 |
PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J)); |
253 |
|
18 |
return; |
254 |
|
18 |
case ARG1: |
255 |
|
18 |
dIntegrateTransport_dv(q.derived(),v.derived(), |
256 |
|
18 |
PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J)); |
257 |
|
18 |
return; |
258 |
|
|
default: |
259 |
|
|
return; |
260 |
|
|
} |
261 |
|
|
} |
262 |
|
|
|
263 |
|
|
template <class Derived> |
264 |
|
|
template <class Config_t, class Tangent_t, class Jacobian_t> |
265 |
|
18 |
void LieGroupBase<Derived>::dIntegrateTransport_dq( |
266 |
|
|
const Eigen::MatrixBase<Config_t > & q, |
267 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
268 |
|
|
const Eigen::MatrixBase<Jacobian_t> & J) const |
269 |
|
|
{ |
270 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t); |
271 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t); |
272 |
|
|
//EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t); |
273 |
|
36 |
derived().dIntegrateTransport_dq_impl(q.derived(), |
274 |
|
|
v.derived(), |
275 |
|
18 |
PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J)); |
276 |
|
|
} |
277 |
|
|
|
278 |
|
|
template <class Derived> |
279 |
|
|
template <class Config_t, class Tangent_t, class Jacobian_t> |
280 |
|
18 |
void LieGroupBase<Derived>::dIntegrateTransport_dv(const Eigen::MatrixBase<Config_t > & q, |
281 |
|
|
const Eigen::MatrixBase<Tangent_t> & v, |
282 |
|
|
const Eigen::MatrixBase<Jacobian_t> & J) const |
283 |
|
|
{ |
284 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t , ConfigVector_t); |
285 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t , TangentVector_t); |
286 |
|
|
//EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(JacobianOut_t, JacobianMatrix_t); |
287 |
|
36 |
derived().dIntegrateTransport_dv_impl(q.derived(), |
288 |
|
|
v.derived(), |
289 |
|
18 |
PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J)); |
290 |
|
|
} |
291 |
|
|
|
292 |
|
|
/** |
293 |
|
|
* @brief Interpolation between two joint's configurations |
294 |
|
|
* |
295 |
|
|
* @param[in] q0 Initial configuration to interpolate |
296 |
|
|
* @param[in] q1 Final configuration to interpolate |
297 |
|
|
* @param[in] u u in [0;1] position along the interpolation. |
298 |
|
|
* |
299 |
|
|
* @return The interpolated configuration (q0 if u = 0, q1 if u = 1) |
300 |
|
|
*/ |
301 |
|
|
template <class Derived> |
302 |
|
|
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t> |
303 |
|
83957 |
void LieGroupBase<Derived>::interpolate( |
304 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
305 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
306 |
|
|
const Scalar & u, |
307 |
|
|
const Eigen::MatrixBase<ConfigOut_t> & qout) const |
308 |
|
|
{ |
309 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t , ConfigVector_t); |
310 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t , ConfigVector_t); |
311 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, ConfigVector_t); |
312 |
✓✗ |
83957 |
Derived().interpolate_impl(q0, q1, u, PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout)); |
313 |
|
83957 |
} |
314 |
|
|
|
315 |
|
|
template <class Derived> |
316 |
|
|
template <class Config_t> |
317 |
|
28972 |
void LieGroupBase<Derived>::normalize |
318 |
|
|
(const Eigen::MatrixBase<Config_t> & qout) const |
319 |
|
|
{ |
320 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t); |
321 |
|
28972 |
return derived().normalize_impl(PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout)); |
322 |
|
|
} |
323 |
|
|
|
324 |
|
|
template <class Derived> |
325 |
|
|
template <class Config_t> |
326 |
|
126946 |
bool LieGroupBase<Derived>::isNormalized |
327 |
|
|
(const Eigen::MatrixBase<Config_t> & qin, const Scalar& prec) const |
328 |
|
|
{ |
329 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t); |
330 |
|
126946 |
return derived().isNormalized_impl(qin, prec); |
331 |
|
|
} |
332 |
|
|
|
333 |
|
|
/** |
334 |
|
|
* @brief Generate a random joint configuration, normalizing quaternions when necessary. |
335 |
|
|
* |
336 |
|
|
* \warning Do not take into account the joint limits. To shoot a configuration uniformingly |
337 |
|
|
* depending on joint limits, see randomConfiguration |
338 |
|
|
* |
339 |
|
|
* @return The joint configuration |
340 |
|
|
*/ |
341 |
|
|
template <class Derived> |
342 |
|
|
template <class Config_t> |
343 |
|
4580 |
void LieGroupBase<Derived>::random |
344 |
|
|
(const Eigen::MatrixBase<Config_t> & qout) const |
345 |
|
|
{ |
346 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Config_t, ConfigVector_t); |
347 |
|
4580 |
return derived().random_impl(PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout)); |
348 |
|
|
} |
349 |
|
|
|
350 |
|
|
template <class Derived> |
351 |
|
|
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t> |
352 |
|
127742 |
void LieGroupBase<Derived>::randomConfiguration( |
353 |
|
|
const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit, |
354 |
|
|
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit, |
355 |
|
|
const Eigen::MatrixBase<ConfigOut_t> & qout) const |
356 |
|
|
{ |
357 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t , ConfigVector_t); |
358 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t , ConfigVector_t); |
359 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigOut_t, ConfigVector_t); |
360 |
|
255484 |
derived ().randomConfiguration_impl(lower_pos_limit.derived(), |
361 |
|
|
upper_pos_limit.derived(), |
362 |
|
127742 |
PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout)); |
363 |
|
127740 |
} |
364 |
|
|
|
365 |
|
|
template <class Derived> |
366 |
|
|
template <class ConfigL_t, class ConfigR_t, class Tangent_t> |
367 |
|
138763 |
void LieGroupBase<Derived>::difference( |
368 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
369 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
370 |
|
|
const Eigen::MatrixBase<Tangent_t> & d) const |
371 |
|
|
{ |
372 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t); |
373 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t); |
374 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(Tangent_t, TangentVector_t); |
375 |
|
138763 |
derived().difference_impl(q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(Tangent_t,d)); |
376 |
|
138763 |
} |
377 |
|
|
|
378 |
|
|
template <class Derived> |
379 |
|
|
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t> |
380 |
|
1988 |
void LieGroupBase<Derived>::dDifference(const Eigen::MatrixBase<ConfigL_t> & q0, |
381 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
382 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & J) const |
383 |
|
|
{ |
384 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t); |
385 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t); |
386 |
|
|
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(JacobianOut_t, JacobianMatrix_t); |
387 |
|
|
PINOCCHIO_STATIC_ASSERT(arg==ARG0||arg==ARG1, arg_SHOULD_BE_ARG0_OR_ARG1); |
388 |
|
1988 |
derived().template dDifference_impl<arg> (q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J)); |
389 |
|
1988 |
} |
390 |
|
|
|
391 |
|
|
template <class Derived> |
392 |
|
|
template<class ConfigL_t, class ConfigR_t, class JacobianOut_t> |
393 |
|
220 |
void LieGroupBase<Derived>::dDifference(const Eigen::MatrixBase<ConfigL_t> & q0, |
394 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
395 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & J, |
396 |
|
|
const ArgumentPosition arg) const |
397 |
|
|
{ |
398 |
✓✓✗✓
|
220 |
assert((arg==ARG0||arg==ARG1) && "arg should be either ARG0 or ARG1"); |
399 |
|
|
|
400 |
✓✓✗ |
220 |
switch (arg) |
401 |
|
|
{ |
402 |
|
110 |
case ARG0: |
403 |
|
110 |
dDifference<ARG0>(q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J)); |
404 |
|
110 |
return; |
405 |
|
110 |
case ARG1: |
406 |
|
110 |
dDifference<ARG1>(q0.derived(), q1.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J)); |
407 |
|
110 |
return; |
408 |
|
|
default: |
409 |
|
|
return; |
410 |
|
|
} |
411 |
|
|
} |
412 |
|
|
|
413 |
|
|
template <class Derived> |
414 |
|
|
template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t> |
415 |
|
|
void LieGroupBase<Derived>::dDifference(const Eigen::MatrixBase<ConfigL_t> & q0, |
416 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
417 |
|
|
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
418 |
|
|
int self, |
419 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & Jout, |
420 |
|
|
const AssignmentOperatorType op) const |
421 |
|
|
{ |
422 |
|
|
PINOCCHIO_UNUSED_VARIABLE(self); |
423 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t); |
424 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t); |
425 |
|
|
assert(Jin.cols() == nv()); |
426 |
|
|
assert(Jout.cols() == nv()); |
427 |
|
|
assert(Jout.rows() == Jin.rows()); |
428 |
|
|
derived().template dDifference_product_impl<arg>( |
429 |
|
|
q0.derived(), q1.derived(), |
430 |
|
|
Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout), |
431 |
|
|
false, op); |
432 |
|
|
} |
433 |
|
|
|
434 |
|
|
template <class Derived> |
435 |
|
|
template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t> |
436 |
|
|
void LieGroupBase<Derived>::dDifference(const Eigen::MatrixBase<ConfigL_t> & q0, |
437 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
438 |
|
|
int self, |
439 |
|
|
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
440 |
|
|
const Eigen::MatrixBase<JacobianOut_t> & Jout, |
441 |
|
|
const AssignmentOperatorType op) const |
442 |
|
|
{ |
443 |
|
|
PINOCCHIO_UNUSED_VARIABLE(self); |
444 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t); |
445 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t); |
446 |
|
|
assert(Jin.rows() == nv()); |
447 |
|
|
assert(Jout.rows() == nv()); |
448 |
|
|
assert(Jout.cols() == Jin.cols()); |
449 |
|
|
derived().template dDifference_product_impl<arg>( |
450 |
|
|
q0.derived(), q1.derived(), |
451 |
|
|
Jin.derived(), PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout), |
452 |
|
|
true, op); |
453 |
|
|
} |
454 |
|
|
|
455 |
|
|
template <class Derived> |
456 |
|
|
template <class ConfigL_t, class ConfigR_t> |
457 |
|
|
typename LieGroupBase<Derived>::Scalar |
458 |
|
28848 |
LieGroupBase<Derived>::squaredDistance( |
459 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
460 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1) const |
461 |
|
|
{ |
462 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t); |
463 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t); |
464 |
|
28848 |
return derived().squaredDistance_impl(q0.derived(), q1.derived()); |
465 |
|
|
} |
466 |
|
|
|
467 |
|
|
template <class Derived> |
468 |
|
|
template <class ConfigL_t, class ConfigR_t> |
469 |
|
|
typename LieGroupBase<Derived>::Scalar |
470 |
|
28694 |
LieGroupBase<Derived>::distance( |
471 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
472 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1) const |
473 |
|
|
{ |
474 |
|
28694 |
return sqrt(squaredDistance(q0.derived(), q1.derived())); |
475 |
|
|
} |
476 |
|
|
|
477 |
|
|
template <class Derived> |
478 |
|
|
template <class ConfigL_t, class ConfigR_t> |
479 |
|
146 |
bool LieGroupBase<Derived>::isSameConfiguration( |
480 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
481 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
482 |
|
|
const Scalar & prec) const |
483 |
|
|
{ |
484 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigL_t, ConfigVector_t); |
485 |
|
|
EIGEN_STATIC_ASSERT_SAME_VECTOR_SIZE(ConfigR_t, ConfigVector_t); |
486 |
|
146 |
return derived().isSameConfiguration_impl(q0.derived(), q1.derived(), prec); |
487 |
|
|
} |
488 |
|
|
|
489 |
|
|
// ----------------- API that allocates memory ---------------------------- // |
490 |
|
|
|
491 |
|
|
|
492 |
|
|
template <class Derived> |
493 |
|
|
template <class Config_t, class Tangent_t> |
494 |
|
|
typename LieGroupBase<Derived>::ConfigVector_t |
495 |
|
89472 |
LieGroupBase<Derived>::integrate(const Eigen::MatrixBase<Config_t> & q, |
496 |
|
|
const Eigen::MatrixBase<Tangent_t> & v) const |
497 |
|
|
{ |
498 |
✓✗ |
89472 |
ConfigVector_t qout(nq()); |
499 |
✗✗ |
89472 |
integrate(q.derived(), v.derived(), qout); |
500 |
|
89472 |
return qout; |
501 |
|
|
} |
502 |
|
|
|
503 |
|
|
template <class Derived> |
504 |
|
|
template <class ConfigL_t, class ConfigR_t> |
505 |
|
83920 |
typename LieGroupBase<Derived>::ConfigVector_t LieGroupBase<Derived>::interpolate( |
506 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
507 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
508 |
|
|
const Scalar & u) const |
509 |
|
|
{ |
510 |
✓✗ |
83920 |
ConfigVector_t qout(nq()); |
511 |
✗✗ |
83920 |
interpolate(q0.derived(), q1.derived(), u, qout); |
512 |
|
83920 |
return qout; |
513 |
|
|
} |
514 |
|
|
|
515 |
|
|
template <class Derived> |
516 |
|
|
typename LieGroupBase<Derived>::ConfigVector_t |
517 |
|
1950 |
LieGroupBase<Derived>::random() const |
518 |
|
|
{ |
519 |
✓✗ |
1950 |
ConfigVector_t qout(nq()); |
520 |
✓✗ |
1950 |
random(qout); |
521 |
|
1950 |
return qout; |
522 |
|
|
} |
523 |
|
|
|
524 |
|
|
template <class Derived> |
525 |
|
|
template <class ConfigL_t, class ConfigR_t> |
526 |
|
|
typename LieGroupBase<Derived>::ConfigVector_t |
527 |
|
86188 |
LieGroupBase<Derived>::randomConfiguration |
528 |
|
|
(const Eigen::MatrixBase<ConfigL_t> & lower_pos_limit, |
529 |
|
|
const Eigen::MatrixBase<ConfigR_t> & upper_pos_limit) const |
530 |
|
|
{ |
531 |
✓✗ |
86188 |
ConfigVector_t qout(nq()); |
532 |
✗✗ |
86188 |
randomConfiguration(lower_pos_limit.derived(), upper_pos_limit.derived(), qout); |
533 |
|
86188 |
return qout; |
534 |
|
|
} |
535 |
|
|
|
536 |
|
|
template <class Derived> |
537 |
|
|
template <class ConfigL_t, class ConfigR_t> |
538 |
|
106501 |
typename LieGroupBase<Derived>::TangentVector_t LieGroupBase<Derived>::difference( |
539 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
540 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1) const |
541 |
|
|
{ |
542 |
✓✗ |
106501 |
TangentVector_t diff(nv()); |
543 |
✗✗ |
106501 |
difference(q0.derived(), q1.derived(), diff); |
544 |
|
106501 |
return diff; |
545 |
|
|
} |
546 |
|
|
|
547 |
|
|
// ----------------- Default implementations ------------------------------ // |
548 |
|
|
template <class Derived> |
549 |
|
|
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
550 |
|
|
void LieGroupBase<Derived>::dIntegrate_product_impl( |
551 |
|
|
const Config_t & q, |
552 |
|
|
const Tangent_t & v, |
553 |
|
|
const JacobianIn_t & Jin, |
554 |
|
|
JacobianOut_t & Jout, |
555 |
|
|
bool dIntegrateOnTheLeft, |
556 |
|
|
const ArgumentPosition arg, |
557 |
|
|
const AssignmentOperatorType op) const |
558 |
|
|
{ |
559 |
|
|
Index nv_ (nv()); |
560 |
|
|
JacobianMatrix_t J (nv_, nv_); |
561 |
|
|
dIntegrate(q, v, J, arg); |
562 |
|
|
switch (op) { |
563 |
|
|
case SETTO: |
564 |
|
|
if(dIntegrateOnTheLeft) Jout = J * Jin; |
565 |
|
|
else Jout = Jin * J; |
566 |
|
|
return; |
567 |
|
|
case ADDTO: |
568 |
|
|
if(dIntegrateOnTheLeft) Jout += J * Jin; |
569 |
|
|
else Jout += Jin * J; |
570 |
|
|
return; |
571 |
|
|
case RMTO: |
572 |
|
|
if(dIntegrateOnTheLeft) Jout -= J * Jin; |
573 |
|
|
else Jout -= Jin * J; |
574 |
|
|
return; |
575 |
|
|
} |
576 |
|
|
} |
577 |
|
|
|
578 |
|
|
template <class Derived> |
579 |
|
|
template <ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianIn_t, class JacobianOut_t> |
580 |
|
|
void LieGroupBase<Derived>::dDifference_product_impl(const ConfigL_t & q0, |
581 |
|
|
const ConfigR_t & q1, |
582 |
|
|
const JacobianIn_t & Jin, |
583 |
|
|
JacobianOut_t & Jout, |
584 |
|
|
bool dDifferenceOnTheLeft, |
585 |
|
|
const AssignmentOperatorType op) const |
586 |
|
|
{ |
587 |
|
|
Index nv_ (nv()); |
588 |
|
|
JacobianMatrix_t J (nv_, nv_); |
589 |
|
|
dDifference<arg>(q0, q1, J); |
590 |
|
|
switch (op) { |
591 |
|
|
case SETTO: |
592 |
|
|
if(dDifferenceOnTheLeft) Jout = J * Jin; |
593 |
|
|
else Jout = Jin * J; |
594 |
|
|
return; |
595 |
|
|
case ADDTO: |
596 |
|
|
if(dDifferenceOnTheLeft) Jout += J * Jin; |
597 |
|
|
else Jout += Jin * J; |
598 |
|
|
return; |
599 |
|
|
case RMTO: |
600 |
|
|
if(dDifferenceOnTheLeft) Jout -= J * Jin; |
601 |
|
|
else Jout -= Jin * J; |
602 |
|
|
return; |
603 |
|
|
} |
604 |
|
|
} |
605 |
|
|
|
606 |
|
|
template <class Derived> |
607 |
|
|
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t> |
608 |
|
59473 |
void LieGroupBase<Derived>::interpolate_impl( |
609 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
610 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
611 |
|
|
const Scalar & u, |
612 |
|
|
const Eigen::MatrixBase<ConfigOut_t> & qout) const |
613 |
|
|
{ |
614 |
✓✓ |
59473 |
if (u == 0) PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout) = q0; |
615 |
✓✓ |
38977 |
else if(u == 1) PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout) = q1; |
616 |
|
|
else |
617 |
|
|
{ |
618 |
✓✗✓✗
✓✗ |
18481 |
TangentVector_t vdiff(u * difference(q0, q1)); |
619 |
✓✗ |
18481 |
integrate(q0.derived(), vdiff, PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout)); |
620 |
|
|
} |
621 |
|
59473 |
} |
622 |
|
|
|
623 |
|
|
template <class Derived> |
624 |
|
|
template <class ConfigL_t, class ConfigR_t> |
625 |
|
|
typename LieGroupBase<Derived>::Scalar |
626 |
|
24682 |
LieGroupBase<Derived>::squaredDistance_impl( |
627 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
628 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1) const |
629 |
|
|
{ |
630 |
✓✓✗✓
✗ |
24682 |
TangentVector_t t(nv()); |
631 |
✓✗ |
24682 |
difference(q0.derived(), q1.derived(), t); |
632 |
✓✗ |
24694 |
return t.squaredNorm(); |
633 |
|
|
} |
634 |
|
|
|
635 |
|
|
template <class Derived> |
636 |
|
|
template <class ConfigL_t, class ConfigR_t> |
637 |
|
74 |
bool LieGroupBase<Derived>::isSameConfiguration_impl( |
638 |
|
|
const Eigen::MatrixBase<ConfigL_t> & q0, |
639 |
|
|
const Eigen::MatrixBase<ConfigR_t> & q1, |
640 |
|
|
const Scalar & prec) const |
641 |
|
|
{ |
642 |
|
74 |
return q0.isApprox(q1, prec); |
643 |
|
|
} |
644 |
|
|
|
645 |
|
|
template <class Derived> |
646 |
|
|
typename LieGroupBase <Derived>::Index |
647 |
|
262109 |
LieGroupBase <Derived>::nq () const |
648 |
|
|
{ |
649 |
|
262109 |
return derived().nq(); |
650 |
|
|
} |
651 |
|
|
|
652 |
|
|
template <class Derived> |
653 |
|
|
typename LieGroupBase <Derived>::Index |
654 |
|
131666 |
LieGroupBase <Derived>::nv () const |
655 |
|
|
{ |
656 |
|
131666 |
return derived().nv(); |
657 |
|
|
} |
658 |
|
|
|
659 |
|
|
template <class Derived> |
660 |
|
|
typename LieGroupBase <Derived>::ConfigVector_t |
661 |
|
90 |
LieGroupBase <Derived>::neutral () const |
662 |
|
|
{ |
663 |
|
90 |
return derived().neutral(); |
664 |
|
|
} |
665 |
|
|
|
666 |
|
|
template <class Derived> |
667 |
|
37 |
std::string LieGroupBase <Derived>::name () const |
668 |
|
|
{ |
669 |
|
37 |
return derived().name(); |
670 |
|
|
} |
671 |
|
|
|
672 |
|
|
} // namespace pinocchio |
673 |
|
|
|
674 |
|
|
#endif // __pinocchio_multibody_liegroup_liegroup_operation_base_hxx__ |