spline.hpp
Go to the documentation of this file.
1
11#ifndef _parameteric_curves_spline_hpp
12#define _parameteric_curves_spline_hpp
13
15
16#include <boost/archive/text_iarchive.hpp>
17#include <boost/archive/text_oarchive.hpp>
18#include <boost/serialization/split_member.hpp>
19#include <boost/serialization/vector.hpp>
20#include <fstream>
24#include <vector>
25namespace parametriccurves {
26
31template <typename Point, typename T_Point>
32T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c,
33 Point const& d) {
34 T_Point res;
35 res.push_back(a);
36 res.push_back(b);
37 res.push_back(c);
38 res.push_back(d);
39 return res;
40}
41
42template <typename Numeric, Eigen::Index Dim, typename Point, typename T_Point>
44 Point const& c, Point const& d,
45 const Numeric min,
46 const Numeric max) {
47 T_Point coeffs = make_cubic_vector<Point, T_Point>(a, b, c, d);
48 return Polynomial<Numeric, Dim, Point>(coeffs.begin(), coeffs.end(), min,
49 max);
50}
55template <typename Point, typename T_Point>
56T_Point make_quintic_vector(Point const& a, Point const& b, Point const& c,
57 Point const& d, Point const& e, Point const& f) {
58 T_Point res;
59 res.push_back(a);
60 res.push_back(b);
61 res.push_back(c);
62 res.push_back(d);
63 res.push_back(e);
64 res.push_back(f);
65 return res;
66}
67
68template <typename Numeric, Eigen::Index Dim, typename Point, typename T_Point>
70 Point const& c, Point const& d,
71 Point const& e, Point const& f,
72 const Numeric min,
73 const Numeric max) {
74 T_Point coeffs = make_quintic_vector<Point, T_Point>(a, b, c, d, e, f);
75 return Polynomial<Numeric, Dim, Point>(coeffs.begin(), coeffs.end(), min,
76 max);
77}
78
83template <typename Numeric = double, Eigen::Index Dim = Eigen::Dynamic,
84 typename Point = Eigen::Matrix<Numeric, Dim, 1>,
85 typename SplineBase = Polynomial<Numeric, Dim, Point> >
86struct Spline : public AbstractCurve<Numeric, Point> {
87 typedef Point point_t;
88 typedef std::vector<Point, Eigen::aligned_allocator<Point> > t_point_t;
89 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
90 typedef Numeric time_t;
91 typedef Numeric num_t;
92 typedef SplineBase spline_t;
93 typedef typename std::vector<spline_t, Eigen::aligned_allocator<spline_t> >
95 typedef typename t_spline_t::iterator it_spline_t;
96 typedef typename t_spline_t::const_iterator cit_spline_t;
99
100 public:
102
104
107 Spline(const t_spline_t& subSplines)
108 : curve_abc_t(subSplines.front().tmin(), subSplines.back().tmax()),
109 subSplines_(subSplines) {}
110
112 Spline(const Spline& other)
113 : curve_abc_t(other.subSplines_.front().tmin(),
114 other.subSplines_.front().tmax()),
115 subSplines_(other.subSplines_) {}
116
119
120 public:
121 /* Given a set of waypoints (x_i*) and timestep (t_i), it provides the unique
122 * set of cubic splines fulfulling those 4 restrictions :
123 * - x_i(t_i) = x_i* ; this means that the curve passes through each waypoint
124 * - x_i(t_i+1) = x_i+1* ;
125 * - its derivative is continous at t_i+1
126 * - its 2nd derivative is continous at t_i+1
127 * more details in paper "Task-Space Trajectories via Cubic Spline
128 * Optimization" By J. Zico Kolter and Andrew Y.ng (ICRA 2009) */
129 template <typename In>
130 void createSplineFromWayPoints(In wayPointsBegin, In wayPointsEnd) {
131 std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
132 if (size < 1) {
133 throw; // TODO
134 }
135 subSplines_.clear();
136 subSplines_.reserve(size);
137
138 // refer to the paper to understand all this.
139 MatrixX h1 = MatrixX::Zero(size, size);
140 MatrixX h2 = MatrixX::Zero(size, size);
141 MatrixX h3 = MatrixX::Zero(size, size);
142 MatrixX h4 = MatrixX::Zero(size, size);
143 MatrixX h5 = MatrixX::Zero(size, size);
144 MatrixX h6 = MatrixX::Zero(size, size);
145
146 MatrixX a = MatrixX::Zero(size, Dim);
147 MatrixX b = MatrixX::Zero(size, Dim);
148 MatrixX c = MatrixX::Zero(size, Dim);
149 MatrixX d = MatrixX::Zero(size, Dim);
150 MatrixX x = MatrixX::Zero(size, Dim);
151 In it(wayPointsBegin), next(wayPointsBegin);
152 ++next;
153
154 for (std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i) {
155 num_t const dTi((*next).first - (*it).first);
156 num_t const dTi_sqr(dTi * dTi);
157 num_t const dTi_cube(dTi_sqr * dTi);
158 // filling matrices values
159 h3(i, i) = -3 / dTi_sqr;
160 h3(i, i + 1) = 3 / dTi_sqr;
161 h4(i, i) = -2 / dTi;
162 h4(i, i + 1) = -1 / dTi;
163 h5(i, i) = 2 / dTi_cube;
164 h5(i, i + 1) = -2 / dTi_cube;
165 h6(i, i) = 1 / dTi_sqr;
166 h6(i, i + 1) = 1 / dTi_sqr;
167 if (i + 2 < size) {
168 In it2(next);
169 ++it2;
170 num_t const dTi_1((*it2).first - (*next).first);
171 num_t const dTi_1sqr(dTi_1 * dTi_1);
172 // this can be optimized but let's focus on clarity as long as not
173 // needed
174 h1(i + 1, i) = 2 / dTi;
175 h1(i + 1, i + 1) = 4 / dTi + 4 / dTi_1;
176 h1(i + 1, i + 2) = 2 / dTi_1;
177 h2(i + 1, i) = -6 / dTi_sqr;
178 h2(i + 1, i + 1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
179 h2(i + 1, i + 2) = 6 / dTi_1sqr;
180 }
181 x.row(i) = (*it).second.transpose();
182 }
183 // adding last x
184 x.row(size - 1) = (*it).second.transpose();
185 a = x;
187 b = h1 * h2 * x; // h1 * b = h2 * x => b = (h1)^-1 * h2 * x
188 c = h3 * x + h4 * b;
189 d = h5 * x + h6 * b;
190 it = wayPointsBegin, next = wayPointsBegin;
191 ++next;
192
193 for (int i = 0; next != wayPointsEnd; ++i, ++it, ++next) {
194 Numeric min = (*it).first;
195 Numeric max = (*next).first;
196 Point a_ = a.row(i) - b.row(i) * min + c.row(i) * min * min -
197 d.row(i) * min * min * min;
198 Point b_ = b.row(i) - 2 * c.row(i) * min + 3 * d.row(i) * min * min;
199 Point c_ = c.row(i) - 3 * d.row(i) * min;
200 Point d_ = d.row(i);
201 subSplines_.push_back(create_cubic<Numeric, Dim, Point, t_point_t>(
202 a_, b_, c_, d_, min, max));
203 }
204 Numeric min = (*it).first;
205 Point a_ = a.row(size - 1) - b.row(size - 1) * min +
206 c.row(size - 1) * min * min - d.row(size - 1) * min * min * min;
207 Point b_ = b.row(size - 1) - 2 * c.row(size - 1) * min +
208 3 * d.row(size - 1) * min * min;
209 Point c_ = c.row(size - 1) - 3 * d.row(size - 1) * min;
210 Point d_ = d.row(size - 1);
211 subSplines_.push_back(
212 create_cubic<Numeric, Dim, Point, t_point_t>(a_, b_, c_, d_, min, min));
213
214 this->t_min = subSplines_.front().tmin();
215 this->t_max = subSplines_.back().tmax();
216 return;
217 }
218
219 template <typename In>
220 void createSplineFromWayPointsConstr(In wayPointsBegin, In wayPointsEnd,
221 const spline_constraints& constraints) {
222 std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
223 if (size < 1) throw; // TODO
224 subSplines_.clear();
225 subSplines_.reserve(size);
226 spline_constraints cons = constraints;
227 In it(wayPointsBegin), next(wayPointsBegin), end(wayPointsEnd - 1);
228 ++next;
229 for (std::size_t i(0); next != end; ++next, ++it, ++i)
230 compute_one_spline<In>(it, next, cons, subSplines_);
231 compute_end_spline<In>(it, next, cons, subSplines_);
232 return;
233 }
234
235 public:
236 virtual const point_t operator()(const time_t& t) const {
237 if ((t < subSplines_.front().tmin() || t > subSplines_.back().tmax())) {
238 throw std::out_of_range("t is out of range");
239 }
240 for (cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++it) {
241 if (t >= (it->tmin()) && t <= (it->tmax())) {
242 return it->operator()(t);
243 }
244 }
245 const point_t dummy;
246 return dummy;
247 }
248
249 virtual const point_t derivate(const time_t& t,
250 const std::size_t& order) const {
251 if ((t < subSplines_.front().tmin() || t > subSplines_.back().tmax())) {
252 throw std::out_of_range("derivative call out of range");
253 }
254 for (cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++it) {
255 if (t >= (it->tmin()) && t <= (it->tmax())) {
256 return it->derivate(t, order);
257 }
258 }
259
260 const point_t dummy;
261 return dummy;
262 }
263
264 virtual const std::size_t& size() const { return subSplines_[0].size(); }
265 const t_spline_t& getSubsplines() const { return subSplines_; }
266
267 virtual bool setInitialPoint(const point_t& /*x_init*/) { return false; }
268 virtual bool setInitialPoint(const num_t& /*x_init*/) { return false; }
269
270 protected:
271 /*Attributes*/
273
274 private:
275 template <typename In>
276 void compute_one_spline(In wayPointsBegin, In wayPointsNext,
277 spline_constraints& constraints,
278 t_spline_t& subSplines) const {
279 const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
280 const point_t &b0 = constraints.init_vel, c0 = constraints.init_acc / 2.;
281 const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
282 const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt;
283 const point_t d0 = (a1 - a0 - b0 * dt - c0 * dt_2) / dt_3;
284
285 Point a_ =
286 a0 - b0 * init_t + c0 * init_t * init_t - d0 * init_t * init_t * init_t;
287 Point b_ = b0 - 2 * c0 * init_t + 3 * d0 * init_t * init_t;
288 Point c_ = c0 - 3 * d0 * init_t;
289 Point d_ = d0;
290 subSplines.push_back(create_cubic<Numeric, Dim, Point, t_point_t>(
291 a_, b_, c_, d_, init_t, end_t));
292
293 constraints.init_vel = subSplines.back().derivate(end_t, 1);
294 constraints.init_acc = subSplines.back().derivate(end_t, 2);
295 }
296
297 template <typename In>
298 void compute_end_spline(In wayPointsBegin, In wayPointsNext,
299 spline_constraints& constraints,
300 t_spline_t& subSplines) const {
301 const point_t &a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
302 const point_t &b0 = constraints.init_vel, b1 = constraints.end_vel,
303 c0 = constraints.init_acc / 2., c1 = constraints.end_acc;
304 const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
305 const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt,
306 dt_4 = dt_3 * dt, dt_5 = dt_4 * dt;
307 // solving a system of four linear eq with 4 unknows: d0, e0
308 const point_t alpha_0 = a1 - a0 - b0 * dt - c0 * dt_2;
309 const point_t alpha_1 = b1 - b0 - 2 * c0 * dt;
310 const point_t alpha_2 = c1 - 2 * c0;
311 const num_t x_d_0 = dt_3, x_d_1 = 3 * dt_2, x_d_2 = 6 * dt;
312 const num_t x_e_0 = dt_4, x_e_1 = 4 * dt_3, x_e_2 = 12 * dt_2;
313 const num_t x_f_0 = dt_5, x_f_1 = 5 * dt_4, x_f_2 = 20 * dt_3;
314
315 point_t d, e, f;
316 Eigen::MatrixXd rhs = Eigen::MatrixXd::Zero(3, Dim);
317 rhs.row(0) = alpha_0;
318 rhs.row(1) = alpha_1;
319 rhs.row(2) = alpha_2;
320 Eigen::Matrix3d eq = Eigen::Matrix3d::Zero();
321 eq(0, 0) = x_d_0;
322 eq(0, 1) = x_e_0;
323 eq(0, 2) = x_f_0;
324 eq(1, 0) = x_d_1;
325 eq(1, 1) = x_e_1;
326 eq(1, 2) = x_f_1;
327 eq(2, 0) = x_d_2;
328 eq(2, 1) = x_e_2;
329 eq(2, 2) = x_f_2;
330 rhs = eq.inverse().eval() * rhs;
331 d = rhs.row(0);
332 e = rhs.row(1);
333 f = rhs.row(2);
334 num_t min = init_t;
335 Numeric min2 = min * min;
336 Numeric min3 = min2 * min;
337 Numeric min4 = min3 * min;
338 Numeric min5 = min4 * min;
339 Point a_ = a0 - b0 * min + c0 * min2 - d * min3 + e * min4 - f * min5;
340 Point b_ = b0 - 2 * c0 * min + 3 * d * min2 - 4 * e * min3 + 5 * f * min4;
341 Point c_ = c0 - 3 * d * min + 6 * e * min2 - 10 * f * min3;
342 Point d_ = d - 4 * e * min + 10 * f * min2;
343 Point e_ = e - 5 * f * min;
344 Point f_ = f;
345
346 subSplines.push_back(create_quintic<Numeric, Dim, Point, t_point_t>(
347 a_, b_, c_, d_, e_, f_, init_t, end_t));
348 }
349
350 // Serialization of the class
352 template <class Archive>
353 void save(Archive& ar, const unsigned int /*version*/) const {
354 ar& subSplines_;
355
356 return;
357 }
358
359 template <class Archive>
360 void load(Archive& ar, const unsigned int /*version*/) {
361 ar& subSplines_;
362
363 this->t_min = subSplines_.front().tmin();
364 this->t_max = subSplines_.back().tmax();
365 return;
366 }
367
368 BOOST_SERIALIZATION_SPLIT_MEMBER()
369
370 public:
371 bool loadFromFile(const std::string& filename) {
372 std::ifstream ifs(filename.c_str());
373 if (ifs) {
374 boost::archive::text_iarchive ia(ifs);
375 Spline& cubic_spline = *static_cast<Spline*>(this);
376 ia >> cubic_spline;
377 } else {
378 const std::string exception_message(filename +
379 " does not seem to be a valid file.");
380 throw std::invalid_argument(exception_message);
381 return false;
382 }
383 return true;
384 }
385
387 bool saveToFile(const std::string& filename) const {
388 std::ofstream ofs(filename.c_str());
389 if (ofs) {
390 boost::archive::text_oarchive oa(ofs);
391 oa << *static_cast<const Spline*>(this);
392 } else {
393 const std::string exception_message(filename +
394 " does not seem to be a valid file.");
395 throw std::invalid_argument(exception_message);
396 return false;
397 }
398 return true;
399 }
400
401 // BOOST_SERIALIZATION_SPLIT_MEMBER()
402};
403} // namespace parametriccurves
404#endif //_CLASS_EXACTCUBIC
void load(Archive &ar, Eigen::Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols > &m, const unsigned int)
Definition: eigen-matrix.hpp:50
void save(Archive &ar, const Eigen::Matrix< _Scalar, _Rows, _Cols, _Options, _MaxRows, _MaxCols > &m, const unsigned int)
Definition: eigen-matrix.hpp:38
Definition: abstract-curve.hpp:16
T_Point make_quintic_vector(Point const &a, Point const &b, Point const &c, Point const &d, Point const &e, Point const &f)
Creates coefficient vector of a quintic spline defined on the interval [tBegin, tEnd]....
Definition: spline.hpp:56
Polynomial< Numeric, Dim, Point > create_quintic(Point const &a, Point const &b, Point const &c, Point const &d, Point const &e, Point const &f, const Numeric min, const Numeric max)
Definition: spline.hpp:69
T_Point make_cubic_vector(Point const &a, Point const &b, Point const &c, Point const &d)
Creates coefficient vector of a cubic spline defined on the interval [tBegin, tEnd]....
Definition: spline.hpp:32
Polynomial< Numeric, Dim, Point > create_cubic(Point const &a, Point const &b, Point const &c, Point const &d, const Numeric min, const Numeric max)
Definition: spline.hpp:43
void PseudoInverse(_Matrix_Type_ &pinvmat)
Definition: MathDefs.h:28
Eigen::Matrix< Numeric, 3, 1 > Point
Definition: effector_spline.h:28
double Numeric
Definition: effector_spline.h:26
std::vector< Point, Eigen::aligned_allocator< Point > > T_Point
Definition: effector_spline.h:29
Definition of a cubic spline.
Represents a curve of dimension Dim is Safe is false, no verification is made on the evaluation of th...
Definition: abstract-curve.hpp:21
virtual const time_t tmin() const
Definition: abstract-curve.hpp:47
virtual const time_t tmax() const
Definition: abstract-curve.hpp:48
Represents a Polynomialf arbitrary order defined on the interval [tBegin, tEnd]. It follows the equat...
Definition: polynomial.hpp:32
Represents a set of cubic splines defining a continuous function crossing each of the waypoint given ...
Definition: spline.hpp:86
Eigen::Matrix< Numeric, Eigen::Dynamic, Eigen::Dynamic > MatrixX
Definition: spline.hpp:89
Spline(const Spline &other)
Copy Constructor.
Definition: spline.hpp:112
void createSplineFromWayPoints(In wayPointsBegin, In wayPointsEnd)
Definition: spline.hpp:130
bool loadFromFile(const std::string &filename)
Definition: spline.hpp:371
std::vector< Point, Eigen::aligned_allocator< Point > > t_point_t
Definition: spline.hpp:88
SplineBase spline_t
Definition: spline.hpp:92
const t_spline_t & getSubsplines() const
Definition: spline.hpp:265
AbstractCurve< Numeric, Point > curve_abc_t
Definition: spline.hpp:98
Spline(const t_spline_t &subSplines)
Constructor.
Definition: spline.hpp:107
~Spline()
Destructor.
Definition: spline.hpp:118
void createSplineFromWayPointsConstr(In wayPointsBegin, In wayPointsEnd, const spline_constraints &constraints)
Definition: spline.hpp:220
std::vector< spline_t, Eigen::aligned_allocator< spline_t > > t_spline_t
Definition: spline.hpp:94
virtual const point_t derivate(const time_t &t, const std::size_t &order) const
Definition: spline.hpp:249
Spline()
Constructor.
Definition: spline.hpp:103
virtual bool setInitialPoint(const point_t &)
Definition: spline.hpp:267
virtual const point_t operator()(const time_t &t) const
Definition: spline.hpp:236
Point point_t
Definition: spline.hpp:87
curve_constraints< point_t > spline_constraints
Definition: spline.hpp:97
Numeric time_t
Definition: spline.hpp:90
t_spline_t::const_iterator cit_spline_t
Definition: spline.hpp:96
friend class boost::serialization::access
Definition: spline.hpp:351
Numeric num_t
Definition: spline.hpp:91
virtual bool setInitialPoint(const num_t &)
Definition: spline.hpp:268
bool saveToFile(const std::string &filename) const
Saved a Derived object as a text file.
Definition: spline.hpp:387
t_spline_t::iterator it_spline_t
Definition: spline.hpp:95
virtual const std::size_t & size() const
Definition: spline.hpp:264
t_spline_t subSplines_
Definition: spline.hpp:272
Definition: curve-constraint.hpp:18
point_t init_vel
Definition: curve-constraint.hpp:27
point_t init_acc
Definition: curve-constraint.hpp:28