Merge branch 'export_plot' into 'devel'

Export plot

See merge request loco-3d/curves!42

README.md

@@ -18,48 +18,6 @@ To do so, tools are provided to:
 The library is template-based, thus generic:  the curves can be of any dimension, and can be implemented in double, float  ...
 
 
-----------
-Example of use for and end-effector trajectory
--------------
-The library comes with an helper class to automatically generate end-effector trajectories.
-For instance, to create a 2 second long trajectory from the point (0,0,0) to (1,1,0), with a waypoint
-at (0.5,0.5,0.5), one can use the following code:
-
-```cpp
-typedef std::pair<double, Eigen::Vector3d> Waypoint;
-typedef std::vector<Waypoint> T_Waypoint;
-
-// loading helper class namespace
-using namespace spline::helpers;
-
-// Create waypoints
-waypoints.push_back(std::make_pair(0., Eigen::Vector3d(0,0,0)));
-waypoints.push_back(std::make_pair(1., Eigen::Vector3d(0.5,0.5,0.5)));
-waypoints.push_back(std::make_pair(2., Eigen::Vector3d(1,1,0)));
-
-exact_cubic_t* eff_traj = effector_spline(waypoints.begin(),waypoints.end());
-
-// evaluate spline
-(*eff_traj)(0.); // (0,0,0)
-(*eff_traj)(2.); // (1,1,0)
-```
-If rotation of the effector must be considered, the code is almost the same:
-
-```cpp
-// initial rotation is 0, end rotation is a rotation by Pi around x axis
-quat_t init_rot(0,0,0,1), end_rot(1,0,0,0);
-effector_spline_rotation eff_traj_rot(waypoints.begin(),waypoints.end(), init_quat, end_quat);
-
-// evaluate spline
-eff_traj_rot(0.); // (0,0,0,0,0,0,1)
-eff_traj_rot(1.); // (0.5,0.5,0.5,0.707107,0,0,0.707107) // Pi/2 around x axis
-eff_traj_rot(2.); // (0,0,0,1,0,0,0)
-```
-
-Additional parameters for the same methods an be used to specify parameters for the take off and
-landing phases: height and duration of the phase, and along which normal.
-Please refer to the Main.cpp files to see all the unit tests and possibilities offered by the library
-
 Installation
 -------------
 
