| 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/6✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
|
8 | return new exact_cubic_t(splines); |
| 127 | 4 | } | |
| 128 | } // namespace helpers | ||
| 129 | } // namespace ndcurves | ||
| 130 | #endif //_CLASS_EFFECTORSPLINE | ||
| 131 |