GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/liegroup/special-orthogonal.hpp
Date: 2025-04-30 16:14:33
Exec Total Coverage
Lines: 253 285 88.8%
Branches: 247 839 29.4%
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 13 static ConfigVector_t neutral()
124 {
125 13 ConfigVector_t n;
126
2/8
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 13 n << Scalar(1), Scalar(0);
127 13 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 31795 static Index nq()
382 {
383 31795 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
2/4
✓ Branch 1 taken 8065 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8065 times.
 16130 assert(quaternion::isNormalized(quat0));
413
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());
414
2/4
✓ Branch 1 taken 8065 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8065 times.
 16130 assert(quaternion::isNormalized(quat1));
415
416
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) =
417
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));
418 16130 }
419
420 template<ArgumentPosition arg, class ConfigL_t, class ConfigR_t, class JacobianOut_t>
421 364 static void dDifference_impl(
422 const Eigen::MatrixBase<ConfigL_t> & q0,
423 const Eigen::MatrixBase<ConfigR_t> & q1,
424 const Eigen::MatrixBase<JacobianOut_t> & J)
425 {
426 typedef typename SE3::Matrix3 Matrix3;
427
428
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());
429
2/4
✓ Branch 1 taken 182 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 182 times.
 364 assert(quaternion::isNormalized(quat0));
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 quat1(q1.derived().data());
431
2/4
✓ Branch 1 taken 182 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 182 times.
 364 assert(quaternion::isNormalized(quat1));
432
433 // TODO: check whether the merge with 2.6.9 is correct
434
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;
435
1/2
✓ Branch 1 taken 182 times.
✗ Branch 2 not taken.
 364 const Matrix3 R = q.matrix();
436
437 if (arg == ARG0)
438 {
439
1/2
✓ Branch 1 taken 71 times.
✗ Branch 2 not taken.
 142 JacobianMatrix_t J1;
440
1/2
✓ Branch 1 taken 71 times.
✗ Branch 2 not taken.
 142 Jlog3(R, J1);
441
442
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();
443 }
444 else if (arg == ARG1)
445 {
446
1/2
✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
 222 Jlog3(R, J);
447 }
448 /*
449 const Quaternion_t quat_diff = quat0.conjugate() * quat1;
450
451 if (arg == ARG0) {
452 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
453 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
454 JacobianMatrix_t J1;
455 quaternion::Jlog3(quat_diff, J1);
456 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
457 const Matrix3 R = (quat_diff).matrix();
458
459 PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J).noalias() = - J1 * R.transpose();
460 } else if (arg == ARG1) {
461 quaternion::Jlog3(quat_diff, J);
462 }
463 */
464 364 }
465
466 template<class ConfigIn_t, class Velocity_t, class ConfigOut_t>
467 17390 static void integrate_impl(
468 const Eigen::MatrixBase<ConfigIn_t> & q,
469 const Eigen::MatrixBase<Velocity_t> & v,
470 const Eigen::MatrixBase<ConfigOut_t> & qout)
471 {
472
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());
473
2/4
✓ Branch 1 taken 8703 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8703 times.
 17390 assert(quaternion::isNormalized(quat));
474
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());
475
476
1/2
✓ Branch 1 taken 8703 times.
✗ Branch 2 not taken.
 17390 Quaternion_t pOmega;
477
1/2
✓ Branch 1 taken 8703 times.
✗ Branch 2 not taken.
 17390 quaternion::exp3(v, pOmega);
478
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;
479
1/2
✓ Branch 1 taken 8703 times.
✗ Branch 2 not taken.
 17390 quaternion::firstOrderNormalize(quat_map);
480
2/4
✓ Branch 1 taken 8703 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 8703 times.
 17390 assert(quaternion::isNormalized(quat_map));
481 17390 }
482
483 template<class Config_t, class Jacobian_t>
484 80 static void integrateCoeffWiseJacobian_impl(
485 const Eigen::MatrixBase<Config_t> & q, const Eigen::MatrixBase<Jacobian_t> & J)
486 {
487
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");
488
489 typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Jacobian_t) JacobianPlainType;
490 typedef typename SE3::Vector3 Vector3;
491 typedef typename SE3::Matrix3 Matrix3;
492
493
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());
494
2/4
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 40 times.
 80 assert(quaternion::isNormalized(quat_map));
495
496 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
497 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
498 Eigen::Matrix<Scalar, NQ, NV, JacobianPlainType::Options | Eigen::RowMajor>
499
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 80 Jexp3QuatCoeffWise;
500
501 Scalar theta;
502
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 80 const Vector3 v = quaternion::log3(quat_map, theta);
503
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 80 quaternion::Jexp3CoeffWise(v, Jexp3QuatCoeffWise);
504
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 80 Matrix3 Jlog;
505
1/2
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
 80 Jlog3(theta, v, Jlog);
506 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
507
508 // if(quat_map.w() >= 0.) // comes from the log3 for quaternions which may change the
509 // sign.
510
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
511 // sign.
512
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;
513 else
514
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;
515
516 // Jexp3(quat_map,PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J).template
517 // topLeftCorner<NQ,NV>());
518 80 }
519
520 template<class Config_t, class Tangent_t, class JacobianOut_t>
521 115 static void dIntegrate_dq_impl(
522 const Eigen::MatrixBase<Config_t> & /*q*/,
523 const Eigen::MatrixBase<Tangent_t> & v,
524 const Eigen::MatrixBase<JacobianOut_t> & J,
525 const AssignmentOperatorType op = SETTO)
526 {
527 115 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J);
528
3/4
✓ Branch 0 taken 61 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 115 switch (op)
529 {
530 113 case SETTO:
531
2/4
✓ Branch 2 taken 61 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 35 times.
✗ Branch 6 not taken.
 113 Jout = exp3(-v);
532 113 break;
533 1 case ADDTO:
534
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 Jout += exp3(-v);
535 1 break;
536 1 case RMTO:
537
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 Jout -= exp3(-v);
538 1 break;
539 ✗ default:
540 ✗ assert(false && "Wrong Op requesed value");
541 break;
542 }
543 115 }
544
545 template<class Config_t, class Tangent_t, class JacobianOut_t>
546 195 static void dIntegrate_dv_impl(
547 const Eigen::MatrixBase<Config_t> & /*q*/,
548 const Eigen::MatrixBase<Tangent_t> & v,
549 const Eigen::MatrixBase<JacobianOut_t> & J,
550 const AssignmentOperatorType op = SETTO)
551 {
552
3/4
✓ Branch 0 taken 101 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 195 switch (op)
553 {
554 193 case SETTO:
555 193 Jexp3<SETTO>(v, J.derived());
556 193 break;
557 1 case ADDTO:
558 1 Jexp3<ADDTO>(v, J.derived());
559 1 break;
560 1 case RMTO:
561 1 Jexp3<RMTO>(v, J.derived());
562 1 break;
563 ✗ default:
564 ✗ assert(false && "Wrong Op requesed value");
565 break;
566 }
567 195 }
568
569 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
570 331 static void dIntegrateTransport_dq_impl(
571 const Eigen::MatrixBase<Config_t> & /*q*/,
572 const Eigen::MatrixBase<Tangent_t> & v,
573 const Eigen::MatrixBase<JacobianIn_t> & Jin,
574 const Eigen::MatrixBase<JacobianOut_t> & J_out)
575 {
576 typedef typename SE3::Matrix3 Matrix3;
577 331 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out);
578
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);
579
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;
580 331 }
581
582 template<class Config_t, class Tangent_t, class JacobianIn_t, class JacobianOut_t>
583 3 static void dIntegrateTransport_dv_impl(
584 const Eigen::MatrixBase<Config_t> & /*q*/,
585 const Eigen::MatrixBase<Tangent_t> & v,
586 const Eigen::MatrixBase<JacobianIn_t> & Jin,
587 const Eigen::MatrixBase<JacobianOut_t> & J_out)
588 {
589 typedef typename SE3::Matrix3 Matrix3;
590 3 JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t, J_out);
591 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
592 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
593
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Matrix3 Jtmp3;
594
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 Jexp3<SETTO>(v, Jtmp3);
595 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
596
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;
597 3 }
598
599 template<class Config_t, class Tangent_t, class Jacobian_t>
600 1 static void dIntegrateTransport_dq_impl(
601 const Eigen::MatrixBase<Config_t> & /*q*/,
602 const Eigen::MatrixBase<Tangent_t> & v,
603 const Eigen::MatrixBase<Jacobian_t> & J_out)
604 {
605 typedef typename SE3::Matrix3 Matrix3;
606 1 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J_out);
607
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);
608
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Jout = Jtmp3 * Jout;
609 1 }
610
611 template<class Config_t, class Tangent_t, class Jacobian_t>
612 1 static void dIntegrateTransport_dv_impl(
613 const Eigen::MatrixBase<Config_t> & /*q*/,
614 const Eigen::MatrixBase<Tangent_t> & v,
615 const Eigen::MatrixBase<Jacobian_t> & J_out)
616 {
617 typedef typename SE3::Matrix3 Matrix3;
618 1 Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t, J_out);
619 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
620 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
621
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Matrix3 Jtmp3;
622
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Jexp3<SETTO>(v, Jtmp3);
623 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
624
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Jout = Jtmp3 * Jout;
625 1 }
626
627 template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
628 3063 static void interpolate_impl(
629 const Eigen::MatrixBase<ConfigL_t> & q0,
630 const Eigen::MatrixBase<ConfigR_t> & q1,
631 const Scalar & u,
632 const Eigen::MatrixBase<ConfigOut_t> & qout)
633 {
634
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());
635
2/4
✓ Branch 1 taken 3063 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3063 times.
 3063 assert(quaternion::isNormalized(quat0));
