start and end velocity constraints

.gitignore

 # Build and Release Folders
 bin/
 lib/
+build/
 
 # temp files
 .*~
+*.user

include/spline/curve_abc.h

@@ -42,7 +42,7 @@ struct  curve_abc : std::unary_function<Time, Point>
 	///  \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 = 0;
+    virtual point_t operator()(time_t t) const = 0;
 /*Operations*/
 
 /*Helpers*/

include/spline/exact_cubic.h

@@ -59,10 +59,16 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
 	exact_cubic(In wayPointsBegin, In wayPointsEnd)
         : curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
 
+
+    ///\brief Constructor
+    ///\param subSplines: vector of subsplines
+    exact_cubic(const t_spline_t& subSplines)
+        : curve_abc_t(), subSplines_(subSplines) {}
+
 	///\brief Destructor
     ~exact_cubic(){}
 
-    protected:
+    private:
     template<typename In>
     t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd) const
     {
@@ -158,7 +164,22 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
                 return it->operator()(t);
 			}
 		}
-	}
+    }
+
+    ///  \brief Evaluation of the derivative spline at time t.
+    ///  \param t : the time when to evaluate the spine
+    ///  \param return : the value x(t)
+    virtual point_t derivate(time_t t, std::size_t order) const
+    {
+        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->min()) && t <= (it->max()))
+            {
+                return it->derivate(t, order);
+            }
+        }
+    }
 	/*Operations*/
 
 	/*Helpers*/

include/spline/exact_cubic_vel_acc_cons.h

@@ -39,7 +39,7 @@ namespace spline
 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 curve_abc<Time, Numeric, Dim, Safe, Point>
