GCC Code Coverage Report

Directory: ./
File: include/ndcurves/so3_smooth.h
Date: 2025-03-05 17:18:30
Exec Total Coverage
Lines: 149 170 87.6%
Branches: 136 288 47.2%
Line Branch Exec Source
1 #ifdef CURVES_WITH_PINOCCHIO_SUPPORT
2
3 #ifndef NDCURVES_SO3_SMOOTH_H
4 #define NDCURVES_SO3_SMOOTH_H
5
6 #include <Eigen/Geometry>
7 #include <boost/math/constants/constants.hpp>
8 #include <boost/serialization/split_free.hpp>
9 #include <boost/serialization/vector.hpp>
10 #include <pinocchio/fwd.hpp>
11 #include <pinocchio/spatial/explog.hpp>
12
13 #include "ndcurves/MathDefs.h"
14 #include "ndcurves/constant_curve.h"
15 #include "ndcurves/curve_abc.h"
16 #include "ndcurves/polynomial.h"
17
18 namespace ndcurves {
19
20 /// \class SO3Smooth.
21 /// \brief Represents a smooth interpolation in SO3, using an analytical
22 /// interpolation between 2 rotation.
23 ///
24 template <typename Time = double, typename Numeric = Time, bool Safe = false>
25 struct SO3Smooth : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t> {
26 typedef Numeric Scalar;
27 typedef matrix3_t point_t;
28 typedef matrix3_t_cst_ref point_t_ref;
29 typedef point3_t point_derivate_t;
30 typedef Time time_t;
31 typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t> curve_abc_t;
32 typedef constant_curve<Time, Numeric, Safe, point_derivate_t>
33 curve_derivate_t;
34 typedef SO3Smooth<Time, Numeric, Safe> SO3Smooth_t;
35 typedef polynomial<Time, Numeric, Safe, point1_t> min_jerk_t;
36
37 public:
38 /* Constructors - destructors */
39 /// \brief Empty constructor. Curve obtained this way can not perform other
40 /// class functions.
41 ///
42 11 SO3Smooth()
43 : curve_abc_t(),
44
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 init_rot_(point_t::Identity()),
45
2/4
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
 11 end_rot_(point_t::Identity()),
46 11 t_min_(0.0),
47 11 t_max_(1.0),
48
3/6
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11 times.
✗ Branch 8 not taken.
 11 min_jerk_(min_jerk_t::MinimumJerk(point1_t(0.0), point1_t(1.0), t_min_,
49 t_max_)),
50
3/6
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11 times.
✗ Branch 8 not taken.
 11 rot_diff_(pinocchio::log3(init_rot_.transpose() * end_rot_)),
51
1/2
✓ Branch 2 taken 11 times.
✗ Branch 3 not taken.
 22 dt_(1e-3) {}
52
53 /// \brief constructor with initial and final rotation and time bounds
54 3 SO3Smooth(const quaternion_t& init_quat, const quaternion_t& end_quat,
55 const time_t t_min, const time_t t_max)
56 : curve_abc_t(),
57 3 t_min_(t_min),
58 3 t_max_(t_max),
59
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 min_jerk_(min_jerk_t::MinimumJerk(point1_t(0.0), point1_t(1.0), t_min_,
60 t_max_)),
61
3/6
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 3 times.
✗ Branch 9 not taken.
 6 dt_(1e-3) {
62
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 quaternion_t tmp_init_quat = init_quat;
63
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 quaternion_t tmp_end_quat = end_quat;
64
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 tmp_init_quat.normalized();
65
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 tmp_end_quat.normalized();
66
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 init_rot_ = tmp_init_quat.toRotationMatrix();
67
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 end_rot_ = tmp_end_quat.toRotationMatrix();
68
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 rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_);
69
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 safe_check();
70 3 }
71
72 /// \brief constructor with initial and final rotation expressed as rotation
73 /// matrix and time bounds
74 1 SO3Smooth(const matrix3_t& init_rot, const matrix3_t& end_rot,
75 const time_t t_min, const time_t t_max)
76 : curve_abc_t(),
77 1 init_rot_(init_rot),
78
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 end_rot_(end_rot),
79 1 t_min_(t_min),
80 1 t_max_(t_max),
81
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.
 1 min_jerk_(min_jerk_t::MinimumJerk(point1_t(0.0), point1_t(1.0), t_min_,
82 t_max_)),
83
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.
 1 rot_diff_(pinocchio::log3(init_rot_.transpose() * end_rot_)),
84
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 dt_(1e-3) {
85
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 safe_check();
86 1 }
87
88 /// \brief constructor with initial and final rotation, time bounds are set to
89 /// [0;1]
90 1 SO3Smooth(const quaternion_t& init_rot, const quaternion_t& end_rot)
91 : curve_abc_t(),
92 1 t_min_(0.),
93 1 t_max_(1.),
94
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.
 1 min_jerk_(min_jerk_t::MinimumJerk(point1_t(0.0), point1_t(1.0), t_min_,
95 t_max_)),
96
3/6
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
 2 dt_(1e-3) {
97
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t init_rot_normalized = init_rot.normalized();
98
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t end_rot_normalized = end_rot.normalized();
99
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 init_rot_ = init_rot_normalized.toRotationMatrix();
100
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 end_rot_ = end_rot_normalized.toRotationMatrix();
101
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.
 1 rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_);
102
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 safe_check();
103 1 }
104
105 /// \brief constructor with initial and final rotation expressed as rotation
106 /// matrix, time bounds are set to [0;1]
107 1 SO3Smooth(const matrix3_t& init_rot, const matrix3_t& end_rot)
108 : curve_abc_t(),
109 1 init_rot_(init_rot),
110
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 end_rot_(end_rot),
111 1 t_min_(0.),
112 1 t_max_(1.),
113
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.
 1 min_jerk_(min_jerk_t::MinimumJerk(point1_t(0.0), point1_t(1.0), t_min_,
114 t_max_)),
115
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 2 dt_(1e-3) {
116
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.
 1 rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_);
