From 844e49b75cfe89daa5cc44492dc6916394f466ed Mon Sep 17 00:00:00 2001
From: Steve Tonneau <stonneau@axle.laas.fr>
Date: Fri, 25 Nov 2016 13:49:58 +0100
Subject: [PATCH] externalized helper function to build cubic and quintic
splines
---
include/spline/cubic_spline.h | 8 ++
include/spline/cubic_zero_vel_acc.h | 150 +++++-----------------------
include/spline/exact_cubic.h | 24 ++---
include/spline/quintic_spline.h | 8 ++
include/spline/spline_curve.h | 11 +-
src/tests/spline_test/Main.cpp | 3 +-
6 files changed, 63 insertions(+), 141 deletions(-)
diff --git a/include/spline/cubic_spline.h b/include/spline/cubic_spline.h
index 0e866a6..db9af91 100644
--- a/include/spline/cubic_spline.h
+++ b/include/spline/cubic_spline.h
@@ -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
diff --git a/include/spline/cubic_zero_vel_acc.h b/include/spline/cubic_zero_vel_acc.h
index e35168d..2093e25 100644
--- a/include/spline/cubic_zero_vel_acc.h
+++ b/include/spline/cubic_zero_vel_acc.h
@@ -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
diff --git a/include/spline/exact_cubic.h b/include/spline/exact_cubic.h
index bbe488b..3de5908 100644
--- a/include/spline/exact_cubic.h
+++ b/include/spline/exact_cubic.h
@@ -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*/
diff --git a/include/spline/quintic_spline.h b/include/spline/quintic_spline.h
index c80a5e7..5860a6f 100644
--- a/include/spline/quintic_spline.h
+++ b/include/spline/quintic_spline.h
@@ -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
diff --git a/include/spline/spline_curve.h b/include/spline/spline_curve.h
index d3807ae..da5f481 100644
--- a/include/spline/spline_curve.h
+++ b/include/spline/spline_curve.h
@@ -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:
diff --git a/src/tests/spline_test/Main.cpp b/src/tests/spline_test/Main.cpp
index 276b03b..6880949 100644
--- a/src/tests/spline_test/Main.cpp
+++ b/src/tests/spline_test/Main.cpp
@@ -1,5 +1,6 @@
#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;
--
GitLab