+struct cubic_zero_vel : public exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point>
 {
     typedef Point 	point_t;
     typedef T_Point t_point_t;
@@ -47,26 +47,65 @@ struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
     typedef Time 	time_t;
     typedef Numeric	num_t;
     typedef spline_curve<time_t, Numeric, Dim, Safe, point_t, t_point_t> spline_t;
-    typedef curve_abc<Time, Numeric, Dim, Safe, Point> curve_abc_t;
+    typedef exact_cubic<time_t, Numeric, Dim, Safe, point_t, t_point_t> 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;
 
+    public:
+    struct spline_constraints
+    {
+        spline_constraints():
+            init_vel(point_t::Zero()),init_acc(init_vel),end_vel(init_vel),end_acc(init_vel),
+            init_normal(init_vel),end_normal(init_vel) {}
+
+        spline_constraints(const point_t& n0, point_t& n1):
+            init_vel(point_t::Zero()),init_acc(init_vel),end_vel(init_vel),end_acc(init_vel),
+            init_normal(n0),end_normal(n1) {}
+
+       ~spline_constraints(){}
+        point_t init_vel;
+        point_t init_acc;
+        point_t end_vel;
+        point_t end_acc;
+        point_t init_normal;
+        point_t end_normal;
+    };
+
 	/* 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 end           of a waypoint container
-	template<typename In>
-    cubic_zero_vel(In wayPointsBegin, In wayPointsEnd)
-        : curve_abc_t(), subSplines_(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
+    template<typename In>
+    cubic_zero_vel(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints = spline_constraints())
+        : exact_cubic_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints)) {}
 
 	///\brief Destructor
     ~cubic_zero_vel(){}
 
-    protected:
+    private:
+    MatrixX setVelConstraintsAndComputeB(const spline_constraints& constraints,
+                                         const Eigen::Ref<MatrixX> x,
+                                         Eigen::Ref<MatrixX> h1, Eigen::Ref<MatrixX> h2) const
+    {
+        std::size_t size(h1.rows());
+        MatrixX cons =  MatrixX::Zero(h1.rows(), Dim); // constraint matrix on b
+        cons.row(0) = constraints.init_vel;
+        cons.row(size-1) = constraints.end_vel;
+
+
+        h1(0,0)= 1; // activating init velocity constraint
+        h1(size-1,size-1)= 1; // activating end velocity constraint
+
+        MatrixX b = MatrixX::Zero(size,Dim);
+        MatrixX h1inv = h1.inverse();
+        b = h1inv * (h2 *x + cons); //h1 * b = h2 * x => b = (h1)^-1 * h2 * x
+        return b;
+    }
+
     template<typename In>
-    t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd) const
+    t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints) const
     {
         std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
         if(Safe && size < 1)
@@ -84,12 +123,10 @@ struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
         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)
@@ -102,13 +139,8 @@ struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
             h3(i,i+1) =  3 / dTi_sqr;
             h4(i,i)   = -2 / dTi;
             h4(i,i+1) = -1 / dTi;
-            num_t coeff_h5(-2);
-            if(i == 0)
-            {
-                coeff_h5 = 1;
-            }
-            h5(i,i)   = -coeff_h5 / dTi_cube;
-            h5(i,i+1) =  coeff_h5 / dTi_cube;
+            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)
@@ -129,12 +161,8 @@ struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
         // adding last x
         x.row(size-1)= (*it).second.transpose();
         a= x;
-        h1(0,0)= 1;
-        h3(0,0)= 0;h3(0,1)= 0;
-        h4(0,0)= 0;h4(0,1)= 0;
-        h6(0,0)= 0;h6(0,1)= 0;
-        PseudoInverse(h1);
-        b = h1 * h2 * x; //h1 * b = h2 * x => b = (h1)^-1 * h2 * x
+        // init velocity given by a constraint on b0
+        MatrixX b = setVelConstraintsAndComputeB(constraints, x, h1, h2);
         c = h3 * x + h4 * b;
         d = h5 * x + h6 * b;
         it= wayPointsBegin, next=wayPointsBegin; ++ next;
@@ -143,8 +171,8 @@ struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
             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));
         }
-        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));
+        //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;
     }
 
@@ -153,31 +181,6 @@ struct cubic_zero_vel : public curve_abc<Time, Numeric, Dim, Safe, Point>
     cubic_zero_vel(const cubic_zero_vel&);
     cubic_zero_vel& operator=(const cubic_zero_vel&);
     /* 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().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->min()) && t <= (it->max()))
-            {
-                return it->operator()(t);
-            }
-        }
-    }
-    /*Operations*/
-
-    /*Helpers*/
-    public:
-    num_t virtual min() const{return subSplines_.front().min();}
-    num_t virtual max() const{return subSplines_.back().max();}
-    /*Helpers*/
-
     /*Attributes*/
     public:
     const t_spline_t subSplines_;

include/spline/spline_curve.h

@@ -129,6 +129,33 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
             currentPoint_ += cdt *coefficients_.col(i);
         return currentPoint_;
     }
+
+
+    ///  \brief Evaluation of the derivative spline at time t.
+    ///  \param t : the time when to evaluate the spine
+    ///  \param return : the value x(t)
+    point_t derivate(time_t t, std::size_t order) const
+    {
+        if((t < t_min_ || t > t_max_) && Safe){ throw std::out_of_range("TODO");}
+        time_t const dt (t-t_min_);
+        time_t cdt(1);
+        point_t currentPoint_ = point_t::Zero();
+        for(int i = order; i < order_+1; ++i, cdt*=dt)
+        {
+            currentPoint_ += cdt *coefficients_.col(i) * fact(i, order);
+        }
+        return currentPoint_;
+    }
+
+    private:
+    num_t fact(const std::size_t n, const std::size_t order) const
+    {
+        std::size_t res(1);
+        for(std::size_t i = 0; i < order; ++i)
+            res *= n-i;
+        return res;
+    }
+
 /*Operations*/
 
 /*Helpers*/

src/tests/spline_test/Main.cpp

@@ -16,7 +16,9 @@ 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 cubic_zero_vel <double, double, 3, true, point_t> cubic_zero_vel_t;
 typedef bezier_curve  <double, double, 3, true, point_t> bezier_curve_t;
+typedef cubic_zero_vel_t::spline_constraints spline_constraints_t;
 typedef std::pair<double, point_t> Waypoint;
 typedef std::vector<Waypoint> T_Waypoint;
 
