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