GCC Code Coverage Report

Directory: ./
File: include/pinocchio/spatial/explog-quaternion.hpp
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 78 97 80.4%
Branches: 95 221 43.0%
Line Branch Exec Source
1 //
2 // Copyright (c) 2018-2021 CNRS INRIA
3 //
4
5 #ifndef __pinocchio_spatial_explog_quaternion_hpp__
6 #define __pinocchio_spatial_explog_quaternion_hpp__
7
8 #include "pinocchio/math/quaternion.hpp"
9 #include "pinocchio/spatial/explog.hpp"
10 #include "pinocchio/utils/static-if.hpp"
11
12 namespace pinocchio
13 {
14 namespace quaternion
15 {
16
17 ///
18 /// \brief Exp: so3 -> SO3 (quaternion)
19 ///
20 /// \returns the integral of the velocity vector as a quaternion.
21 ///
22 /// \param[in] v The angular velocity vector.
23 /// \param[out] qout The quaternion where the result is stored.
24 ///
25 template<typename Vector3Like, typename QuaternionLike>
26 void
27 6176 exp3(const Eigen::MatrixBase<Vector3Like> & v, Eigen::QuaternionBase<QuaternionLike> & quat_out)
28 {
29 EIGEN_STATIC_ASSERT_VECTOR_ONLY(Vector3Like);
30
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 3088 times.
 6176 assert(v.size() == 3);
31
32 typedef typename Vector3Like::Scalar Scalar;
33 enum
34 {
35 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options
36 };
37 typedef Eigen::Quaternion<typename QuaternionLike::Scalar, Options> QuaternionPlain;
38 6176 const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
39
40
1/2
✓ Branch 1 taken 3088 times.
✗ Branch 2 not taken.
 6176 const Scalar t2 = v.squaredNorm();
41 6176 const Scalar t = math::sqrt(t2 + eps * eps);
42
43
3/6
✓ Branch 0 taken 21 times.
✓ Branch 1 taken 3067 times.
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 6176 static const Scalar ts_prec =
44
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 42 TaylorSeriesExpansion<Scalar>::template precision<3>(); // Precision for the Taylor series
45 // expansion.
46
47
2/4
✓ Branch 1 taken 3088 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3088 times.
✗ Branch 5 not taken.
 6176 Eigen::AngleAxis<Scalar> aa(t, v / t);
48
1/2
✓ Branch 1 taken 3088 times.
✗ Branch 2 not taken.
 6176 QuaternionPlain quat_then(aa);
49
50 // order 4 Taylor expansion in theta / (order 2 in t2)
51
1/2
✓ Branch 1 taken 3088 times.
✗ Branch 2 not taken.
 6176 QuaternionPlain quat_else;
52 6176 const Scalar t2_2 = t2 / 4; // theta/2 squared
53
2/4
✓ Branch 1 taken 3088 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3088 times.
✗ Branch 5 not taken.
 6176 quat_else.vec() =
54
1/2
✓ Branch 1 taken 3088 times.
✗ Branch 2 not taken.
 6176 Scalar(0.5) * (Scalar(1) - t2_2 / Scalar(6) + t2_2 * t2_2 / Scalar(120)) * v;
55
1/2
✓ Branch 1 taken 3088 times.
✗ Branch 2 not taken.
 6176 quat_else.w() = Scalar(1) - t2_2 / 2 + t2_2 * t2_2 / 24;
56
57 using ::pinocchio::internal::if_then_else;
58
2/2
✓ Branch 0 taken 12352 times.
✓ Branch 1 taken 3088 times.
 30880 for (Eigen::DenseIndex k = 0; k < 4; ++k)
59 {
60
2/4
✓ Branch 1 taken 12352 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2496 times.
✗ Branch 5 not taken.
 24704 quat_out.coeffs().coeffRef(k) = if_then_else(
61
2/4
✓ Branch 1 taken 10740 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10740 times.
✗ Branch 5 not taken.
 24704 ::pinocchio::internal::GT, t2, ts_prec, quat_then.coeffs().coeffRef(k),
62
1/2
✓ Branch 2 taken 10740 times.
✗ Branch 3 not taken.
 24704 quat_else.coeffs().coeffRef(k));
63 }
64 6176 }
65
66 /// \brief Exp: so3 -> SO3 (quaternion)
67 ///
68 /// \returns the integral of the velocity vector as a quaternion.
69 ///
70 /// \param[in] v The angular velocity vector.
71 ///
72 template<typename Vector3Like>
73 Eigen::
74 Quaternion<typename Vector3Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
75 exp3(const Eigen::MatrixBase<Vector3Like> & v)
76 {
77 typedef Eigen::Quaternion<
78 typename Vector3Like::Scalar, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options>
79 ReturnType;
80 ReturnType res;
81 exp3(v, res);
82 return res;
83 }
84
85 /// \brief The se3 -> SE3 exponential map, using quaternions to represent the output rotation.
86 ///
87 /// \returns the integral of the twist motion over unit time.
88 ///
89 /// \param[in] motion the spatial motion.
90 /// \param[out] q the output transform in \f$\mathbb{R}^3 x S^3\f$.
91 template<typename MotionDerived, typename Config_t>
92 5331 void exp6(const MotionDense<MotionDerived> & motion, Eigen::MatrixBase<Config_t> & qout)
93 {
94 enum
95 {
96 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Config_t)::Options
97 };
98 typedef typename Config_t::Scalar Scalar;
99 typedef typename MotionDerived::Vector3 Vector3;
100 typedef Eigen::Quaternion<Scalar, Options> Quaternion_t;
101 5331 const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
102
103
1/2
✓ Branch 1 taken 2867 times.
✗ Branch 2 not taken.
 5331 const typename MotionDerived::ConstAngularType & w = motion.angular();
104
1/2
✓ Branch 1 taken 2867 times.
✗ Branch 2 not taken.
 5331 const typename MotionDerived::ConstLinearType & v = motion.linear();
105
106
1/2
✓ Branch 1 taken 2867 times.
✗ Branch 2 not taken.
 5331 const Scalar t2 = w.squaredNorm() + eps * eps;
107 5331 const Scalar t = math::sqrt(t2);
108
109 Scalar ct, st;
110 5331 SINCOS(t, &st, &ct);
111
112 5331 const Scalar inv_t2 = Scalar(1) / t2;
113 5331 const Scalar ts_prec =
114
1/2
✓ Branch 1 taken 2867 times.
✗ Branch 2 not taken.
 5331 TaylorSeriesExpansion<Scalar>::template precision<3>(); // Taylor expansion precision
115
116 using ::pinocchio::internal::if_then_else;
117 using ::pinocchio::internal::LT;
118
119 10662 const Scalar alpha_wxv = if_then_else(
120 LT, t, ts_prec,
121 ✗ Scalar(0.5) - t2 / Scalar(24), // then: use Taylor expansion
122
1/2
✓ Branch 1 taken 2867 times.
✗ Branch 2 not taken.
 5331 (Scalar(1) - ct) * inv_t2 // else
123 );
124
125 10662 const Scalar alpha_w2 = if_then_else(
126
1/2
✓ Branch 1 taken 2867 times.
✗ Branch 2 not taken.
 5331 LT, t, ts_prec, Scalar(1) / Scalar(6) - t2 / Scalar(120), (t - st) * inv_t2 / t);
127
128 // linear part
129
5/10
✓ Branch 1 taken 2464 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 403 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 2464 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 403 times.
✗ Branch 8 not taken.
✓ Branch 9 taken 2464 times.
✗ Branch 10 not taken.
 5331 Eigen::Map<Vector3> trans_(qout.derived().template head<3>().data());
130
9/18
✓ Branch 1 taken 2867 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2867 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2867 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2867 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2867 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2867 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2867 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2867 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2867 times.
✗ Branch 26 not taken.
 5331 trans_.noalias() = v + alpha_wxv * w.cross(v) + alpha_w2 * w.cross(w.cross(v));
131
132 // quaternion part
133 typedef Eigen::Map<Quaternion_t> QuaternionMap_t;
134
2/4
✓ Branch 2 taken 2867 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 2867 times.
✗ Branch 7 not taken.
 5331 QuaternionMap_t quat_(qout.derived().template tail<4>().data());
135
1/2
✓ Branch 1 taken 2867 times.
✗ Branch 2 not taken.
 5331 exp3(w, quat_);
136 5331 }
137
138 /// \brief The se3 -> SE3 exponential map, using quaternions to represent the output rotation.
139 ///
140 /// \returns the integral of the twist motion over unit time.
141 ///
142 /// \param[in] motion the spatial motion.
143 template<typename MotionDerived>
144 Eigen::Matrix<
145 typename MotionDerived::Scalar,
146 7,
147 1,
148 PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options>
149 ✗ exp6(const MotionDense<MotionDerived> & motion)
150 {
151 typedef typename MotionDerived::Scalar Scalar;
152 enum
153 {
154 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionDerived::Vector3)::Options
155 };
156 typedef Eigen::Matrix<Scalar, 7, 1, Options> ReturnType;
157
158 ✗ ReturnType qout;
159 ✗ exp6(motion, qout);
160 ✗ return qout;
161 }
162
163 /// \brief The se3 -> SE3 exponential map, using quaternions to represent the output rotation.
164 ///
165 /// \returns the integral of the spatial velocity over unit time.
166 ///
167 /// \param[in] vec6 the vector representing the spatial velocity.
168 /// \param[out] qout the output transform in R^3 x S^3.
169 template<typename Vector6Like, typename Config_t>
170 5331 void exp6(const Eigen::MatrixBase<Vector6Like> & vec6, Eigen::MatrixBase<Config_t> & qout)
171 {
172
2/5
✓ Branch 2 taken 2867 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 403 times.
✗ Branch 6 not taken.
 5331 MotionRef<const Vector6Like> nu(vec6.derived());
173
1/2
✓ Branch 1 taken 2867 times.
✗ Branch 2 not taken.
 5331 ::pinocchio::quaternion::exp6(nu, qout);
174 5331 }
175
176 /// \brief The se3 -> SE3 exponential map, using quaternions to represent the output rotation.
177 ///
178 /// \returns the integral of the spatial velocity over unit time.
179 ///
180 /// \param[in] vec6 the vector representing the spatial velocity.
181 template<typename Vector6Like>
182 Eigen::
183 Matrix<typename Vector6Like::Scalar, 7, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options>
184 ✗ exp6(const Eigen::MatrixBase<Vector6Like> & vec6)
185 {
186 typedef typename Vector6Like::Scalar Scalar;
187 enum
188 {
189 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(Vector6Like)::Options
190 };
191 typedef Eigen::Matrix<Scalar, 7, 1, Options> ReturnType;
192
193 ✗ ReturnType qout;
194 ✗ ::pinocchio::quaternion::exp6(vec6, qout);
195 ✗ return qout;
196 }
197
198 /// \brief Same as \ref log3 but with a unit quaternion as input.
199 ///
200 /// \param[in] quat the unit quaternion.
201 /// \param[out] theta the angle value (resuling from compurations).
202 ///
203 /// \return The angular velocity vector associated to the rotation matrix.
204 ///
205 template<typename QuaternionLike>
206 Eigen::Matrix<
207 typename QuaternionLike::Scalar,
208 3,
209 1,
210 PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options>
211 522 log3(
212 const Eigen::QuaternionBase<QuaternionLike> & quat, typename QuaternionLike::Scalar & theta)
213 {
214 typedef typename QuaternionLike::Scalar Scalar;
215 enum
216 {
217 Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options
218 };
219 typedef Eigen::Matrix<Scalar, 3, 1, Options> Vector3;
220
221
1/2
✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
 522 Vector3 res;
222
2/4
✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 261 times.
✗ Branch 5 not taken.
 522 const Scalar norm_squared = quat.vec().squaredNorm();
223
224 static const Scalar eps = Eigen::NumTraits<Scalar>::epsilon();
225
4/8
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 259 times.
✓ Branch 3 taken 2 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 2 times.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
 522 static const Scalar ts_prec = TaylorSeriesExpansion<Scalar>::template precision<2>();
226 522 const Scalar norm = math::sqrt(norm_squared + eps * eps);
227
228 using ::pinocchio::internal::GE;
229 using ::pinocchio::internal::if_then_else;
230 using ::pinocchio::internal::LT;
231
232
2/4
✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 261 times.
✗ Branch 5 not taken.
 522 const Scalar pos_neg = if_then_else(GE, quat.w(), Scalar(0), Scalar(+1), Scalar(-1));
233
234
1/2
✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
 522 Eigen::Quaternion<Scalar, Options> quat_pos;
235
2/4
✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 261 times.
✗ Branch 5 not taken.
 522 quat_pos.w() = pos_neg * quat.w();
236
4/8
✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 261 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 261 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 261 times.
✗ Branch 11 not taken.
 522 quat_pos.vec() = pos_neg * quat.vec();
237
238
2/4
✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 261 times.
✗ Branch 5 not taken.
 522 const Scalar theta_2 = math::atan2(norm, quat_pos.w()); // in [0,pi]
239
1/2
✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
 522 const Scalar y_x = norm / quat_pos.w(); // nonnegative
240
2/4
✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 261 times.
✗ Branch 5 not taken.
 522 const Scalar y_x_sq = norm_squared / (quat_pos.w() * quat_pos.w());
241
242 1044 theta = if_then_else(
243 ✗ LT, norm_squared, ts_prec, Scalar(2.) * (Scalar(1) - y_x_sq / Scalar(3)) * y_x,
244
1/2
✓ Branch 1 taken 261 times.
✗ Branch 2 not taken.
 522 Scalar(2.) * theta_2);
245
246 522 const Scalar th2_2 = theta * theta / Scalar(4);
247 1044 const Scalar inv_sinc = if_then_else(
248 LT, norm_squared, ts_prec,
249 ✗ Scalar(2) * (Scalar(1) + th2_2 / Scalar(6) + Scalar(7) / Scalar(360) * th2_2 * th2_2),
250
1/2
✓ Branch 2 taken 261 times.
✗ Branch 3 not taken.
 522 theta / math::sin(theta_2));
251
252
2/2
✓ Branch 0 taken 783 times.
✓ Branch 1 taken 261 times.
 2088 for (Eigen::DenseIndex k = 0; k < 3; ++k)
253 {
254 // res[k] = if_then_else(LT, norm_squared, ts_prec,
255 // Scalar(2) * (Scalar(1) + y_x_sq / Scalar(6) - y_x_sq*y_x_sq /
256 // Scalar(9)) * pos_neg * quat.vec()[k], inv_sinc * pos_neg *
257 // quat.vec()[k]);
258
3/6
✓ Branch 1 taken 783 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 783 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 783 times.
✗ Branch 8 not taken.
 1566 res[k] = inv_sinc * quat_pos.vec()[k];
259 }
260 1044 return res;
261 }
262
263 ///
264 /// \brief Log: SO3 -> so3.
265 ///
266 /// Pseudo-inverse of log from \f$ SO3 -> { v \in so3, ||v|| \le pi } \f$.
267 ///
268 /// \param[in] quat The unit quaternion representing a certain rotation.
269 ///
270 /// \return The angular velocity vector associated to the quaternion.
271 ///
272 template<typename QuaternionLike>
273 Eigen::Matrix<
274 typename QuaternionLike::Scalar,
275 3,
276 1,
277 PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Vector3)::Options>
278 130 log3(const Eigen::QuaternionBase<QuaternionLike> & quat)
279 {
280 typename QuaternionLike::Scalar theta;
281
1/2
✓ Branch 2 taken 130 times.
✗ Branch 3 not taken.
 260 return log3(quat.derived(), theta);
282 }
283
284 ///
285 /// \brief Derivative of \f$ q = \exp{\mathbf{v} + \delta\mathbf{v}} \f$ where \f$
286 /// \delta\mathbf{v} \f$
287 /// is a small perturbation of \f$ \mathbf{v} \f$ at identity.
288 ///
289 /// \returns The Jacobian of the quaternion components variation.
290 ///
291 template<typename Vector3Like, typename Matrix43Like>
292 1 void Jexp3CoeffWise(
293 const Eigen::MatrixBase<Vector3Like> & v, const Eigen::MatrixBase<Matrix43Like> & Jexp)
294 {
295 // EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Matrix43Like,4,3);
296
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 assert(Jexp.rows() == 4 && Jexp.cols() == 3 && "Jexp does have the right size.");
297 1 Matrix43Like & Jout = PINOCCHIO_EIGEN_CONST_CAST(Matrix43Like, Jexp);
298
299 typedef typename Vector3Like::Scalar Scalar;
300
301
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 const Scalar n2 = v.squaredNorm();
302 1 const Scalar n = math::sqrt(n2);
303 1 const Scalar theta = Scalar(0.5) * n;
304 1 const Scalar theta2 = Scalar(0.25) * n2;
305
306
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 if (n2 > math::sqrt(Eigen::NumTraits<Scalar>::epsilon()))
307 {
308 Scalar c, s;
309 1 SINCOS(theta, &s, &c);
310
4/8
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 Jout.template topRows<3>().noalias() =
311
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 ((Scalar(0.5) / n2) * (c - 2 * s / n)) * v * v.transpose();
312
4/8
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 Jout.template topRows<3>().diagonal().array() += s / n;
313
5/10
✓ 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.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 1 Jout.template bottomRows<1>().noalias() = -s / (2 * n) * v.transpose();
314 }
315 else
316 {
317 ✗ Jout.template topRows<3>().noalias() =
318 ✗ (-Scalar(1) / Scalar(12) + n2 / Scalar(480)) * v * v.transpose();
319 ✗ Jout.template topRows<3>().diagonal().array() += Scalar(0.5) * (1 - theta2 / 6);
320 ✗ Jout.template bottomRows<1>().noalias() =
321 ✗ (Scalar(-0.25) * (Scalar(1) - theta2 / 6)) * v.transpose();
322 }
323 1 }
324
325 ///
326 ///  \brief Computes the Jacobian of log3 operator for a unit quaternion.
327 ///
328 /// \param[in] quat A unit quaternion representing the input rotation.
329 /// \param[out] Jlog The resulting Jacobian of the log operator.
330 ///
331 template<typename QuaternionLike, typename Matrix3Like>
332 ✗ void Jlog3(
333 const Eigen::QuaternionBase<QuaternionLike> & quat,
334 const Eigen::MatrixBase<Matrix3Like> & Jlog)
335 {
336 typedef typename QuaternionLike::Scalar Scalar;
337 typedef Eigen::Matrix<
338 Scalar, 3, 1, PINOCCHIO_EIGEN_PLAIN_TYPE(typename QuaternionLike::Coefficients)::Options>
339 Vector3;
340
341 Scalar t;
342 ✗ Vector3 w(log3(quat, t));
343 ✗ pinocchio::Jlog3(t, w, PINOCCHIO_EIGEN_CONST_CAST(Matrix3Like, Jlog));
344 }
345 } // namespace quaternion
346 } // namespace pinocchio
347
348 #endif // ifndef __pinocchio_spatial_explog_quaternion_hpp__
349