vel acc constraints for bezier curves

include/spline/bezier_curve.h

@@ -12,6 +12,7 @@
 
 #include "curve_abc.h"
 #include "bernstein.h"
+#include "curve_constraint.h"
 
 #include "MathDefs.h"
 
@@ -34,7 +35,9 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
 	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 */
@@ -49,18 +52,36 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
 	, size_(std::distance(PointsBegin, PointsEnd))
     , degree_(size_-1)
     , bernstein_(spline::makeBernstein<num_t>(degree_))
-	{
+    {
         assert(bernstein_.size() == size_);
 		In it(PointsBegin);
         if(Safe && (size_<1 || minBound >= maxBound))
-		{
             throw std::out_of_range("can't create bezier min bound is higher than max bound"); // TODO
-		}
-		for(; it != PointsEnd; ++it)
-		{
-			pts_.push_back(*it);
-		}
-	}
+        for(; it != PointsEnd; ++it)
+            pts_.push_back(*it);
+    }
+
+
+    ///\brief Constructor
+    /// This constructor will add 4 points (2 after the first one, 2 before the last one)
+    /// to ensure that velocity and acceleration constraints are respected
+    ///\param PointsBegin, PointsEnd : the points parametering the Bezier curve
+    ///\param constraints : constraints applying on start / end velocities and acceleration
+    ///
+    template<typename In>
+    bezier_curve(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints, const time_t minBound=0, const time_t maxBound=1)
+    : minBound_(minBound)
+    , maxBound_(maxBound)
+    , size_(std::distance(PointsBegin, PointsEnd)+4)
+    , degree_(size_-1)
+    , bernstein_(spline::makeBernstein<num_t>(degree_))
+    {
+        if(Safe && (size_<1 || minBound >= maxBound))
+            throw std::out_of_range("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)
+            pts_.push_back(*cit);
+    }
 
 	///\brief Destructor
 	~bezier_curve()
@@ -86,8 +107,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
             throw std::out_of_range("can't evaluate bezier curve, out of range"); // TODO
         }
 		else
