GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/liegroup/special-orthogonal.hpp
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 253 285 88.8%
Branches: 254 869 29.2%
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 20243 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 20243 times.
✗ Branch 2 not taken.
 20243 const Scalar tr = R.trace();
66
67
3/8
✓ Branch 0 taken 5 times.
✓ Branch 1 taken 20238 times.
✓ Branch 3 taken 5 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 20243 static const Scalar PI_value = PI<Scalar>();
68
69 using internal::if_then_else;
70
0/6
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 40486 Scalar theta = if_then_else(
71 ✗ internal::GT, tr, Scalar(2),
72 ✗ Scalar(0), // then
73
1/2
✓ Branch 1 taken 20243 times.
✗ Branch 2 not taken.
 20243 if_then_else(
74 ✗ internal::LT, tr, Scalar(-2),
75
1/2
✓ Branch 1 taken 20243 times.
✗ Branch 2 not taken.
 20243 if_then_else(
76
1/6
✓ Branch 1 taken 20243 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 20243 internal::GE, R(1, 0), Scalar(0), PI_value,
77 ✗ static_cast<Scalar>(-PI_value)), // then
78
1/2
✓ Branch 1 taken 20243 times.
✗ Branch 2 not taken.
 20243 if_then_else(
79 internal::GT, tr,
80 ✗ static_cast<Scalar>(Scalar(2) - Scalar(1e-2)), // TODO: change value
81
2/12
✓ Branch 1 taken 20243 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20243 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 20243 static_cast<Scalar>(asin((R(1, 0) - R(0, 1)) / Scalar(2))), // then
82
1/2
✓ Branch 1 taken 20243 times.
✗ Branch 2 not taken.
 20243 if_then_else(
83
1/6
✓ Branch 1 taken 20243 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 20243 internal::GE, R(1, 0), Scalar(0),
84 ✗ static_cast<Scalar>(acos(tr / Scalar(2))), // then
85
1/8
✓ Branch 1 taken 20243 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 20243 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
2/13
✓ Branch 1 taken 20243 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 20107 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
 20243 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 20243 return theta;
98 }
99
100 template<typename Matrix2Like>
101 130 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 130 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 39809 static Index nq()
113 {
114 39809 return NQ;
115 }
116
117 /// Get dimension of Lie Group tangent space
118 14641 static Index nv()
119 {
120 14641 return NV;
121 }
122
123 12 static ConfigVector_t neutral()
124 {
125 12 ConfigVector_t n;
126
2/8
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 12 n << Scalar(1), Scalar(0);
127 12 return n;
128 }
129
130 170 static std::string name()
131 {
132
1/2
✓ Branch 2 taken 170 times.
✗ Branch 3 not taken.
 170 return std::string("SO(2)");
133 }
134
135 template<class ConfigL_t, class ConfigR_t, class Tangent_t>
136 25238 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
1/2
✓ Branch 1 taken 12619 times.
✗ Branch 2 not taken.
 25238 Matrix2 R; // R0.transpose() * R1;
142
3/10
✓ Branch 1 taken 12619 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12619 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12619 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 25238 R(0, 0) = R(1, 1) = q0.dot(q1);
143
5/18
✓ Branch 1 taken 12619 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12619 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12619 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12619 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12619 times.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
 25238 R(1, 0) = q0(0) * q1(1) - q0(1) * q1(0);
144
2/8
✓ Branch 1 taken 12619 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12619 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 25238 R(0, 1) = -R(1, 0);
145
2/6
✓ Branch 1 taken 12619 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 12619 times.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
 25238 PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d)[0] = log(R);
146 25238 }
147
148 template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
149 260 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
1/2
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
 260 Matrix2 R; // R0.transpose() * R1;
155
3/10
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 130 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 260 R(0, 0) = R(1, 1) = q0.dot(q1);
156
5/18
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 130 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 130 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 130 times.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
 260 R(1, 0) = q0(0) * q1(1) - q0(1) * q1(0);
157
2/8
✓ Branch 1 taken 130 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 130 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 260 R(0, 1) = -R(1, 0);
158
159
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 260 Scalar w(Jlog(R));
160
0/4
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 260 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).coeffRef(0, 0) = ((arg == ARG0) ? -w : w);
161 260 }
162
163 template<class ConfigIn_t, class Velocity_t, class ConfigOut_t>
164 25806 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 25806 ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout);
170
171
1/2
✓ Branch 1 taken 12908 times.
✗ Branch 2 not taken.
 25806 const Scalar ca = q(0);
172
1/2
✓ Branch 1 taken 12908 times.
✗ Branch 2 not taken.
 25806 const Scalar sa = q(1);
173
1/2
✓ Branch 1 taken 12908 times.
✗ Branch 2 not taken.
 25806 const Scalar & omega = v(0);
174
175
2/4
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2 Scalar cosOmega, sinOmega;
176
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 25806 SINCOS(omega, &sinOmega, &cosOmega);
177 // TODO check the cost of atan2 vs SINCOS
178
179
8/16
✓ Branch 1 taken 12908 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12908 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
 25806 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
1/2
✓ Branch 1 taken 12908 times.
✗ Branch 2 not taken.
 25806 const Scalar norm2 = out.squaredNorm();
183
5/10
✓ Branch 1 taken 12908 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
 25806 out *= (3 - norm2) / 2;
184
5/7
✓ Branch 1 taken 2 times.
✓ Branch 2 taken 12906 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 2 times.
✓ Branch 5 taken 12906 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 2 times.
 25806 assert(pinocchio::isNormalized(out));
185 25806 }
186
187 template<class Config_t, class Jacobian_t>
188 80 static void integrateCoeffWiseJacobian_impl(
189 const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J)
190 {
191
2/4
✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 40 times.
✗ Branch 7 not taken.
 80 assert(J.rows() == nq() && J.cols() == nv() && "J is not of the right dimension");
192 80 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J);
193
3/6
✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 40 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 40 times.
✗ Branch 9 not taken.
 80 Jout << -q[1], q[0];
194 80 }
195
196 template<class Config_t, class Tangent_t, class JacobianOut_t>
197 96 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 96 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
204
1/4
✓ Branch 0 taken 48 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 96 switch (op)
205 {
206 96 case SETTO:
207
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 96 Jout(0, 0) = Scalar(1);
208 96 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 96 }
220
221 template<class Config_t, class Tangent_t, class JacobianOut_t>
222 176 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 176 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
229
1/4
✓ Branch 0 taken 88 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 176 switch (op)
230 {
231 176 case SETTO:
232
0/4
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 176 Jout(0, 0) = Scalar(1);
233 176 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 176 }
245
246 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
247 325 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 325 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout) = Jin;
254 325 }
255
256 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
257 1 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 1 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, Jout) = Jin;
264 1 }
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 9180 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 9180 ConfigOut_t & out = PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout);
290
291
3/53
✓ Branch 1 taken 9180 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 9180 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 9180 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 42 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 45 not taken.
✗ Branch 46 not taken.
✗ Branch 47 not taken.
✗ Branch 48 not taken.
✗ Branch 49 not taken.
✗ Branch 50 not taken.
✗ Branch 51 not taken.
✗ Branch 52 not taken.
✗ Branch 53 not taken.
✗ Branch 54 not taken.
✗ Branch 55 not taken.
✗ Branch 56 not taken.
✗ Branch 57 not taken.
✗ Branch 58 not taken.
 9180 assert(
292 check_expression_if_real<Scalar>(abs(q0.norm() - 1) < 1e-8)
293 && "initial configuration not normalized");
294
3/53
✓ Branch 1 taken 9180 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 9180 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 9180 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 22 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 25 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 31 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
✗ Branch 38 not taken.
✗ Branch 39 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 42 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 45 not taken.
✗ Branch 46 not taken.
✗ Branch 47 not taken.
✗ Branch 48 not taken.
✗ Branch 49 not taken.
✗ Branch 50 not taken.
✗ Branch 51 not taken.
✗ Branch 52 not taken.
✗ Branch 53 not taken.
✗ Branch 54 not taken.
✗ Branch 55 not taken.
✗ Branch 56 not taken.
✗ Branch 57 not taken.
✗ Branch 58 not taken.
 9180 assert(
295 check_expression_if_real<Scalar>(abs(q1.norm() - 1) < 1e-8)
296 && "final configuration not normalized");
297
1/2
✓ Branch 1 taken 9180 times.
✗ Branch 2 not taken.
 9180 const Scalar cosTheta = q0.dot(q1);
298
4/14
✓ Branch 1 taken 9180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9180 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9180 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9180 times.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
 9180 const Scalar sinTheta = q0(0) * q1(1) - q0(1) * q1(0);
299
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 9180 const Scalar theta = atan2(sinTheta, cosTheta);
300
2/15
✓ Branch 1 taken 9180 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 9180 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
 9180 assert(check_expression_if_real<Scalar>(fabs(sin(theta) - sinTheta) < 1e-8));
301
302
3/8
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 9179 times.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 9180 static const Scalar PI_value = PI<Scalar>();
303
3/10
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 9179 times.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 15 not taken.
✗ Branch 16 not taken.
 9180 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
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 9180 const Scalar abs_theta = fabs(theta);
308
1/10
✓ Branch 1 taken 9180 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 9180 out[0] = if_then_else(
309 ✗ LT, abs_theta, static_cast<Scalar>(1e-6),
310
2/14
✓ Branch 1 taken 9180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9180 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
 9180 static_cast<Scalar>((Scalar(1) - u) * q0[0] + u * q1[0]), // then
311
1/2
✓ Branch 1 taken 9180 times.
✗ Branch 2 not taken.
 9180 if_then_else(
312 LT, abs_theta, PI_value_lower, // else
313 ✗ static_cast<Scalar>(
314
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 18360 (sin((Scalar(1) - u) * theta) / sinTheta) * q0[0]
315
2/4
✓ Branch 1 taken 9180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9180 times.
✗ Branch 5 not taken.
 9180 + (sin(u * theta) / sinTheta) * q1[0]), // then
316
2/4
✓ Branch 1 taken 9180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9180 times.
✗ Branch 5 not taken.
 9180 q0(0) // cos(theta0) // else
317 ));
318
319
1/10
✓ Branch 1 taken 9180 times.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
 9180 out[1] = if_then_else(
320 ✗ LT, abs_theta, static_cast<Scalar>(1e-6),
321
2/14
✓ Branch 1 taken 9180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9180 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
 9180 static_cast<Scalar>((Scalar(1) - u) * q0[1] + u * q1[1]), // then
322
1/2
✓ Branch 1 taken 9180 times.
✗ Branch 2 not taken.
 9180 if_then_else(
323 LT, abs_theta, PI_value_lower, // else
324 ✗ static_cast<Scalar>(
325
0/2
✗ Branch 1 not taken.
✗ Branch 2 not taken.
 18360 (sin((Scalar(1) - u) * theta) / sinTheta) * q0[1]
326
2/4
✓ Branch 1 taken 9180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9180 times.
✗ Branch 5 not taken.
 9180 + (sin(u * theta) / sinTheta) * q1[1]), // then
327
2/4
✓ Branch 1 taken 9180 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9180 times.
✗ Branch 5 not taken.
 9180 q0(1) // sin(theta0) // else
328 ));
329 9180 }
330
331 template<class Config_t>
332 6127 static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout)
333 {
334 6127 pinocchio::normalize(qout.const_cast_derived());
335 6127 }
336
337 template<class Config_t>
338 30602 static bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin, const Scalar & prec)
339 {
340 30602 const Scalar norm = qin.norm();
341 using std::abs;
342 30602 return abs(norm - Scalar(1.0)) < prec;
343 }
344
345 template<class Config_t>
346 25390 static void random_impl(const Eigen::MatrixBase<Config_t> & qout)
347 {
348 25390 Config_t & out = PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout);
349
350
3/8
✓ Branch 0 taken 29 times.
✓ Branch 1 taken 12669 times.
✓ Branch 3 taken 29 times.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 25390 static const Scalar PI_value = PI<Scalar>();
351
0/16
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
 25390 const Scalar angle = -PI_value + Scalar(2) * PI_value * ((Scalar)rand()) / RAND_MAX;
