Directory: | ./ |
---|---|
File: | include/pinocchio/multibody/liegroup/special-orthogonal.hpp |
Date: | 2024-08-27 18:20:05 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 146 | 273 | 53.5% |
Branches: | 109 | 434 | 25.1% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | // | ||
2 | // Copyright (c) 2016-2021 CNRS INRIA | ||
3 | // | ||
4 | |||
5 | #ifndef __pinocchio_multibody_liegroup_special_orthogonal_operation_hpp__ | ||
6 | #define __pinocchio_multibody_liegroup_special_orthogonal_operation_hpp__ | ||
7 | |||
8 | #include <limits> | ||
9 | |||
10 | #include "pinocchio/spatial/explog.hpp" | ||
11 | #include "pinocchio/math/quaternion.hpp" | ||
12 | #include "pinocchio/multibody/liegroup/liegroup-base.hpp" | ||
13 | #include "pinocchio/utils/static-if.hpp" | ||
14 | |||
15 | namespace pinocchio | ||
16 | { | ||
17 | template<int Dim, typename Scalar, int Options = 0> | ||
18 | struct SpecialOrthogonalOperationTpl | ||
19 | { | ||
20 | }; | ||
21 | |||
22 | template<int Dim, typename Scalar, int Options> | ||
23 | struct traits<SpecialOrthogonalOperationTpl<Dim, Scalar, Options>> | ||
24 | { | ||
25 | }; | ||
26 | |||
27 | template<typename _Scalar, int _Options> | ||
28 | struct traits<SpecialOrthogonalOperationTpl<2, _Scalar, _Options>> | ||
29 | { | ||
30 | typedef _Scalar Scalar; | ||
31 | enum | ||
32 | { | ||
33 | Options = _Options, | ||
34 | NQ = 2, | ||
35 | NV = 1 | ||
36 | }; | ||
37 | }; | ||
38 | |||
39 | template<typename _Scalar, int _Options> | ||
40 | struct traits<SpecialOrthogonalOperationTpl<3, _Scalar, _Options>> | ||
41 | { | ||
42 | typedef _Scalar Scalar; | ||
43 | enum | ||
44 | { | ||
45 | Options = _Options, | ||
46 | NQ = 4, | ||
47 | NV = 3 | ||
48 | }; | ||
49 | }; | ||
50 | |||
51 | template<typename _Scalar, int _Options> | ||
52 | struct SpecialOrthogonalOperationTpl<2, _Scalar, _Options> | ||
53 | : public LieGroupBase<SpecialOrthogonalOperationTpl<2, _Scalar, _Options>> | ||
54 | { | ||
55 | PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(SpecialOrthogonalOperationTpl); | ||
56 | typedef Eigen::Matrix<Scalar, 2, 2> Matrix2; | ||
57 | typedef typename Eigen::NumTraits<Scalar>::Real RealScalar; | ||
58 | |||
59 | template<typename Matrix2Like> | ||
60 | 136 | static typename Matrix2Like::Scalar log(const Eigen::MatrixBase<Matrix2Like> & R) | |
61 | { | ||
62 | typedef typename Matrix2Like::Scalar Scalar; | ||
63 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2); | ||
64 | |||
65 |
1/2✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
|
136 | const Scalar tr = R.trace(); |
66 | |||
67 |
3/4✓ Branch 0 taken 1 times.
✓ Branch 1 taken 135 times.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
136 | static const Scalar PI_value = PI<Scalar>(); |
68 | |||
69 | using internal::if_then_else; | ||
70 | 272 | Scalar theta = if_then_else( | |
71 | ✗ | internal::GT, tr, Scalar(2), | |
72 | ✗ | Scalar(0), // then | |
73 |
1/2✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
|
136 | if_then_else( |
74 | ✗ | internal::LT, tr, Scalar(-2), | |
75 |
1/2✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
|
136 | if_then_else( |
76 |
1/2✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
|
136 | internal::GE, R(1, 0), Scalar(0), PI_value, |
77 | ✗ | static_cast<Scalar>(-PI_value)), // then | |
78 |
1/2✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
|
136 | if_then_else( |
79 | internal::GT, tr, | ||
80 | ✗ | static_cast<Scalar>(Scalar(2) - Scalar(1e-2)), // TODO: change value | |
81 |
2/4✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 136 times.
✗ Branch 5 not taken.
|
136 | static_cast<Scalar>(asin((R(1, 0) - R(0, 1)) / Scalar(2))), // then |
82 |
1/2✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
|
136 | if_then_else( |
83 |
1/2✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
|
136 | internal::GE, R(1, 0), Scalar(0), |
84 | ✗ | static_cast<Scalar>(acos(tr / Scalar(2))), // then | |
85 |
1/2✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
|
136 | static_cast<Scalar>(-acos(tr / Scalar(2))))))); |
86 | |||
87 | // const bool pos = (R (1, 0) > Scalar(0)); | ||
88 | // if (tr > Scalar(2)) theta = Scalar(0); // acos((3-1)/2) | ||
89 | // else if (tr < Scalar(-2)) theta = (pos ? PI_value : -PI_value); // acos((-1-1)/2) | ||
90 | // Around 0, asin is numerically more stable than acos because | ||
91 | // acos(x) = PI/2 - x and asin(x) = x (the precision of x is not lost in PI/2). | ||
92 | // else if (tr > Scalar(2) - 1e-2) theta = asin ((R(1,0) - R(0,1)) / Scalar(2)); | ||
93 | // else theta = (pos ? acos (tr/Scalar(2)) : -acos(tr/Scalar(2))); | ||
94 |
1/4✓ Branch 1 taken 136 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
|
136 | assert(check_expression_if_real<Scalar>(theta == theta) && "theta is NaN"); // theta != NaN |
95 | // assert ((cos (theta) * R(0,0) + sin (theta) * R(1,0) > 0) && | ||
96 | // (cos (theta) * R(1,0) - sin (theta) * R(0,0) < 1e-6)); | ||
97 | 136 | return theta; | |
98 | } | ||
99 | |||
100 | template<typename Matrix2Like> | ||
101 | ✗ | static typename Matrix2Like::Scalar Jlog(const Eigen::MatrixBase<Matrix2Like> &) | |
102 | { | ||
103 | typedef typename Matrix2Like::Scalar Scalar; | ||
104 | EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix2Like, 2, 2); | ||
105 | ✗ | return Scalar(1); | |
106 | } | ||
107 | |||
108 | /// Get dimension of Lie Group vector representation | ||
109 | /// | ||
110 | /// For instance, for SO(3), the dimension of the vector representation is | ||
111 | /// 4 (quaternion) while the dimension of the tangent space is 3. | ||
112 | ✗ | static Index nq() | |
113 | { | ||
114 | ✗ | return NQ; | |
115 | } | ||
116 | |||
117 | /// Get dimension of Lie Group tangent space | ||
118 | ✗ | static Index nv() | |
119 | { | ||
120 | ✗ | return NV; | |
121 | } | ||
122 | |||
123 | 1 | static ConfigVector_t neutral() | |
124 | { | ||
125 | 1 | ConfigVector_t n; | |
126 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | n << Scalar(1), Scalar(0); |
127 | 1 | return n; | |
128 | } | ||
129 | |||
130 | ✗ | static std::string name() | |
131 | { | ||
132 | ✗ | return std::string("SO(2)"); | |
133 | } | ||
134 | |||
135 | template<class ConfigL_t, class ConfigR_t, class Tangent_t> | ||
136 | ✗ | static void difference_impl( | |
137 | const Eigen::MatrixBase<ConfigL_t> & q0, | ||
138 | const Eigen::MatrixBase<ConfigR_t> & q1, | ||
139 | const Eigen::MatrixBase<Tangent_t> & d) | ||
140 | { | ||
141 | ✗ | Matrix2 R; // R0.transpose() * R1; | |
142 | ✗ | R(0, 0) = R(1, 1) = q0.dot(q1); | |
143 | ✗ | R(1, 0) = q0(0) * q1(1) - q0(1) * q1(0); | |
144 | ✗ | R(0, 1) = -R(1, 0); | |
145 | ✗ | PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d)[0] = log(R); | |
146 | } | ||
147 | |||
148 | template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t> | ||
149 | ✗ | static void dDifference_impl( | |
150 | const Eigen::MatrixBase<ConfigL_t> & q0, | ||
151 | const Eigen::MatrixBase<ConfigR_t> & q1, | ||
152 | const Eigen::MatrixBase<JacobianOut_t> & J) | ||
153 | { | ||
154 | ✗ | Matrix2 R; // R0.transpose() * R1; | |
155 | ✗ | R(0, 0) = R(1, 1) = q0.dot(q1); | |
156 | ✗ | R(1, 0) = q0(0) * q1(1) - q0(1) * q1(0); | |
157 | ✗ | R(0, 1) = -R(1, 0); | |
158 | |||
159 | ✗ | Scalar w(Jlog(R)); | |
160 | ✗ | PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).coeffRef(0, 0) = ((arg == ARG0) ? -w : w); | |
161 | } | ||
162 | |||
163 | template<class ConfigIn_t, class Velocity_t, class ConfigOut_t> | ||
164 | ✗ | static void integrate_impl( | |
165 | const Eigen::MatrixBase<ConfigIn_t> & q, | ||
166 | const Eigen::MatrixBase<Velocity_t> & v, | ||
167 | const Eigen::MatrixBase<ConfigOut_t> & qout) | ||
168 | { | ||
169 | ✗ | ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout); | |
170 | |||
171 | ✗ | const Scalar ca = q(0); | |
172 | ✗ | const Scalar sa = q(1); | |
173 | ✗ | const Scalar & omega = v(0); | |
174 | |||
175 | Scalar cosOmega, sinOmega; | ||
176 | ✗ | SINCOS(omega, &sinOmega, &cosOmega); | |
177 | // TODO check the cost of atan2 vs SINCOS | ||
178 | |||
179 | ✗ | out << cosOmega * ca - sinOmega * sa, sinOmega * ca + cosOmega * sa; | |
180 | // First order approximation of the normalization of the unit complex | ||
181 | // See quaternion::firstOrderNormalize for equations. | ||
182 | ✗ | const Scalar norm2 = out.squaredNorm(); | |
183 | ✗ | out *= (3 - norm2) / 2; | |
184 | ✗ | assert(pinocchio::isNormalized(out)); | |
185 | } | ||
186 | |||
187 | template<class Config_t, class Jacobian_t> | ||
188 | ✗ | static void integrateCoeffWiseJacobian_impl( | |
189 | const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J) | ||
190 | { | ||
191 | ✗ | assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension"); | |
192 | ✗ | Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J); | |
193 | ✗ | Jout << -q[1], q[0]; | |
194 | } | ||
195 | |||
196 | template<class Config_t, class Tangent_t, class JacobianOut_t> | ||
197 | ✗ | static void dIntegrate_dq_impl( | |
198 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
199 | const Eigen::MatrixBase<Tangent_t> & /*v*/, | ||
200 | const Eigen::MatrixBase<JacobianOut_t> & J, | ||
201 | const AssignmentOperatorType op = SETTO) | ||
202 | { | ||
203 | ✗ | JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J); | |
204 | ✗ | switch (op) | |
205 | { | ||
206 | ✗ | case SETTO: | |
207 | ✗ | Jout(0, 0) = Scalar(1); | |
208 | ✗ | break; | |
209 | ✗ | case ADDTO: | |
210 | ✗ | Jout(0, 0) += Scalar(1); | |
211 | ✗ | break; | |
212 | ✗ | case RMTO: | |
213 | ✗ | Jout(0, 0) -= Scalar(1); | |
214 | ✗ | break; | |
215 | ✗ | default: | |
216 | ✗ | assert(false && "Wrong Op requesed value"); | |
217 | break; | ||
218 | } | ||
219 | } | ||
220 | |||
221 | template<class Config_t, class Tangent_t, class JacobianOut_t> | ||
222 | ✗ | static void dIntegrate_dv_impl( | |
223 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
224 | const Eigen::MatrixBase<Tangent_t> & /*v*/, | ||
225 | const Eigen::MatrixBase<JacobianOut_t> & J, | ||
226 | const AssignmentOperatorType op = SETTO) | ||
227 | { | ||
228 | ✗ | JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J); | |
229 | ✗ | switch (op) | |
230 | { | ||
231 | ✗ | case SETTO: | |
232 | ✗ | Jout(0, 0) = Scalar(1); | |
233 | ✗ | break; | |
234 | ✗ | case ADDTO: | |
235 | ✗ | Jout(0, 0) += Scalar(1); | |
236 | ✗ | break; | |
237 | ✗ | case RMTO: | |
238 | ✗ | Jout(0, 0) -= Scalar(1); | |
239 | ✗ | break; | |
240 | ✗ | default: | |
241 | ✗ | assert(false && "Wrong Op requesed value"); | |
242 | break; | ||
243 | } | ||
244 | } | ||
245 | |||
246 | template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> | ||
247 | ✗ | static void dIntegrateTransport_dq_impl( | |
248 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
249 | const Eigen::MatrixBase<Tangent_t> & /*v*/, | ||
250 | const Eigen::MatrixBase<JacobianIn_t> & Jin, | ||
251 | const Eigen::MatrixBase<JacobianOut_t> & Jout) | ||
252 | { | ||
253 | ✗ | PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout) = Jin; | |
254 | } | ||
255 | |||
256 | template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> | ||
257 | ✗ | static void dIntegrateTransport_dv_impl( | |
258 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
259 | const Eigen::MatrixBase<Tangent_t> & /*v*/, | ||
260 | const Eigen::MatrixBase<JacobianIn_t> & Jin, | ||
261 | const Eigen::MatrixBase<JacobianOut_t> & Jout) | ||
262 | { | ||
263 | ✗ | PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout) = Jin; | |
264 | } | ||
265 | |||
266 | template<class Config_t, class Tangent_t, class Jacobian_t> | ||
267 | ✗ | static void dIntegrateTransport_dq_impl( | |
268 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
269 | const Eigen::MatrixBase<Tangent_t> & /*v*/, | ||
270 | const Eigen::MatrixBase<Jacobian_t> & /*J*/) | ||
271 | { | ||
272 | } | ||
273 | |||
274 | template<class Config_t, class Tangent_t, class Jacobian_t> | ||
275 | ✗ | static void dIntegrateTransport_dv_impl( | |
276 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
277 | const Eigen::MatrixBase<Tangent_t> & /*v*/, | ||
278 | const Eigen::MatrixBase<Jacobian_t> & /*J*/) | ||
279 | { | ||
280 | } | ||
281 | |||
282 | template<class ConfigL_t, class ConfigR_t, class ConfigOut_t> | ||
283 | ✗ | static void interpolate_impl( | |
284 | const Eigen::MatrixBase<ConfigL_t> & q0, | ||
285 | const Eigen::MatrixBase<ConfigR_t> & q1, | ||
286 | const Scalar & u, | ||
287 | const Eigen::MatrixBase<ConfigOut_t> & qout) | ||
288 | { | ||
289 | ✗ | ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout); | |
290 | |||
291 | ✗ | assert( | |
292 | check_expression_if_real<Scalar>(abs(q0.norm() - 1) < 1e-8) | ||
293 | && "initial configuration not normalized"); | ||
294 | ✗ | assert( | |
295 | check_expression_if_real<Scalar>(abs(q1.norm() - 1) < 1e-8) | ||
296 | && "final configuration not normalized"); | ||
297 | ✗ | const Scalar cosTheta = q0.dot(q1); | |
298 | ✗ | const Scalar sinTheta = q0(0) * q1(1) - q0(1) * q1(0); | |
299 | ✗ | const Scalar theta = atan2(sinTheta, cosTheta); | |
300 | ✗ | assert(check_expression_if_real<Scalar>(fabs(sin(theta) - sinTheta) < 1e-8)); | |
301 | |||
302 | ✗ | static const Scalar PI_value = PI<Scalar>(); | |
303 | ✗ | static const Scalar PI_value_lower = PI_value - static_cast<Scalar>(1e-6); | |
304 | using namespace internal; | ||
305 | |||
306 | // const Scalar theta0 = atan2(q0(1), q0(0)); | ||
307 | ✗ | const Scalar abs_theta = fabs(theta); | |
308 | ✗ | out[0] = if_then_else( | |
309 | ✗ | LT, abs_theta, static_cast<Scalar>(1e-6), | |
310 | ✗ | static_cast<Scalar>((Scalar(1) - u) * q0[0] + u * q1[0]), // then | |
311 | ✗ | if_then_else( | |
312 | LT, abs_theta, PI_value_lower, // else | ||
313 | ✗ | static_cast<Scalar>( | |
314 | ✗ | (sin((Scalar(1) - u) * theta) / sinTheta) * q0[0] | |
315 | ✗ | + (sin(u * theta) / sinTheta) * q1[0]), // then | |
316 | ✗ | q0(0) // cos(theta0) // else | |
317 | )); | ||
318 | |||
319 | ✗ | out[1] = if_then_else( | |
320 | ✗ | LT, abs_theta, static_cast<Scalar>(1e-6), | |
321 | ✗ | static_cast<Scalar>((Scalar(1) - u) * q0[1] + u * q1[1]), // then | |
322 | ✗ | if_then_else( | |
323 | LT, abs_theta, PI_value_lower, // else | ||
324 | ✗ | static_cast<Scalar>( | |
325 | ✗ | (sin((Scalar(1) - u) * theta) / sinTheta) * q0[1] | |
326 | ✗ | + (sin(u * theta) / sinTheta) * q1[1]), // then | |
327 | ✗ | q0(1) // sin(theta0) // else | |
328 | )); | ||
329 | } | ||
330 | |||
331 | template<class Config_t> | ||
332 | ✗ | static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) | |
333 | { | ||
334 | ✗ | pinocchio::normalize(qout.const_cast_derived()); | |
335 | } | ||
336 | |||
337 | template<class Config_t> | ||
338 | ✗ | static bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin, const Scalar & prec) | |
339 | { | ||
340 | ✗ | const Scalar norm = qin.norm(); | |
341 | using std::abs; | ||
342 | ✗ | return abs(norm - Scalar(1.0)) < prec; | |
343 | } | ||
344 | |||
345 | template<class Config_t> | ||
346 | 10 | static void random_impl(const Eigen::MatrixBase<Config_t> & qout) | |
347 | { | ||
348 | 10 | Config_t & out = PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout); | |
349 | |||
350 |
3/4✓ Branch 0 taken 2 times.
✓ Branch 1 taken 3 times.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
10 | static const Scalar PI_value = PI<Scalar>(); |
351 | 10 | const Scalar angle = -PI_value + Scalar(2) * PI_value * ((Scalar)rand()) / RAND_MAX; | |
352 |
2/4✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5 times.
✗ Branch 5 not taken.
|
10 | SINCOS(angle, &out(1), &out(0)); |
353 | 10 | } | |
354 | |||
355 | template<class ConfigL_t, class ConfigR_t, class ConfigOut_t> | ||
356 | 14 | static void randomConfiguration_impl( | |
357 | const Eigen::MatrixBase<ConfigL_t> &, | ||
358 | const Eigen::MatrixBase<ConfigR_t> &, | ||
359 | const Eigen::MatrixBase<ConfigOut_t> & qout) | ||
360 | { | ||
361 | 14 | random_impl(qout); | |
362 | 14 | } | |
363 | }; // struct SpecialOrthogonalOperationTpl<2,_Scalar,_Options> | ||
364 | |||
365 | template<typename _Scalar, int _Options> | ||
366 | struct SpecialOrthogonalOperationTpl<3, _Scalar, _Options> | ||
367 | : public LieGroupBase<SpecialOrthogonalOperationTpl<3, _Scalar, _Options>> | ||
368 | { | ||
369 | PINOCCHIO_LIE_GROUP_TPL_PUBLIC_INTERFACE(SpecialOrthogonalOperationTpl); | ||
370 | |||
371 | typedef Eigen::Quaternion<Scalar> Quaternion_t; | ||
372 | typedef Eigen::Map<Quaternion_t> QuaternionMap_t; | ||
373 | typedef Eigen::Map<const Quaternion_t> ConstQuaternionMap_t; | ||
374 | typedef SE3Tpl<Scalar, Options> SE3; | ||
375 | typedef typename Eigen::NumTraits<Scalar>::Real RealScalar; | ||
376 | |||
377 | /// Get dimension of Lie Group vector representation | ||
378 | /// | ||
379 | /// For instance, for SO(3), the dimension of the vector representation is | ||
380 | /// 4 (quaternion) while the dimension of the tangent space is 3. | ||
381 | 6978 | static Index nq() | |
382 | { | ||
383 | 6978 | return NQ; | |
384 | } | ||
385 | |||
386 | /// Get dimension of Lie Group tangent space | ||
387 | ✗ | static Index nv() | |
388 | { | ||
389 | ✗ | return NV; | |
390 | } | ||
391 | |||
392 | 4 | static ConfigVector_t neutral() | |
393 | { | ||
394 | 4 | ConfigVector_t n; | |
395 | 4 | n.setZero(); | |
396 | 4 | n[3] = Scalar(1); | |
397 | 4 | return n; | |
398 | } | ||
399 | |||
400 | ✗ | static std::string name() | |
401 | { | ||
402 | ✗ | return std::string("SO(3)"); | |
403 | } | ||
404 | |||
405 | template<class ConfigL_t, class ConfigR_t, class Tangent_t> | ||
406 | 260 | static void difference_impl( | |
407 | const Eigen::MatrixBase<ConfigL_t> & q0, | ||
408 | const Eigen::MatrixBase<ConfigR_t> & q1, | ||
409 | const Eigen::MatrixBase<Tangent_t> & d) | ||
410 | { | ||
411 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 130 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
260 | ConstQuaternionMap_t quat0(q0.derived().data()); |
412 |
2/4✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 130 times.
|
260 | assert(quaternion::isNormalized( |
413 | quat0, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
414 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 130 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
260 | ConstQuaternionMap_t quat1(q1.derived().data()); |
415 |
2/4✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 130 times.
|
260 | assert(quaternion::isNormalized( |
416 | quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
417 | |||
418 |
2/4✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 125 times.
✗ Branch 6 not taken.
|
260 | PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d) = |
419 |
2/4✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
|
260 | quaternion::log3(Quaternion_t(quat0.conjugate() * quat1)); |
420 | 260 | } | |
421 | |||
422 | template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t> | ||
423 | 12 | static void dDifference_impl( | |
424 | const Eigen::MatrixBase<ConfigL_t> & q0, | ||
425 | const Eigen::MatrixBase<ConfigR_t> & q1, | ||
426 | const Eigen::MatrixBase<JacobianOut_t> & J) | ||
427 | { | ||
428 | typedef typename SE3::Matrix3 Matrix3; | ||
429 | |||
430 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
12 | ConstQuaternionMap_t quat0(q0.derived().data()); |
431 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
|
12 | assert(quaternion::isNormalized( |
432 | quat0, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
433 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
12 | ConstQuaternionMap_t quat1(q1.derived().data()); |
434 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
|
12 | assert(quaternion::isNormalized( |
435 | quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
436 | |||
437 | // TODO: check whether the merge with 2.6.9 is correct | ||
438 |
2/4✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
|
12 | const Quaternion_t q = quat0.conjugate() * quat1; |
439 |
1/2✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
|
12 | const Matrix3 R = q.matrix(); |
440 | |||
441 | if (arg == ARG0) | ||
442 | { | ||
443 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | JacobianMatrix_t J1; |
444 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | Jlog3(R, J1); |
445 | |||
446 |
5/10✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 3 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 3 times.
✗ Branch 15 not taken.
|
6 | PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).noalias() = -J1 * R.transpose(); |
447 | } | ||
448 | else if (arg == ARG1) | ||
449 | { | ||
450 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | Jlog3(R, J); |
451 | } | ||
452 | /* | ||
453 | const Quaternion_t quat_diff = quat0.conjugate() * quat1; | ||
454 | |||
455 | if (arg == ARG0) { | ||
456 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
457 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED | ||
458 | JacobianMatrix_t J1; | ||
459 | quaternion::Jlog3(quat_diff, J1); | ||
460 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
461 | const Matrix3 R = (quat_diff).matrix(); | ||
462 | |||
463 | PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).noalias() = - J1 * R.transpose(); | ||
464 | } else if (arg == ARG1) { | ||
465 | quaternion::Jlog3(quat_diff, J); | ||
466 | } | ||
467 | */ | ||
468 | 12 | } | |
469 | |||
470 | template<class ConfigIn_t, class Velocity_t, class ConfigOut_t> | ||
471 | 442 | static void integrate_impl( | |
472 | const Eigen::MatrixBase<ConfigIn_t> & q, | ||
473 | const Eigen::MatrixBase<Velocity_t> & v, | ||
474 | const Eigen::MatrixBase<ConfigOut_t> & qout) | ||
475 | { | ||
476 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 221 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
442 | ConstQuaternionMap_t quat(q.derived().data()); |
477 |
2/4✓ Branch 1 taken 221 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 221 times.
|
442 | assert(quaternion::isNormalized( |
478 | quat, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
479 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 221 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
442 | QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).data()); |
480 | |||
481 |
1/2✓ Branch 1 taken 221 times.
✗ Branch 2 not taken.
|
442 | Quaternion_t pOmega; |
482 |
1/2✓ Branch 1 taken 221 times.
✗ Branch 2 not taken.
|
442 | quaternion::exp3(v, pOmega); |
483 |
2/4✓ Branch 1 taken 221 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 221 times.
✗ Branch 5 not taken.
|
442 | quat_map = quat * pOmega; |
484 |
1/2✓ Branch 1 taken 221 times.
✗ Branch 2 not taken.
|
442 | quaternion::firstOrderNormalize(quat_map); |
485 |
2/4✓ Branch 1 taken 221 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 221 times.
|
442 | assert(quaternion::isNormalized( |
486 | quat_map, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
487 | 442 | } | |
488 | |||
489 | template<class Config_t, class Jacobian_t> | ||
490 | ✗ | static void integrateCoeffWiseJacobian_impl( | |
491 | const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J) | ||
492 | { | ||
493 | ✗ | assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension"); | |
494 | |||
495 | typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType; | ||
496 | typedef typename SE3::Vector3 Vector3; | ||
497 | typedef typename SE3::Matrix3 Matrix3; | ||
498 | |||
499 | ✗ | ConstQuaternionMap_t quat_map(q.derived().data()); | |
500 | ✗ | assert(quaternion::isNormalized( | |
501 | quat_map, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
502 | |||
503 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
504 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED | ||
505 | Eigen::Matrix<Scalar, NQ, NV, JacobianPlainType::Options | Eigen::RowMajor> | ||
506 | ✗ | Jexp3QuatCoeffWise; | |
507 | |||
508 | Scalar theta; | ||
509 | ✗ | const Vector3 v = quaternion::log3(quat_map, theta); | |
510 | ✗ | quaternion::Jexp3CoeffWise(v, Jexp3QuatCoeffWise); | |
511 | ✗ | Matrix3 Jlog; | |
512 | ✗ | Jlog3(theta, v, Jlog); | |
513 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
514 | |||
515 | // if(quat_map.w() >= 0.) // comes from the log3 for quaternions which may change the | ||
516 | // sign. | ||
517 | ✗ | if (quat_map.coeffs()[3] >= 0.) // comes from the log3 for quaternions which may change the | |
518 | // sign. | ||
519 | ✗ | PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).noalias() = Jexp3QuatCoeffWise * Jlog; | |
520 | else | ||
521 | ✗ | PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).noalias() = -Jexp3QuatCoeffWise * Jlog; | |
522 | |||
523 | // Jexp3(quat_map,PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template | ||
524 | // topLeftCorner<NQ,NV>()); | ||
525 | } | ||
526 | |||
527 | template<class Config_t, class Tangent_t, class JacobianOut_t> | ||
528 | 11 | static void dIntegrate_dq_impl( | |
529 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
530 | const Eigen::MatrixBase<Tangent_t> & v, | ||
531 | const Eigen::MatrixBase<JacobianOut_t> & J, | ||
532 | const AssignmentOperatorType op = SETTO) | ||
533 | { | ||
534 | 11 | JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J); | |
535 |
3/4✓ Branch 0 taken 9 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
11 | switch (op) |
536 | { | ||
537 | 9 | case SETTO: | |
538 |
2/4✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 9 times.
✗ Branch 6 not taken.
|
9 | Jout = exp3(-v); |
539 | 9 | break; | |
540 | 1 | case ADDTO: | |
541 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | Jout += exp3(-v); |
542 | 1 | break; | |
543 | 1 | case RMTO: | |
544 |
2/4✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
|
1 | Jout -= exp3(-v); |
545 | 1 | break; | |
546 | ✗ | default: | |
547 | ✗ | assert(false && "Wrong Op requesed value"); | |
548 | break; | ||
549 | } | ||
550 | 11 | } | |
551 | |||
552 | template<class Config_t, class Tangent_t, class JacobianOut_t> | ||
553 | 11 | static void dIntegrate_dv_impl( | |
554 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
555 | const Eigen::MatrixBase<Tangent_t> & v, | ||
556 | const Eigen::MatrixBase<JacobianOut_t> & J, | ||
557 | const AssignmentOperatorType op = SETTO) | ||
558 | { | ||
559 |
3/4✓ Branch 0 taken 9 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
11 | switch (op) |
560 | { | ||
561 | 9 | case SETTO: | |
562 | 9 | Jexp3<SETTO>(v, J.derived()); | |
563 | 9 | break; | |
564 | 1 | case ADDTO: | |
565 | 1 | Jexp3<ADDTO>(v, J.derived()); | |
566 | 1 | break; | |
567 | 1 | case RMTO: | |
568 | 1 | Jexp3<RMTO>(v, J.derived()); | |
569 | 1 | break; | |
570 | ✗ | default: | |
571 | ✗ | assert(false && "Wrong Op requesed value"); | |
572 | break; | ||
573 | } | ||
574 | 11 | } | |
575 | |||
576 | template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> | ||
577 | 1 | static void dIntegrateTransport_dq_impl( | |
578 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
579 | const Eigen::MatrixBase<Tangent_t> & v, | ||
580 | const Eigen::MatrixBase<JacobianIn_t> & Jin, | ||
581 | const Eigen::MatrixBase<JacobianOut_t> & J_out) | ||
582 | { | ||
583 | typedef typename SE3::Matrix3 Matrix3; | ||
584 | 1 | JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out); | |
585 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | const Matrix3 Jtmp3 = exp3(-v); |
586 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | Jout.noalias() = Jtmp3 * Jin; |
587 | 1 | } | |
588 | |||
589 | template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> | ||
590 | 1 | static void dIntegrateTransport_dv_impl( | |
591 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
592 | const Eigen::MatrixBase<Tangent_t> & v, | ||
593 | const Eigen::MatrixBase<JacobianIn_t> & Jin, | ||
594 | const Eigen::MatrixBase<JacobianOut_t> & J_out) | ||
595 | { | ||
596 | typedef typename SE3::Matrix3 Matrix3; | ||
597 | 1 | JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out); | |
598 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
599 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED | ||
600 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Matrix3 Jtmp3; |
601 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Jexp3<SETTO>(v, Jtmp3); |
602 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
603 |
3/6✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
|
1 | Jout.noalias() = Jtmp3 * Jin; |
604 | 1 | } | |
605 | |||
606 | template<class Config_t, class Tangent_t, class Jacobian_t> | ||
607 | 1 | static void dIntegrateTransport_dq_impl( | |
608 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
609 | const Eigen::MatrixBase<Tangent_t> & v, | ||
610 | const Eigen::MatrixBase<Jacobian_t> & J_out) | ||
611 | { | ||
612 | typedef typename SE3::Matrix3 Matrix3; | ||
613 | 1 | Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J_out); | |
614 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | const Matrix3 Jtmp3 = exp3(-v); |
615 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | Jout = Jtmp3 * Jout; |
616 | 1 | } | |
617 | |||
618 | template<class Config_t, class Tangent_t, class Jacobian_t> | ||
619 | 1 | static void dIntegrateTransport_dv_impl( | |
620 | const Eigen::MatrixBase<Config_t> & /*q*/, | ||
621 | const Eigen::MatrixBase<Tangent_t> & v, | ||
622 | const Eigen::MatrixBase<Jacobian_t> & J_out) | ||
623 | { | ||
624 | typedef typename SE3::Matrix3 Matrix3; | ||
625 | 1 | Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J_out); | |
626 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
627 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED | ||
628 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Matrix3 Jtmp3; |
629 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Jexp3<SETTO>(v, Jtmp3); |
630 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
631 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | Jout = Jtmp3 * Jout; |
632 | 1 | } | |
633 | |||
634 | template<class ConfigL_t, class ConfigR_t, class ConfigOut_t> | ||
635 | 2 | static void interpolate_impl( | |
636 | const Eigen::MatrixBase<ConfigL_t> & q0, | ||
637 | const Eigen::MatrixBase<ConfigR_t> & q1, | ||
638 | const Scalar & u, | ||
639 | const Eigen::MatrixBase<ConfigOut_t> & qout) | ||
640 | { | ||
641 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
2 | ConstQuaternionMap_t quat0(q0.derived().data()); |
642 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
|
2 | assert(quaternion::isNormalized( |
643 | quat0, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
644 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
2 | ConstQuaternionMap_t quat1(q1.derived().data()); |
645 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
|
2 | assert(quaternion::isNormalized( |
646 | quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
647 | |||
648 |
1/2✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
|
2 | QuaternionMap_t quat_res(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).data()); |
649 | |||
650 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | TangentVector_t w; |
651 |
1/2✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
|
2 | difference_impl(q0, q1, w); |
652 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
|
2 | integrate_impl(q0, u * w, qout); |
653 |
2/4✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
|
2 | assert(quaternion::isNormalized( |
654 | quat_res, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
655 | 2 | } | |
656 | |||
657 | template<class ConfigL_t, class ConfigR_t> | ||
658 | 3 | static Scalar squaredDistance_impl( | |
659 | const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1) | ||
660 | { | ||
661 | PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH | ||
662 | PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED | ||
663 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | TangentVector_t t; |
664 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
3 | difference_impl(q0, q1, t); |
665 | PINOCCHIO_COMPILER_DIAGNOSTIC_POP | ||
666 |
1/2✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
|
6 | return t.squaredNorm(); |
667 | } | ||
668 | |||
669 | template<class Config_t> | ||
670 | 8 | static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) | |
671 | { | ||
672 | 8 | pinocchio::normalize(qout.const_cast_derived()); | |
673 | 8 | } | |
674 | |||
675 | template<class Config_t> | ||
676 | 7 | static bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin, const Scalar & prec) | |
677 | { | ||
678 | 7 | const Scalar norm = qin.norm(); | |
679 | using std::abs; | ||
680 | 7 | return abs(norm - Scalar(1.0)) < prec; | |
681 | } | ||
682 | |||
683 | template<class Config_t> | ||
684 | 4676 | static void random_impl(const Eigen::MatrixBase<Config_t> & qout) | |
685 | { | ||
686 |
1/5✗ Branch 2 not taken.
✓ Branch 3 taken 2338 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
|
4676 | QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout).data()); |
687 |
1/2✓ Branch 1 taken 2338 times.
✗ Branch 2 not taken.
|
4676 | quaternion::uniformRandom(quat_map); |
688 | |||
689 |
2/4✓ Branch 1 taken 2338 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 2338 times.
|
4676 | assert(quaternion::isNormalized( |
690 | quat_map, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
691 | 4676 | } | |
692 | |||
693 | template<class ConfigL_t, class ConfigR_t, class ConfigOut_t> | ||
694 | 4692 | static void randomConfiguration_impl( | |
695 | const Eigen::MatrixBase<ConfigL_t> &, | ||
696 | const Eigen::MatrixBase<ConfigR_t> &, | ||
697 | const Eigen::MatrixBase<ConfigOut_t> & qout) | ||
698 | { | ||
699 | 4692 | random_impl(qout); | |
700 | 4692 | } | |
701 | |||
702 | template<class ConfigL_t, class ConfigR_t> | ||
703 | 6 | static bool isSameConfiguration_impl( | |
704 | const Eigen::MatrixBase<ConfigL_t> & q0, | ||
705 | const Eigen::MatrixBase<ConfigR_t> & q1, | ||
706 | const Scalar & prec) | ||
707 | { | ||
708 | 6 | ConstQuaternionMap_t quat1(q0.derived().data()); | |
709 | 6 | assert(quaternion::isNormalized( | |
710 | quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
711 | 6 | ConstQuaternionMap_t quat2(q1.derived().data()); | |
712 | 6 | assert(quaternion::isNormalized( | |
713 | quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); | ||
714 | |||
715 | 12 | return quaternion::defineSameRotation(quat1, quat2, prec); | |
716 | } | ||
717 | }; // struct SpecialOrthogonalOperationTpl<3,_Scalar,_Options> | ||
718 | |||
719 | } // namespace pinocchio | ||
720 | |||
721 | #endif // ifndef __pinocchio_multibody_liegroup_special_orthogonal_operation_hpp__ | ||
722 |