#include "curves/bezier_curve.h"
#include "curves/polynomial.h"
#include "curves/exact_cubic.h"
#include "curves/curve_constraint.h"
#include "curves/curve_conversion.h"
#include "curves/bernstein.h"
#include "curves/cubic_hermite_spline.h"
#include "curves/piecewise_curve.h"
#include "python_definitions.h"
#include "python_variables.h"
#include <vector>
#include <eigenpy/memory.hpp>
#include <eigenpy/eigenpy.hpp>
#include <boost/python.hpp>
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
using namespace curves;
typedef double real;
typedef Eigen::Vector3d point_t;
typedef Eigen::Vector3d tangent_t;
typedef std::pair<point_t, tangent_t> pair_point_tangent_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;
typedef std::vector<pair_point_tangent_t,Eigen::aligned_allocator<pair_point_tangent_t> > t_pair_point_tangent_t;
typedef curves::bezier_curve <real, real, 3, true, point_t> bezier3_t;
typedef curves::bezier_curve <real, real, 6, true, point6_t> bezier6_t;
typedef curves::polynomial <real, real, 3, true, point_t, t_point_t> polynomial_t;
typedef curves::exact_cubic <real, real, 3, true, point_t, t_point_t> exact_cubic_t;
typedef curves::cubic_hermite_spline <real, real, 3, true, point_t> cubic_hermite_spline_t;
typedef polynomial_t::coeff_t coeff_t;
typedef std::pair<real, point_t> waypoint_t;
typedef std::vector<waypoint_t, Eigen::aligned_allocator<point_t> > t_waypoint_t;
typedef curves::piecewise_curve <real, real, 3, true, point_t, t_point_t, polynomial_t> piecewise_polynomial_curve_t;
typedef curves::piecewise_curve <real, real, 3, true, point_t, t_point_t, bezier3_t> piecewise_bezier3_curve_t;
typedef curves::piecewise_curve <real, real, 6, true, point6_t, t_point6_t, bezier6_t> piecewise_bezier6_curve_t;
typedef curves::piecewise_curve <real, real, 3, true, point_t, t_point_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
typedef curves::Bern<double> bernstein_t;
typedef curves::curve_constraints<point_t> curve_constraints_t;
typedef curves::curve_constraints<point6_t> curve_constraints6_t;
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bernstein_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier3_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier6_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(polynomial_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(exact_cubic_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(cubic_hermite_spline_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints_t)
namespace curves
{
using namespace boost::python;
/* Template constructor bezier */
template <typename Bezier, typename PointList, typename T_Point>
Bezier* wrapBezierConstructorTemplate(const PointList& array, const real T_min =0., const real T_max =1.)
{
T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
return new Bezier(asVector.begin(), asVector.end(), T_min, T_max);
}
template <typename Bezier, typename PointList, typename T_Point, typename CurveConstraints>
Bezier* wrapBezierConstructorConstraintsTemplate(const PointList& array, const CurveConstraints& constraints,
const real T_min =0., const real T_max =1.)
{
T_Point asVector = vectorFromEigenArray<PointList, T_Point>(array);
return new Bezier(asVector.begin(), asVector.end(), constraints, T_min, T_max);
}
/* End Template constructor bezier */
/*3D constructors bezier */
bezier3_t* wrapBezierConstructor3(const point_list_t& array)
{
return wrapBezierConstructorTemplate<bezier3_t, point_list_t, t_point_t>(array) ;
}
bezier3_t* wrapBezierConstructorBounds3(const point_list_t& array, const real T_min, const real T_max)
{
return wrapBezierConstructorTemplate<bezier3_t, point_list_t, t_point_t>(array, T_min, T_max) ;
}
bezier3_t* wrapBezierConstructor3Constraints(const point_list_t& array, const curve_constraints_t& constraints)
{
return wrapBezierConstructorConstraintsTemplate<bezier3_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints) ;
}
bezier3_t* wrapBezierConstructorBounds3Constraints(const point_list_t& array, const curve_constraints_t& constraints,
const real T_min, const real T_max)
{
return wrapBezierConstructorConstraintsTemplate<bezier3_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints, T_min, T_max) ;
}
/*END 3D constructors bezier */
/*6D constructors bezier */
bezier6_t* wrapBezierConstructor6(const point_list6_t& array)
{
return wrapBezierConstructorTemplate<bezier6_t, point_list6_t, t_point6_t>(array) ;
}
bezier6_t* wrapBezierConstructorBounds6(const point_list6_t& array, const real T_min, const real T_max)
{
return wrapBezierConstructorTemplate<bezier6_t, point_list6_t, t_point6_t>(array, T_min, T_max) ;
}
bezier6_t* wrapBezierConstructor6Constraints(const point_list6_t& array, const curve_constraints6_t& 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 T_min, const real T_max)
{
return wrapBezierConstructorConstraintsTemplate<bezier6_t, point_list6_t, t_point6_t, curve_constraints6_t>(array, constraints, T_min, T_max) ;
}
/*END 6D constructors bezier */
/* Wrap Cubic hermite spline */
t_pair_point_tangent_t getPairsPointTangent(const point_list_t& points, const point_list_t& tangents)
{
t_pair_point_tangent_t res;
if (points.size() != tangents.size())
{
throw std::length_error("size of points and tangents must be the same");
}
for(int i =0;i<points.cols();++i)
{
res.push_back(pair_point_tangent_t(tangents.col(i), tangents.col(i)));
}
return res;
}
cubic_hermite_spline_t* wrapCubicHermiteSplineConstructor(const point_list_t& points, const point_list_t& tangents, const time_waypoints_t& time_pts)
{
t_pair_point_tangent_t ppt = getPairsPointTangent(points, tangents);
std::vector<real> time_control_pts;
for( int i =0; i<time_pts.size(); ++i)
{
time_control_pts.push_back(time_pts[i]);
}
return new cubic_hermite_spline_t(ppt.begin(), ppt.end(), time_control_pts);
}
/* End wrap Cubic hermite spline */
/* Wrap polynomial */
polynomial_t* wrapSplineConstructor(const coeff_t& array)
{
return new polynomial_t(array, 0., 1.);
}
/* End wrap polynomial */
/* Wrap piecewise curve */
piecewise_polynomial_curve_t* wrapPiecewisePolynomialCurveConstructor(const polynomial_t& pol)
{
return new piecewise_polynomial_curve_t(pol);
}
piecewise_bezier3_curve_t* wrapPiecewiseBezier3CurveConstructor(const bezier3_t& bc)
{
return new piecewise_bezier3_curve_t(bc);
}
piecewise_bezier6_curve_t* wrapPiecewiseBezier6CurveConstructor(const bezier6_t& bc)
{
return new piecewise_bezier6_curve_t(bc);
}
piecewise_cubic_hermite_curve_t* wrapPiecewiseCubicHermiteCurveConstructor(const cubic_hermite_spline_t& ch)
{
return new piecewise_cubic_hermite_curve_t(ch);
}
/* end wrap piecewise polynomial curve */
/* Wrap exact cubic spline */
t_waypoint_t getWayPoints(const coeff_t& array, const time_waypoints_t& time_wp)
{
t_waypoint_t res;
for(int i =0;i<array.cols();++i)
{
res.push_back(std::make_pair(time_wp(i), array.col(i)));
}
return res;
}
template <typename BezierType, int dim>
Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> wayPointsToList(const BezierType& self)
{
typedef typename BezierType::t_point_t t_point;
typedef typename BezierType::t_point_t::const_iterator cit_point;
const t_point& wps = self.waypoints();
Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> res (dim, wps.size());
int col = 0;
for(cit_point cit = wps.begin(); cit != wps.end(); ++cit, ++col)
{
res.block<dim,1>(0,col) = *cit;
}
return res;
}
exact_cubic_t* wrapExactCubicConstructor(const coeff_t& array, const time_waypoints_t& time_wp)
{
t_waypoint_t wps = getWayPoints(array, time_wp);
return new exact_cubic_t(wps.begin(), wps.end());
}
exact_cubic_t* wrapExactCubicConstructorConstraint(const coeff_t& array, const time_waypoints_t& time_wp, const curve_constraints_t& constraints)
{
t_waypoint_t wps = getWayPoints(array, time_wp);
return new exact_cubic_t(wps.begin(), wps.end(), constraints);
}
point_t get_init_vel(const curve_constraints_t& c)
{
return c.init_vel;
}
point_t get_init_acc(const curve_constraints_t& c)
{
return c.init_acc;
}
point_t get_end_vel(const curve_constraints_t& c)
{
return c.end_vel;
}
point_t get_end_acc(const curve_constraints_t& c)
{
return c.end_acc;
}
void set_init_vel(curve_constraints_t& c, const point_t& val)
{
c.init_vel = val;
}
void set_init_acc(curve_constraints_t& c, const point_t& val)
{
c.init_acc = val;
}
void set_end_vel(curve_constraints_t& c, const point_t& val)
{
c.end_vel = val;
}
void set_end_acc(curve_constraints_t& c, const point_t& val)
{
c.end_acc = val;
}
/* End wrap exact cubic spline */
BOOST_PYTHON_MODULE(curves)
{
/** BEGIN eigenpy init**/
eigenpy::enableEigenPy();
eigenpy::enableEigenPySpecific<point_t,point_t>();
eigenpy::enableEigenPySpecific<ret_point_t,ret_point_t>();
eigenpy::enableEigenPySpecific<point_list_t,point_list_t>();
eigenpy::enableEigenPySpecific<point6_t,point6_t>();
eigenpy::enableEigenPySpecific<ret_point6_t,ret_point6_t>();
eigenpy::enableEigenPySpecific<point_list6_t,point_list6_t>();
eigenpy::enableEigenPySpecific<coeff_t,coeff_t>();
/*eigenpy::exposeAngleAxis();
eigenpy::exposeQuaternion();*/
/** END eigenpy init**/
/** BEGIN bezier curve 6**/
class_<bezier6_t>
("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())
.def("derivate", &bezier6_t::derivate)
.def("compute_derivate", &bezier6_t::compute_derivate)
.def("compute_primitive", &bezier6_t::compute_primitive)
.def("waypoints", &wayPointsToList<bezier6_t,6>)
.def_readonly("degree", &bezier6_t::degree_)
.def_readonly("nbWaypoints", &bezier6_t::size_)
;
/** END bezier curve**/
/** BEGIN bezier curve**/
class_<bezier3_t>
("bezier3", no_init)
.def("__init__", make_constructor(&wrapBezierConstructor3))
.def("__init__", make_constructor(&wrapBezierConstructorBounds3))
.def("__init__", make_constructor(&wrapBezierConstructor3Constraints))
.def("__init__", make_constructor(&wrapBezierConstructorBounds3Constraints))
.def("min", &bezier3_t::min)
.def("max", &bezier3_t::max)
.def("__call__", &bezier3_t::operator())
.def("derivate", &bezier3_t::derivate)
.def("compute_derivate", &bezier3_t::compute_derivate)
.def("compute_primitive", &bezier3_t::compute_primitive)
.def("waypoints", &wayPointsToList<bezier3_t,3>)
.def_readonly("degree", &bezier3_t::degree_)
.def_readonly("nbWaypoints", &bezier3_t::size_)
;
/** 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 polynomial curve function**/
class_<polynomial_t>("polynomial", init<const polynomial_t::coeff_t, const real, const real >())
.def("__init__", make_constructor(&wrapSplineConstructor))
.def("min", &polynomial_t::min)
.def("max", &polynomial_t::max)
.def("__call__", &polynomial_t::operator())
.def("derivate", &polynomial_t::derivate)
.def("serialize", &polynomial_t::saveAsText)
;
/** END polynomial function**/
/** BEGIN piecewise curve function **/
class_<piecewise_polynomial_curve_t>
("piecewise_polynomial_curve", no_init)
.def("__init__", make_constructor(&wrapPiecewisePolynomialCurveConstructor))
.def("min", &piecewise_polynomial_curve_t::min)
.def("max", &piecewise_polynomial_curve_t::max)
.def("__call__", &piecewise_polynomial_curve_t::operator())
.def("derivate", &piecewise_polynomial_curve_t::derivate)
.def("add_curve", &piecewise_polynomial_curve_t::add_curve)
.def("is_continuous", &piecewise_polynomial_curve_t::is_continuous)
;
class_<piecewise_bezier3_curve_t>
("piecewise_bezier3_curve", no_init)
.def("__init__", make_constructor(&wrapPiecewiseBezier3CurveConstructor))
.def("min", &piecewise_bezier3_curve_t::min)
.def("max", &piecewise_bezier3_curve_t::max)
.def("__call__", &piecewise_bezier3_curve_t::operator())
.def("derivate", &piecewise_bezier3_curve_t::derivate)
.def("add_curve", &piecewise_bezier3_curve_t::add_curve)
.def("is_continuous", &piecewise_bezier3_curve_t::is_continuous)
;
class_<piecewise_bezier6_curve_t>
("piecewise_bezier6_curve", no_init)
.def("__init__", make_constructor(&wrapPiecewiseBezier6CurveConstructor))
.def("min", &piecewise_bezier6_curve_t::min)
.def("max", &piecewise_bezier6_curve_t::max)
.def("__call__", &piecewise_bezier6_curve_t::operator())
.def("derivate", &piecewise_bezier6_curve_t::derivate)
.def("add_curve", &piecewise_bezier6_curve_t::add_curve)
.def("is_continuous", &piecewise_bezier6_curve_t::is_continuous)
;
class_<piecewise_cubic_hermite_curve_t>
("piecewise_cubic_hermite_curve", no_init)
.def("__init__", make_constructor(&wrapPiecewiseCubicHermiteCurveConstructor))
.def("min", &piecewise_cubic_hermite_curve_t::min)
.def("max", &piecewise_cubic_hermite_curve_t::max)
.def("__call__", &piecewise_cubic_hermite_curve_t::operator())
.def("derivate", &piecewise_cubic_hermite_curve_t::derivate)
.def("add_curve", &piecewise_cubic_hermite_curve_t::add_curve)
.def("is_continuous", &piecewise_cubic_hermite_curve_t::is_continuous)
;
/** END piecewise curve function **/
/** BEGIN exact_cubic curve**/
class_<exact_cubic_t>
("exact_cubic", no_init)
.def("__init__", make_constructor(&wrapExactCubicConstructor))
.def("__init__", make_constructor(&wrapExactCubicConstructorConstraint))
.def("min", &exact_cubic_t::min)
.def("max", &exact_cubic_t::max)
.def("__call__", &exact_cubic_t::operator())
.def("derivate", &exact_cubic_t::derivate)
.def("getNumberSplines", &exact_cubic_t::getNumberSplines)
.def("getSplineAt", &exact_cubic_t::getSplineAt)
;
/** END exact_cubic curve**/
/** BEGIN cubic_hermite_spline **/
class_<cubic_hermite_spline_t>
("cubic_hermite_spline", no_init)
.def("__init__", make_constructor(&wrapCubicHermiteSplineConstructor))
.def("min", &cubic_hermite_spline_t::min)
.def("max", &cubic_hermite_spline_t::max)
.def("__call__", &cubic_hermite_spline_t::operator())
.def("derivate", &cubic_hermite_spline_t::derivate)
;
/** END cubic_hermite_spline **/
/** BEGIN curve constraints**/
class_<curve_constraints_t>
("curve_constraints", init<>())
.add_property("init_vel", &get_init_vel, &set_init_vel)
.add_property("init_acc", &get_init_acc, &set_init_acc)
.add_property("end_vel", &get_end_vel, &set_end_vel)
.add_property("end_acc", &get_end_acc, &set_end_acc)
;
/** END curve constraints**/
/** BEGIN bernstein polynomial**/
class_<bernstein_t>
("bernstein", init<const unsigned int, const unsigned int>())
.def("__call__", &bernstein_t::operator())
;
/** END bernstein polynomial**/
/** BEGIN curves conversion**/
def("polynomial_from_bezier", polynomial_from_curve<polynomial_t,bezier3_t>);
def("polynomial_from_hermite", polynomial_from_curve<polynomial_t,cubic_hermite_spline_t>);
def("bezier_from_hermite", bezier_from_curve<bezier3_t,cubic_hermite_spline_t>);
def("bezier_from_polynomial", bezier_from_curve<bezier3_t,polynomial_t>);
def("hermite_from_bezier", hermite_from_curve<cubic_hermite_spline_t, bezier3_t>);
def("hermite_from_polynomial", hermite_from_curve<cubic_hermite_spline_t, polynomial_t>);
/** END curves conversion**/
}
} // namespace curves