Skip to content

GitLab

Commit ab3e8f97 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

binding splines in python

parent f93635f9
Hide whitespace changes
Inline Side-by-side
include/spline/spline_curve.h
View file @ ab3e8f97
......@@ -49,7 +49,7 @@ struct spline_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
/// by the number of the columns -1.
///\param min: LOWER bound on interval definition of the spline
///\param max: UPPER bound on interval definition of the spline
spline_curve(const coeff_t_ref coefficients, const time_t min, const time_t max)
spline_curve(const coeff_t& coefficients, const time_t min, const time_t max)
: curve_abc_t(),
coefficients_(coefficients), t_min_(min), t_max_(max), dim_(Dim), order_(coefficients_.cols()-1)
{
......
python/spline_python.cpp
View file @ ab3e8f97
#include "spline/bezier_curve.h"
#include "spline/spline_curve.h"
#include <vector>
......@@ -10,12 +11,14 @@
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
typedef double real;
typedef Eigen::Vector3d point_t;
typedef Eigen::Matrix<real, Eigen::Dynamic,3> point_list_t;
typedef Eigen::Matrix<real, 3, Eigen::Dynamic> point_list_t;
typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
typedef std::pair<real, point_t> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
typedef spline::bezier_curve <real, real, 3, true, point_t> bezier_t;
typedef spline::spline_curve <real, real, 3, true, point_t, t_point_t> spline_curve_t;
typedef spline_curve_t::coeff_t coeff_t;
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_t)
......@@ -27,8 +30,8 @@ using namespace boost::python;
t_point_t vectorFromEigenArray(const point_list_t& array)
{
t_point_t res;
for(int i =0;i<array.rows();++i)
res.push_back(array.row(i));
for(int i =0;i<array.cols();++i)
res.push_back(array.col(i));
return res;
}
......@@ -45,6 +48,13 @@ bezier_t* wrapBezierConstructorBounds(const point_list_t& array, const real lb,
return new bezier_t(asVector.begin(), asVector.end(), lb, ub);
}
spline_curve_t* wrapSplineConstructor(const coeff_t& array)
{
return new spline_curve_t(array, 0., 1.);
}
BOOST_PYTHON_MODULE(spline)
{
/** BEGIN eigenpy init**/
......@@ -52,10 +62,12 @@ BOOST_PYTHON_MODULE(spline)
eigenpy::enableEigenPySpecific<point_t,point_t>();
eigenpy::enableEigenPySpecific<point_list_t,point_list_t>();
eigenpy::enableEigenPySpecific<coeff_t,coeff_t>();
/*eigenpy::exposeAngleAxis();
eigenpy::exposeQuaternion();*/
/** END eigenpy init**/
/** BEGIN bezier curve**/
class_<bezier_t>
("bezier", no_init)
.def("__init__", make_constructor(&wrapBezierConstructor))
......@@ -64,56 +76,21 @@ BOOST_PYTHON_MODULE(spline)
.def("max", &bezier_t::max)
.def("__call__", &bezier_t::operator())
;
/** END bezier curve**/
/** BEGIN spline curve function**/
class_<spline_curve_t>("spline", init<const spline_curve_t::coeff_t, const real, const real >())
.def("__init__", make_constructor(&wrapSplineConstructor))
.def("min", &spline_curve_t::min)
.def("max", &spline_curve_t::max)
.def("__call__", &spline_curve_t::operator())
.def("derivate", &spline_curve_t::derivate)
;
/** END cubic function**/
/** END eigenpy init**/
/** BEGIN enum types **/
/*#ifdef CLP_FOUND
enum_<SolverLP>("SolverLP")
.value("SOLVER_LP_QPOASES", SOLVER_LP_QPOASES)
.value("SOLVER_LP_CLP", SOLVER_LP_CLP)
.export_values();
#else
enum_<SolverLP>("SolverLP")
.value("SOLVER_LP_QPOASES", SOLVER_LP_QPOASES)
.export_values();
#endif
enum_<EquilibriumAlgorithm>("EquilibriumAlgorithm")
.value("EQUILIBRIUM_ALGORITHM_LP", EQUILIBRIUM_ALGORITHM_LP)
.value("EQUILIBRIUM_ALGORITHM_LP2", EQUILIBRIUM_ALGORITHM_LP2)
.value("EQUILIBRIUM_ALGORITHM_DLP", EQUILIBRIUM_ALGORITHM_DLP)
.value("EQUILIBRIUM_ALGORITHM_PP", EQUILIBRIUM_ALGORITHM_PP)
.value("EQUILIBRIUM_ALGORITHM_IP", EQUILIBRIUM_ALGORITHM_IP)
.value("EQUILIBRIUM_ALGORITHM_DIP", EQUILIBRIUM_ALGORITHM_DIP)
.export_values();
enum_<LP_status>("LP_status")
.value("LP_STATUS_UNKNOWN", LP_STATUS_UNKNOWN)
.value("LP_STATUS_OPTIMAL", LP_STATUS_OPTIMAL)
.value("LP_STATUS_INFEASIBLE", LP_STATUS_INFEASIBLE)
.value("LP_STATUS_UNBOUNDED", LP_STATUS_UNBOUNDED)
.value("LP_STATUS_MAX_ITER_REACHED", LP_STATUS_MAX_ITER_REACHED)
.value("LP_STATUS_ERROR", LP_STATUS_ERROR)
.export_values();*/
/** END enum types **/
/*bool (Equilibrium::*setNewContacts)
(const MatrixX3ColMajor&, const MatrixX3ColMajor&, const double, const EquilibriumAlgorithm) = &Equilibrium::setNewContacts;
class_<Equilibrium>("Equilibrium", init<std::string, double, unsigned int, optional <SolverLP, bool, const unsigned int, const bool> >())
.def("useWarmStart", &Equilibrium::useWarmStart)
.def("setUseWarmStart", &Equilibrium::setUseWarmStart)
.def("getName", &Equilibrium::getName)
.def("getAlgorithm", &Equilibrium::getAlgorithm)
.def("setNewContacts", setNewContacts)
.def("computeEquilibriumRobustness", wrapComputeQuasiEquilibriumRobustness)
.def("computeEquilibriumRobustness", wrapComputeEquilibriumRobustness)
.def("getPolytopeInequalities", wrapGetPolytopeInequalities)
;*/
}
} // namespace spline
python/test/test.py
View file @ ab3e8f97
from spline import bezier
from spline import bezier, spline
from numpy import matrix
waypoints = matrix([[1.,2.,3.],[4.,5.,6.]]).transpose()
#testing bezier curve
a = bezier(matrix([[1.,2.,3.],[4.,5.,6.]]))
a = bezier(matrix([[1.,2.,3.],[4.,5.,6.]]), -1., 3.)
a = bezier(waypoints)
a = bezier(waypoints, -1., 3.)
a.min()
a.max()
a(0.4)
#testing spline function
a = spline(waypoints)
a = spline(waypoints, -1., 3.)
a.min()
a.max()
a(0.4)
assert((a.derivate(0.4,0) == a(0.4)).all())
a.derivate(0.4,2)
src/tests/spline_test/Main.cpp
View file @ ab3e8f97
......@@ -16,7 +16,7 @@ namespace spline
{
typedef Eigen::Vector3d point_t;
typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
typedef spline_curve <double, double, 3, true, point_t, t_point_t> cubic_function_t;
typedef spline_curve <double, double, 3, true, point_t, t_point_t> spline_curve_t;
typedef exact_cubic <double, double, 3, true, point_t> exact_cubic_t;
typedef spline_deriv_constraint <double, double, 3, true, point_t> spline_deriv_constraint_t;
typedef bezier_curve <double, double, 3, true, point_t> bezier_curve_t;
......@@ -26,7 +26,7 @@ typedef std::vector<Waypoint> T_Waypoint;
typedef Eigen::Matrix<double,1,1> point_one;
typedef spline_curve<double, double, 1, true, point_one> cubic_function_one;
typedef spline_curve<double, double, 1, true, point_one> spline_curve_one;
typedef exact_cubic <double, double, 1, true, point_one> exact_cubic_one;
typedef std::pair<double, point_one> WaypointOne;
typedef std::vector<WaypointOne> T_WaypointOne;
......@@ -78,7 +78,7 @@ void CubicFunctionTest(bool& error)
vec.push_back(b);
vec.push_back(c);
vec.push_back(d);
cubic_function_t cf(vec.begin(), vec.end(), 0, 1);
spline_curve_t cf(vec.begin(), vec.end(), 0, 1);
point_t res1;
res1 =cf(0);
point_t x0(1,2,3);
......@@ -97,7 +97,7 @@ void CubicFunctionTest(bool& error)
vec.push_back(b);
vec.push_back(c);
vec.push_back(d);
cubic_function_t cf2(vec, 0.5, 1);
spline_curve_t cf2(vec, 0.5, 1);
res1 = cf2(0.5);
ComparePoints(x0, res1, errMsg + "x3 ", error);
error = true;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment