bindings for constraints bezier

python/spline_python.cpp

@@ -60,57 +60,56 @@ T_Point vectorFromEigenArray(const PointList& array)
     return res;
 }
 
-bezier_t* wrapBezierConstructor(const point_list_t& array)
+template <typename Bezier, typename PointList, typename T_Point>
+Bezier* wrapBezierConstructorTemplate(const PointList& array, const real lb = 0., const real ub =1.)
 {
-    t_point_t asVector = vectorFromEigenArray<point_list_t, t_point_t>(array);
-    return new bezier_t(asVector.begin(), asVector.end());
+    T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
+    return new Bezier(asVector.begin(), asVector.end(), lb, ub);
 }
 
+template <typename Bezier, typename PointList, typename T_Point, typename CurveConstraints>
+Bezier* wrapBezierConstructorConstraintsTemplate(const PointList& array, const CurveConstraints& constraints, const real lb = 0., const real ub =1.)
+{
+    T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
+    return new Bezier(asVector.begin(), asVector.end(), constraints, lb, ub);
+}
 
+/*3D constructors */
+bezier_t* wrapBezierConstructor(const point_list_t& array)
+{
+    return wrapBezierConstructorTemplate<bezier_t, point_list_t, t_point_t>(array) ;
+}
 bezier_t* wrapBezierConstructorBounds(const point_list_t& array, 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(), lb, ub);
+    return wrapBezierConstructorTemplate<bezier_t, point_list_t, t_point_t>(array, 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);
+    return wrapBezierConstructorConstraintsTemplate<bezier_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints) ;
 }
-
-
-bezier_t* wrapBezierConstructorConstraintsBounds(const point_list_t& array, const curve_constraints_t& constraints, const real lb, const real ub)
+bezier_t* wrapBezierConstructorBoundsConstraints(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);
+    return wrapBezierConstructorConstraintsTemplate<bezier_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints, lb, ub) ;
 }
-
+/*END 3D constructors */
+/*6D constructors */
 bezier6_t* wrapBezierConstructor6(const point_list6_t& array)
 {
-    t_point6_t asVector = vectorFromEigenArray<point_list6_t, t_point6_t>(array);
-    return new bezier6_t(asVector.begin(), asVector.end());
+    return wrapBezierConstructorTemplate<bezier6_t, point_list6_t, t_point6_t>(array) ;
 }
-
-
 bezier6_t* wrapBezierConstructorBounds6(const point_list6_t& array, 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(), lb, ub);
+    return wrapBezierConstructorTemplate<bezier6_t, point_list6_t, t_point6_t>(array, 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);
+    return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, 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);
+    return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, constraints, lb, ub) ;
 }
+/*END 6D constructors */
 
 spline_curve_t* wrapSplineConstructor(const coeff_t& array)
 {
@@ -221,6 +220,8 @@ BOOST_PYTHON_MODULE(spline)
         ("bezier6", no_init)
             .def("__init__", make_constructor(&wrapBezierConstructor6))
             .def("__init__", make_constructor(&wrapBezierConstructorBounds6))
+            //.def("__init__", make_constructor(&wrapBezierConstructor6Constraints))
+            //.def("__init__", make_constructor(&wrapBezierConstructorBounds6Constraints))
             .def("min", &bezier6_t::min)
             .def("max", &bezier6_t::max)
             .def("__call__", &bezier6_t::operator())
@@ -238,6 +239,8 @@ BOOST_PYTHON_MODULE(spline)
         ("bezier", no_init)
             .def("__init__", make_constructor(&wrapBezierConstructor))
             .def("__init__", make_constructor(&wrapBezierConstructorBounds))
+            .def("__init__", make_constructor(&wrapBezierConstructorConstraints))
+            .def("__init__", make_constructor(&wrapBezierConstructorBoundsConstraints))
             .def("min", &bezier_t::min)
             .def("max", &bezier_t::max)
             .def("__call__", &bezier_t::operator())
@@ -274,7 +277,7 @@ BOOST_PYTHON_MODULE(spline)
     /** END bezier curve**/
 
 
-    /** BEGIN spline constraints**/
+    /** BEGIN curve constraints**/
     class_<curve_constraints_t>
         ("curve_constraints", init<>())
             .add_property("init_vel", &get_init_vel, &set_init_vel)
@@ -282,7 +285,7 @@ BOOST_PYTHON_MODULE(spline)
             .add_property("end_vel", &get_end_vel, &set_end_vel)
             .add_property("end_acc", &get_end_acc, &set_end_acc)
         ;
-    /** END spline constraints**/
+    /** END curve constraints**/
 
 
     /** BEGIN spline_deriv_constraints**/

python/test/test.py

-from spline import bezier, spline, exact_cubic, curve_constraints, spline_deriv_constraint
+from spline import bezier, bezier6, spline, exact_cubic, curve_constraints, spline_deriv_constraint
 
 from numpy import matrix
+from numpy.linalg import norm
 
 waypoints = matrix([[1.,2.,3.],[4.,5.,6.]]).transpose()
+waypoints6 = matrix([[1.,2.,3.,7.,5.,5.],[4.,5.,6.,4.,5.,6.]]).transpose()
 time_waypoints = matrix([0.,1.])
 
 #testing bezier curve
+a = bezier6(waypoints6)
 a = bezier(waypoints, -1., 3.)
 
-a = bezier(waypoints)
 assert(a.degree == a.nbWaypoints -1)
 a.min()
 a.max()
@@ -31,6 +33,18 @@ for i in range(10):
 assert(prim(0) == matrix([0.,0.,0.])).all()
 
 
+#testing bezier with constraints
+c = curve_constraints();
+c.init_vel = matrix([0.,1.,1.]);
+c.end_vel  = matrix([0.,1.,1.]);
+c.init_acc = matrix([0.,1.,-1.]);
+c.end_acc  = matrix([0.,100.,1.]);
+
+
+a = bezier(waypoints,c)
+assert norm(a.derivate(0,1) - c.init_vel) < 1e-10
+assert norm(a.derivate(1,2) - c.end_acc) < 1e-10
+
 
 #testing spline function
 a = spline(waypoints)