| 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) - 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 |
void dDifference_impl(const Eigen::MatrixBase<ConfigL_t> & q0, |
|
147 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
148 |
const Eigen::MatrixBase<JacobianOut_t> & J) const |
||
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 (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 |
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) const |
||
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 |
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) const |
||
253 |
{ |
||
254 |
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin; |
||
255 |
} |
||
256 |
|||
257 |
template <class Config_t, class Tangent_t, class Jacobian_t> |
||
258 |
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
||
259 |
const Eigen::MatrixBase<Tangent_t> & /*v*/, |
||
260 |
const Eigen::MatrixBase<Jacobian_t> & /*J*/) const {} |
||
261 |
|||
262 |
template <class Config_t, class Tangent_t, class Jacobian_t> |
||
263 |
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
||
264 |
const Eigen::MatrixBase<Tangent_t> & /*v*/, |
||
265 |
const Eigen::MatrixBase<Jacobian_t> & /*J*/) const {} |
||
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 |
void random_impl (const Eigen::MatrixBase<Config_t>& qout) const |
|
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 |
void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &, |
|
330 |
const Eigen::MatrixBase<ConfigR_t> &, |
||
331 |
const Eigen::MatrixBase<ConfigOut_t> & qout) const |
||
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 |
void dDifference_impl (const Eigen::MatrixBase<ConfigL_t> & q0, |
|
391 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
392 |
const Eigen::MatrixBase<JacobianOut_t> & J) const |
||
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 |
✓✗ | 130 |
JacobianMatrix_t J1; |
405 |
✓✗ | 130 |
quaternion::Jlog3(quat_diff, J1); |
406 |
✓✗ | 130 |
const Matrix3 R = (quat_diff).matrix(); |
407 |
|||
408 |
✓✗✓✗ ✓✗✓✗ ✓✗ |
130 |
PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).noalias() = - J1 * R.transpose(); |
409 |
} else if (arg == ARG1) { |
||
410 |
✓✗ | 210 |
quaternion::Jlog3(quat_diff, J); |
411 |
} |
||
412 |
340 |
} |
|
413 |
|||
414 |
template <class ConfigIn_t, class Velocity_t, class ConfigOut_t> |
||
415 |
10939 |
static void integrate_impl(const Eigen::MatrixBase<ConfigIn_t> & q, |
|
416 |
const Eigen::MatrixBase<Velocity_t> & v, |
||
417 |
const Eigen::MatrixBase<ConfigOut_t> & qout) |
||
418 |
{ |
||
419 |
✓✓✗✓ ✗ |
10939 |
ConstQuaternionMap_t quat(q.derived().data()); |
420 |
✓✗✗✓ |
10939 |
assert(quaternion::isNormalized(quat,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
421 |
✓✓✗✓ ✗ |
10939 |
QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout).data()); |
422 |
|||
423 |
✓✗✓✗ |
10939 |
Quaternion_t pOmega; quaternion::exp3(v,pOmega); |
424 |
✓✗✓✗ |
10939 |
quat_map = quat * pOmega; |
425 |
✓✗ | 10939 |
quaternion::firstOrderNormalize(quat_map); |
426 |
✓✗✗✓ |
10939 |
assert(quaternion::isNormalized(quat_map,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
427 |
10939 |
} |
|
428 |
|||
429 |
template <class Config_t, class Jacobian_t> |
||
430 |
80 |
static void integrateCoeffWiseJacobian_impl(const Eigen::MatrixBase<Config_t> & q, |
|
431 |
const Eigen::MatrixBase<Jacobian_t> & J) |
||
432 |
{ |
||
433 |
✓✗✓✗ ✓✗✓✗ |
80 |
assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension"); |
434 |
|||
435 |
typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType; |
||
436 |
typedef typename SE3::Vector3 Vector3; |
||
437 |
typedef typename SE3::Matrix3 Matrix3; |
||
438 |
|||
439 |
✓✓✗✓ ✗ |
80 |
ConstQuaternionMap_t quat_map(q.derived().data()); |
440 |
✓✗✗✓ |
80 |
assert(quaternion::isNormalized(quat_map,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
441 |
|||
442 |
✓✗ | 80 |
Eigen::Matrix<Scalar,NQ,NV,JacobianPlainType::Options|Eigen::RowMajor> Jexp3QuatCoeffWise; |
443 |
|||
444 |
Scalar theta; |
||
445 |
✓✗ | 80 |
const Vector3 v = quaternion::log3(quat_map,theta); |
446 |
✓✗ | 80 |
quaternion::Jexp3CoeffWise(v,Jexp3QuatCoeffWise); |
447 |
✓✗ | 80 |
Matrix3 Jlog; |
448 |
✓✗ | 80 |
Jlog3(theta,v,Jlog); |
449 |
|||
450 |
// if(quat_map.w() >= 0.) // comes from the log3 for quaternions which may change the sign. |
||
451 |
✓✗✓✓ |
80 |
if(quat_map.coeffs()[3] >= 0.) // comes from the log3 for quaternions which may change the sign. |
452 |
✓✗✓✗ ✓✗ |
38 |
PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).noalias() = Jexp3QuatCoeffWise * Jlog; |
453 |
else |
||
454 |
✓✗✓✗ ✓✗✓✗ |
42 |
PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).noalias() = -Jexp3QuatCoeffWise * Jlog; |
455 |
|||
456 |
// Jexp3(quat_map,PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template topLeftCorner<NQ,NV>()); |
||
457 |
80 |
} |
|
458 |
|||
459 |
template <class Config_t, class Tangent_t, class JacobianOut_t> |
||
460 |
99 |
static void dIntegrate_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
461 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
462 |
const Eigen::MatrixBase<JacobianOut_t> & J, |
||
463 |
const AssignmentOperatorType op=SETTO) |
||
464 |
{ |
||
465 |
99 |
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J); |
|
466 |
✓✓✓✗ |
99 |
switch(op) |
467 |
{ |
||
468 |
97 |
case SETTO: |
|
469 |
✓✗✓✗ |
97 |
Jout = exp3(-v); |
470 |
97 |
break; |
|
471 |
1 |
case ADDTO: |
|
472 |
✓✗✓✗ |
1 |
Jout += exp3(-v); |
473 |
1 |
break; |
|
474 |
1 |
case RMTO: |
|
475 |
✓✗✓✗ |
1 |
Jout -= exp3(-v); |
476 |
1 |
break; |
|
477 |
default: |
||
478 |
assert(false && "Wrong Op requesed value"); |
||
479 |
break; |
||
480 |
} |
||
481 |
99 |
} |
|
482 |
|||
483 |
template <class Config_t, class Tangent_t, class JacobianOut_t> |
||
484 |
179 |
static void dIntegrate_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
485 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
486 |
const Eigen::MatrixBase<JacobianOut_t> & J, |
||
487 |
const AssignmentOperatorType op=SETTO) |
||
488 |
{ |
||
489 |
✓✓✓✗ |
179 |
switch(op) |
490 |
{ |
||
491 |
177 |
case SETTO: |
|
492 |
177 |
Jexp3<SETTO>(v, J.derived()); |
|
493 |
177 |
break; |
|
494 |
1 |
case ADDTO: |
|
495 |
1 |
Jexp3<ADDTO>(v, J.derived()); |
|
496 |
1 |
break; |
|
497 |
1 |
case RMTO: |
|
498 |
1 |
Jexp3<RMTO>(v, J.derived()); |
|
499 |
1 |
break; |
|
500 |
default: |
||
501 |
assert(false && "Wrong Op requesed value"); |
||
502 |
break; |
||
503 |
} |
||
504 |
179 |
} |
|
505 |
|||
506 |
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
||
507 |
1 |
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
508 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
509 |
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
||
510 |
const Eigen::MatrixBase<JacobianOut_t> & J_out) const |
||
511 |
{ |
||
512 |
typedef typename SE3::Matrix3 Matrix3; |
||
513 |
1 |
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out); |
|
514 |
✓✗✓✗ |
1 |
const Matrix3 Jtmp3 = exp3(-v); |
515 |
✓✗✓✗ ✓✗ |
1 |
Jout.noalias() = Jtmp3 * Jin; |
516 |
1 |
} |
|
517 |
|||
518 |
template <class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t> |
||
519 |
1 |
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
520 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
521 |
const Eigen::MatrixBase<JacobianIn_t> & Jin, |
||
522 |
const Eigen::MatrixBase<JacobianOut_t> & J_out) const |
||
523 |
{ |
||
524 |
typedef typename SE3::Matrix3 Matrix3; |
||
525 |
1 |
JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out); |
|
526 |
✓✗ | 1 |
Matrix3 Jtmp3; |
527 |
✓✗ | 1 |
Jexp3<SETTO>(v, Jtmp3); |
528 |
✓✗✓✗ ✓✗ |
1 |
Jout.noalias() = Jtmp3 * Jin; |
529 |
1 |
} |
|
530 |
|||
531 |
template <class Config_t, class Tangent_t, class Jacobian_t> |
||
532 |
1 |
void dIntegrateTransport_dq_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
533 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
534 |
const Eigen::MatrixBase<Jacobian_t> & J_out) const |
||
535 |
{ |
||
536 |
typedef typename SE3::Matrix3 Matrix3; |
||
537 |
1 |
Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out); |
|
538 |
✓✗✓✗ |
1 |
const Matrix3 Jtmp3 = exp3(-v); |
539 |
✓✗✓✗ |
1 |
Jout = Jtmp3 * Jout; |
540 |
1 |
} |
|
541 |
|||
542 |
template <class Config_t, class Tangent_t, class Jacobian_t> |
||
543 |
1 |
void dIntegrateTransport_dv_impl(const Eigen::MatrixBase<Config_t > & /*q*/, |
|
544 |
const Eigen::MatrixBase<Tangent_t> & v, |
||
545 |
const Eigen::MatrixBase<Jacobian_t> & J_out) const |
||
546 |
{ |
||
547 |
typedef typename SE3::Matrix3 Matrix3; |
||
548 |
1 |
Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out); |
|
549 |
✓✗ | 1 |
Matrix3 Jtmp3; |
550 |
✓✗ | 1 |
Jexp3<SETTO>(v, Jtmp3); |
551 |
✓✗✓✗ |
1 |
Jout = Jtmp3 * Jout; |
552 |
1 |
} |
|
553 |
|||
554 |
|||
555 |
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t> |
||
556 |
3062 |
static void interpolate_impl(const Eigen::MatrixBase<ConfigL_t> & q0, |
|
557 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
558 |
const Scalar & u, |
||
559 |
const Eigen::MatrixBase<ConfigOut_t> & qout) |
||
560 |
{ |
||
561 |
✓✓✗✓ ✗ |
3062 |
ConstQuaternionMap_t quat0 (q0.derived().data()); |
562 |
✓✗✗✓ |
3062 |
assert(quaternion::isNormalized(quat0,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
563 |
✓✓✗✓ ✗ |
3062 |
ConstQuaternionMap_t quat1 (q1.derived().data()); |
564 |
✓✗✗✓ |
3062 |
assert(quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
565 |
|||
566 |
✓✗ | 3062 |
QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t,qout).data()); |
567 |
|||
568 |
✓✗✓✗ |
3062 |
quat_map = quat0.slerp(u, quat1); |
569 |
✓✗✗✓ |
3062 |
assert(quaternion::isNormalized(quat_map,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
570 |
3062 |
} |
|
571 |
|||
572 |
template <class ConfigL_t, class ConfigR_t> |
||
573 |
2057 |
static Scalar squaredDistance_impl(const Eigen::MatrixBase<ConfigL_t> & q0, |
|
574 |
const Eigen::MatrixBase<ConfigR_t> & q1) |
||
575 |
{ |
||
576 |
✓✗ | 2057 |
TangentVector_t t; |
577 |
✓✗ | 2057 |
difference_impl(q0, q1, t); |
578 |
✓✗ | 2057 |
return t.squaredNorm(); |
579 |
} |
||
580 |
|||
581 |
template <class Config_t> |
||
582 |
2054 |
static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout) |
|
583 |
{ |
||
584 |
2054 |
Config_t & qout_ = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout); |
|
585 |
2054 |
qout_.normalize(); |
|
586 |
2054 |
} |
|
587 |
|||
588 |
template <class Config_t> |
||
589 |
10209 |
static bool isNormalized_impl (const Eigen::MatrixBase<Config_t> & qin, |
|
590 |
const Scalar& prec) |
||
591 |
{ |
||
592 |
10209 |
const Scalar norm = qin.norm(); |
|
593 |
using std::abs; |
||
594 |
10209 |
return abs(norm - Scalar(1.0)) < prec; |
|
595 |
} |
||
596 |
|||
597 |
template <class Config_t> |
||
598 |
13266 |
void random_impl(const Eigen::MatrixBase<Config_t> & qout) const |
|
599 |
{ |
||
600 |
✓✓✗✓ ✗ |
13266 |
QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).data()); |
601 |
✓✗ | 13266 |
quaternion::uniformRandom(quat_map); |
602 |
|||
603 |
✓✗✗✓ |
13266 |
assert(quaternion::isNormalized(quat_map,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
604 |
13266 |
} |
|
605 |
|||
606 |
template <class ConfigL_t, class ConfigR_t, class ConfigOut_t> |
||
607 |
12456 |
void randomConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> &, |
|
608 |
const Eigen::MatrixBase<ConfigR_t> &, |
||
609 |
const Eigen::MatrixBase<ConfigOut_t> & qout) const |
||
610 |
{ |
||
611 |
12456 |
random_impl(qout); |
|
612 |
12456 |
} |
|
613 |
|||
614 |
template <class ConfigL_t, class ConfigR_t> |
||
615 |
9 |
static bool isSameConfiguration_impl(const Eigen::MatrixBase<ConfigL_t> & q0, |
|
616 |
const Eigen::MatrixBase<ConfigR_t> & q1, |
||
617 |
const Scalar & prec) |
||
618 |
{ |
||
619 |
9 |
ConstQuaternionMap_t quat1(q0.derived().data()); |
|
620 |
9 |
assert(quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
|
621 |
9 |
ConstQuaternionMap_t quat2(q1.derived().data()); |
|
622 |
9 |
assert(quaternion::isNormalized(quat1,RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE))); |
|
623 |
|||
624 |
18 |
return quaternion::defineSameRotation(quat1,quat2,prec); |
|
625 |
} |
||
626 |
}; // struct SpecialOrthogonalOperationTpl<3,_Scalar,_Options> |
||
627 |
|||
628 |
} // namespace pinocchio |
||
629 |
|||
630 |
#endif // ifndef __pinocchio_multibody_liegroup_special_orthogonal_operation_hpp__ |
| Generated by: GCOVR (Version 4.2) |