352
2/6
✓ Branch 1 taken 12698 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12698 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
 25390 SINCOS(angle, &out(1), &out(0));
353 25390 }
354
355 template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
356 24590 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 24590 random_impl(qout);
362 24590 }
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 30589 static Index nq()
382 {
383 30589 return NQ;
384 }
385
386 /// Get dimension of Lie Group tangent space
387 6613 static Index nv()
388 {
389 6613 return NV;
390 }
391
392 26 static ConfigVector_t neutral()
393 {
394 26 ConfigVector_t n;
395
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 26 n.setZero();
396
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.
 26 n[3] = Scalar(1);
397 26 return n;
398 }
399
400 175 static std::string name()
401 {
402
1/2
✓ Branch 2 taken 175 times.
✗ Branch 3 not taken.
 175 return std::string("SO(3)");
403 }
404
405 template<class ConfigL_t, class ConfigR_t, class Tangent_t>
406 16130 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
3/5
✓ Branch 2 taken 7413 times.
✓ Branch 3 taken 652 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 7413 times.
✗ Branch 6 not taken.
 16130 ConstQuaternionMap_t quat0(q0.derived().data());
412
3/7
✓ Branch 1 taken 8065 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8065 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 3 times.
 16130 assert(quaternion::isNormalized(
413 quat0, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
414
3/5
✓ Branch 2 taken 7413 times.
✓ Branch 3 taken 652 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 7413 times.
✗ Branch 6 not taken.
 16130 ConstQuaternionMap_t quat1(q1.derived().data());
415
3/7
✓ Branch 1 taken 8065 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8065 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 3 times.
 16130 assert(quaternion::isNormalized(
416 quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
417
418
2/4
✓ Branch 1 taken 8065 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 645 times.
✗ Branch 6 not taken.
 16130 PINOCCHIO_EIGEN_CONST_CAST(Tangent_t, d) =
419
2/4
✓ Branch 1 taken 8065 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8065 times.
✗ Branch 5 not taken.
 16136 quaternion::log3(Quaternion_t(quat0.conjugate() * quat1));
420 16130 }
421
422 template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
423 364 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
3/5
✓ Branch 2 taken 62 times.
✓ Branch 3 taken 120 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 62 times.
✗ Branch 6 not taken.
 364 ConstQuaternionMap_t quat0(q0.derived().data());
431
2/7
✓ Branch 1 taken 182 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 182 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 364 assert(quaternion::isNormalized(
432 quat0, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
433
3/5
✓ Branch 2 taken 62 times.
✓ Branch 3 taken 120 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 62 times.
✗ Branch 6 not taken.
 364 ConstQuaternionMap_t quat1(q1.derived().data());
434
2/7
✓ Branch 1 taken 182 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 182 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 364 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 182 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 182 times.
✗ Branch 5 not taken.
 364 const Quaternion_t q = quat0.conjugate() * quat1;
439
1/2
✓ Branch 1 taken 182 times.
✗ Branch 2 not taken.
 364 const Matrix3 R = q.matrix();
440
441 if (arg == ARG0)
442 {
443
1/2
✓ Branch 1 taken 71 times.
✗ Branch 2 not taken.
 142 JacobianMatrix_t J1;
444
1/2
✓ Branch 1 taken 71 times.
✗ Branch 2 not taken.
 142 Jlog3(R, J1);
445
446
5/10
✓ Branch 1 taken 71 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 71 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 71 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 71 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 71 times.
✗ Branch 15 not taken.
 142 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J).noalias() = -J1 * R.transpose();
447 }
448 else if (arg == ARG1)
449 {
450
1/2
✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
 222 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 364 }
469
470 template<class ConfigIn_t, class Velocity_t, class ConfigOut_t>
471 17390 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
3/5
✓ Branch 2 taken 7593 times.
✓ Branch 3 taken 1110 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 7593 times.
✗ Branch 6 not taken.
 17390 ConstQuaternionMap_t quat(q.derived().data());
477
3/7
✓ Branch 1 taken 8703 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8703 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 5 times.
 17390 assert(quaternion::isNormalized(
478 quat, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
479
3/5
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 8702 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 17390 QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).data());
480
481
1/2
✓ Branch 1 taken 8703 times.
✗ Branch 2 not taken.
 17390 Quaternion_t pOmega;
482
1/2
✓ Branch 1 taken 8703 times.
✗ Branch 2 not taken.
 17390 quaternion::exp3(v, pOmega);
483
2/4
✓ Branch 1 taken 8703 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8703 times.
✗ Branch 5 not taken.
 17390 quat_map = quat * pOmega;
484
1/2
✓ Branch 1 taken 8703 times.
✗ Branch 2 not taken.
 17390 quaternion::firstOrderNormalize(quat_map);
485
3/7
✓ Branch 1 taken 8703 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8703 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 5 times.
 17390 assert(quaternion::isNormalized(
486 quat_map, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
487 17390 }
488
489 template<class Config_t, class Jacobian_t>
490 80 static void integrateCoeffWiseJacobian_impl(
491 const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J)
492 {
493
2/4
✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 40 times.
✗ Branch 7 not taken.
 80 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
3/5
✓ Branch 2 taken 20 times.
✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 20 times.
✗ Branch 6 not taken.
 80 ConstQuaternionMap_t quat_map(q.derived().data());
500
2/4
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 40 times.
 80 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
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 80 Jexp3QuatCoeffWise;
507
508 Scalar theta;
509
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 80 const Vector3 v = quaternion::log3(quat_map, theta);
510
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 80 quaternion::Jexp3CoeffWise(v, Jexp3QuatCoeffWise);
511
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 80 Matrix3 Jlog;
512
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 80 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
3/4
✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 23 times.
✓ Branch 5 taken 17 times.
 80 if (quat_map.coeffs()[3] >= 0.) // comes from the log3 for quaternions which may change the
518 // sign.
519
3/6
✓ Branch 1 taken 23 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 23 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 23 times.
✗ Branch 9 not taken.
 46 PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J).noalias() = Jexp3QuatCoeffWise * Jlog;
520 else
521
4/8
✓ Branch 1 taken 17 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 17 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 17 times.
✗ Branch 12 not taken.
 34 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 80 }
526
527 template<class Config_t, class Tangent_t, class JacobianOut_t>
528 115 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 115 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
535
3/4
✓ Branch 0 taken 61 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 115 switch (op)
536 {
537 113 case SETTO:
538
2/4
✓ Branch 2 taken 61 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 35 times.
✗ Branch 6 not taken.
 113 Jout = exp3(-v);
539 113 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 115 }
551
552 template<class Config_t, class Tangent_t, class JacobianOut_t>
553 195 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 101 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 195 switch (op)
560 {
561 193 case SETTO:
562 193 Jexp3<SETTO>(v, J.derived());
563 193 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 195 }
575
576 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
577 331 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 331 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out);
585
2/4
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
 331 const Matrix3 Jtmp3 = exp3(-v);
586
3/6
✓ Branch 1 taken 171 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 171 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 171 times.
✗ Branch 8 not taken.
 331 Jout.noalias() = Jtmp3 * Jin;
587 331 }
588
589 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
590 3 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 3 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 3 times.
✗ Branch 2 not taken.
 3 Matrix3 Jtmp3;
601
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Jexp3<SETTO>(v, Jtmp3);
602 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
603
3/6
✓ 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.
 3 Jout.noalias() = Jtmp3 * Jin;
604 3 }
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 3063 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
3/5
✓ Branch 2 taken 3060 times.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 3060 times.
✗ Branch 6 not taken.
 3063 ConstQuaternionMap_t quat0(q0.derived().data());
642
3/7
✓ Branch 1 taken 3063 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3063 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
 3063 assert(quaternion::isNormalized(
643 quat0, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
644
3/5
✓ Branch 2 taken 3060 times.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 3060 times.
✗ Branch 6 not taken.
 3063 ConstQuaternionMap_t quat1(q1.derived().data());
645
3/7
✓ Branch 1 taken 3063 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3063 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
 3063 assert(quaternion::isNormalized(
646 quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
647
648
1/2
✓ Branch 3 taken 3063 times.
✗ Branch 4 not taken.
 3063 QuaternionMap_t quat_res(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).data());
649
650
1/2
✓ Branch 1 taken 3063 times.
✗ Branch 2 not taken.
 3063 TangentVector_t w;
651
1/2
✓ Branch 1 taken 3063 times.
✗ Branch 2 not taken.
 3063 difference_impl(q0, q1, w);
652
2/4
✓ Branch 1 taken 3063 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3063 times.
✗ Branch 5 not taken.
 3063 integrate_impl(q0, u * w, qout);
653
3/7
✓ Branch 1 taken 3063 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3063 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 1 times.
 3063 assert(quaternion::isNormalized(
654 quat_res, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
655 3063 }
656
657 template<class ConfigL_t, class ConfigR_t>
658 2057 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 1030 times.
✗ Branch 2 not taken.
 2057 TangentVector_t t;
664
1/2
✓ Branch 1 taken 1030 times.
✗ Branch 2 not taken.
 2057 difference_impl(q0, q1, t);
665 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
666
1/2
✓ Branch 1 taken 1030 times.
✗ Branch 2 not taken.
 4114 return t.squaredNorm();
667 }
668
669 template<class Config_t>
670 2054 static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout)
671 {
672 2054 pinocchio::normalize(qout.const_cast_derived());
673 2054 }
674
675 template<class Config_t>
676 10209 static bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin, const Scalar & prec)
677 {
678 10209 const Scalar norm = qin.norm();
679 using std::abs;
680 10209 return abs(norm - Scalar(1.0)) < prec;
681 }
682
683 template<class Config_t>
684 18130 static void random_impl(const Eigen::MatrixBase<Config_t> & qout)
685 {
686
3/5
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 9063 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 18130 QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout).data());
687
1/2
✓ Branch 1 taken 9065 times.
✗ Branch 2 not taken.
 18130 quaternion::uniformRandom(quat_map);
688
689
2/7
✓ Branch 1 taken 9065 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 9065 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 18130 assert(quaternion::isNormalized(
690 quat_map, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
691 18130 }
692
693 template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
694 17170 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 17170 random_impl(qout);
700 17170 }
701
702 template<class ConfigL_t, class ConfigR_t>
703 11 static bool isSameConfiguration_impl(
704 const Eigen::MatrixBase<ConfigL_t> & q0,
705 const Eigen::MatrixBase<ConfigR_t> & q1,
706 const Scalar & prec)
707 {
708 11 ConstQuaternionMap_t quat1(q0.derived().data());
709 11 assert(quaternion::isNormalized(
710 quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
711 11 ConstQuaternionMap_t quat2(q1.derived().data());
712 11 assert(quaternion::isNormalized(
713 quat1, RealScalar(PINOCCHIO_DEFAULT_QUATERNION_NORM_TOLERANCE_VALUE)));
714
715 22 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