Commit c2ed8387 authored by stevet's avatar stevet
Browse files

python bindings and tests

parent 3dec2d43
......@@ -5,3 +5,4 @@ build-rel/
# temp files
.*~
*.user
*.pyc
......@@ -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
......
#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
#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
#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
......@@ -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))
......
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)
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)
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
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 #####
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.