19 #ifndef _CLASS_CUBICZEROVELACC
20 #define _CLASS_CUBICZEROVELACC
38 template <
typename Time = double,
typename Numeric =
Time, std::size_t Dim = 3,
bool Safe =
false,
39 typename Point = Eigen::Matrix<Numeric, Dim, 1>,
40 typename T_Point = std::vector<Point, Eigen::aligned_allocator<Point> >,
41 typename SplineBase = polynom<Time, Numeric, Dim, Safe, Point, T_Point> >
45 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic>
MatrixX;
46 typedef Eigen::Matrix<Numeric, 3, 3>
Matrix3;
62 template <
typename In>
65 :
exact_cubic_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints)) {}
74 template <
typename In>
75 void compute_one_spline(In wayPointsBegin, In wayPointsNext,
spline_constraints& constraints,
77 const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
78 const point_t &b0 = constraints.init_vel, c0 = constraints.init_acc / 2.;
79 const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
80 const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt;
81 const point_t d0 = (a1 - a0 - b0 * dt - c0 * dt_2) / dt_3;
82 subSplines.push_back(create_cubic<Time, Numeric, Dim, Safe, Point, T_Point>(a0, b0, c0, d0, init_t, end_t));
83 constraints.init_vel = subSplines.back().derivate(end_t, 1);
84 constraints.init_acc = subSplines.back().derivate(end_t, 2);
87 template <
typename In>
88 void compute_end_spline(In wayPointsBegin, In wayPointsNext,
spline_constraints& constraints,
90 const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
91 const point_t &b0 = constraints.init_vel, b1 = constraints.end_vel, c0 = constraints.init_acc / 2.,
92 c1 = constraints.end_acc;
93 const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
94 const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt, dt_4 = dt_3 * dt, dt_5 = dt_4 * dt;
96 const point_t alpha_0 = a1 - a0 - b0 * dt - c0 * dt_2;
97 const point_t alpha_1 = b1 - b0 - 2 * c0 * dt;
98 const point_t alpha_2 = c1 - 2 * c0;
99 const num_t x_d_0 = dt_3, x_d_1 = 3 * dt_2, x_d_2 = 6 * dt;
100 const num_t x_e_0 = dt_4, x_e_1 = 4 * dt_3, x_e_2 = 12 * dt_2;
101 const num_t x_f_0 = dt_5, x_f_1 = 5 * dt_4, x_f_2 = 20 * dt_3;
104 MatrixX rhs = MatrixX::Zero(3, Dim);
105 rhs.row(0) = alpha_0;
106 rhs.row(1) = alpha_1;
107 rhs.row(2) = alpha_2;
108 Matrix3 eq = Matrix3::Zero(3, 3);
118 rhs = eq.inverse().eval() * rhs;
123 subSplines.push_back(create_quintic<Time, Numeric, Dim, Safe, Point, T_Point>(a0, b0, c0, d, e, f, init_t, end_t));
127 template <
typename In>
129 std::size_t
const size(std::distance(wayPointsBegin, wayPointsEnd));
130 if (Safe && size < 1)
throw;
132 subSplines.reserve(size - 1);
134 In it(wayPointsBegin), next(wayPointsBegin), end(wayPointsEnd - 1);
136 for (std::size_t i(0); next != end; ++next, ++it, ++i) compute_one_spline<In>(it, next, cons, subSplines);
137 compute_end_spline<In>(it, next, cons, subSplines);
145 #endif //_CLASS_CUBICZEROVELACC