Commit 19cafdde authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

Merge remote-tracking branch 'jchemin/topic/add_piecewise_curve_final' into HEAD

parents 4d546b5e dde8663f
......@@ -11,7 +11,7 @@ SET(${PROJECT_NAME}_HEADERS
quintic_spline.h
linear_variable.h
cubic_hermite_spline.h
piecewise_polynomial_curve.h
piecewise_curve.h
)
INSTALL(FILES
......
......@@ -26,20 +26,20 @@ namespace curves{
template<typename _Matrix_Type_>
void PseudoInverse(_Matrix_Type_& pinvmat)
{
Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
_Matrix_Type_ m_sigma = svd.singularValues();
Eigen::JacobiSVD<_Matrix_Type_> svd(pinvmat, Eigen::ComputeFullU | Eigen::ComputeFullV);
_Matrix_Type_ m_sigma = svd.singularValues();
double pinvtoler= 1.e-6; // choose your tolerance widely!
double pinvtoler= 1.e-6; // choose your tolerance widely!
_Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(),pinvmat.rows());
for (long i=0; i<m_sigma.rows(); ++i)
{
if (m_sigma(i) > pinvtoler)
{
m_sigma_inv(i,i)=1.0/m_sigma(i);
}
}
pinvmat = (svd.matrixV()*m_sigma_inv*svd.matrixU().transpose());
_Matrix_Type_ m_sigma_inv = _Matrix_Type_::Zero(pinvmat.cols(),pinvmat.rows());
for (long i=0; i<m_sigma.rows(); ++i)
{
if (m_sigma(i) > pinvtoler)
{
m_sigma_inv(i,i)=1.0/m_sigma(i);
}
}
pinvmat = (svd.matrixV()*m_sigma_inv*svd.matrixU().transpose());
}
} // namespace curves
......
......@@ -32,16 +32,16 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef Point point_t;
typedef Time time_t;
typedef Numeric num_t;
typedef Point point_t;
typedef Time time_t;
typedef Numeric num_t;
typedef curve_constraints<point_t> curve_constraints_t;
typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
typedef typename t_point_t::const_iterator cit_point_t;
typedef bezier_curve<Time, Numeric, Dim, Safe, Point > bezier_curve_t;
/* Constructors - destructors */
public:
public:
/// \brief Constructor.
/// Given the first and last point of a control points set, create the bezier curve.
......@@ -63,7 +63,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
In it(PointsBegin);
if(Safe && (size_<1 || T_max_ <= T_min_))
{
throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
throw std::invalid_argument("can't create bezier min bound is higher than max bound"); // TODO
}
for(; it != PointsEnd; ++it)
{
......@@ -90,7 +90,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
if(Safe && (size_<1 || T_max_ <= T_min_))
{
throw std::out_of_range("can't create bezier min bound is higher than max bound");
throw std::invalid_argument("can't create bezier min bound is higher than max bound");
}
t_point_t updatedList = add_constraints<In>(PointsBegin, PointsEnd, constraints);
for(cit_point_t cit = updatedList.begin(); cit != updatedList.end(); ++cit)
......@@ -99,27 +99,27 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
}
}
///\brief Destructor
~bezier_curve()
{
// NOTHING
}
///\brief Destructor
~bezier_curve()
{
// NOTHING
}
private:
// bezier_curve(const bezier_curve&);
private:
// bezier_curve(const bezier_curve&);
// bezier_curve& operator=(const bezier_curve&);
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the bezier curve at time t.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
public:
/// \brief Evaluation of the bezier curve at time t.
/// \param t : time when to evaluate the curve.
/// \return \f$x(t)\f$ point corresponding on curve at time t.
virtual point_t operator()(const time_t t) const
{
if(Safe &! (T_min_ <= t && t <= T_max_))
{
throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO
throw std::invalid_argument("can't evaluate bezier curve, time t is out of range"); // TODO
}
if (size_ == 1)
{
......@@ -128,7 +128,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
return evalHorner(t);
}
}
}
/// \brief Compute the derived curve at order N.
/// Computes the derivative order N, \f$\frac{d^Nx(t)}{dt^N}\f$ of bezier curve of parametric equation x(t).
......@@ -298,7 +298,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// \return pair containing the first element of both bezier curve obtained.
///
std::pair<bezier_curve_t,bezier_curve_t> split(const Numeric t){
if (t == T_max_)
if (fabs(t-T_max_)<MARGIN)
{
throw std::runtime_error("can't split curve, interval range is equal to original curve");
}
......@@ -326,15 +326,15 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
throw std::out_of_range("In Extract curve : times out of bounds");
}
if(t1 == T_min_ && t2 == T_max_)
if (fabs(t1-T_min_)<MARGIN && fabs(t2-T_max_)<MARGIN)
{
return bezier_curve_t(waypoints().begin(), waypoints().end(), T_min_, T_max_, mult_T_);
}
if(t1 == T_min_)
if (fabs(t1-T_min_)<MARGIN)
{
return split(t2).first;
}
if(t2 == T_max_)
if (fabs(t2-T_max_)<MARGIN)
{
return split(t1).second;
}
......@@ -384,7 +384,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
virtual time_t max() const{return T_max_;}
/*Helpers*/
public:
public:
/// Starting time of cubic hermite spline : T_min_ is equal to first time of control points.
/*const*/ time_t T_min_;
/// Ending time of cubic hermite spline : T_max_ is equal to last time of control points.
......@@ -394,6 +394,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*const*/ std::size_t degree_;
/*const*/ std::vector<Bern<Numeric> > bernstein_;
/*const*/ t_point_t control_points_;
static const double MARGIN;
public:
static bezier_curve_t zero(const time_t T=1.)
......@@ -403,6 +404,10 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
return bezier_curve_t(ts.begin(), ts.end(),0.,T);
}
};
template<typename Time, typename Numeric, std::size_t Dim, bool Safe, typename Point>
const double bezier_curve<Time, Numeric, Dim, Safe, Point>::MARGIN(0.001);
} // namespace curve
#endif //_CLASS_BEZIERCURVE
......@@ -63,15 +63,15 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
/*Attributes*/
public:
/// \brief Constructor.
/// \param wayPointsBegin : an iterator pointing to the first element of a pair(position, derivative) container.
/// \param wayPointsEns : an iterator pointing to the last element of a pair(position, derivative) container.
/// \brief Constructor.
/// \param wayPointsBegin : an iterator pointing to the first element of a pair(position, derivative) container.
/// \param wayPointsEns : an iterator pointing to the last element of a pair(position, derivative) container.
/// \param time_control_points : vector containing time for each waypoint.
///
template<typename In>
cubic_hermite_spline(In PairsBegin, In PairsEnd, const vector_time_t & time_control_points)
{
// Check size of pairs container.
template<typename In>
cubic_hermite_spline(In PairsBegin, In PairsEnd, const vector_time_t & time_control_points)
{
// Check size of pairs container.
std::size_t const size(std::distance(PairsBegin, PairsEnd));
size_ = size;
if(Safe && size < 1)
......@@ -85,30 +85,30 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
control_points_.push_back(*it);
}
setTime(time_control_points);
}
}
/// \brief Destructor.
/// \brief Destructor.
virtual ~cubic_hermite_spline(){}
/*Operations*/
public:
public:
/// \brief Evaluation of the cubic hermite spline at time t.
/// \param t : time when to evaluate the spline.
/// \return \f$p(t)\f$ point corresponding on spline at time t.
///
virtual Point operator()(const Time t) const
virtual Point operator()(const Time t) const
{
if(Safe &! (T_min_ <= t && t <= T_max_))
{
throw std::out_of_range("can't evaluate cubic hermite spline, out of range");
throw std::invalid_argument("can't evaluate cubic hermite spline, out of range");
}
if (size_ == 1)
{
return control_points_.front().first;
return control_points_.front().first;
}
else
{
return evalCubicHermiteSpline(t, 0);
return evalCubicHermiteSpline(t, 0);
}
}
......@@ -139,7 +139,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
computeDurationSplines();
if (!checkDurationSplines())
{
throw std::logic_error("time_splines not monotonous, all spline duration should be superior to 0");
throw std::invalid_argument("time_splines not monotonous, all spline duration should be superior to 0");
}
}
......@@ -214,7 +214,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
//
const Time dt = (t1-t0);
const Time alpha = (t - t0)/dt;
assert(0. <= alpha <= 1. && "alpha must be in [0,1]");
assert(0. <= alpha && alpha <= 1. && "alpha must be in [0,1]");
Numeric h00, h10, h01, h11;
evalCoeffs(alpha,h00,h10,h01,h11,order_derivative);
//std::cout << "for val t="<<t<<" alpha="<<alpha<<" coef : h00="<<h00<<" h10="<<h10<<" h01="<<h01<<" h11="<<h11<<std::endl;
......@@ -353,17 +353,17 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Dim, Safe, Point>
}
return is_positive;
}
/*Operations*/
/*Operations*/
/*Helpers*/
public:
public:
/// \brief Get the minimum time for which the curve is defined
/// \return \f$t_{min}\f$, lower bound of time range.
Time virtual min() const{return time_control_points_.front();}
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$, upper bound of time range.
Time virtual max() const{return time_control_points_.back();}
/*Helpers*/
/*Helpers*/
};
......
......@@ -25,23 +25,23 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
, typename Point= Eigen::Matrix<Numeric, Dim, 1> >
struct curve_abc : std::unary_function<Time, Point>
{
typedef Point point_t;
typedef Time time_t;
typedef Point point_t;
typedef Time time_t;
/* Constructors - destructors */
public:
/// \brief Constructor.
public:
/// \brief Constructor.
curve_abc(){}
/// \brief Destructor.
/// \brief Destructor.
virtual ~curve_abc(){}
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : time when to evaluate the spine
/// \return \f$x(t)\f$, point corresponding on curve at time t.
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : time when to evaluate the spine
/// \return \f$x(t)\f$, point corresponding on curve at time t.
virtual point_t operator()(const time_t t) const = 0;
......@@ -53,18 +53,18 @@ struct curve_abc : std::unary_function<Time, Point>
/*Operations*/
/*Helpers*/
public:
/// \brief Get the minimum time for which the curve is defined.
/// \return \f$t_{min}\f$, lower bound of time range.
virtual time_t min() const = 0;
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$, upper bound of time range.
virtual time_t max() const = 0;
public:
/// \brief Get the minimum time for which the curve is defined.
/// \return \f$t_{min}\f$, lower bound of time range.
virtual time_t min() const = 0;
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$, upper bound of time range.
virtual time_t max() const = 0;
std::pair<time_t, time_t> timeRange() {return std::make_pair(min(), max());}
/*Helpers*/
};
};
} // namespace curves
#endif //_STRUCT_CURVE_ABC
......@@ -22,16 +22,16 @@ namespace curves
template <typename Point>
struct curve_constraints
{
typedef Point point_t;
typedef Point point_t;
curve_constraints():
init_vel(point_t::Zero()),init_acc(init_vel),end_vel(init_vel),end_acc(init_vel){}
~curve_constraints(){}
~curve_constraints(){}
point_t init_vel;
point_t init_acc;
point_t end_vel;
point_t end_acc;
point_t init_vel;
point_t init_acc;
point_t end_vel;
point_t end_acc;
};
} // namespace curves
#endif //_CLASS_CUBICZEROVELACC
......@@ -38,7 +38,7 @@ Polynomial polynomial_from_curve(const curveTypeToConvert& curve)
}
/// \brief Converts a cubic hermite spline or polynomial to a cubic bezier curve.
/// \brief Converts a cubic hermite spline or polynomial of order 3 or less to a cubic bezier curve.
/// \param curve : the polynomial of order 3 or less/cubic hermite spline defined between [Tmin,Tmax] to convert.
/// \return the equivalent cubic bezier curve.
template<typename Bezier, typename curveTypeToConvert>
......
......@@ -41,12 +41,12 @@ template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool S
, typename SplineBase=polynomial<Time, Numeric, Dim, Safe, Point, T_Point> >
struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef Point point_t;
typedef Point point_t;
typedef T_Point t_point_t;
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
typedef Eigen::Matrix<Numeric, 3, 3> Matrix3;
typedef Time time_t;
typedef Numeric num_t;
typedef Time time_t;
typedef Numeric num_t;
typedef SplineBase spline_t;
typedef typename std::vector<spline_t> t_spline_t;
typedef typename t_spline_t::iterator it_spline_t;
......@@ -54,14 +54,14 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
typedef curve_abc<Time, Numeric, Dim, Safe, Point> curve_abc_t;
typedef curve_constraints<point_t> spline_constraints;
/* Constructors - destructors */
public:
/// \brief Constructor.
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
/// \param wayPointsEns : an iterator pointing to the last element of a waypoint container.
/* Constructors - destructors */
public:
/// \brief Constructor.
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
/// \param wayPointsEns : an iterator pointing to the last element of a waypoint container.
///
template<typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd)
template<typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd)
: curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
/// \brief Constructor.
......@@ -82,7 +82,7 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
exact_cubic(const exact_cubic& other)
: curve_abc_t(), subSplines_(other.subSplines_) {}
/// \brief Destructor.
/// \brief Destructor.
virtual ~exact_cubic(){}
std::size_t getNumberSplines()
......@@ -249,13 +249,13 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
private:
//exact_cubic& operator=(const exact_cubic&);
/* Constructors - destructors */
/* Constructors - destructors */
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/*Operations*/
public:
/// \brief Evaluation of the cubic spline at time t.
/// \param t : time when to evaluate the spline
/// \return \f$x(t)\f$ point corresponding on spline at time t.
/// \return \f$x(t)\f$ point corresponding on spline at time t.
///
virtual point_t operator()(const time_t t) const
{
......@@ -295,22 +295,22 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
// this should not happen
throw std::runtime_error("Exact cubic evaluation failed; t is outside bounds");
}
/*Operations*/
/*Operations*/
/*Helpers*/
public:
/*Helpers*/
public:
/// \brief Get the minimum time for which the curve is defined
/// \return \f$t_{min}\f$ lower bound of time range.
num_t virtual min() const{return subSplines_.front().min();}
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$ upper bound of time range.
num_t virtual max() const{return subSplines_.back().max();}
/*Helpers*/
/*Helpers*/
/*Attributes*/
/*Attributes*/
public:
t_spline_t subSplines_; // const
/*Attributes*/
/*Attributes*/
};
} // namespace curves
#endif //_CLASS_EXACTCUBIC
......
......@@ -50,7 +50,7 @@ struct linear_variable{
return *this;
}
static linear_variable_t Zero(size_t dim=0){
static linear_variable_t Zero(){
linear_variable_t w;
w.A_ = matrix_t::Zero();
w.b_ = point_t::Zero();
......
/**
* \file piecewise_polynomial_curve.h
* \brief class allowing to create a piecewise polynomial curve.
* \file piecewise_curve.h
* \brief class allowing to create a piecewise curve.
* \author Jason C.
* \date 05/2019
*/
......@@ -9,61 +9,60 @@
#define _CLASS_PIECEWISE_CURVE
#include "curve_abc.h"
#include "polynomial.h"
#include "curve_conversion.h"
#include <typeinfo>
namespace curves
{
/// \class PiecewiseCurve.
/// \brief Represent a piecewise polynomial curve. We can add some new polynomials to the curve,
/// but the starting time of the polynomial to add should be equal to the ending time of the
/// piecewise_polynomial_curve.<br>\ Example : A piecewise polynomial curve composed of three polynomials pol_0,
/// pol_1 and pol_2 where pol_0 is defined between \f$[T0_{min},T0_{max}]\f$, pol_1 between
/// \f$[T0_{max},T1_{max}]\f$ and pol_2 between \f$[T1_{max},T2_{max}]\f$.
/// On the piecewise polynomial curve, pol_0 is located between \f$[T0_{min},T0_{max}[\f$,
/// pol_1 between \f$[T0_{max},T1_{max}[\f$ and pol_2 between \f$[T1_{max},T2_{max}]\f$.
/// \brief Represent a piecewise curve. We can add some new curve,
/// but the starting time of the curve to add should be equal to the ending time of the actual
/// piecewise_curve.<br>\ Example : A piecewise curve composed of three curves cf0,
/// cf1 and cf2 where cf0 is defined between \f$[T0_{min},T0_{max}]\f$, cf1 between
/// \f$[T0_{max},T1_{max}]\f$ and cf2 between \f$[T1_{max},T2_{max}]\f$.
/// On the piecewise polynomial curve, cf0 is located between \f$[T0_{min},T0_{max}[\f$,
/// cf1 between \f$[T0_{max},T1_{max}[\f$ and cf2 between \f$[T1_{max},T2_{max}]\f$.
///
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> >
typename T_Point= std::vector<Point,Eigen::aligned_allocator<Point> >,
typename Curve= curve_abc<Time, Numeric, Dim, Safe, Point>
>
struct piecewise_polynomial_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
struct piecewise_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
{
typedef Point point_t;
typedef T_Point t_point_t;
typedef Time time_t;
typedef Numeric num_t;
//typedef polynomial <double, double, 3, true, point_t, t_point_t> polynomial_t;
typedef polynomial <double, double, 3, true, point_t, t_point_t> polynomial_t;
typedef typename std::vector < polynomial_t > t_polynomial_t;
typedef std::vector<Time> t_vector_time_t;
public:
/// \brief Constructor.
/// Initialize a piecewise polynomial curve by giving the first polynomial curve.
typedef Point point_t;
typedef T_Point t_point_t;
typedef Time time_t;
typedef Numeric num_t;
typedef Curve curve_t;
typedef typename std::vector < curve_t > t_curve_t;
typedef typename std::vector< Time > t_time_t;
public:
/// \brief Constructor.
/// Initialize a piecewise curve by giving the first curve.
/// \param pol : a polynomial curve.
///
piecewise_polynomial_curve(const polynomial_t& pol)
{
size_ = 0;
add_polynomial_curve(pol);
time_polynomial_curves_.push_back(pol.min());
T_min_ = pol.min();
}
piecewise_curve(const curve_t& cf)
{
size_ = 0;
add_curve(cf);
time_curves_.push_back(cf.min());
T_min_ = cf.min();
}
virtual ~piecewise_polynomial_curve(){}
virtual ~piecewise_curve(){}
virtual Point operator()(const Time t) const
virtual Point operator()(const Time t) const
{
if(Safe &! (T_min_ <= t && t <= T_max_))
{
//std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
throw std::out_of_range("can't evaluate piecewise curve, out of range");
//std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
throw std::out_of_range("can't evaluate piecewise curve, out of range");
}
return polynomial_curves_.at(find_interval(t))(t);
return curves_.at(find_interval(t))(t);
}
/// \brief Evaluate the derivative of order N of curve at time t.
......@@ -75,78 +74,78 @@ struct piecewise_polynomial_curve : public curve_abc<Time, Numeric, Dim, Safe, P
{
if(Safe &! (T_min_ <= t && t <= T_max_))
{
throw std::out_of_range("can't evaluate piecewise curve, out of range");
throw std::invalid_argument("can't evaluate piecewise curve, out of range");
}
return (polynomial_curves_.at(find_interval(t))).derivate(t, order);
return (curves_.at(find_interval(t))).derivate(t, order);
}
/// \brief Add a new polynomial to piecewise curve, which should be defined in \f$[T_{min},T_{max}]\f$ where \f$T_{min}\f$
/// \brief Add a new curve to piecewise curve, which should be defined in \f$[T_{min},T_{max}]\f$ where \f$T_{min}\f$
/// is equal to \f$T_{max}\f$ of the actual piecewise curve.
/// \param pol : polynomial to add.
/// \param cf : curve to add.
///
void add_polynomial_curve(polynomial_t pol)
{
// Check time continuity : Beginning time of pol must be equal to T_max_ of actual piecewise curve.
if (size_!=0 && pol.min()!=T_max_)
{
throw std::out_of_range("Can not add new Polynom to PiecewiseCurve : time discontinuity between T_max_ and pol.min()");