19 #ifndef _CLASS_EFFECTORSPLINE
20 #define _CLASS_EFFECTORSPLINE
22 #include "spline/spline_deriv_constraint.h"
28 typedef Eigen::Matrix<Numeric, 3, 1>
Point;
29 typedef std::vector<Point, Eigen::aligned_allocator<Point> >
T_Point;
32 typedef spline_deriv_constraint<Time, Numeric, 3, true, Point, T_Point>
45 return std::make_pair(source.first + time_offset,
46 (source.second + normal / norm * offset));
51 const Time time_offset) {
56 Point n = normal / norm;
57 Point d = offset / (time_offset * time_offset * time_offset) * -n;
58 Point c = -3 * d * time_offset;
59 Point b = -c * time_offset;
62 points.push_back(from);
67 return spline_t(points.begin(), points.end(), init_time,
68 init_time + time_offset);
75 constraints.end_acc = end_spline.derivate(end_spline.min(), 2);
76 constraints.end_vel = end_spline.derivate(end_spline.min(), 1);
96 template <
typename In>
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) {
105 const Waypoint &inPoint = *wayPointsBegin, endPoint = *(wayPointsEnd - 1);
106 waypoints.push_back(inPoint);
109 compute_offset(inPoint, lift_normal, lift_offset, lift_offset_duration));
111 waypoints.insert(waypoints.end(), wayPointsBegin + 1, wayPointsEnd - 1);
114 compute_offset(endPoint, land_normal, land_offset, -land_offset_duration);
115 waypoints.push_back(landWaypoint);
120 landWaypoint.first, land_offset_duration);
123 land_offset_duration);
127 splines.push_back(end_spline);
132 #endif //_CLASS_EFFECTORSPLINE