-		{
-			num_t dt = (1 - nT);
+        {
+            num_t dt = (1 - nT);
 			switch(size_)
             {
                 case 1 :
@@ -106,7 +127,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
 						+ 3 * pts_[2] * nT * nT * dt 
 						+ pts_[3] * nT * nT *nT;
                 default :
-                    return evalBernstein(dt);
+                    return evalBernstein(nT);
 				break;
 			}
 		}
@@ -154,7 +175,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
     ///  \param order : order of the derivative
     ///  \param t : the time when to evaluate the spine
     ///  \param return : the value x(t)
-    virtual point_t     derivate(const time_t t, const std::size_t order) const
+    virtual point_t derivate(const time_t t, const std::size_t order) const
     {
         bezier_curve_t deriv =compute_derivate(order);
         return deriv(t);
@@ -169,17 +190,41 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
         typename t_point_t::const_iterator pts_it = pts_.begin();
         for(typename std::vector<Bern<Numeric> >::const_iterator cit = bernstein_.begin();
             cit !=bernstein_.end(); ++cit, ++pts_it)
-        {
             res += cit->operator()(u) * (*pts_it);
-        }
         return res;
     }
 
     const t_point_t& waypoints() const {return pts_;}
 
+    private:
+    template<typename In>
+    t_point_t add_constraints(In PointsBegin, In PointsEnd, const curve_constraints_t& constraints)
+    {
+        t_point_t res;
+        point_t P0, P1, P2, P_n_2, P_n_1, PN;
+        P0 = *PointsBegin; PN = *(PointsEnd-1);
+        P1    = P0+ constraints.init_vel / degree_;
+        P_n_1 = PN -constraints.end_vel  / degree_;
+        P2    = constraints.init_acc / (degree_ * (degree_-1)) + 2* P1    - P0;
+        P_n_2 = constraints.end_acc  / (degree_ * (degree_-1)) + 2* P_n_1 - PN;
+
+        res.push_back(P0);
+        res.push_back(P1);
+        res.push_back(P2);
+
+        for(In it = PointsBegin+1; it != PointsEnd-1; ++it)
+            res.push_back(*it);
+
+        res.push_back(P_n_2);
+        res.push_back(P_n_1);
+        res.push_back(PN);
+        return res;
+    }
+
 /*Operations*/
 
 /*Helpers*/
+    public:
 	virtual time_t min() const{return minBound_;}
 	virtual time_t max() const{return maxBound_;}
 /*Helpers*/

python/spline_python.cpp

@@ -38,6 +38,7 @@ typedef std::vector<waypoint_t, Eigen::aligned_allocator<point_t> > t_waypoint_t
 
 typedef spline::spline_deriv_constraint  <real, real, 3, true, point_t, t_point_t> spline_deriv_constraint_t;
 typedef spline::curve_constraints<point_t> curve_constraints_t;
+typedef spline::curve_constraints<point6_t> curve_constraints6_t;
 /*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
 
 EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_t)
@@ -72,6 +73,19 @@ bezier_t* wrapBezierConstructorBounds(const point_list_t& array, const real lb,
     return new bezier_t(asVector.begin(), asVector.end(), lb, ub);
 }
 
+bezier_t* wrapBezierConstructorConstraints(const point_list_t& array, const curve_constraints_t& constraints)
+{
+    t_point_t asVector = vectorFromEigenArray<point_list_t, t_point_t>(array);
+    return new bezier_t(asVector.begin(), asVector.end(), constraints);
+}
+
+
+bezier_t* wrapBezierConstructorConstraintsBounds(const point_list_t& array, const curve_constraints_t& constraints, const real lb, const real ub)
+{
+    t_point_t asVector = vectorFromEigenArray<point_list_t, t_point_t>(array);
+    return new bezier_t(asVector.begin(), asVector.end(), constraints, lb, ub);
+}
+
 bezier6_t* wrapBezierConstructor6(const point_list6_t& array)
 {
     t_point6_t asVector = vectorFromEigenArray<point_list6_t, t_point6_t>(array);
@@ -85,6 +99,19 @@ bezier6_t* wrapBezierConstructorBounds6(const point_list6_t& array, const real l
     return new bezier6_t(asVector.begin(), asVector.end(), lb, ub);
 }
 
+bezier6_t* wrapBezierConstructor6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints)
+{
+    t_point6_t asVector = vectorFromEigenArray<point_list6_t, t_point6_t>(array);
+    return new bezier6_t(asVector.begin(), asVector.end(), constraints);
+}
+
+
+bezier6_t* wrapBezierConstructorBounds6Constraints(const point_list6_t& array, const curve_constraints6_t& constraints, const real lb, const real ub)
+{
+    t_point6_t asVector = vectorFromEigenArray<point_list6_t, t_point6_t>(array);
+    return new bezier6_t(asVector.begin(), asVector.end(), constraints, lb, ub);
+}
+
 spline_curve_t* wrapSplineConstructor(const coeff_t& array)
 {
     return new spline_curve_t(array, 0., 1.);

src/tests/spline_test/Main.cpp

@@ -60,7 +60,7 @@ void ComparePoints(const Eigen::VectorXd& pt1, const Eigen::VectorXd& pt2, const
     if((pt1-pt2).norm() > margin && !notequal)
 	{
 		error = true;
-        std::cout << errmsg << pt1 << " ; " << pt2 << std::endl;
+        std::cout << errmsg << pt1.transpose() << " ; " << pt2.transpose() << std::endl;
 	}
 }
 
@@ -247,6 +247,37 @@ void BezierDerivativeCurveTest(bool& error)
     ComparePoints(point_t::Zero(), cf3.derivate(0.,100), errMsg, error);
 }
 
+void BezierDerivativeCurveConstraintTest(bool& error)
+{
+    std::string errMsg("In test BezierDerivativeCurveConstraintTest ; unexpected result for x ");
+    point_t a(1,2,3);
+    point_t b(2,3,4);
+    point_t c(3,4,5);
+
+    bezier_curve_t::curve_constraints_t constraints;
+    constraints.init_vel = point_t(-1,-1,-1);
+    constraints.init_acc = point_t(-2,-2,-2);
+    constraints.end_vel = point_t(-10,-10,-10);
+    constraints.end_acc = point_t(-20,-20,-20);
+
+    std::vector<point_t> params;
+    params.push_back(a);
+    params.push_back(b);
+
+    params.push_back(c);
+    bezier_curve_t cf3(params.begin(), params.end(), constraints);
+
+    assert(cf3.degree_ == params.size() + 3);
+    assert(cf3.size_   == params.size() + 4);
+
+    ComparePoints(a, cf3(0), errMsg, error);
+    ComparePoints(c, cf3(1), errMsg, error);
+    ComparePoints(constraints.init_vel, cf3.derivate(0.,1), errMsg, error);
+    ComparePoints(constraints.end_vel , cf3.derivate(1.,1), errMsg, error);
+    ComparePoints(constraints.init_acc, cf3.derivate(0.,2), errMsg, error);
+    ComparePoints(constraints.end_vel, cf3.derivate(1.,1), errMsg, error);
+    ComparePoints(constraints.end_acc, cf3.derivate(1.,2), errMsg, error);
+}
 
 
 /*Exact Cubic Function tests*/
@@ -599,6 +630,7 @@ int main(int /*argc*/, char** /*argv[]*/)
     EffectorSplineRotationWayPointRotationTest(error);
     BezierCurveTest(error);
     BezierDerivativeCurveTest(error);
+    BezierDerivativeCurveConstraintTest(error);
 	if(error)
 	{
         std::cout << "There were some errors\n";