@@ -75,20 +33,18 @@ This package is available as binary in [robotpkg/wip](http://robotpkg.openrobots
 To handle this with cmake, use the recursive option to clone the repository.
 For instance, using http:
 ```
-git clone --recursive https://github.com/stonneau/spline.git $SPLINE_DIR
+git clone --recursive https://github.com/loco-3d/curves $CURVES_DIR
 ```
 The library is header only, so the build only serves to build the tests and python bindings:
 
 ```sh
-cd $SPLINE_DIR && mkdir build && cd build
-cmake .. && make
-../bin/tests
+cd $CURVES_DIR && mkdir build && cd build
+cmake .. && make && make test
 ```
 
 If everything went fine you should obtain the following output:
 ```sh
-performing tests...
-no errors found
+100% tests passed, 0 tests failed out of 3
 ```
 ### Optional: Python bindings installation
 To install the Python bindings, in the CMakeLists.txt file, first enable the BUILD_PYTHON_INTERFACE option:
@@ -98,13 +54,18 @@ OPTION (BUILD_PYTHON_INTERFACE "Build the python binding" ON)
 
 Then rebuild the library:
 ```sh
-cd $SPLINE_DIR/build
+cd $CURVES_DIR/build
 cmake -DCMAKE_INSTALL_PREFIX=${DEVEL_DIR}/install ..
 make install
 ```
 The python bindings should then be accessible through the package centroidal_dynamics.
-To see example of use, you can refer to the [test file](https://github.com/stonneau/spline/blob/master/python/test/test.py)
-which is rather self explanatory:
 
-In spite of an exhaustive documentation, please refer to the C++ documentation, which mostly applies
-to python. For the moment, only bezier curves are binded.
+
+Documentation and tutorial
+-------------
+
+For a python tutorial, you can refer to the [jupyter notebook](https://github.com/loco-3d/curves/blob/devel/python/test/sandbox/test.ipynb) . The [test file](https://github.com/loco-3d/curves/blob/master/python/test/test.py)
+is more exhaustive and rather self explanatory.
+
+Please refer to the C++ manual, which mostly applies
+to python.

include/curves/bezier_curve.h

@@ -40,7 +40,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
   typedef typename t_point_t::const_iterator cit_point_t;
   typedef bezier_curve<Time, Numeric, Safe, Point> bezier_curve_t;
   typedef boost::shared_ptr<bezier_curve_t> bezier_curve_ptr_t;
-  typedef piecewise_curve<Time, Numeric, Safe, point_t> piecewise_curve_t;
+  typedef piecewise_curve<Time, Numeric, Safe, point_t, point_t, bezier_curve_t> piecewise_curve_t;
   typedef curve_abc<Time, Numeric, Safe, point_t> curve_abc_t;  // parent class
   typedef typename curve_abc_t::curve_ptr_t curve_ptr_t;
 

include/curves/fwd.h

@@ -29,7 +29,7 @@ template <typename Time, typename Numeric, bool Safe, typename Point,
   struct exact_cubic;
 
 template <typename Time, typename Numeric, bool Safe, typename Point,
-            typename Point_derivate>
+            typename Point_derivate, typename CurveType>
   struct piecewise_curve;
 
 template <typename Time, typename Numeric, bool Safe,typename Point, typename T_Point>
@@ -82,19 +82,19 @@ typedef polynomial<double, double, true, pointX_t, t_pointX_t> polynomial_t;
 typedef exact_cubic<double, double, true, pointX_t,t_pointX_t, polynomial_t> exact_cubic_t;
 typedef bezier_curve<double, double, true, pointX_t> bezier_t;
 typedef cubic_hermite_spline<double, double, true, pointX_t> cubic_hermite_spline_t;
-typedef piecewise_curve <double, double, true, pointX_t,pointX_t> piecewise_t;
+typedef piecewise_curve <double, double, true, pointX_t,pointX_t, curve_abc_t> piecewise_t;
 
 // definition of all curves class with point3 as return type:
 typedef polynomial<double, double, true, point3_t, t_point3_t> polynomial3_t;
 typedef exact_cubic<double, double, true, point3_t,t_point3_t, polynomial_t> exact_cubic3_t;
 typedef bezier_curve<double, double, true, point3_t> bezier3_t;
 typedef cubic_hermite_spline<double, double, true, point3_t> cubic_hermite_spline3_t;
-typedef piecewise_curve <double, double, true, point3_t,point3_t> piecewise3_t;
+typedef piecewise_curve <double, double, true, point3_t,point3_t, curve_3_t> piecewise3_t;
 
 // special curves with return type fixed:
 typedef SO3Linear<double, double, true> SO3Linear_t;
 typedef SE3Curve<double, double, true> SE3Curve_t;
-typedef piecewise_curve <double, double, true,  transform_t, point6_t> piecewise_SE3_t;
+typedef piecewise_curve <double, double, true,  transform_t, point6_t, curve_SE3_t> piecewise_SE3_t;
 
 }
 

include/curves/piecewise_curve.h

@@ -25,7 +25,7 @@ namespace curves {
 ///
 template <typename Time = double, typename Numeric = Time, bool Safe = false,
           typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
-          typename Point_derivate = Point >
+          typename Point_derivate = Point, typename CurveType = curve_abc<Time, Numeric, Safe, Point,Point_derivate> >
 struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_derivate> {
   typedef Point point_t;
   typedef Point_derivate   point_derivate_t;
@@ -33,11 +33,12 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
   typedef std::vector<point_derivate_t, Eigen::aligned_allocator<point_derivate_t> > t_point_derivate_t;
   typedef Time time_t;
   typedef Numeric num_t;
-  typedef curve_abc<Time, Numeric, Safe, point_t,point_derivate_t> curve_t; // parent class
+  typedef curve_abc<Time, Numeric, Safe, point_t,point_derivate_t> base_curve_t; // parent class
+  typedef CurveType curve_t; // contained curves base class
   typedef boost::shared_ptr<curve_t> curve_ptr_t;
   typedef typename std::vector<curve_ptr_t> t_curve_ptr_t;
   typedef typename std::vector<Time> t_time_t;
-  typedef piecewise_curve<Time, Numeric, Safe, Point,Point_derivate> piecewise_curve_t;
+  typedef piecewise_curve<Time, Numeric, Safe, Point,Point_derivate,CurveType> piecewise_curve_t;
  public:
   /// \brief Empty constructor. Add at least one curve to call other class functions.
   ///
@@ -97,7 +98,7 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
     return true;
   }
 
-  virtual bool isApprox(const curve_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
+  virtual bool isApprox(const base_curve_t* other, const Numeric prec = Eigen::NumTraits<Numeric>::dummy_precision()) const{
     const piecewise_curve_t* other_cast = dynamic_cast<const piecewise_curve_t*>(other);
     if(other_cast)
       return isApprox(*other_cast,prec);
@@ -435,7 +436,7 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
     if (version) {
       // Do something depending on version ?
     }
-    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_t);
+    ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(base_curve_t);
     ar& boost::serialization::make_nvp("dim", dim_);
     ar& boost::serialization::make_nvp("curves", curves_);
     ar& boost::serialization::make_nvp("time_curves", time_curves_);
@@ -446,8 +447,8 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
 };  // End struct piecewise curve
 
 
-template <typename Time, typename Numeric, bool Safe, typename Point, typename Point_derivate>
-const double piecewise_curve<Time, Numeric, Safe, Point,Point_derivate >::MARGIN(0.001);
+template <typename Time, typename Numeric, bool Safe, typename Point, typename Point_derivate, typename CurveType>
+const double piecewise_curve<Time, Numeric, Safe, Point,Point_derivate, CurveType >::MARGIN(0.001);
 
 }  // namespace curves
 

python/CMakeLists.txt

@@ -24,7 +24,16 @@ IF(APPLE)
   SET_TARGET_PROPERTIES(${PY_NAME} PROPERTIES SUFFIX ".so")
 ENDIF(APPLE)
 
-INSTALL(TARGETS ${PY_NAME} DESTINATION ${PYTHON_SITELIB})
+INSTALL(TARGETS ${PY_NAME} DESTINATION ${PYTHON_SITELIB}/${PY_NAME})
+
+install (FILES plot/plot.py
+  deploy/__init__.py
+  DESTINATION ${PYTHON_SITELIB}/${PY_NAME})
+  
+install (FILES deploy/optimization/__init__.py
+  DESTINATION ${PYTHON_SITELIB}/${PY_NAME}/optimization)
+  
 
 ADD_PYTHON_UNIT_TEST("python-curves" "python/test/test.py" "python")
 ADD_PYTHON_UNIT_TEST("python-optimization" "python/test/optimization.py" "python")
+ADD_PYTHON_UNIT_TEST("python-notebook" "python/test/notebook.py" "python")

python/curves_python.cpp

@@ -176,6 +176,21 @@ piecewise_SE3_t* wrapPiecewiseSE3EmptyConstructor() {
   return new piecewise_SE3_t();
 }
 
+typedef bezier_t::piecewise_curve_t piecewise_bezier_t;
+piecewise_bezier_t* wrapPiecewiseBezierConstructor(const bezier_t::bezier_curve_ptr_t& curve) {
+  return new piecewise_bezier_t(curve);
+}
+piecewise_bezier_t* wrapPiecewiseBezierEmptyConstructor() {
+  return new piecewise_bezier_t();
+}
+typedef bezier_linear_variable_t::piecewise_curve_t piecewise_linear_bezier_t;
+piecewise_linear_bezier_t* wrapPiecewiseBezierLinearConstructor(const bezier_linear_variable_t::bezier_curve_ptr_t& curve) {
+  return new piecewise_linear_bezier_t(curve);
+}
+piecewise_linear_bezier_t* wrapPiecewiseBezierLinearEmptyConstructor() {
+  return new piecewise_linear_bezier_t();
+}
+
 static piecewise_t discretPointToPolynomialC0(const pointX_list_t& points,
                                                                const time_waypoints_t& time_points) {
   t_pointX_t points_list = vectorFromEigenArray<pointX_list_t, t_pointX_t>(points);
@@ -494,7 +509,7 @@ BOOST_PYTHON_MODULE(curves) {
 
 
   /** BEGIN bezier3 curve**/
-  class_<bezier3_t, bases<curve_3_t> >("bezier3", init<>())
+  class_<bezier3_t, bases<curve_3_t>, boost::shared_ptr<bezier3_t> >("bezier3", init<>())
       .def("__init__", make_constructor(&wrapBezier3Constructor))
       .def("__init__", make_constructor(&wrapBezier3ConstructorBounds))
       .def("__init__", make_constructor(&wrapBezier3ConstructorConstraints))
@@ -519,7 +534,7 @@ BOOST_PYTHON_MODULE(curves) {
       ;
   /** END bezier3 curve**/
   /** BEGIN bezier curve**/
-  class_<bezier_t, bases<curve_abc_t> >("bezier", init<>())
+  class_<bezier_t, bases<curve_abc_t>, boost::shared_ptr<bezier_t> >("bezier", init<>())
       .def("__init__", make_constructor(&wrapBezierConstructor))
       .def("__init__", make_constructor(&wrapBezierConstructorBounds))
       .def("__init__", make_constructor(&wrapBezierConstructorConstraints))
@@ -567,7 +582,7 @@ BOOST_PYTHON_MODULE(curves) {
       .def("isZero", &linear_variable_t::isZero)
       .def("norm", &linear_variable_t::norm);
 
-  class_<bezier_linear_variable_t>("bezier_linear_variable", no_init)
+  class_<bezier_linear_variable_t, bases<curve_abc_t>, boost::shared_ptr<bezier_linear_variable_t> >("bezier_linear_variable", no_init)
       .def("__init__", make_constructor(&wrapBezierLinearConstructor))
       .def("__init__", make_constructor(&wrapBezierLinearConstructorBounds))
       .def("min", &bezier_linear_variable_t::min)
@@ -708,6 +723,68 @@ BOOST_PYTHON_MODULE(curves) {
       .def(bp::self != bp::self)
       ;
 
+  class_<piecewise_bezier_t, bases<curve_abc_t> >("piecewise_bezier", init<>())
+      .def("__init__",
+           make_constructor(&wrapPiecewiseBezierConstructor, default_call_policies(), arg("curve")),
+           "Create a peicewise Bezier curve containing the given curve.")
+          .def("__init__", make_constructor(&wrapPiecewiseBezierEmptyConstructor),
+          "Create an empty piecewise-Beziercurve.")
+      .def("append", &piecewise_bezier_t::add_curve_ptr,
+           "Add a new curve to piecewise curve, which should be defined in T_{min},T_{max}] "
+           "where T_{min} is equal toT_{max} of the actual piecewise curve.")
+      .def("is_continuous", &piecewise_bezier_t::is_continuous,
+           "Check if the curve is continuous at the given order.",args("self,order"))
+      .def("curve_at_index", &piecewise_bezier_t::curve_at_index,
+           return_value_policy<copy_const_reference>())
+      .def("curve_at_time", &piecewise_bezier_t::curve_at_time, return_value_policy<copy_const_reference>())
+      .def("num_curves", &piecewise_bezier_t::num_curves)
+      .def("saveAsText", &piecewise_bezier_t::saveAsText<piecewise_bezier_t>, bp::args("filename"),
+           "Saves *this inside a text file.")
+      .def("loadFromText", &piecewise_bezier_t::loadFromText<piecewise_bezier_t>,
+           bp::args("filename"), "Loads *this from a text file.")
+      .def("saveAsXML", &piecewise_bezier_t::saveAsXML<piecewise_bezier_t>,
+           bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
+      .def("loadFromXML", &piecewise_bezier_t::loadFromXML<piecewise_bezier_t>,
+           bp::args("filename", "tag_name"), "Loads *this from a XML file.")
+      .def("saveAsBinary", &piecewise_bezier_t::saveAsBinary<piecewise_bezier_t>,
+           bp::args("filename"), "Saves *this inside a binary file.")
+      .def("loadFromBinary", &piecewise_bezier_t::loadFromBinary<piecewise_bezier_t>,
+           bp::args("filename"), "Loads *this from a binary file.")
+      .def(bp::self == bp::self)
+      .def(bp::self != bp::self)
+      ;
+
+  class_<piecewise_linear_bezier_t, bases<curve_abc_t> >("piecewise_bezier_linear", init<>())
+      .def("__init__",
+           make_constructor(&wrapPiecewiseBezierLinearConstructor, default_call_policies(), arg("curve")),
+           "Create a peicewise Bezier curve containing the given curve.")
+          .def("__init__", make_constructor(&wrapPiecewiseBezierLinearEmptyConstructor),
+          "Create an empty piecewise-Beziercurve.")
+      .def("append", &piecewise_linear_bezier_t::add_curve_ptr,
+           "Add a new curve to piecewise curve, which should be defined in T_{min},T_{max}] "
+           "where T_{min} is equal toT_{max} of the actual piecewise curve.")
+      .def("is_continuous", &piecewise_linear_bezier_t::is_continuous,
+           "Check if the curve is continuous at the given order.",args("self,order"))
+      .def("curve_at_index", &piecewise_linear_bezier_t::curve_at_index,
+           return_value_policy<copy_const_reference>())
+      .def("curve_at_time", &piecewise_linear_bezier_t::curve_at_time, return_value_policy<copy_const_reference>())
+      .def("num_curves", &piecewise_linear_bezier_t::num_curves)
+      .def("saveAsText", &piecewise_linear_bezier_t::saveAsText<piecewise_linear_bezier_t>, bp::args("filename"),
+           "Saves *this inside a text file.")
+      .def("loadFromText", &piecewise_linear_bezier_t::loadFromText<piecewise_linear_bezier_t>,
+           bp::args("filename"), "Loads *this from a text file.")
+      .def("saveAsXML", &piecewise_linear_bezier_t::saveAsXML<piecewise_linear_bezier_t>,
+           bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
+      .def("loadFromXML", &piecewise_linear_bezier_t::loadFromXML<piecewise_linear_bezier_t>,
+           bp::args("filename", "tag_name"), "Loads *this from a XML file.")
+      .def("saveAsBinary", &piecewise_linear_bezier_t::saveAsBinary<piecewise_linear_bezier_t>,
+           bp::args("filename"), "Saves *this inside a binary file.")
+      .def("loadFromBinary", &piecewise_linear_bezier_t::loadFromBinary<piecewise_linear_bezier_t>,
+           bp::args("filename"), "Loads *this from a binary file.")
+      .def(bp::self == bp::self)
+      .def(bp::self != bp::self)
+      ;
+
   class_<piecewise_SE3_t, bases<curve_SE3_t> >("piecewise_SE3", init<>())
       .def("__init__", make_constructor(&wrapPiecewiseSE3Constructor, default_call_policies(), arg("curve")),
       "Create a piecewise-se3 curve containing the given se3 curve.")

python/deploy/__init__.py

+#!/usr/bin/env python
+# Copyright (c) 2019 CNRS
+# Author : Steve Tonneau
+
+from curves import * 
+import plot
+

python/deploy/optimization/__init__.py

+#!/usr/bin/env python
+# Copyright (c) 2019 CNRS
+# Author : Steve Tonneau
+
+from curves.curves.optimization import *

python/test/sandbox/plot_bezier.py → python/plot/plot.py

 import eigenpy
 import matplotlib.pyplot as plt
+from mpl_toolkits.mplot3d import Axes3D
 import numpy as np
 from numpy import array
 
@@ -63,7 +64,6 @@ if __name__ == '__main__':
     a = bezier(waypoints)
 
     fig = plt.figure()
-    # ax = fig.add_subplot(131, projection="3d")
     ax = fig.add_subplot(121)
     plotBezier2D(a, axes=[1, 0], ax=ax)
     plotControlPoints2D(a, axes=[1, 0], ax=ax)

python/python_variables.cpp

@@ -56,12 +56,13 @@ linear_variable_t* wayPointsToLists(const bezier_linear_variable_t& self) {
   const t_point& wps = self.waypoints();
   // retrieve num variables.
   std::size_t dim = wps[0].B().cols();
-  Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> matrices(dim, wps.size() * 3);
-  Eigen::Matrix<real, Eigen::Dynamic, 1> vectors = Eigen::Matrix<real, Eigen::Dynamic, 1>::Zero(3 * wps.size());
+  std::size_t dimRows = wps[0].c().rows();
+  Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> matrices(dim, wps.size() * dimRows);
+  Eigen::Matrix<real, Eigen::Dynamic, 1> vectors = Eigen::Matrix<real, Eigen::Dynamic, 1>::Zero(dimRows * wps.size());
   int i = 0;
   for (cit_point cit = wps.begin(); cit != wps.end(); ++cit, ++i) {
-    matrices.block(0, i * 3, dim, 3) = cit->B().transpose();
-    vectors.segment<3>(i * 3, i * 3 + 2) = cit->c();
+    matrices.block(0, i * dimRows, dim, dimRows) = cit->B().transpose();
+    vectors.segment(i * dimRows, dimRows) = cit->c();
   }
   return new linear_variable_t(matrices.transpose(), vectors.transpose());
 }

python/test/notebook.py

+import os
+import unittest
+from math import sqrt
+
+import eigenpy
+import numpy as np
+from numpy import array, array_equal, isclose, random, zeros, identity, dot, hstack, vstack
+from numpy.linalg import norm
+
+from curves import (CURVES_WITH_PINOCCHIO_SUPPORT, Quaternion, SE3Curve, SO3Linear, bezier, bezier3,
+                    cubic_hermite_spline, curve_constraints, exact_cubic,polynomial,
+                    piecewise, piecewise_SE3, convert_to_polynomial,convert_to_bezier,convert_to_hermite)
+
+eigenpy.switchToNumpyArray()
+
+
+
+#importing the bezier curve class
+from curves import (bezier)
+
+#dummy methods
+def plot(*karrgs):
+        pass
+
+
+
+class TestNotebook(unittest.TestCase):
+    # def print_str(self, inStr):
+    #   print inStr
+    #   return
+
+    def test_notebook(self):
+        print("test_notebook")
+            
+        #We describe a degree 3 curve as a Bezier curve with 4 control points
+        waypoints = array([[1., 2., 3.], [-4., -5., -6.], [4., 5., 6.], [7., 8., 9.]]).transpose()
+        ref = bezier(waypoints)
+
+
+        numSamples = 10; fNumSamples = float(numSamples)
+        ptsTime = [ (ref(float(t) / fNumSamples), float(t) / fNumSamples) for t in range(numSamples+1)]
+
+
+        from curves.optimization import (problem_definition, setup_control_points)
+
+        #dimension of our problem (here 3 as our curve is 3D)
+        dim = 3
+        refDegree = 3
+        pD = problem_definition(dim)
+        pD.degree = refDegree #we want to fit a curve of the same degree as the reference curve for the sanity check
+
+        #generates the variable bezier curve with the parameters of problemDefinition
+        problem = setup_control_points(pD)
+        #for now we only care about the curve itself
+        variableBezier = problem.bezier()
+
+        linearVariable = variableBezier(0.)
+
+        #least square form of ||Ax-b||**2 
+        def to_least_square(A, b):
+            return dot(A.T, A), - dot(A.T, b)
+
+        def genCost(variableBezier, ptsTime):
+            #first evaluate variableBezier for each time sampled
+            allsEvals = [(variableBezier(time), pt) for (pt,time) in ptsTime]
+            #then compute the least square form of the cost for each points
+            allLeastSquares = [to_least_square(el.B(), el.c() + pt) for (el, pt) in  allsEvals]
+            #and finally sum the costs
+            Ab = [sum(x) for x in zip(*allLeastSquares)]
+            return Ab[0], Ab[1]
+        A, b = genCost(variableBezier, ptsTime)
+
+
+        def quadprog_solve_qp(P, q, G=None, h=None, C=None, d=None, verbose=False):
+            return  zeros(P.shape[0])
+
+        res = quadprog_solve_qp(A, b)
+
+
+
+
+        def evalAndPlot(variableBezier, res):
+            fitBezier = variableBezier.evaluate(res.reshape((-1,1)) ) 
+            return fitBezier
+            
+        fitBezier = evalAndPlot(variableBezier, res)
+
+
+
+        pD.degree = refDegree - 1
+
+
+        problem = setup_control_points(pD)
+        variableBezier = problem.bezier()
+
+        A, b = genCost(variableBezier, ptsTime)
+        res = quadprog_solve_qp(A, b)
+        fitBezier = evalAndPlot(variableBezier, res)
+            
+
+        from curves.optimization import constraint_flag
+
+        pD.flag = constraint_flag.INIT_POS | constraint_flag.END_POS
+        #set initial position
+        pD.init_pos = array([ptsTime[ 0][0]]).T
+        #set end position
+        pD.end_pos   = array([ptsTime[-1][0]]).T
+        problem = setup_control_points(pD)
+        variableBezier = problem.bezier()
+
+
+        prob = setup_control_points(pD)
+        variableBezier = prob.bezier()
+        A, b = genCost(variableBezier, ptsTime)
+        res = quadprog_solve_qp(A, b)
+        _ = evalAndPlot(variableBezier, res)
+            
+
+
+        #values are 0 by default, so if the constraint is zero this can be skipped
+        pD.init_vel = array([[0., 0., 0.]]).T
+        pD.init_acc = array([[0., 0., 0.]]).T
+        pD.end_vel = array([[0., 0., 0.]]).T
+        pD.end_acc = array([[0., 0., 0.]]).T
+        pD.flag = constraint_flag.END_POS | constraint_flag.INIT_POS | constraint_flag.INIT_VEL  | constraint_flag.END_VEL  | constraint_flag.INIT_ACC  | constraint_flag.END_ACC
+
+        err = False
+        try:
+            prob = setup_control_points(pD)
+        except RuntimeError:
+            err = True
+        assert err
+
+
+        pD.degree = refDegree + 4
+        prob = setup_control_points(pD)
+        variableBezier = prob.bezier()
+        A, b = genCost(variableBezier, ptsTime)
+        res = quadprog_solve_qp(A, b)
+        fitBezier = evalAndPlot(variableBezier, res)
+
+
+
+        pD.degree = refDegree + 60
+        prob = setup_control_points(pD)
+        variableBezier = prob.bezier()
+        A, b = genCost(variableBezier, ptsTime)
+        #regularization matrix 
+        reg = identity(A.shape[1]) * 0.001
+        res = quadprog_solve_qp(A + reg, b)
+        fitBezier = evalAndPlot(variableBezier, res)
+
+
+        #set initial / terminal constraints
+        pD.flag = constraint_flag.END_POS | constraint_flag.INIT_POS
+        pD.degree = refDegree
+        prob = setup_control_points(pD)
+        variableBezier = prob.bezier()
+
+        #get value of the curve first order derivative at t = 0.8
+        t08Constraint = variableBezier.derivate(0.8,1)
+        target = zeros(3) 
+
+        A, b = genCost(variableBezier, ptsTime)
+        #solve optimization problem with quadprog
+
+        res = quadprog_solve_qp(A, b, C=t08Constraint.B(), d=target - t08Constraint.c())
+        fitBezier = evalAndPlot(variableBezier, res)
+
+        #returns a curve composed of the split curves, 2 in our case
+        piecewiseCurve = ref.split(array([[0.6]]).T)
+
+        #displaying the obtained curves
+
+        #first, split the variable curve
+        piecewiseCurve = variableBezier.split(array([[0.4, 0.8]]).T)
+
+        constrainedCurve = piecewiseCurve.curve_at_index(1)
+
+        #find the number of variables
+        problemSize = prob.numVariables * dim
+        #find the number of constraints, as many as waypoints
+        nConstraints = constrainedCurve.nbWaypoints
+
+        waypoints = constrainedCurve.waypoints()
+
+        ineqMatrix = zeros((nConstraints, problemSize))
+        ineqVector = zeros(nConstraints)
+
+
+        #finding the z equation of each control point
+        for i in range(nConstraints):
+            wayPoint = constrainedCurve.waypointAtIndex(i)
+            ineqMatrix[i,:] = wayPoint.B()[2,:]
+            ineqVector[i] =  -wayPoint.c()[2]
+
+            
+        res = quadprog_solve_qp(A, b, G=ineqMatrix, h = ineqVector)
+        fitBezier = variableBezier.evaluate(res.reshape((-1,1)) ) 
+
+
+
+if __name__ == '__main__':
+    unittest.main()

python/test/sandbox/qp.py

-from __future__ import print_function
-
-import quadprog
-from numpy import array, dot, hstack, vstack
-from scipy.optimize import linprog
-
-
-def quadprog_solve_qp(P, q, G=None, h=None, C=None, d=None, verbose=False):