GCC Code Coverage Report

Directory: ./
File: include/ndcurves/helpers/effector_spline.h
Date: 2025-03-05 17:18:30
Exec Total Coverage
Lines: 40 42 95.2%
Branches: 43 90 47.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_EFFECTORSPLINE
20 #define _CLASS_EFFECTORSPLINE
21
22 #include "ndcurves/exact_cubic.h"
23
24 namespace ndcurves {
25 namespace helpers {
26 typedef double Numeric;
27 typedef double Time;
28 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> Point;
29 typedef std::vector<Point, Eigen::aligned_allocator<Point> > T_Point;
30 typedef std::pair<double, Point> Waypoint;
31 typedef std::vector<Waypoint> T_Waypoint;
32 typedef exact_cubic<Time, Numeric, true, Point, T_Point> exact_cubic_t;
33 typedef exact_cubic_t::spline_constraints spline_constraints_t;
34 typedef exact_cubic_t::t_spline_t t_spline_t;
35 typedef exact_cubic_t::spline_t spline_t;
36
37 /// \brief Compute time such that the equation from source to offsetpoint is
38 /// necessarily a line.
39 8 Waypoint compute_offset(const Waypoint& source, const Point& normal,
40 const Numeric offset, const Time time_offset) {
41
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 Numeric norm = normal.norm();
42
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 8 times.
 8 if (norm < 0.) {
43 ✗ throw std::runtime_error("Norm of normal is less than 0!");
44 }
45
1/2
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
 8 return std::make_pair(source.first + time_offset,
46
4/8
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 8 times.
✗ Branch 11 not taken.
 24 (source.second + normal / norm * offset));
47 }
48
49 /// \brief Compute spline from land way point to end point.
50 /// Constraints are null velocity and acceleration.
51 4 spline_t make_end_spline(const Point& normal, const Point& from,
52 const Numeric offset, const Time init_time,
53 const Time time_offset) {
54
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 Numeric norm = normal.norm();
55
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 4 times.
 4 if (norm < 0.) {
56 ✗ throw std::runtime_error("Norm of normal is less than 0!");
57 }
58
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 Point n = normal / norm;
59
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 4 Point d = offset / (time_offset * time_offset * time_offset) * -n;
60
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 4 Point c = -3 * d * time_offset;
61
3/6
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 4 times.
✗ Branch 8 not taken.
 4 Point b = -c * time_offset;
62
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 T_Point points;
63
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 points.push_back(from);
64
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 points.push_back(b);
65
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 points.push_back(c);
66
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 points.push_back(d);
67 return spline_t(points.begin(), points.end(), init_time,
68
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 8 init_time + time_offset);
69 4 }
70
71 /// \brief Compute end velocity : along landing normal and respecting time.
72 4 spline_constraints_t compute_required_offset_velocity_acceleration(
73 const spline_t& end_spline, const Time /*time_offset*/) {
74 4 spline_constraints_t constraints(end_spline.dim());
75
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 constraints.end_acc = end_spline.derivate(end_spline.min(), 2);
76
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 constraints.end_vel = end_spline.derivate(end_spline.min(), 1);
77 4 return constraints;
78 }
79
80 /// \brief Helper method to create a spline typically used to
81 /// guide the 3d trajectory of a robot end effector.
82 /// Given a set of waypoints, and the normal vector of the start and
83 /// ending positions, automatically create the spline such that:
84 /// + init and end velocities / accelerations are 0.
85 /// + the effector lifts and lands exactly in the direction of the specified
86 /// normals. \param wayPointsBegin : an iterator pointing to the first element
87 /// of a waypoint container. \param wayPointsEnd : an iterator pointing to
88 /// the last element of a waypoint container. \param lift_normal : normal
89 /// to be followed by end effector at take-off. \param land_normal : normal
90 /// to be followed by end effector at landing. \param lift_offset : length
91 /// of the straight line along normal at take-off. \param land_offset :
92 /// length of the straight line along normal at landing. \param
93 /// lift_offset_duration : time travelled along straight line at take-off.
94 /// \param land_offset_duration : time travelled along straight line at landing.
95 ///
96 template <typename In>
97 4 exact_cubic_t* effector_spline(
98 In wayPointsBegin, In wayPointsEnd,
99 const Point& lift_normal = Eigen::Vector3d::UnitZ(),
100 const Point& land_normal = Eigen::Vector3d::UnitZ(),
101 const Numeric lift_offset = 0.02, const Numeric land_offset = 0.02,
102 const Time lift_offset_duration = 0.02,
103 const Time land_offset_duration = 0.02) {
104 4 T_Waypoint waypoints;
105
1/2
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 const Waypoint &inPoint = *wayPointsBegin, endPoint = *(wayPointsEnd - 1);
106
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 waypoints.push_back(inPoint);
107 // adding initial offset
108
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 4 waypoints.push_back(
109 compute_offset(inPoint, lift_normal, lift_offset, lift_offset_duration));
110 // inserting all waypoints but last
111
1/2
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
 4 waypoints.insert(waypoints.end(), wayPointsBegin + 1, wayPointsEnd - 1);
112 // inserting waypoint to start landing
113
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 const Waypoint& landWaypoint =
114 compute_offset(endPoint, land_normal, land_offset, -land_offset_duration);
115
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 waypoints.push_back(landWaypoint);
116 // specifying end velocity constraint such that landing will be in straight
117 // line
118 4 spline_t end_spline =
119 4 make_end_spline(land_normal, landWaypoint.second, land_offset,
120
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 landWaypoint.first, land_offset_duration);
121
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 spline_constraints_t constraints =
122 compute_required_offset_velocity_acceleration(end_spline,
123 land_offset_duration);
124
1/2
✓ Branch 3 taken 4 times.
✗ Branch 4 not taken.
 4 exact_cubic_t splines(waypoints.begin(), waypoints.end(), constraints);
125
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 splines.add_curve(end_spline);
126
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 return new exact_cubic_t(splines);
127 4 }
128 } // namespace helpers
129 } // namespace ndcurves
130 #endif //_CLASS_EFFECTORSPLINE
131