Skip to content
GitLab
Commit 844e49b7 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

externalized helper function to build cubic and quintic splines

parent 6ebf1138
Hide whitespace changes
Inline Side-by-side
include/spline/cubic_spline.h
View file @ 844e49b7
......@@ -33,6 +33,14 @@ T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c, Point
res.push_back(a);res.push_back(b);res.push_back(c);res.push_back(d);
return res;
}
template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point, typename T_Point>
spline_curve<Time,Numeric,Dim,Safe,Point,T_Point> create_cubic(Point const& a, Point const& b, Point const& c, Point const &d,
const Time min, const Time max)
{
T_Point coeffs = make_cubic_vector<Point, T_Point>(a,b,c,d);
return spline_curve<Time,Numeric,Dim,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), min, max);
}
}
#endif //_STRUCT_CUBICSPLINE
include/spline/cubic_zero_vel_acc.h
View file @ 844e49b7
......@@ -20,8 +20,7 @@
#ifndef _CLASS_CUBICZEROVELACC
#define _CLASS_CUBICZEROVELACC
#include "curve_abc.h"
#include "cubic_spline.h"
#include "exact_cubic.h"
#include "MathDefs.h"
......@@ -30,23 +29,28 @@
namespace spline
{
/// \class ExactCubic
/// \class cubic_zero_vel.
/// \brief Represents a set of cubic splines defining a continuous function
/// crossing each of the waypoint given in its initialization
/// crossing each of the waypoint given in its initialization. Additional constraints
/// are used to increase the order of the last and first splines, to start and finish
/// trajectory with zero velocity and acceleration. Thus the first and last splines
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false
, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
///
template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
typename Point= Eigen::Matrix<Numeric, Dim, 1>,
typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> > >
struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point>
{
typedef Point point_t;
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
typedef Time time_t;
typedef Numeric num_t;
typedef cubic_spline<time_t, Numeric, Dim, Safe, Point> cubic_spline_t;
typedef quintic_spline<time_t, Numeric, Dim, Safe, Point> cubic_spline_t;
typedef typename std::vector<cubic_spline_t*> T_cubic;
typedef typename T_cubic::iterator IT_cubic;
typedef typename T_cubic::const_iterator CIT_cubic;
typedef Point point_t;
typedef T_Point t_point_t;
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
typedef Time time_t;
typedef Numeric num_t;
typedef spline_curve<time_t, Numeric, Dim, Safe, point_t, t_point_t> spline_t;
typedef exact_cubic<Time, Numeric, Dim, Safe, Point> exact_cubic_t;
typedef typename std::vector<spline_t> t_spline_t;
typedef typename t_spline_t::iterator it_spline_t;
typedef typename t_spline_t::const_iterator cit_spline_t;
/* Constructors - destructors */
public:
......@@ -54,119 +58,19 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param wayPointsBegin : an iterator pointing to the first element of a waypoint container
///\param wayPointsEns : an iterator pointing to the end of a waypoint container
template<typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd)
cubic_zero_vel(In wayPointsBegin, In wayPointsEnd)
: exact_cubic_t(wayPointsBegin, wayPointsEnd)
{
std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
if(Safe && size < 1)
{
throw; // TODO
}
// refer to the paper to understand all this.
MatrixX h1 = MatrixX::Zero(size, size);
MatrixX h2 = MatrixX::Zero(size, size);
MatrixX h3 = MatrixX::Zero(size, size);
MatrixX h4 = MatrixX::Zero(size, size);
MatrixX h5 = MatrixX::Zero(size, size);
MatrixX h6 = MatrixX::Zero(size, size);
MatrixX a = MatrixX::Zero(size, Dim);
MatrixX b = MatrixX::Zero(size, Dim);
MatrixX c = MatrixX::Zero(size, Dim);
MatrixX d = MatrixX::Zero(size, Dim);
MatrixX x = MatrixX::Zero(size, Dim);
In it(wayPointsBegin), next(wayPointsBegin);
++next;
for(std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i)
{
num_t const dTi((*next).first - (*it).first);
num_t const dTi_sqr(dTi * dTi);
num_t const dTi_cube(dTi_sqr * dTi);
// filling matrices values
h3(i,i) = -3 / dTi_sqr;
h3(i,i+1) = 3 / dTi_sqr;
h4(i,i) = -2 / dTi;
h4(i,i+1) = -1 / dTi;
h5(i,i) = 2 / dTi_cube;
h5(i,i+1) = -2 / dTi_cube;
h6(i,i) = 1 / dTi_sqr;
h6(i,i+1) = 1 / dTi_sqr;
if( i+2 < size)
{
In it2(next); ++ it2;
num_t const dTi_1((*it2).first - (*next).first);
num_t const dTi_1sqr(dTi_1 * dTi_1);
// this can be optimized but let's focus on clarity as long as not needed
h1(i+1, i) = 2 / dTi;
h1(i+1, i+1) = 4 / dTi + 4 / dTi_1;
h1(i+1, i+2) = 2 / dTi_1;
h2(i+1, i) = -6 / dTi_sqr;
h2(i+1, i+1) = (6 / dTi_1sqr) - (6 / dTi_sqr);
h2(i+1, i+2) = 6 / dTi_1sqr;
}
x.row(i)= (*it).second.transpose();
}
// adding last x
x.row(size-1)= (*it).second.transpose();
a= x;
PseudoInverse(h1);
b = h1 * h2 * x; //h1 * b = h2 * x => b = (h1)^-1 * h2 * x
c = h3 * x + h4 * b;
d = h5 * x + h6 * b;
it= wayPointsBegin, next=wayPointsBegin; ++ next;
for(int i=0; next != wayPointsEnd; ++i, ++it, ++next)
{
subSplines_.push_back(new cubic_spline_t(a.row(i), b.row(i), c.row(i), d.row(i), (*it).first, (*next).first));
}
subSplines_.push_back(new cubic_spline_t(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1), (*it).first, (*it).first));
// NOTHING
}
///\brief Destructor
~exact_cubic()
{
for(IT_cubic it = subSplines_.begin(); it != subSplines_.end(); ++ it)
{
delete(*it);
}
}
private:
exact_cubic(const exact_cubic&);
exact_cubic& operator=(const exact_cubic&);
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : the time when to evaluate the spine
/// \param return : the value x(t)
virtual point_t operator()(time_t t) const
{
if(Safe && (t < subSplines_.front()->t_min_ || t > subSplines_.back()->t_max_)){throw std::out_of_range("TODO");}
for(CIT_cubic it = subSplines_.begin(); it != subSplines_.end(); ++ it)
{
if(t >= ((*it)->t_min_) && t <= ((*it)->t_max_))
{
return (*it)->operator()(t);
}
}
}
/*Operations*/
/*Helpers*/
public:
num_t virtual min() const{return subSplines_.front()->t_min_;}
num_t virtual max() const{return subSplines_.back()->t_max_;}
/*Helpers*/
~cubic_zero_vel(){}
/*Attributes*/
private:
T_cubic subSplines_;
/*Attributes*/
cubic_zero_vel(const cubic_zero_vel&);
cubic_zero_vel& operator=(const cubic_zero_vel&);
/* Constructors - destructors */
};
}
#endif //_CLASS_CUBICZEROVELACC
......
include/spline/exact_cubic.h
View file @ 844e49b7
......@@ -48,6 +48,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
typedef typename std::vector<spline_t> t_spline_t;
typedef typename t_spline_t::iterator it_spline_t;
typedef typename t_spline_t::const_iterator cit_spline_t;
typedef curve_abc<Time, Numeric, Dim, Safe, Point> curve_abc_t;
/* Constructors - destructors */
public:
......@@ -56,7 +57,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param wayPointsEns : an iterator pointing to the end of a waypoint container
template<typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd)
: subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd))
: curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd))
{
}
......@@ -131,19 +132,14 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
it= wayPointsBegin, next=wayPointsBegin; ++ next;
for(int i=0; next != wayPointsEnd; ++i, ++it, ++next)
{
add_cubic(a.row(i), b.row(i), c.row(i), d.row(i),(*it).first, (*next).first, subSplines);
subSplines.push_back(
create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>(a.row(i), b.row(i), c.row(i), d.row(i),(*it).first, (*next).first));
}
add_cubic(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1),(*it).first, (*it).first, subSplines);
subSplines.push_back(
create_cubic<Time,Numeric,Dim,Safe,Point,T_Point>(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1), (*it).first, (*it).first));
return subSplines;
}
void add_cubic(point_t const& a, point_t const& b, point_t const& c, point_t const &d,
const time_t min, const time_t max, t_spline_t& subSplines) const
{
t_point_t coeffs = make_cubic_vector<point_t, t_point_t>(a,b,c,d);
subSplines.push_back(spline_t(coeffs.begin(),coeffs.end(), min, max));
}
private:
exact_cubic(const exact_cubic&);
exact_cubic& operator=(const exact_cubic&);
......@@ -156,10 +152,10 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \param return : the value x(t)
virtual point_t operator()(time_t t) const
{
if(Safe && (t < subSplines_.front().t_min_ || t > subSplines_.back().t_max_)){throw std::out_of_range("TODO");}
if(Safe && (t < subSplines_.front().min() || t > subSplines_.back().max())){throw std::out_of_range("TODO");}
for(cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++ it)
{
if(t >= (it->t_min_) && t <= (it->t_max_))
if(t >= (it->min()) && t <= (it->max()))
{
return it->operator()(t);
}
......@@ -169,8 +165,8 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Helpers*/
public:
num_t virtual min() const{return subSplines_.front().t_min_;}
num_t virtual max() const{return subSplines_.back().t_max_;}
num_t virtual min() const{return subSplines_.front().min();}
num_t virtual max() const{return subSplines_.back().max();}
/*Helpers*/
/*Attributes*/
......
include/spline/quintic_spline.h
View file @ 844e49b7
......@@ -35,6 +35,14 @@ T_Point make_quintic_vector(Point const& a, Point const& b, Point const& c,
res.push_back(d);res.push_back(e);res.push_back(f);
return res;
}
template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point, typename T_Point>
spline_curve<Time,Numeric,Dim,Safe,Point,T_Point> create_quintic(Point const& a, Point const& b, Point const& c, Point const &d, Point const &e, Point const &f,
const Time min, const Time max)
{
T_Point coeffs = make_quintic_vector<Point, T_Point>(a,b,c,d,e,f);
return spline_curve<Time,Numeric,Dim,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), min, max);
}
}
#endif //_STRUCT_QUINTIC_SPLINE
include/spline/spline_curve.h
View file @ 844e49b7
......@@ -39,6 +39,7 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
typedef typename t_point_t::const_iterator cit_point_t;
typedef Time time_t;
typedef Numeric num_t;
typedef curve_abc<Time, Numeric, Dim, Safe, Point> curve_abc_t;
/* Constructors - destructors */
public:
///\brief Constructor
......@@ -48,7 +49,8 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param min: LOWER bound on interval definition of the spline
///\param max: UPPER bound on interval definition of the spline
spline_curve(const T_Point& coefficients, const time_t min, const time_t max)
:coefficients_(coefficients), t_min_(min), t_max_(max), dim_(Dim), order_(coefficients_.size()+1)
: curve_abc_t(),
coefficients_(coefficients), t_min_(min), t_max_(max), dim_(Dim), order_(coefficients_.size()+1)
{
if(Safe)
{
......@@ -71,7 +73,8 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
///\param max: UPPER bound on interval definition of the spline
template<typename In>
spline_curve(In zeroOrderCoefficient, In out, const time_t min, const time_t max)
:coefficients_(init_coeffs(zeroOrderCoefficient, out)), t_min_(min), t_max_(max), dim_(Dim), order_(coefficients_.size()+1)
:coefficients_(init_coeffs(zeroOrderCoefficient, out)), dim_(Dim), order_(coefficients_.size()+1),
t_min_(min), t_max_(max)
{
if(Safe)
{
......@@ -131,9 +134,11 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Attributes*/
public:
t_point_t coefficients_;
time_t t_min_, t_max_;
std::size_t dim_;
std::size_t order_;
private:
time_t t_min_, t_max_;
/*Attributes*/
private:
......
src/tests/spline_test/Main.cpp
View file @ 844e49b7
#include "spline/exact_cubic.h"
#include "spline/cubic_zero_vel_acc.h"
#include "spline/bezier_curve.h"
#include "spline/spline_curve.h"
......@@ -14,7 +15,7 @@ namespace spline
typedef Eigen::Vector3d point_t;
typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
typedef spline_curve <double, double, 3, true, point_t, t_point_t> cubic_function_t;
typedef exact_cubic <double, double, 3, true, point_t> exact_cubic_t;
typedef exact_cubic <double, double, 3, true, point_t> exact_cubic_t;
typedef bezier_curve <double, double, 3, true, point_t> bezier_curve_t;
typedef std::pair<double, point_t> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment