python bindings and tests

.gitignore

@@ -5,3 +5,4 @@ build-rel/
 # temp files
 .*~
 *.user
+*.pyc

python/CMakeLists.txt

@@ -3,10 +3,10 @@ STRING(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
 ADD_REQUIRED_DEPENDENCY("eigenpy")
 
 # Define the wrapper library that wraps our library
-add_library( ${PY_NAME} SHARED spline_python.cpp )
+add_library( ${PY_NAME} SHARED spline_python.cpp python_variables.cpp python_variables.h )
 #~ target_link_libraries( centroidal_dynamics ${Boost_LIBRARIES} ${PROJECT_NAME} )
 # don't prepend wrapper library name with lib
-set_target_properties( ${PY_NAME} PROPERTIES PREFIX "" )
+set_target_properties( ${PY_NAME} PROPERTIES PREFIX ""  )
 
 IF(APPLE)
   # We need to change the extension for python bindings

python/python_definitions.h

+#include "hpp/spline/bezier_curve.h"
+#include "hpp/spline/linear_variable.h"
+
+#include <vector>
+
+#ifndef _DEFINITION_PYTHON_BINDINGS
+#define _DEFINITION_PYTHON_BINDINGS
+
+
+namespace spline
+{
+
+typedef double real;
+typedef Eigen::Vector3d point_t;
+typedef Eigen::VectorXd vectorX_t;
+typedef Eigen::Matrix<double, 6, 1, 0, 6, 1> point6_t;
+typedef Eigen::Matrix<double, 3, 1, 0, 3, 1> ret_point_t;
+typedef Eigen::Matrix<double, 6, 1, 0, 6, 1> ret_point6_t;
+typedef Eigen::VectorXd time_waypoints_t;
+typedef Eigen::Matrix<real, 3, Eigen::Dynamic> point_list_t;
+typedef Eigen::Matrix<real, 6, Eigen::Dynamic> point_list6_t;
+typedef std::vector<point_t,Eigen::aligned_allocator<point_t> >  t_point_t;
+typedef std::vector<point6_t,Eigen::aligned_allocator<point6_t> >  t_point6_t;
+typedef std::pair<real, point_t> Waypoint;
+typedef std::vector<Waypoint> T_Waypoint;
+typedef std::pair<real, point6_t> Waypoint6;
+typedef std::vector<Waypoint6> T_Waypoint6;
+
+
+
+template <typename PointList, typename T_Point>
+T_Point vectorFromEigenArray(const PointList& array)
+{
+    T_Point res;
+    for(int i =0;i<array.cols();++i)
+        res.push_back(array.col(i));
+    return res;
+}
+
+} //namespace spline.
+
+
+#endif //_DEFINITION_PYTHON_BINDINGS

python/python_variables.cpp

+#include "python_variables.h"
+#include "python_definitions.h"
+
+#include <Eigen/Core>
+
+
+namespace spline
+{
+
+std::vector<linear_variable_3_t> matrix3DFromEigenArray(const point_list_t& matrices, const point_list_t& vectors)
+{
+    assert(vectors.cols() * 3  == matrices.cols() ) ;
+    std::vector<linear_variable_3_t> res;
+    for(int i =0;i<vectors.cols();++i)
+        res.push_back(linear_variable_3_t(matrices.block<3,3>(0,i*3), vectors.col(i)));
+    return res;
+}
+
+variables_3_t fillWithZeros(const linear_variable_3_t& var, const std::size_t totalvar, const std::size_t i)
+{
+    variables_3_t res;
+    std::vector<linear_variable_3_t>& vars = res.variables_;
+    for (std::size_t idx = 0; idx < i; ++idx)
+        vars.push_back(linear_variable_3_t::Zero());
+    vars.push_back(var);
+    for (std::size_t idx = i+1; idx < totalvar; ++idx)
+        vars.push_back(linear_variable_3_t::Zero());
+    return res;
+}
+
+std::vector<variables_3_t> computeLinearControlPoints(const point_list_t& matrices, const point_list_t& vectors)
+{
+    std::vector<variables_3_t> res;
+    std::vector<linear_variable_3_t> variables = matrix3DFromEigenArray(matrices, vectors);
+    // now need to fill all this with zeros...
+    std::size_t totalvar = variables.size();
+    for (std::size_t i = 0; i < totalvar; ++i)
+        res.push_back( fillWithZeros(variables[i],totalvar,i));
+    return res;
+}
+
+/*linear variable control points*/
+bezier_linear_variable_t* wrapBezierLinearConstructor(const point_list_t& matrices, const point_list_t& vectors)
+{
+    std::vector<variables_3_t> asVector = computeLinearControlPoints(matrices, vectors);
+    return new bezier_linear_variable_t(asVector.begin(), asVector.end(), 1.) ;
+}
+
+bezier_linear_variable_t* wrapBezierLinearConstructorBounds(const point_list_t& matrices, const point_list_t& vectors, const real ub)
+{
+    std::vector<variables_3_t> asVector = computeLinearControlPoints(matrices, vectors);
+    return new bezier_linear_variable_t(asVector.begin(), asVector.end(), ub) ;
+}
+
+
+LinearControlPointsHolder*
+        wayPointsToLists(const bezier_linear_variable_t& self)
+{
+    typedef typename bezier_linear_variable_t::t_point_t t_point;
+    typedef typename bezier_linear_variable_t::t_point_t::const_iterator cit_point;
+    const t_point& wps = self.waypoints();
+    // retrieve num variables.
+    std::size_t dim = wps[0].variables_.size()*3;
+    Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> matrices (dim,wps.size() * 3);
+    Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> vectors  (dim,wps.size());
+    int col = 0;
+    for(cit_point cit = wps.begin(); cit != wps.end(); ++cit, ++col)
+    {
+        const std::vector<linear_variable_3_t>& variables = cit->variables_;
+        int i = 0;
+        for(std::vector<linear_variable_3_t>::const_iterator varit = variables.begin();
+            varit != variables.end(); ++varit, i+=3)
+        {
+            vectors.block<3,1>(i,col)   =  varit->b_;
+            matrices.block<3,3>(i,col*3) = varit->A_;
+        }
+    }
+    LinearControlPointsHolder* res (new LinearControlPointsHolder);
+    res->res = std::make_pair(matrices, vectors);
+    return res;
+}
+
+
+// does not include end time
+LinearBezierVector* split(const bezier_linear_variable_t& self,  const vectorX_t& times)
+{
+    LinearBezierVector* res (new LinearBezierVector);
+    bezier_linear_variable_t current = self;
+    real current_time = 0.;
+    real tmp;
+    for(int i = 0; i < times.rows(); ++i)
+    {
+        tmp =times[i];
+        std::pair<bezier_linear_variable_t, bezier_linear_variable_t> pairsplit = current.split(tmp-current_time);
+        res->beziers_.push_back(pairsplit.first);
+        current = pairsplit.second;
+        current_time += tmp-current_time;
+    }
+    res->beziers_.push_back(current);
+    return res;
+}
+
+}
+ // namespace spline

python/python_variables.h

+#include "hpp/spline/bezier_curve.h"
+#include "hpp/spline/linear_variable.h"
+
+#include "python_definitions.h"
+
+#include <vector>
+
+#ifndef _VARIABLES_PYTHON_BINDINGS
+#define _VARIABLES_PYTHON_BINDINGS
+
+
+namespace spline
+{
+static const int dim = 3;
+typedef spline::linear_variable<dim, real> linear_variable_3_t;
+typedef spline::variables<linear_variable_3_t> variables_3_t;
+typedef spline::bezier_curve  <real, real, dim, true, variables_3_t> bezier_linear_variable_t;
+
+
+/*linear variable control points*/
+bezier_linear_variable_t* wrapBezierLinearConstructor(const point_list_t& matrices, const point_list_t& vectors);
+
+bezier_linear_variable_t* wrapBezierLinearConstructorBounds
+    (const point_list_t& matrices, const point_list_t& vectors, const real ub);
+
+typedef std::pair<Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic>,
+                  Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> > linear_points_t;
+
+struct LinearControlPointsHolder
+{
+    linear_points_t res;
+    Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> A() {return res.first;}
+    Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> b() {return res.second;}
+};
+
+LinearControlPointsHolder* wayPointsToLists(const bezier_linear_variable_t& self);
+
+struct LinearBezierVector
+{
+    std::vector<bezier_linear_variable_t> beziers_;
+    std::size_t size() {return beziers_.size();}
+    bezier_linear_variable_t* at(std::size_t i)
+    {
+        assert (i<size());
+        return new bezier_linear_variable_t(beziers_[i]);
+    }
+};
+
+// does not include end time
+LinearBezierVector* split(const bezier_linear_variable_t& self,  const vectorX_t& times);
+} //namespace spline.
+
+
+#endif //_VARIABLES_PYTHON_BINDINGS

python/spline_python.cpp

@@ -6,6 +6,8 @@
 #include "hpp/spline/bezier_polynom_conversion.h"
 #include "hpp/spline/bernstein.h"
 
+#include "python_definitions.h"
+#include "python_variables.h"
 
 #include <vector>
 
@@ -15,20 +17,7 @@
 #include <boost/python.hpp>
 
 /*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
-typedef double real;
-typedef Eigen::Vector3d point_t;
-typedef Eigen::Matrix<double, 6, 1, 0, 6, 1> point6_t;
-typedef Eigen::Matrix<double, 3, 1, 0, 3, 1> ret_point_t;
-typedef Eigen::Matrix<double, 6, 1, 0, 6, 1> ret_point6_t;
-typedef Eigen::VectorXd time_waypoints_t;
-typedef Eigen::Matrix<real, 3, Eigen::Dynamic> point_list_t;
-typedef Eigen::Matrix<real, 6, Eigen::Dynamic> point_list6_t;
-typedef std::vector<point_t,Eigen::aligned_allocator<point_t> >  t_point_t;
-typedef std::vector<point6_t,Eigen::aligned_allocator<point6_t> >  t_point6_t;
-typedef std::pair<real, point_t> Waypoint;
-typedef std::vector<Waypoint> T_Waypoint;
-typedef std::pair<real, point6_t> Waypoint6;
-typedef std::vector<Waypoint6> T_Waypoint6;
+using namespace  spline;
 
 typedef spline::bezier_curve  <real, real, 3, true, point_t> bezier_t;
 typedef spline::bezier_curve  <real, real, 6, true, point6_t> bezier6_t;
@@ -57,14 +46,6 @@ EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_deriv_constraint_t)
 namespace spline
 {
 using namespace boost::python;
-template <typename PointList, typename T_Point>
-T_Point vectorFromEigenArray(const PointList& array)
-{
-    T_Point res;
-    for(int i =0;i<array.cols();++i)
-        res.push_back(array.col(i));
-    return res;
-}
 
 template <typename Bezier, typename PointList, typename T_Point>
 Bezier* wrapBezierConstructorTemplate(const PointList& array, const real ub =1.)
@@ -260,6 +241,37 @@ BOOST_PYTHON_MODULE(hpp_spline)
     /** END bezier curve**/
 
 
+    /** BEGIN variable points bezier curve**/
+    class_<LinearControlPointsHolder>
+        ("LinearWaypoint", no_init)
+        .def_readonly("A", &LinearControlPointsHolder::A)
+        .def_readonly("b", &LinearControlPointsHolder::b)
+        ;
+
+    class_<LinearBezierVector>
+        ("bezierVarVector", no_init)
+        .def_readonly("size", &LinearBezierVector::size)
+        .def("at", &LinearBezierVector::at, return_value_policy<manage_new_object>())
+        ;
+
+    class_<bezier_linear_variable_t>
+        ("bezierVar", no_init)
+            .def("__init__", make_constructor(&wrapBezierLinearConstructor))
+            .def("__init__", make_constructor(&wrapBezierLinearConstructorBounds))
+            .def("min", &bezier_linear_variable_t::min)
+            .def("max", &bezier_linear_variable_t::max)
+            //.def("__call__", &bezier_linear_control_t::operator())
+            .def("derivate", &bezier_linear_variable_t::derivate)
+            .def("compute_derivate", &bezier_linear_variable_t::compute_derivate)
+            .def("compute_primitive", &bezier_linear_variable_t::compute_primitive)
+            .def("split", &split, return_value_policy<manage_new_object>())
+            .def("waypoints", &wayPointsToLists, return_value_policy<manage_new_object>())
+            .def_readonly("degree", &bezier_linear_variable_t::degree_)
+            .def_readonly("nbWaypoints", &bezier_linear_variable_t::size_)
+        ;
+    /** END variable points bezier curve**/
+
+
     /** BEGIN spline curve function**/
     class_<polynom_t>("polynom",  init<const polynom_t::coeff_t, const real, const real >())
             .def("__init__", make_constructor(&wrapSplineConstructor))

python/test/variable/qp_traj/convex_hull.py

+import quadprog
+from numpy import array, dot, vstack, hstack, asmatrix, identity, cross
+from numpy.linalg import norm
+
+from scipy.spatial import ConvexHull
+import numpy as np
+
+def genConvexHullLines(points):
+         hull = ConvexHull(points)
+         lineList = [points[el] for el in hull.vertices] + [points[hull.vertices[0]]]
+         lineList = [array(el[:2].tolist() + [0.]) for el in lineList]
+         return [[lineList[i],lineList[i+1]] for i in range(len(hull.vertices))], lineList[:-1]
+         #now compute lines
+
+
+def getLineFromSegment(line):
+        a = line[0]; b = line[1]; c = a.copy() ; c[2] = 1.
+        normal = cross((b-a),(c-a))
+        normal /= norm(normal)
+        # get inequality
+        dire = b - a
+        coeff = normal
+        rhs = a.dot(normal)
+        return (coeff, array([rhs]))
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+
+#generate right of the line
+def genFromLine(line, num_points, ranges, existing_points = []):
+        coeff, rhs = getLineFromSegment(line)
+        num_gen = 0
+        gen = existing_points + [line[0][:2], line[1][:2]]
+        while(len(gen) < num_points):
+                pt = array([np.random.uniform(ranges[0][0], ranges[0][1]), np.random.uniform(ranges[1][0], ranges[1][1])])
+                if coeff[:2].dot(pt) <= rhs :
+                        gen += [pt]
+        return genConvexHullLines(gen)
+        
+if __name__ == '__main__':
+        genFromLine([array([0.5,0.,0.]),array([0.5,0.5,0.])],5)
+        genFromLine([array([0.5,0.,0.]),array([0.5,-0.5,0.])],5)
+        

python/test/variable/qp_traj/plot_cord.py

+import matplotlib.pyplot as plt
+import numpy as np
+
+
+def plotBezier(bez, color):
+        step = 100.
+        points1 =  np.array([(bez(i/step*bez.max())[0][0],bez(i/step*bez.max())[1][0]) for i in range(int(step))])
+        x = points1[:,0]
+        y = points1[:,1]
+        plt.plot(x,y,color,linewidth=2.0)
+        
+def plotControlPoints(bez, color):
+        wps = bez.waypoints()
+        wps = np.array([wps[:2,i] for i in range(wps.shape[1]) ])
+        x = wps[:,0]
+        y = wps[:,1]
+        plt.scatter(x,y,color=color)        
+        
+def plotPoly(lines, color):
+        step = 1000.
+        for line in lines:
+                a_0 = line[0]
+                b_0 = line[1]
+                pointsline =  np.array([ a_0 * i / step + b_0 * (1. - i / step) for i in range(int(step))])
+                xl = pointsline[:,0]
+                yl = pointsline[:,1]
+                plt.plot(xl,yl,color,linewidth=0.5)

python/test/variable/qp_traj/qp.py

+import quadprog
+from numpy import array, dot, vstack, hstack, asmatrix, identity
+
+#min (1/2)x' P x + q' x  
+#subject to  G x <= h
+#subject to  C x  = d
+def quadprog_solve_qp(P, q, G=None, h=None, C=None, d=None):
+    qp_G = .5 * (P + P.T)   # make sure P is symmetric
+    qp_a = -q
+    if C is not None:
+        if G is not None:
+                qp_C = -vstack([C, G]).T
+                qp_b = -hstack([d, h])   
+        else:
+                qp_C = -C.transpose()
+                qp_b = -d 
+        meq = C.shape[0]
+    else:  # no equality constraint 
+        qp_C = -G.T
+        qp_b = -h
+        meq = 0
+    return quadprog.solve_qp(qp_G, qp_a, qp_C, qp_b, meq)[0]
+
+
+#min ||Ax-b||**2 
+#subject to  G x <= h
+#subject to  C x  = d
+def solve_least_square(A,b,G=None, h=None, C=None, d=None):
+        P = dot(A.T, A)
+        #~ q = 2*dot(b, A).reshape(b.shape[0])
+        q = 2*dot(b, A)
+        return quadprog_solve_qp(P, q, G, h)
+
+
+#min q' x  
+#subject to  G x <= h
+#subject to  C x  = d
+def solve_lp(q, G=None, h=None, C=None, d=None):
+    qp_G = identity(q.shape[0]) * 0.000001
+    qp_a = -q
+    if C is not None:
+        if G is not None:
+                qp_C = -vstack([C, G]).T
+                qp_b = -hstack([d, h])  
+        else:
+                qp_C = -C.transpose()
+                qp_b = -d 
+        meq = C.shape[0]
+    else:  # no equality constraint 
+        qp_C = -G.T
+        qp_b = -h
+        meq = 0
+    return quadprog.solve_qp(qp_G, qp_a, qp_C, qp_b, meq)[0]
+        
+
+if __name__ == '__main__':
+        
+        from numpy.linalg import norm
+        
+        A = array([[1., 2., 0.], [-8., 3., 2.], [0., 1., 1.]])
+        b = array([3., 2., 3.])
+        P = dot(A.T, A)
+        q = 2*dot(b, A).reshape((3,))
+        G = array([[1., 2., 1.], [2., 0., 1.], [-1., 2., -1.]])
+        h = array([3., 2., -2.]).reshape((3,))
+
+        res2 = solve_least_square(A, b, G, h)
+        res1 =  quadprog_solve_qp(P, q, G, h)
+        print res1
+        print res2

python/test/variable/qp_traj/qp_cord.py

+from spline import *
+from varBezier import varBezier
+from convex_hull import *
+
+from qp import solve_lp, quadprog_solve_qp
+
+
+from numpy import matrix, array, zeros, ones, diag, cross, vstack, identity
+from numpy.linalg import norm
+
+__EPS = 1e-6
+
+#### helpers for stacking matrices ####
+def concat(m1,m2):
+        if (type(m1) == type(None)):
+                return m2
+                
+        return vstack([m1,m2]).reshape([-1,m2.shape[-1]])
+        
+def concatvec(m1,m2):
+        if (type(m1) == type(None)):
+                return m2
+        return array(m1.tolist()+m2.tolist())
+
+#### helpers for stacking matrices ####
+
+#constraint to lie between 2 extremities of a segment
+def segmentConstraint(varBez, a, b, wpIndex, constraintVarIndex, totalAddVarConstraints):
+        mat, vec = varBez.matrixFromWaypoints(wpIndex)
+        vec = b - vec   
+        resmat = zeros([mat.shape[0],mat.shape[1]+totalAddVarConstraints])
+        resmat[:,:mat.shape[1]]=mat
+        resmat[:, mat.shape[1]+constraintVarIndex-1]= b-a
+        return (resmat, vec)
+
+#constraint to lie one side of a line
+def lineConstraint(varBez, C, d, totalAddVarConstraints):
+        resmat = None
+        resvec = None
+        for wpIndex in range(varBez.bezier.nbWaypoints):
+                mat, vec = varBez.matrixFromWaypoints(wpIndex)
+                mat = C.dot(mat)
+                vec = d - C.dot(vec)
+                resmat = concat(resmat, mat)
+                resvec = concatvec(resvec, vec)
+        augmented = zeros([resmat.shape[0],resmat.shape[1]+totalAddVarConstraints])
+        augmented[:,:resmat.shape[1]]=resmat
+        return (augmented, resvec)
+
+
+##### cost function integrals #####
+#compute bezier primitive of variable waypoints given these waypoints
+def compute_primitive(wps):
+        new_degree = (len(wps)-1+1)
+        current_sum = [zeros(wps[0][0].shape),0]
+        n_wp = [(current_sum[0],0.)]
+        for wp in wps:
+                current_sum[0] = current_sum[0] + wp[0]
+                current_sum[1] = current_sum[1] + wp[1]
+                n_wp +=[(current_sum[0]/new_degree, current_sum[1]/new_degree)]
+        return n_wp
+        
+#cost function for analytical integral cost of squared acceleration
+def accelerationcost(bezVar):
+        acc = bezVar.bezier.compute_derivate(2)        
+        
+        hacc = varBezier(); hacc.bezier = acc
+        wps_i=[[],[],[]]
+        for i in range(3): #integrate for each axis
+                for j in range(acc.nbWaypoints):
+                        A_i = hacc.matrixFromWaypoints(j)[0][i,:].reshape([1,-1])
+                        b_i = hacc.matrixFromWaypoints(j)[1][i]
+                        wps_i[i] += [(A_i.transpose().dot(A_i), b_i*b_i) ]
+        # now integrate each bezier curve
+        for i, wps in enumerate(wps_i):
+              wps_i[i] = compute_primitive(wps)
+        
+        resmat = wps_i[0][-1][0]; resvec = wps_i[0][-1][1]
+        for i in range(1,3):
+                resmat = resmat + wps_i[i][-1][0]
+                resvec = resvec + wps_i[i][-1][1]
+        return (resmat, resvec)
+##### cost function integrals #####

python/test/variable/qp_traj/test_cord.py

+from spline import *
+from numpy import matrix, array, zeros, ones, diag, cross
+from numpy.linalg import norm
+
+__EPS = 1e-6
+
+
+from varBezier import varBezier
+from qp_cord import *
+from plot_cord import *
+
+
+idxFile = 0
+import string
+import uuid; uuid.uuid4().hex.upper()[0:6]
+
+
+######################## generate problems ########################
+def genBezierInput(numvars = 3):
+        valDep = array([np.random.uniform(0., 1.), np.random.uniform(0.,5.), 0.])
+        valEnd = array([np.random.uniform(5., 10.), np.random.uniform(0.,5.), 0.])
+        return varBezier([valDep]+["" for _ in range(numvars)]+[valEnd], 1.)
+        
+def genSplit(numCurves):
+        splits = []
+        lastval = np.random.uniform(0., 1.)
+        for i in range(numCurves):
+                splits += [lastval]
+                lastval += np.random.uniform(0., 1.)
+        return [el / lastval for el in splits[:-1]]
+                
+
+def getRightMostLine(ptList):
+        pt1 = array([0.,0.,0.])
+        pt2 = array([0.,0.,0.])
+        for pt in ptList:
+                if pt[0] > pt1[0]:
+                       pt1 =  pt
+                elif pt[0] > pt2[0]:
+                       pt2 =  pt
+        if pt1[1] < pt2[1]:
+                return [pt2,pt1]
+        else:
+                return [pt1,pt2]
+######################## generate problems ########################
+
+######################## solve a given problem ########################
+
+#inequality constraint from line
+def getLineFromSegment(line):
+        a = line[0]; b = line[1]; c = a.copy() ; c[2] = 1.