11#ifndef _parameteric_curves_spline_hpp
12#define _parameteric_curves_spline_hpp
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>
31template <
typename Po
int,
typename T_Po
int>
42template <
typename Numeric, Eigen::Index Dim,
typename Po
int,
typename T_Po
int>
44 Point
const&
c, Point
const&
d,
55template <
typename Po
int,
typename T_Po
int>
57 Point
const&
d, Point
const&
e, Point
const&
f) {
68template <
typename Numeric, Eigen::Index Dim,
typename Po
int,
typename T_Po
int>
70 Point
const&
c, Point
const&
d,
71 Point
const&
e, Point
const&
f,
83template <
typename Numeric = double, Eigen::Index Dim = Eigen::Dynamic,
84 typename Point = Eigen::Matrix<Numeric, Dim, 1>,
85 typename SplineBase = Polynomial<Numeric, Dim, Point> >
88 typedef std::vector<Point, Eigen::aligned_allocator<Point> >
t_point_t;
89 typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic>
MatrixX;
93 typedef typename std::vector<spline_t, Eigen::aligned_allocator<spline_t> >
129 template <
typename In>
155 num_t const dTi((*next).first - (*it).first);
170 num_t const dTi_1((*it2).first - (*next).first);
181 x.row(
i) = (*it).second.transpose();
184 x.row(
size - 1) = (*it).second.transpose();
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;
204 Numeric min = (*it).first;
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;
219 template <
typename In>
238 throw std::out_of_range(
"t is out of range");
241 if (
t >= (
it->tmin()) &&
t <= (
it->tmax())) {
242 return it->operator()(
t);
250 const std::size_t&
order)
const {
252 throw std::out_of_range(
"derivative call out of range");
255 if (
t >= (
it->tmin()) &&
t <= (
it->tmax())) {
275 template <
typename In>
297 template <
typename In>
316 Eigen::MatrixXd
rhs = Eigen::MatrixXd::Zero(3, Dim);
320 Eigen::Matrix3d
eq = Eigen::Matrix3d::Zero();
335 Numeric
min2 = min * min;
342 Point
d_ =
d - 4 *
e * min + 10 *
f *
min2;
343 Point
e_ =
e - 5 *
f * min;
352 template <
class Archive>
353 void save(
Archive&
ar,
const unsigned int )
const {
359 template <
class Archive>
360 void load(
Archive&
ar,
const unsigned int ) {
363 this->
t_min = subSplines_.front().tmin();
364 this->
t_max = subSplines_.back().tmax();
368 BOOST_SERIALIZATION_SPLIT_MEMBER()
374 boost::archive::text_iarchive
ia(
ifs);
379 " does not seem to be a valid file.");
390 boost::archive::text_oarchive
oa(
ofs);
394 " does not seem to be a valid file.");
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
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
time_t t_max
Definition abstract-curve.hpp:65
time_t t_min
Definition abstract-curve.hpp:64
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
Evaluation of the derivative spline at time t.
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
Evaluation of the cubic spline at time t.
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 end_acc
Definition curve-constraint.hpp:30
point_t init_vel
Definition curve-constraint.hpp:27
point_t init_acc
Definition curve-constraint.hpp:28
point_t end_vel
Definition curve-constraint.hpp:29