GCC Code Coverage Report

Directory: ./
File: include/ndcurves/helpers/effector_spline_rotation.h
Date: 2025-03-05 17:18:30
Exec Total Coverage
Lines: 70 93 75.3%
Branches: 49 112 43.8%
Line Branch Exec Source
1 /**
2 * \file exact_cubic.h
3 * \brief class allowing to create an Exact cubic spline.
4 * \author Steve T.
5 * \version 0.1
6 * \date 06/17/2013
7 *
8 * This file contains definitions for the ExactCubic class.
9 * Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique
10 * set of cubic splines fulfulling those 4 restrictions :
11 * - x_i(t_i) = x_i* ; this means that the curve passes trough each waypoint
12 * - x_i(t_i+1) = x_i+1* ;
13 * - its derivative is continous at t_i+1
14 * - its acceleration is continous at t_i+1
15 * more details in paper "Task-Space Trajectories via Cubic Spline Optimization"
16 * By J. Zico Kolter and Andrew Y.ng (ICRA 2009)
17 */
18
19 #ifndef _CLASS_EFFECTOR_SPLINE_ROTATION
20 #define _CLASS_EFFECTOR_SPLINE_ROTATION
21
22 #include <Eigen/Geometry>
23
24 #include "ndcurves/curve_abc.h"
25 #include "ndcurves/helpers/effector_spline.h"
26
27 namespace ndcurves {
28 namespace helpers {
29 typedef Eigen::Matrix<Numeric, 4, 1> quat_t;
30 typedef Eigen::Ref<quat_t> quat_ref_t;
31 typedef const Eigen::Ref<const quat_t> quat_ref_const_t;
32 typedef Eigen::Matrix<Numeric, 7, 1> config_t;
33 typedef curve_abc<Time, Numeric, false, quat_t> curve_abc_quat_t;
34 typedef std::pair<Numeric, quat_t> waypoint_quat_t;
35 typedef std::vector<waypoint_quat_t> t_waypoint_quat_t;
36 typedef Eigen::Matrix<Numeric, 1, 1> point_one_dim_t;
37 typedef exact_cubic<Numeric, Numeric, false, point_one_dim_t>
38 exact_cubic_constraint_one_dim;
39 typedef std::pair<Numeric, point_one_dim_t> waypoint_one_dim_t;
40 typedef std::vector<waypoint_one_dim_t> t_waypoint_one_dim_t;
41
42 class rotation_spline : public curve_abc_quat_t {
43 public:
44 7 rotation_spline(quat_ref_const_t quat_from = quat_t(0, 0, 0, 1),
45 quat_ref_const_t quat_to = quat_t(0, 0, 0, 1),
46 const double min = 0., const double max = 1.)
47 7 : curve_abc_quat_t(),
48
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 quat_from_(quat_from.data()),
49
1/2
✓ Branch 2 taken 7 times.
✗ Branch 3 not taken.
 7 quat_to_(quat_to.data()),
50 7 dim_(4),
51 7 min_(min),
52 7 max_(max),
53
1/2
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 14 time_reparam_(computeWayPoints()) {}
54
55 44 ~rotation_spline() {}
56
57 /* Copy Constructors / operator=*/
58 rotation_spline& operator=(const rotation_spline& from) {
59 quat_from_ = from.quat_from_;
60 quat_to_ = from.quat_to_;
61 dim_ = from.dim_;
62 min_ = from.min_;
63 max_ = from.max_;
64 time_reparam_ = exact_cubic_constraint_one_dim(from.time_reparam_);
65 return *this;
66 }
67 /* Copy Constructors / operator=*/
68
69 3 quat_t operator()(const Numeric t) const {
70
2/2
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 2 times.
 3 if (t <= min()) return quat_from_.coeffs();
71
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
 2 if (t >= max()) return quat_to_.coeffs();
72 // normalize u
73 2 Numeric u = (t - min()) / (max() - min());
74 // re-parametrize u
75
3/6
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 2 times.
✗ Branch 10 not taken.
 4 return quat_from_.slerp(time_reparam_(u)[0], quat_to_).coeffs();
76 }
77
78 /**
79 * @brief isApprox check if other and *this are approximately equals.
80 * Only two curves of the same class can be approximately equals, for
81 * comparison between different type of curves see isEquivalent
82 * @param other the other curve to check
83 * @param prec the precision threshold, default
84 * Eigen::NumTraits<Numeric>::dummy_precision()
85 * @return true is the two curves are approximately equals
86 */
87 ✗ bool isApprox(
88 const rotation_spline& other,
89 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
90 ✗ return ndcurves::isApprox<Numeric>(min_, other.min_) &&
91 ✗ ndcurves::isApprox<Numeric>(max_, other.max_) &&
92 ✗ dim_ == other.dim_ && quat_from_.isApprox(other.quat_from_, prec) &&
93 ✗ quat_to_.isApprox(other.quat_to_, prec) &&
94 ✗ time_reparam_.isApprox(other.time_reparam_, prec);
95 }
96
97 ✗ virtual bool isApprox(
98 const curve_abc_quat_t* other,
99 const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const {
100 const rotation_spline* other_cast =
101 ✗ dynamic_cast<const rotation_spline*>(other);
102 ✗ if (other_cast)
103 ✗ return isApprox(*other_cast, prec);
104 else
105 ✗ return false;
106 }
107
108 ✗ virtual bool operator==(const rotation_spline& other) const {
109 ✗ return isApprox(other);
110 }
111
112 ✗ virtual bool operator!=(const rotation_spline& other) const {
113 ✗ return !(*this == other);
114 }
115
116 ✗ virtual quat_t derivate(time_t /*t*/, std::size_t /*order*/) const {
117 ✗ throw std::runtime_error(
118 ✗ "TODO quaternion spline does not implement derivate");
119 }
120
121 /// \brief Compute the derived curve at order N.
122 /// \param order : order of derivative.
123 /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the
124 /// curve.
125 ✗ curve_abc_quat_t* compute_derivate_ptr(const std::size_t /*order*/) const {
126 ✗ throw std::logic_error(
127 ✗ "Compute derivate for quaternion spline is not implemented yet.");
128 }
129
130 /// \brief Initialize time re-parametrization for spline.
131 7 exact_cubic_constraint_one_dim computeWayPoints() const {
132 7 t_waypoint_one_dim_t waypoints;
133
4/8
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
 7 waypoints.push_back(std::make_pair(0, point_one_dim_t::Zero()));
134
4/8
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 7 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 7 times.
✗ Branch 11 not taken.
 7 waypoints.push_back(std::make_pair(1, point_one_dim_t::Ones()));
135
1/2
✓ Branch 3 taken 7 times.
✗ Branch 4 not taken.
 14 return exact_cubic_constraint_one_dim(waypoints.begin(), waypoints.end());
136 7 }
137
138 /// \brief Get dimension of curve.
139 /// \return dimension of curve.
140 9 virtual std::size_t dim() const { return dim_; }
141 /// \brief Get the minimum time for which the curve is defined.
142 /// \return \f$t_{min}\f$, lower bound of time range.
143 16 virtual time_t min() const { return min_; }
144 /// \brief Get the maximum time for which the curve is defined.
145 /// \return \f$t_{max}\f$, upper bound of time range.
146 10 virtual time_t max() const { return max_; }
147 /// \brief Get the degree of the curve.
148 /// \return \f$degree\f$, the degree of the curve.
149 ✗ virtual std::size_t degree() const { return 1; }
150
151 /*Attributes*/
152 Eigen::Quaterniond quat_from_; // const
153 Eigen::Quaterniond quat_to_; // const
154 std::size_t dim_; // const
155 double min_; // const
156 double max_; // const
157 exact_cubic_constraint_one_dim time_reparam_; // const
158 /*Attributes*/
159 }; // End class rotation_spline
160
161 typedef exact_cubic<Time, Numeric, false, quat_t,
162 std::vector<quat_t, Eigen::aligned_allocator<quat_t> >,
163 rotation_spline>
164 exact_cubic_quat_t;
165
166 /// \class effector_spline_rotation.
167 /// \brief Represents a trajectory for and end effector.
168 /// uses the method effector_spline to create a spline trajectory.
169 /// Additionally, handles the rotation of the effector as follows:
170 /// does not rotate during the take off and landing phase,
171 /// then uses a SLERP algorithm to interpolate the rotation in the quaternion
172 /// space.
173 class effector_spline_rotation {
174 /* Constructors - destructors */
175 public:
176 /// \brief Constructor.
177 /// Given a set of waypoints, and the normal vector of the start and
178 /// ending positions, automatically create the spline such that:
179 /// + init and end velocities / accelerations are 0
180 /// + the effector lifts and lands exactly in the direction of the specified
181 /// normals. \param wayPointsBegin : an iterator pointing to the first
182 /// element of a waypoint container. \param wayPointsEnd : an iterator
183 /// pointing to the last element of a waypoint container. \param to_quat : 4D
184 /// vector, quaternion indicating rotation at take off(x, y, z, w). \param
185 /// land_quat : 4D vector, quaternion indicating rotation at landing
186 /// (x, y, z, w). \param lift_normal : normal to be followed by end
187 /// effector at take-off. \param land_normal : normal to be followed by
188 /// end effector at landing. \param lift_offset : length of the straight
189 /// line along normal at take-off. \param land_offset : length of the
190 /// straight line along normal at landing. \param lift_offset_duration : time
191 /// travelled along straight line at take-off. \param land_offset_duration :
192 /// time travelled along straight line at landing.
193 ///
194 template <typename In>
195 2 effector_spline_rotation(In wayPointsBegin, In wayPointsEnd,
196 quat_ref_const_t& to_quat = quat_t(0, 0, 0, 1),
197 quat_ref_const_t& land_quat = quat_t(0, 0, 0, 1),
198 const Point& lift_normal = Eigen::Vector3d::UnitZ(),
199 const Point& land_normal = Eigen::Vector3d::UnitZ(),
200 const Numeric lift_offset = 0.02,
201 const Numeric land_offset = 0.02,
202 const Time lift_offset_duration = 0.02,
203 const Time land_offset_duration = 0.02)
204 4 : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal,
205 land_normal, lift_offset, land_offset,
206 lift_offset_duration, land_offset_duration)),
207 2 to_quat_(to_quat.data()),
208 2 land_quat_(land_quat.data()),
209 2 time_lift_offset_(spline_->min() + lift_offset_duration),
210 2 time_land_offset_(spline_->max() - land_offset_duration),
211 4 quat_spline_(simple_quat_spline()) {
212 // NOTHING
213 2 }
214
215 /// \brief Constructor.
216 /// Given a set of waypoints, and the normal vector of the start and
217 /// ending positions, automatically create the spline such that:
218 /// + init and end velocities / accelerations are 0
219 /// + the effector lifts and lands exactly in the direction of the specified
220 /// normals. \param wayPointsBegin : an iterator pointing to the first
221 /// element of a waypoint container. \param wayPointsEnd : an iterator
222 /// pointing to the last element of a waypoint container. \param
223 /// quatWayPointsBegin : en iterator pointing to the first element of a 4D
224 /// vector (x, y, z, w) container of
225 /// quaternions indicating rotation at specific time steps.
226 /// \param quatWayPointsEnd : en iterator pointing to the last element of
227 /// a 4D vector (x, y, z, w) container of
228 /// quaternions indicating rotation at specific time steps.
229 /// \param lift_normal : normal to be followed by end effector at
230 /// take-off. \param land_normal : normal to be followed by end
231 /// effector at landing. \param lift_offset : length of the straight
232 /// line along normal at take-off. \param land_offset : length of the
233 /// straight line along normal at landing. \param lift_offset_duration : time
234 /// travelled along straight line at take-off. \param land_offset_duration :
235 /// time travelled along straight line at landing.
236 ///
237 template <typename In, typename InQuat>
238 1 effector_spline_rotation(In wayPointsBegin, In wayPointsEnd,
239 InQuat quatWayPointsBegin, InQuat quatWayPointsEnd,
240 const Point& lift_normal = Eigen::Vector3d::UnitZ(),
241 const Point& land_normal = Eigen::Vector3d::UnitZ(),
242 const Numeric lift_offset = 0.02,
243 const Numeric land_offset = 0.02,
244 const Time lift_offset_duration = 0.02,
245 const Time land_offset_duration = 0.02)
246 2 : spline_(effector_spline(wayPointsBegin, wayPointsEnd, lift_normal,
247 land_normal, lift_offset, land_offset,
248 lift_offset_duration, land_offset_duration)),
249 1 to_quat_((quatWayPointsBegin->second).data()),
250
2/4
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 land_quat_(((quatWayPointsEnd - 1)->second).data()),
251 1 time_lift_offset_(spline_->min() + lift_offset_duration),
252 1 time_land_offset_(spline_->max() - land_offset_duration),
253 2 quat_spline_(quat_spline(quatWayPointsBegin, quatWayPointsEnd)) {
254 // NOTHING
255 1 }
256
257
1/2
✓ Branch 0 taken 3 times.
✗ Branch 1 not taken.
 6 virtual ~effector_spline_rotation() { delete spline_; }
258 /* Constructors - destructors */
259
260 /*Helpers*/
261 Numeric min() const { return spline_->min(); }
262 Numeric max() const { return spline_->max(); }
263 /*Helpers*/
264
265 /*Operations*/
266 /// \brief Evaluation of the effector position and rotation at time t.
267 /// \param t : the time when to evaluate the spline.
268 /// \return A 7D vector where the 3 first values are the 3D position and the
269 /// 4 last are the quaternion describing the rotation.
270 ///
271 15 config_t operator()(const Numeric t) const {
272 15 config_t res;
273
2/4
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
 15 res.head<3>() = (*spline_)(t);
274
2/4
✓ Branch 2 taken 15 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15 times.
✗ Branch 6 not taken.
 15 res.tail<4>() = interpolate_quat(t);
275 15 return res;
276 }
277
278 15 quat_t interpolate_quat(const Numeric t) const {
279
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 9 times.
 15 if (t <= time_lift_offset_) return to_quat_.coeffs();
280
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 3 times.
 9 if (t >= time_land_offset_) return land_quat_.coeffs();
281 3 return quat_spline_(t);
282 }
283
284 private:
285 2 exact_cubic_quat_t simple_quat_spline() const {
286 2 std::vector<rotation_spline> splines;
287
4/8
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 2 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 2 times.
✗ Branch 12 not taken.
 2 splines.push_back(rotation_spline(to_quat_.coeffs(), land_quat_.coeffs(),
288 2 time_lift_offset_, time_land_offset_));
289
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 return exact_cubic_quat_t(splines);
290 2 }
291
292 template <typename InQuat>
293 1 exact_cubic_quat_t quat_spline(InQuat quatWayPointsBegin,
294 InQuat quatWayPointsEnd) const {
295
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (std::distance(quatWayPointsBegin, quatWayPointsEnd) < 1) {
296 ✗ return simple_quat_spline();
297 }
298 1 exact_cubic_quat_t::t_spline_t splines;
299 1 InQuat it(quatWayPointsBegin);
300 1 Time init = time_lift_offset_;
301
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Eigen::Quaterniond current_quat = to_quat_;
302
2/2
✓ Branch 1 taken 3 times.
✓ Branch 2 taken 1 times.
 4 for (; it != quatWayPointsEnd; ++it) {
303
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 splines.push_back(
304
3/6
✓ Branch 3 taken 3 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 3 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 3 times.
✗ Branch 11 not taken.
 6 rotation_spline(current_quat.coeffs(), it->second, init, it->first));
305
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 current_quat = it->second;
306 3 init = it->first;
307 }
308
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.
 2 splines.push_back(rotation_spline(
309 2 current_quat.coeffs(), land_quat_.coeffs(), init, time_land_offset_));
310
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 return exact_cubic_quat_t(splines);
311 1 }
312 /*Operations*/
313
314 public:
315 /*Attributes*/
316 const exact_cubic_t* spline_;
317 const Eigen::Quaterniond to_quat_;
318 const Eigen::Quaterniond land_quat_;
319 const double time_lift_offset_;
320 const double time_land_offset_;
321 const exact_cubic_quat_t quat_spline_;
322 /*Attributes*/
323 }; // End class effector_spline_rotation
324
325 } // namespace helpers
326 } // namespace ndcurves
327 #endif //_CLASS_EFFECTOR_SPLINE_ROTATION
328