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

include/curves/CMakeLists.txt

@@ -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

include/curves/MathDefs.h

@@ -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

include/curves/bezier_curve.h

@@ -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
 

include/curves/cubic_hermite_spline.h

@@ -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*/
 
 };
 

include/curves/curve_abc.h

@@ -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
 

include/curves/curve_constraint.h

@@ -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

include/curves/curve_conversion.h

@@ -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>

include/curves/exact_cubic.h

@@ -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");