@@ -311,6 +313,24 @@ void ExactCubicOneDimTest(bool& error)
 	ComparePoints(one, res1, errmsg, error);
 }
 
+void CheckWayPointConstraint(const std::string& errmsg, const double step, const spline::T_Waypoint& wayPoints, const exact_cubic_t* curve, bool& error )
+{
+    point_t res1;
+    for(double i = 0; i <= 1; i = i + step)
+    {
+        res1 = (*curve)(i);
+        ComparePoints(point_t(i,i,i), res1, errmsg, error);
+    }
+}
+
+void CheckDerivative(const std::string& errmsg, const double eval_point, const std::size_t order, const point_t& target, const exact_cubic_t* curve, bool& error )
+{
+    point_t res1;
+    res1 = curve->derivate(eval_point,order);
+    ComparePoints(target, res1, errmsg, error);
+}
+
+
 void ExactCubicPointsCrossedTest(bool& error)
 {
 	spline::T_Waypoint waypoints;
@@ -318,25 +338,52 @@ void ExactCubicPointsCrossedTest(bool& error)
 	{
 		waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
 	}
-	exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
-	point_t res1;
-	for(double i = 0; i <= 1; i = i + 0.2)
-	{
-		res1 = exactCubic(i);
-		std::string errmsg("Error While checking that given wayPoints are crossed (expected / obtained)");
-		ComparePoints(point_t(i,i,i), res1, errmsg, error);
-	}
+    exact_cubic_t exactCubic(waypoints.begin(), waypoints.end());
+    std::string errmsg("Error While checking that given wayPoints are crossed (expected / obtained)");
+    CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
+
 }
 
-int main(int argc, char *argv[])
+void ExactCubicVelocityConstraintsTest(bool& error)
+{
+    spline::T_Waypoint waypoints;
+    for(double i = 0; i <= 1; i = i + 0.2)
+    {
+        waypoints.push_back(std::make_pair(i,point_t(i,i,i)));
+    }
+    std::string errmsg("Error in ExactCubicVelocityConstraintsTest (1); while checking that given wayPoints are crossed (expected / obtained)");
+    spline_constraints_t constraints;
+    cubic_zero_vel_t exactCubic(waypoints.begin(), waypoints.end());
+    // now check that init and end velocity are 0
+    CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
+    std::string errmsg3("Error in ExactCubicVelocityConstraintsTest (2); while checking derivative (expected / obtained)");
+    // now check derivatives
+    CheckDerivative(errmsg3,0,1,constraints.init_vel,&exactCubic, error);
+    CheckDerivative(errmsg3,1,1,constraints.end_vel,&exactCubic, error);
+
+    constraints.end_vel = point_t(1,2,3);
+    constraints.init_vel = point_t(-1,-2,-3);
+    std::string errmsg2("Error in ExactCubicVelocityConstraintsTest (3); while checking that given wayPoints are crossed (expected / obtained)");
+    cubic_zero_vel_t exactCubic2(waypoints.begin(), waypoints.end(),constraints);
+    CheckWayPointConstraint(errmsg2, 0.2, waypoints, &exactCubic2, error);
+
+    std::string errmsg4("Error in ExactCubicVelocityConstraintsTest (4); while checking derivative (expected / obtained)");
+    // now check derivatives
+    CheckDerivative(errmsg4,0,1,constraints.init_vel,&exactCubic2, error);
+    CheckDerivative(errmsg4,1,1,constraints.end_vel,&exactCubic2, error);
+}
+
+
+int main(int /*argc*/, char* /*argv[]*/)
 {
 	std::cout << "performing tests... \n";
 	bool error = false;
-	CubicFunctionTest(error);
+    CubicFunctionTest(error);
 	ExactCubicNoErrorTest(error);
 	ExactCubicPointsCrossedTest(error); // checks that given wayPoints are crossed
 	ExactCubicTwoPointsTest(error);
-	ExactCubicOneDimTest(error);
+    ExactCubicOneDimTest(error);
+    ExactCubicVelocityConstraintsTest(error);
 	//BezierCurveTest(error);
 	if(error)
 	{