117
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 safe_check();
118 1 }
119
120 // Generate functions:
121
122 /* Generators. */
123 /// \brief Empty constructor. Curve obtained this way can not perform other
124 /// class functions.
125 ///
126 1 void generate() {
127 1 init_rot_.setIdentity();
128 1 end_rot_.setIdentity();
129 1 t_min_ = 0.0;
130 1 t_max_ = 1.0;
131
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.
 1 min_jerk_t::MinimumJerk(min_jerk_, point1_t(0.0), point1_t(1.0), t_min_,
132 t_max_);
133
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_);
134 1 safe_check();
135 1 }
136
137 /// \brief constructor with initial and final rotation and time bounds
138 6 void generate(const matrix3_t& init_rot, const matrix3_t& end_rot,
139 const time_t t_min, const time_t t_max) {
140 6 init_rot_ = init_rot;
141 6 end_rot_ = end_rot;
142 6 t_min_ = t_min;
143 6 t_max_ = t_max;
144
3/6
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 6 min_jerk_t::MinimumJerk(min_jerk_, point1_t(0.0), point1_t(1.0), t_min_,
145 t_max_);
146
2/4
✓ Branch 2 taken 6 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6 times.
✗ Branch 6 not taken.
 6 rot_diff_ = pinocchio::log3(init_rot_.transpose() * end_rot_);
147 6 safe_check();
148 6 }
149
150 /// \brief constructor with initial and final rotation expressed as rotation
151 /// matrix and time bounds
152 1 void generate(const quaternion_t& init_rot, const quaternion_t& end_rot,
153 const time_t t_min, const time_t t_max) {
154
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t init_rot_normalized = init_rot.normalized();
155
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t end_rot_normalized = end_rot.normalized();
156
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.
 1 generate(init_rot_normalized.toRotationMatrix(),
157 end_rot_normalized.toRotationMatrix(), t_min, t_max);
158 1 }
159
160 1 void generate(const quaternion_t& init_rot, const quaternion_t& end_rot) {
161
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t init_rot_normalized = init_rot.normalized();
162
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 quaternion_t end_rot_normalized = end_rot.normalized();
163
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.
 1 generate(init_rot_normalized.toRotationMatrix(),
164 end_rot_normalized.toRotationMatrix(), 0.0, 1.0);
165 1 }
166
167 /// \brief constructor with initial and final rotation expressed as rotation
168 /// matrix, time bounds are set to [0;1]
169 1 void generate(const matrix3_t& init_rot, const matrix3_t& end_rot) {
170 1 generate(init_rot, end_rot, 0.0, 1.0);
171 1 }
172
173 /// \brief Destructor
174 36 virtual ~SO3Smooth() {}
175
176 /// \brief Copy constructor
177 1 SO3Smooth(const SO3Smooth& other)
178 : curve_abc_t(),
179 1 init_rot_(other.init_rot_),
180
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 end_rot_(other.end_rot_),
181 1 t_min_(other.t_min_),
182 1 t_max_(other.t_max_),
183
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 min_jerk_(other.min_jerk_),
184
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 rot_diff_(other.rot_diff_),
185
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 2 dt_(1e-3) {}
186
187 /// \brief Evaluation of the SO3Smooth at time t using \$f R(t) = R1 *
188 /// exp3(min_jerk(t) log3(R1.T * R2)) \$f. \param t : time when to evaluate
189 /// the spline. \return \f$x(t)\f$ point corresponding on spline at time t.
190 65679 virtual point_t operator()(const time_t t) const {
191
2/4
✓ Branch 0 taken 65679 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 65679 times.
 65679 if ((t < t_min_ || t > t_max_) && Safe) {
192 ✗ throw std::invalid_argument(
193 "error in SO3Smooth : time t to evaluate derivative should be in "
194 "range [Tmin, Tmax] of the curve");
195 }
196
5/10
✓ Branch 2 taken 65679 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 65679 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 65679 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 65679 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 65679 times.
✗ Branch 15 not taken.
 131358 return init_rot_ * pinocchio::exp3(min_jerk_(t)[0] * rot_diff_);
197 }
198
199 /**
200 * @brief isApprox check if other and *this are approximately equals.
201 * Only two curves of the same class can be approximately equals, for
202 * comparison between different type of curves see isEquivalent
203 * @param other the other curve to check
204 * @param prec the precision threshold, default
205 * Eigen::NumTraits<Numeric>::dummy_precision()
206 * @return true is the two curves are approximately equals
207 */
208 3 bool isApprox(
209 const SO3Smooth_t& other,
210 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
211
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 return ndcurves::isApprox<Numeric>(t_min_, other.min()) &&
212 3 ndcurves::isApprox<Numeric>(t_max_, other.max()) &&
213
2/4
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 9 init_rot_.isApprox(other.init_rot_, prec) &&
214
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 6 end_rot_.isApprox(other.end_rot_, prec);
215 }
216
217 ✗ virtual bool isApprox(
218 const curve_abc_t* other,
219 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
220 ✗ const SO3Smooth_t* other_cast = dynamic_cast<const SO3Smooth_t*>(other);
221 ✗ if (other_cast)
222 ✗ return isApprox(*other_cast, prec);
223 else
224 ✗ return false;
225 }
226
227 3 virtual bool operator==(const SO3Smooth_t& other) const {
228 3 return isApprox(other);
229 }
230
231 ✗ virtual bool operator!=(const SO3Smooth_t& other) const {
232 ✗ return !(*this == other);
233 }
234
235 /// \brief Evaluation of the derivative of order N of spline at time t.
236 /// \param t : the time when to evaluate the spline.
237 /// \param order : order of derivative.
238 /// \return \f$ \frac{d^N x(t)}{dt^N} \f$ point corresponding on derivative
239 /// spline at time t. Specifically
240 /// \f$ \frac{d R(t)}{dt} = R1 * \frac{d exp^{min_jerk(t) diff}{dw} *
241 /// \frac{d min_jerk(t)}{dt} diff \f$ with \f$ R^3 \f$ -> \f$ SO3 \f$
242 /// : \f$ R = exp(w) \f$ or \frac{d R(t)}{dt} = = R1 *
243 /// Jexp3(min_jerk(t) diff) * \frac{d min_jerk(t)}{dt} diff \f$. We
244 /// use the pinocchio library in order to compute the exp3, log3, and
245 /// Jexp3 fonctions.
246 37994 virtual point_derivate_t derivate(const time_t t,
247 const std::size_t order) const {
248
1/2
✓ Branch 2 taken 37994 times.
✗ Branch 3 not taken.
 37994 point_derivate_t out = point_derivate_t::Zero();
249
2/4
✓ Branch 0 taken 37994 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✓ Branch 3 taken 37994 times.
 37994 if ((t < t_min_ || t > t_max_) && Safe) {
250 ✗ throw std::invalid_argument(
251 "error in SO3Smooth : time t to evaluate derivative should be in "
252 "range [Tmin, Tmax] of the curve");
253 }
254
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 37994 times.
 37994 if (order > 2) {
255 ✗ throw std::invalid_argument("Order must be in {1, 2}.");
256
2/2
✓ Branch 0 taken 9498 times.
✓ Branch 1 taken 28496 times.
 37994 } else if (order == 2) {
257 9498 double t1 = t;
258 9498 double t2 = t + dt_;
259
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 9498 times.
 9498 if (t2 > t_max_) {
260 ✗ t1 = t - dt_;
261 ✗ t2 = t;
262 }
263
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 9498 times.
 9498 if (t1 < t_min_) {
264 ✗ t1 = t_min_;
265 ✗ t2 = t_max_;
266 }
267
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 9498 times.
 9498 if (t1 == t2) {
268 ✗ out.setZero();
269 } else {
270
5/10
✓ Branch 1 taken 9498 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 9498 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 9498 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 9498 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 9498 times.
✗ Branch 14 not taken.
 9498 out = (derivate(t1, 1) - derivate(t2, 1)) / (t2 - t1);
271 }
272
1/2
✓ Branch 0 taken 28496 times.
✗ Branch 1 not taken.
 28496 } else if (order == 1) {
273
1/2
✓ Branch 1 taken 28496 times.
✗ Branch 2 not taken.
 28496 matrix3_t jexp;
274
4/8
✓ Branch 1 taken 28496 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28496 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28496 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 28496 times.
✗ Branch 11 not taken.
 28496 pinocchio::Jexp3(min_jerk_(t)[0] * rot_diff_, jexp);
275
4/8
✓ Branch 1 taken 28496 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28496 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28496 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 28496 times.
✗ Branch 11 not taken.
 28496 out.noalias() =
276
3/6
✓ Branch 1 taken 28496 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 28496 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 28496 times.
✗ Branch 8 not taken.
 56992 init_rot_ * jexp * min_jerk_.derivate(t, 1)[0] * rot_diff_;
277 } else {
278 ✗ throw std::invalid_argument("Order must be > 0 ");
279 }
280 37994 return out;
281 }
282
283 ✗ curve_derivate_t compute_derivate(const std::size_t /*order*/) const {
284 ✗ throw std::runtime_error("This function has not been implemented yet.");
285 }
286
287 /// \brief Compute the derived curve at order N.
288 /// \param order : order of derivative.
289 /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the
290 /// curve.
291 ✗ curve_derivate_t* compute_derivate_ptr(const std::size_t order) const {
292 ✗ return new curve_derivate_t(compute_derivate(order));
293 }
294
295 /*Helpers*/
296 /// \brief Get dimension of curve.
297 /// \return dimension of curve.
298 14 std::size_t virtual dim() const { return 3; };
299 /// \brief Get the minimum time for which the curve is defined
300 /// \return \f$t_{min}\f$ lower bound of time range.
301 20 time_t min() const { return t_min_; }
302 /// \brief Get the maximum time for which the curve is defined.
303 /// \return \f$t_{max}\f$ upper bound of time range.
304 20 time_t max() const { return t_max_; }
305 /// \brief Get the degree of the curve.
306 /// \return \f$degree\f$, the degree of the curve.
307 14 virtual std::size_t degree() const { return 5; }
308 14 matrix3_t_cst_ref get_init_rotation() const { return init_rot_; }
309 14 matrix3_t_cst_ref get_end_rotation() const { return end_rot_; }
310
311 /*Attributes*/
312 point_t init_rot_; /*! @brief Initial rotation. */
313 point_t end_rot_; /*! @brief End rotation. */
314 time_t t_min_; /*! @brief Start time of the trajectory. */
315 time_t t_max_; /*! @brief End time of the trajectory. */
316 min_jerk_t
317 min_jerk_; /*! @brief Min jerk trajectory from t_min_ and t_max_. */
318 point3_t
319 rot_diff_; /*! @brief The difference between init_rot_ and end_rot_. */
320 const double dt_; /*! @brief Finite differentiation delta time for the
321 acceleration computation. */
322
323 // Serialization of the class
324 friend class boost::serialization::access;
325
326 template <class Archive>
327 6 void load(Archive& ar, const unsigned int version) {
328 if (version) {
329 // Do something depending on version ?
330 }
331
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 ar >> BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
332
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 ar >> boost::serialization::make_nvp("init_rotation", init_rot_);
333
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 ar >> boost::serialization::make_nvp("end_rotation", end_rot_);
334
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 ar >> boost::serialization::make_nvp("t_min", t_min_);
335
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 ar >> boost::serialization::make_nvp("t_max", t_max_);
336
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 ar >> boost::serialization::make_nvp("min_jerk", min_jerk_);
337
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 ar >> boost::serialization::make_nvp("rot_diff", rot_diff_);
338 6 }
339
340 template <class Archive>
341 6 void save(Archive& ar, const unsigned int version) const {
342 if (version) {
343 // Do something depending on version ?
344 }
345
1/2
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
 6 ar << BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
346
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 ar << boost::serialization::make_nvp("init_rotation", init_rot_);
347
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 ar << boost::serialization::make_nvp("end_rotation", end_rot_);
348
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 ar << boost::serialization::make_nvp("t_min", t_min_);
349
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 ar << boost::serialization::make_nvp("t_max", t_max_);
350
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 ar << boost::serialization::make_nvp("min_jerk", min_jerk_);
351
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 6 ar << boost::serialization::make_nvp("rot_diff", rot_diff_);
352 6 }
353
354 12 BOOST_SERIALIZATION_SPLIT_MEMBER()
355
356 /// \brief Log: SO3 -> so3.
357 ///
358 /// Pseudo-inverse of log from \f$ SO3 -> { v \in so3, ||v|| \le pi } \f$.
359 ///
360 /// Code from pinocchio.
361 ///
362 /// \param[in] R the rotation matrix. ///
363 /// \return The angular velocity vector associated to the rotation matrix.
364 ///
365 point3_t log3(const matrix3_t& R) {
366 Scalar theta;
367 static const Scalar PI_value = boost::math::constants::pi<Scalar>();
368
369 point3_t res;
370 const Scalar tr = R.trace();
371 if (tr > Scalar(3))
372 theta = 0; // acos((3-1)/2)
373 else if (tr < Scalar(-1))
374 theta = PI_value; // acos((-1-1)/2)
375 else
376 theta = acos((tr - Scalar(1)) / Scalar(2));
377 if (!std::isfinite(theta)) {
378 throw std::runtime_error("theta contains some NaN");
379 }
380
381 // From runs of hpp-constraints/tests/logarithm.cc: 1e-6 is too small.
382 if (theta < PI_value - 1e-2) {
383 const Scalar t =
384 ((theta >
385 std::pow(std::numeric_limits<Scalar>::epsilon(),
386 Scalar(1) /
387 Scalar(4))) // precision of taylor serie of degree 3
388 ? theta / sin(theta)
389 : Scalar(1)) /
390 Scalar(2);
391 res(0) = t * (R(2, 1) - R(1, 2));
392 res(1) = t * (R(0, 2) - R(2, 0));
393 res(2) = t * (R(1, 0) - R(0, 1));
394 } else {
395 // 1e-2: A low value is not required since the computation is
396 // using explicit formula. However, the precision of this method
397 // is the square root of the precision with the antisymmetric
398 // method (Nominal case).
399 const Scalar cphi = cos(theta - PI_value);
400 const Scalar beta = theta * theta / (Scalar(1) + cphi);
401 point3_t tmp((R.diagonal().array() + cphi) * beta);
402 res(0) = (R(2, 1) > R(1, 2) ? Scalar(1) : Scalar(-1)) *
403 (tmp[0] > Scalar(0) ? sqrt(tmp[0]) : Scalar(0));
404 res(1) = (R(0, 2) > R(2, 0) ? Scalar(1) : Scalar(-1)) *
405 (tmp[1] > Scalar(0) ? sqrt(tmp[1]) : Scalar(0));
406 res(2) = (R(1, 0) > R(0, 1) ? Scalar(1) : Scalar(-1)) *
407 (tmp[2] > Scalar(0) ? sqrt(tmp[2]) : Scalar(0));
408 }
409
410 return res;
411 }
412
413 private:
414 13 void safe_check() {
415 if (Safe) {
416
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 13 times.
 13 if (t_min_ > t_max_) {
417 ✗ throw std::invalid_argument("Tmin should be inferior to Tmax");
418 }
419 }
420 13 }
421
422 }; // struct SO3Smooth
423
424 } // namespace ndcurves
425
426 DEFINE_CLASS_TEMPLATE_VERSION(
427 SINGLE_ARG(typename Time, typename Numeric, bool Safe),
428 SINGLE_ARG(ndcurves::SO3Smooth<Time, Numeric, Safe>))
429
430 #endif // NDCURVES_SO3_SMOOTH_H
431 #endif // CURVES_WITH_PINOCCHIO_SUPPORT
432