636
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());
637
2/4
✓ Branch 1 taken 3063 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3063 times.
 3063 assert(quaternion::isNormalized(quat1));
638
639
1/2
✓ Branch 3 taken 3063 times.
✗ Branch 4 not taken.
 3063 QuaternionMap_t quat_res(PINOCCHIO_EIGEN_CONST_CAST(ConfigOut_t, qout).data());
640
641
1/2
✓ Branch 1 taken 3063 times.
✗ Branch 2 not taken.
 3063 TangentVector_t w;
642
1/2
✓ Branch 1 taken 3063 times.
✗ Branch 2 not taken.
 3063 difference_impl(q0, q1, w);
643
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);
644
2/4
✓ Branch 1 taken 3063 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3063 times.
 3063 assert(quaternion::isNormalized(quat_res));
645 3063 }
646
647 template<class ConfigL_t, class ConfigR_t>
648 2057 static Scalar squaredDistance_impl(
649 const Eigen::MatrixBase<ConfigL_t> & q0, const Eigen::MatrixBase<ConfigR_t> & q1)
650 {
651 PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH
652 PINOCCHIO_COMPILER_DIAGNOSTIC_IGNORED_MAYBE_UNINITIALIZED
653
1/2
✓ Branch 1 taken 1030 times.
✗ Branch 2 not taken.
 2057 TangentVector_t t;
654
1/2
✓ Branch 1 taken 1030 times.
✗ Branch 2 not taken.
 2057 difference_impl(q0, q1, t);
655 PINOCCHIO_COMPILER_DIAGNOSTIC_POP
656
1/2
✓ Branch 1 taken 1030 times.
✗ Branch 2 not taken.
 4114 return t.squaredNorm();
657 }
658
659 template<class Config_t>
660 2054 static void normalize_impl(const Eigen::MatrixBase<Config_t> & qout)
661 {
662 2054 pinocchio::normalize(qout.const_cast_derived());
663 2054 }
664
665 template<class Config_t>
666 10209 static bool isNormalized_impl(const Eigen::MatrixBase<Config_t> & qin, const Scalar & prec)
667 {
668 10209 const Scalar norm = qin.norm();
669 using std::abs;
670 10209 return abs(norm - Scalar(1.0)) < prec;
671 }
672
673 template<class Config_t>
674 18934 static void random_impl(const Eigen::MatrixBase<Config_t> & qout)
675 {
676
3/5
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 9465 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
 18934 QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(Config_t, qout).data());
677
1/2
✓ Branch 1 taken 9467 times.
✗ Branch 2 not taken.
 18934 quaternion::uniformRandom(quat_map);
678
679
2/4
✓ Branch 1 taken 9467 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 9467 times.
 18934 assert(quaternion::isNormalized(quat_map));
680 18934 }
681
682 template<class ConfigL_t, class ConfigR_t, class ConfigOut_t>
683 17974 static void randomConfiguration_impl(
684 const Eigen::MatrixBase<ConfigL_t> &,
685 const Eigen::MatrixBase<ConfigR_t> &,
686 const Eigen::MatrixBase<ConfigOut_t> & qout)
687 {
688 17974 random_impl(qout);
689 17974 }
690
691 template<class ConfigL_t, class ConfigR_t>
692 11 static bool isSameConfiguration_impl(
693 const Eigen::MatrixBase<ConfigL_t> & q0,
694 const Eigen::MatrixBase<ConfigR_t> & q1,
695 const Scalar & prec)
696 {
697 11 ConstQuaternionMap_t quat1(q0.derived().data());
698 11 assert(quaternion::isNormalized(quat1));
699 11 ConstQuaternionMap_t quat2(q1.derived().data());
700 11 assert(quaternion::isNormalized(quat1));
701
702 22 return quaternion::defineSameRotation(quat1, quat2, prec);
703 }
704 }; // struct SpecialOrthogonalOperationTpl<3,_Scalar,_Options>
705
706 } // namespace pinocchio
707
708 #endif // ifndef __pinocchio_multibody_liegroup_special_orthogonal_operation_hpp__
709