19 #ifndef _CLASS_EXACTCUBIC
20 #define _CLASS_EXACTCUBIC
37 template <
typename Time = double,
typename Numeric =
Time,
bool Safe =
false,
38 typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
39 typename T_Point = std::vector<Point, Eigen::aligned_allocator<Point> >,
40 typename SplineBase = polynomial<Time, Numeric, Safe, Point, T_Point> >
45 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic>
MatrixX;
46 typedef Eigen::Matrix<Numeric, 3, 3>
Matrix3;
71 template <
typename In>
73 t_spline_t subSplines = computeWayPoints<In>(wayPointsBegin, wayPointsEnd);
74 for (
cit_spline_t it = subSplines.begin(); it != subSplines.end(); ++it) {
84 template <
typename In>
86 t_spline_t subSplines = computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints);
87 for (
cit_spline_t it = subSplines.begin(); it != subSplines.end(); ++it) {
95 for (
cit_spline_t it = subSplines.begin(); it != subSplines.end(); ++it) {
111 boost::shared_ptr<spline_t> s_ptr = boost::dynamic_pointer_cast<spline_t>(this->
curves_.at(index));
115 throw std::runtime_error(
116 "Parent piecewise curve do not contain only curves created from exact_cubic class methods");
127 return polynomial_t(coeffs.begin(), coeffs.end(), t_min, t_max);
138 return polynomial_t(coeffs.begin(), coeffs.end(), t_min, t_max);
147 template <
typename In>
148 t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd)
const {
149 const std::size_t
dim = wayPointsBegin->second.size();
150 const std::size_t size = std::distance(wayPointsBegin, wayPointsEnd);
151 if (Safe && size < 1) {
152 throw std::length_error(
"size of waypoints must be superior to 0");
155 subSplines.reserve(size);
157 MatrixX h1 = MatrixX::Zero(size, size);
158 MatrixX h2 = MatrixX::Zero(size, size);
159 MatrixX h3 = MatrixX::Zero(size, size);
160 MatrixX h4 = MatrixX::Zero(size, size);
161 MatrixX h5 = MatrixX::Zero(size, size);
162 MatrixX h6 = MatrixX::Zero(size, size);
168 In it(wayPointsBegin), next(wayPointsBegin);
171 for (std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i) {
172 num_t const dTi((*next).first - (*it).first);
173 num_t const dTi_sqr(dTi * dTi);
174 num_t const dTi_cube(dTi_sqr * dTi);
176 h3(i, i) = -3 / dTi_sqr;
177 h3(i, i + 1) = 3 / dTi_sqr;
179 h4(i, i + 1) = -1 / dTi;
180 h5(i, i) = 2 / dTi_cube;
181 h5(i, i + 1) = -2 / dTi_cube;
182 h6(i, i) = 1 / dTi_sqr;
183 h6(i, i + 1) = 1 / dTi_sqr;
187 num_t const dTi_1((*it2).first - (*next).first);
188 num_t const dTi_1sqr(dTi_1 * dTi_1);
190 h1(i + 1, i) = 2 / dTi;
191 h1(i + 1, i + 1) = 4 / dTi + 4 / dTi_1;
192 h1(i + 1, i + 2) = 2 / dTi_1;
193 h2(i + 1, i) = -6 / dTi_sqr;
194 h2(i + 1, i + 1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
195 h2(i + 1, i + 2) = 6 / dTi_1sqr;
197 x.row(i) = (*it).second.transpose();
200 x.row(size - 1) = (*it).second.transpose();
208 it = wayPointsBegin, next = wayPointsBegin;
210 for (
int i = 0; next != wayPointsEnd; ++i, ++it, ++next) {
211 subSplines.push_back(create_cubic(a.row(i), b.row(i), c.row(i), d.row(i), (*it).first, (*next).first));
216 template <
typename In>
218 std::size_t
const size(std::distance(wayPointsBegin, wayPointsEnd));
219 if (Safe && size < 1) {
220 throw std::length_error(
"number of waypoints should be superior to one");
223 subSplines.reserve(size - 1);
225 In it(wayPointsBegin), next(wayPointsBegin), end(wayPointsEnd - 1);
227 for (std::size_t i(0); next != end; ++next, ++it, ++i) {
228 compute_one_spline<In>(it, next, cons, subSplines);
230 compute_end_spline<In>(it, next, cons, subSplines);
234 template <
typename In>
235 void compute_one_spline(In wayPointsBegin, In wayPointsNext,
spline_constraints& constraints,
237 const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
238 const point_t &b0 = constraints.init_vel, c0 = constraints.init_acc / 2.;
239 const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
240 const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt;
241 const point_t d0 = (a1 - a0 - b0 * dt - c0 * dt_2) / dt_3;
242 subSplines.push_back(create_cubic(a0, b0, c0, d0, init_t, end_t));
243 constraints.init_vel = subSplines.back().derivate(end_t, 1);
244 constraints.init_acc = subSplines.back().derivate(end_t, 2);
247 template <
typename In>
248 void compute_end_spline(In wayPointsBegin, In wayPointsNext,
spline_constraints& constraints,
250 const std::size_t
dim = wayPointsBegin->second.size();
251 const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
252 const point_t &b0 = constraints.init_vel, b1 = constraints.end_vel, c0 = constraints.init_acc / 2.,
253 c1 = constraints.end_acc;
254 const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
255 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;
257 const point_t alpha_0 = a1 - a0 - b0 * dt - c0 * dt_2;
258 const point_t alpha_1 = b1 - b0 - 2 * c0 * dt;
259 const point_t alpha_2 = c1 - 2 * c0;
260 const num_t x_d_0 = dt_3, x_d_1 = 3 * dt_2, x_d_2 = 6 * dt;
261 const num_t x_e_0 = dt_4, x_e_1 = 4 * dt_3, x_e_2 = 12 * dt_2;
262 const num_t x_f_0 = dt_5, x_f_1 = 5 * dt_4, x_f_2 = 20 * dt_3;
265 rhs.row(0) = alpha_0;
266 rhs.row(1) = alpha_1;
267 rhs.row(2) = alpha_2;
268 Matrix3 eq = Matrix3::Zero(3, 3);
278 rhs = eq.inverse().eval() * rhs;
282 subSplines.push_back(create_quintic(a0, b0, c0, d, e, f, init_t, end_t));
287 friend class boost::serialization::access;
288 template <
class Archive>
289 void serialize(Archive& ar,
const unsigned int version) {
298 DEFINE_CLASS_TEMPLATE_VERSION(SINGLE_ARG(
typename Time,
typename Numeric,
bool Safe,
typename Point,
typename T_Point,
299 typename SplineBase),
301 #endif //_CLASS_EXACTCUBIC