GCC Code Coverage Report


Directory: ./
File: include/ndcurves/so3_linear.h
Date: 2025-03-05 17:18:30
Exec Total Coverage
Lines: 129 136 94.9%
Branches: 127 234 54.3%

Line Branch Exec Source
1 #ifndef _STRUCT_SO3_LINEAR_H
2 #define _STRUCT_SO3_LINEAR_H
3
4 #include <Eigen/Geometry>
5 #include <boost/math/constants/constants.hpp>
6 #include <boost/serialization/split_free.hpp>
7 #include <boost/serialization/vector.hpp>
8
9 #include "MathDefs.h"
10 #include "constant_curve.h"
11 #include "curve_abc.h"
12
13 namespace ndcurves {
14
15 /// \class SO3Linear.
16 /// \brief Represents a linear interpolation in SO3, using the slerp method
17 /// provided by Eigen::Quaternion
18 ///
19 ///
20 template <typename Time = double, typename Numeric = Time, bool Safe = false>
21 struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t> {
22 typedef Numeric Scalar;
23 typedef matrix3_t point_t;
24 typedef point3_t point_derivate_t;
25 typedef Eigen::Quaternion<Scalar> quaternion_t;
26 typedef Time time_t;
27 typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t> curve_abc_t;
28 typedef constant_curve<Time, Numeric, Safe, point_derivate_t>
29 curve_derivate_t;
30 typedef SO3Linear<Time, Numeric, Safe> SO3Linear_t;
31
32 public:
33 /* Constructors - destructors */
34 /// \brief Empty constructor. Curve obtained this way can not perform other
35 /// class functions.
36 ///
37 29 SO3Linear()
38 : curve_abc_t(),
39 29 dim_(3),
40 29 init_rot_(),
41
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
29 end_rot_(),
42
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
29 angular_vel_(),
43 29 T_min_(0),
44
1/2
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
58 T_max_(0) {}
45
46 /// \brief constructor with initial and final rotation and time bounds
47 9 SO3Linear(const quaternion_t& init_rot, const quaternion_t& end_rot,
48 const time_t t_min, const time_t t_max)
49 : curve_abc_t(),
50 9 dim_(3),
51 9 init_rot_(init_rot),
52
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
9 end_rot_(end_rot),
53
3/6
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9 times.
✗ Branch 8 not taken.
9 angular_vel_(computeAngularVelocity(init_rot.toRotationMatrix(),
54 end_rot.toRotationMatrix(), t_min,
55 t_max)),
56 9 T_min_(t_min),
57
1/2
✓ Branch 2 taken 9 times.
✗ Branch 3 not taken.
9 T_max_(t_max) {
58
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
9 safe_check();
59 9 }
60
61 /// \brief constructor with initial and final rotation expressed as rotation
62 /// matrix and time bounds
63 38 SO3Linear(const matrix3_t& init_rot, const matrix3_t& end_rot,
64 const time_t t_min, const time_t t_max)
65 : curve_abc_t(),
66 38 dim_(3),
67 38 init_rot_(quaternion_t(init_rot)),
68
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
38 end_rot_(quaternion_t(end_rot)),
69
1/2
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
38 angular_vel_(computeAngularVelocity(init_rot, end_rot, t_min, t_max)),
70 38 T_min_(t_min),
71
1/2
✓ Branch 2 taken 38 times.
✗ Branch 3 not taken.
38 T_max_(t_max) {
72
2/2
✓ Branch 1 taken 37 times.
✓ Branch 2 taken 1 times.
38 safe_check();
73 38 }
74
75 /// \brief constructor with initial and final rotation, time bounds are set to
76 /// [0;1]
77 SO3Linear(const quaternion_t& init_rot, const quaternion_t& end_rot)
78 : curve_abc_t(),
79 dim_(3),
80 init_rot_(init_rot),
81 end_rot_(end_rot),
82 angular_vel_(computeAngularVelocity(
83 init_rot.toRotationMatrix(), end_rot.toRotationMatrix(), 0., 1.)),
84 T_min_(0.),
85 T_max_(1.) {
86 safe_check();
87 }
88
89 /// \brief constructor with initial and final rotation expressed as rotation
90 /// matrix, time bounds are set to [0;1]
91 SO3Linear(const matrix3_t& init_rot, const matrix3_t& end_rot)
92 : curve_abc_t(),
93 dim_(3),
94 init_rot_(quaternion_t(init_rot)),
95 end_rot_(quaternion_t(end_rot)),
96 angular_vel_(computeAngularVelocity(init_rot, end_rot, 0., 1.)),
97 T_min_(0.),
98 T_max_(1.) {
99 safe_check();
100 }
101
102 /// \brief Destructor
103 282 virtual ~SO3Linear() {}
104
105 1 SO3Linear(const SO3Linear& other)
106 1 : dim_(other.dim_),
107 1 init_rot_(other.init_rot_),
108
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
1 end_rot_(other.end_rot_),
109
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
1 angular_vel_(other.angular_vel_),
110 1 T_min_(other.T_min_),
111
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
2 T_max_(other.T_max_) {}
112
113 47 point3_t computeAngularVelocity(const matrix3_t& init_rot,
114 const matrix3_t& end_rot, const double t_min,
115 const double t_max) {
116
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 47 times.
47 if (t_min == t_max) {
117 return point3_t::Zero();
118 } else {
119
6/12
✓ Branch 1 taken 47 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 47 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 47 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 47 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 47 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 47 times.
✗ Branch 17 not taken.
47 return log3(init_rot.transpose() * end_rot) / (t_max - t_min);
120 }
121 }
122
123 11942 quaternion_t computeAsQuaternion(const time_t t) const {
124
6/6
✓ Branch 0 taken 11939 times.
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 2 times.
✓ Branch 3 taken 11937 times.
✓ Branch 4 taken 5 times.
✓ Branch 5 taken 11937 times.
11942 if (Safe & !(T_min_ <= t && t <= T_max_)) {
125
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
5 throw std::invalid_argument(
126 "can't evaluate bezier curve, time t is out of range"); // TODO
127 }
128
3/4
✓ Branch 0 taken 50 times.
✓ Branch 1 taken 11887 times.
✓ Branch 3 taken 50 times.
✗ Branch 4 not taken.
11937 if (t >= T_max_) return end_rot_;
129
3/4
✓ Branch 0 taken 130 times.
✓ Branch 1 taken 11757 times.
✓ Branch 3 taken 130 times.
✗ Branch 4 not taken.
11887 if (t <= T_min_) return init_rot_;
130 11757 Scalar u = (t - T_min_) / (T_max_ - T_min_);
131
1/2
✓ Branch 1 taken 11757 times.
✗ Branch 2 not taken.
11757 return init_rot_.slerp(u, end_rot_);
132 }
133
134 /// \brief Evaluation of the SO3Linear at time t using Eigen slerp.
135 /// \param t : time when to evaluate the spline.
136 /// \return \f$x(t)\f$ point corresponding on spline at time t.
137 11936 virtual point_t operator()(const time_t t) const {
138
1/2
✓ Branch 2 taken 11931 times.
✗ Branch 3 not taken.
11936 return computeAsQuaternion(t).toRotationMatrix();
139 }
140
141 /**
142 * @brief isApprox check if other and *this are approximately equals.
143 * Only two curves of the same class can be approximately equals, for
144 * comparison between different type of curves see isEquivalent
145 * @param other the other curve to check
146 * @param prec the precision threshold, default
147 * Eigen::NumTraits<Numeric>::dummy_precision()
148 * @return true is the two curves are approximately equals
149 */
150 13 bool isApprox(
151 const SO3Linear_t& other,
152 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
153
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
26 return ndcurves::isApprox<Numeric>(T_min_, other.min()) &&
154
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
13 ndcurves::isApprox<Numeric>(T_max_, other.max()) &&
155
2/4
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
13 dim_ == other.dim() &&
156
3/6
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 13 times.
✗ Branch 7 not taken.
26 init_rot_.toRotationMatrix().isApprox(
157
2/4
✓ Branch 0 taken 13 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 13 times.
✗ Branch 4 not taken.
39 other.init_rot_.toRotationMatrix(), prec) &&
158
4/6
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 13 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 10 times.
✓ Branch 7 taken 3 times.
26 end_rot_.toRotationMatrix().isApprox(
159
1/2
✓ Branch 1 taken 13 times.
✗ Branch 2 not taken.
26 other.end_rot_.toRotationMatrix(), prec);
160 }
161
162 8 virtual bool isApprox(
163 const curve_abc_t* other,
164 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
165
1/2
✓ Branch 0 taken 8 times.
✗ Branch 1 not taken.
8 const SO3Linear_t* other_cast = dynamic_cast<const SO3Linear_t*>(other);
166
1/2
✓ Branch 0 taken 8 times.
✗ Branch 1 not taken.
8 if (other_cast)
167 8 return isApprox(*other_cast, prec);
168 else
169 return false;
170 }
171
172 5 virtual bool operator==(const SO3Linear_t& other) const {
173 5 return isApprox(other);
174 }
175
176 3 virtual bool operator!=(const SO3Linear_t& other) const {
177 3 return !(*this == other);
178 }
179
180 /// \brief Evaluation of the derivative of order N of spline at time t.
181 /// \param t : the time when to evaluate the spline.
182 /// \param order : order of derivative.
183 /// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derivative
184 /// spline at time t.
185 2046 virtual point_derivate_t derivate(const time_t t,
186 const std::size_t order) const {
187
4/4
✓ Branch 0 taken 2045 times.
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 2044 times.
2046 if ((t < T_min_ || t > T_max_) && Safe) {
188
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
2 throw std::invalid_argument(
189 "error in SO3_linear : time t to evaluate derivative should be in "
190 "range [Tmin, Tmax] of the curve");
191 }
192
4/6
✓ Branch 0 taken 1160 times.
✓ Branch 1 taken 884 times.
✓ Branch 2 taken 1160 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 1160 times.
2044 if (order > 1 || t > T_max_ || t < T_min_) {
193
1/2
✓ Branch 2 taken 884 times.
✗ Branch 3 not taken.
884 return point3_t::Zero(3);
194
2/2
✓ Branch 0 taken 1155 times.
✓ Branch 1 taken 5 times.
1160 } else if (order == 1) {
195 1155 return angular_vel_;
196 } else {
197
1/2
✓ Branch 2 taken 5 times.
✗ Branch 3 not taken.
5 throw std::invalid_argument("Order must be > 0 ");
198 }
199 }
200
201 2 curve_derivate_t compute_derivate(const std::size_t order) const {
202
1/2
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
2 return curve_derivate_t(derivate(T_min_, order), T_min_, T_max_);
203 }
204
205 /// \brief Compute the derived curve at order N.
206 /// \param order : order of derivative.
207 /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the
208 /// curve.
209 curve_derivate_t* compute_derivate_ptr(const std::size_t order) const {
210 return new curve_derivate_t(compute_derivate(order));
211 }
212
213 /*Helpers*/
214 /// \brief Get dimension of curve.
215 /// \return dimension of curve.
216 13 std::size_t virtual dim() const { return dim_; };
217 /// \brief Get the minimum time for which the curve is defined
218 /// \return \f$t_{min}\f$ lower bound of time range.
219 39 time_t min() const { return T_min_; }
220 /// \brief Get the maximum time for which the curve is defined.
221 /// \return \f$t_{max}\f$ upper bound of time range.
222 37 time_t max() const { return T_max_; }
223 /// \brief Get the degree of the curve.
224 /// \return \f$degree\f$, the degree of the curve.
225 virtual std::size_t degree() const { return 1; }
226 29 matrix3_t getInitRotation() const { return init_rot_.toRotationMatrix(); }
227 29 matrix3_t getEndRotation() const { return end_rot_.toRotationMatrix(); }
228 matrix3_t getInitRotation() { return init_rot_.toRotationMatrix(); }
229 matrix3_t getEndRotation() { return end_rot_.toRotationMatrix(); }
230
231 /*Helpers*/
232
233 /*Attributes*/
234 std::size_t dim_; // const
235 quaternion_t init_rot_, end_rot_;
236 point3_t angular_vel_; // const
237 time_t T_min_, T_max_; // const
238 /*Attributes*/
239
240 // Serialization of the class
241 friend class boost::serialization::access;
242
243 template <class Archive>
244 58 void load(Archive& ar, const unsigned int version) {
245 if (version) {
246 // Do something depending on version ?
247 }
248
2/4
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 29 times.
✗ Branch 6 not taken.
58 ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
249
1/2
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
58 ar >> boost::serialization::make_nvp("dim", dim_);
250
2/4
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 29 times.
✗ Branch 5 not taken.
58 matrix3_t init, end;
251
1/2
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
58 ar >> boost::serialization::make_nvp("init_rotation", init);
252
1/2
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
58 ar >> boost::serialization::make_nvp("end_rotation", end);
253
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
58 init_rot_ = quaternion_t(init);
254
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
58 end_rot_ = quaternion_t(end);
255
1/2
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
58 ar >> boost::serialization::make_nvp("angular_vel", angular_vel_);
256
1/2
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
58 ar >> boost::serialization::make_nvp("T_min", T_min_);
257
1/2
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
58 ar >> boost::serialization::make_nvp("T_max", T_max_);
258 58 }
259
260 template <class Archive>
261 58 void save(Archive& ar, const unsigned int version) const {
262 if (version) {
263 // Do something depending on version ?
264 }
265
2/4
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 29 times.
✗ Branch 6 not taken.
58 ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
266
1/2
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
58 ar << boost::serialization::make_nvp("dim", dim_);
267
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
58 matrix3_t init_matrix(getInitRotation());
268
1/2
✓ Branch 1 taken 29 times.
✗ Branch 2 not taken.
58 matrix3_t end_matrix(getEndRotation());
269
1/2
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
58 ar << boost::serialization::make_nvp("init_rotation", init_matrix);
270
1/2
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
58 ar << boost::serialization::make_nvp("end_rotation", end_matrix);
271
1/2
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
58 ar << boost::serialization::make_nvp("angular_vel", angular_vel_);
272
1/2
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
58 ar << boost::serialization::make_nvp("T_min", T_min_);
273
1/2
✓ Branch 2 taken 29 times.
✗ Branch 3 not taken.
58 ar << boost::serialization::make_nvp("T_max", T_max_);
274 58 }
275
276 116 BOOST_SERIALIZATION_SPLIT_MEMBER()
277
278 /// \brief Log: SO3 -> so3.
279 ///
280 /// Pseudo-inverse of log from \f$ SO3 -> { v \in so3, ||v|| \le pi } \f$.
281 ///
282 /// Code from pinocchio.
283 ///
284 /// \param[in] R the rotation matrix. ///
285 /// \return The angular velocity vector associated to the rotation matrix.
286 ///
287 47 point3_t log3(const matrix3_t& R) {
288 Scalar theta;
289 static const Scalar PI_value = boost::math::constants::pi<Scalar>();
290
291 47 point3_t res;
292 47 const Scalar tr = R.trace();
293
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 46 times.
47 if (tr > Scalar(3))
294 1 theta = 0; // acos((3-1)/2)
295
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 46 times.
46 else if (tr < Scalar(-1))
296 theta = PI_value; // acos((-1-1)/2)
297 else
298 46 theta = acos((tr - Scalar(1)) / Scalar(2));
299
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 47 times.
47 if (!std::isfinite(theta)) {
300 throw std::runtime_error("theta contains some NaN");
301 }
302
303 // From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
304
2/2
✓ Branch 0 taken 44 times.
✓ Branch 1 taken 3 times.
47 if (theta < PI_value - 1e-2) {
305 44 const Scalar t =
306 ((theta >
307 44 std::pow(std::numeric_limits<Scalar>::epsilon(),
308 Scalar(1) /
309 Scalar(4))) // precision of taylor serie of degree 3
310
2/2
✓ Branch 0 taken 39 times.
✓ Branch 1 taken 5 times.
44 ? theta / sin(theta)
311 : Scalar(1)) /
312 Scalar(2);
313 44 res(0) = t * (R(2, 1) - R(1, 2));
314 44 res(1) = t * (R(0, 2) - R(2, 0));
315 44 res(2) = t * (R(1, 0) - R(0, 1));
316 } else {
317 // 1e-2: A low value is not required since the computation is
318 // using explicit formula. However, the precision of this method
319 // is the square root of the precision with the antisymmetric
320 // method (Nominal case).
321 3 const Scalar cphi = cos(theta - PI_value);
322 3 const Scalar beta = theta * theta / (Scalar(1) + cphi);
323
5/10
✓ 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.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 3 times.
✗ Branch 14 not taken.
3 point3_t tmp((R.diagonal().array() + cphi) * beta);
324
4/8
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 3 times.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
6 res(0) = (R(2, 1) > R(1, 2) ? Scalar(1) : Scalar(-1)) *
325
3/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 3 times.
✗ Branch 7 not taken.
3 (tmp[0] > Scalar(0) ? sqrt(tmp[0]) : Scalar(0));
326
4/8
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 3 times.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
6 res(1) = (R(0, 2) > R(2, 0) ? Scalar(1) : Scalar(-1)) *
327
2/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
3 (tmp[1] > Scalar(0) ? sqrt(tmp[1]) : Scalar(0));
328
4/8
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 3 times.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
6 res(2) = (R(1, 0) > R(0, 1) ? Scalar(1) : Scalar(-1)) *
329
2/6
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 3 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
3 (tmp[2] > Scalar(0) ? sqrt(tmp[2]) : Scalar(0));
330 }
331
332 47 return res;
333 }
334
335 private:
336 47 void safe_check() {
337 if (Safe) {
338
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 46 times.
47 if (T_min_ > T_max_) {
339
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
1 throw std::invalid_argument("Tmin should be inferior to Tmax");
340 }
341 }
342 46 }
343
344 }; // struct SO3Linear
345
346 } // namespace ndcurves
347
348 DEFINE_CLASS_TEMPLATE_VERSION(
349 SINGLE_ARG(typename Time, typename Numeric, bool Safe),
350 SINGLE_ARG(ndcurves::SO3Linear<Time, Numeric, Safe>))
351
352 #endif // _STRUCT_SO3_LINEAR_H
353