Commit 58dd2dc4 authored by Steve Tonneau's avatar Steve Tonneau
Browse files

added 6d bezier curves

parent 2b906c02
......@@ -124,6 +124,8 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
if(derived_wp.empty())
derived_wp.push_back(point_t::Zero());
bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),minBound_,maxBound_);
std::cout << "deriv size" << deriv.size_ << std::endl;
std::cout << "val size" << size_ << std::endl;
assert(deriv.size_ +1 == this->size_);
return deriv.compute_derivate(order-1);
}
......
......@@ -13,14 +13,21 @@
/*** 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;
typedef spline::bezier_curve <real, real, 3, true, point_t> bezier_t;
typedef spline::bezier_curve <real, real, 6, true, point6_t> bezier6_t;
typedef spline::spline_curve <real, real, 3, true, point_t, t_point_t> spline_curve_t;
typedef spline::exact_cubic <real, real, 3, true, point_t, t_point_t> exact_cubic_t;
typedef spline_curve_t::coeff_t coeff_t;
......@@ -33,6 +40,7 @@ typedef spline_deriv_constraint_t::spline_constraints spline_constraints_t;
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier6_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_curve_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(exact_cubic_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_constraints_t)
......@@ -41,10 +49,10 @@ EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_deriv_constraint_t)
namespace spline
{
using namespace boost::python;
t_point_t vectorFromEigenArray(const point_list_t& array)
template <typename PointList, typename T_Point>
T_Point vectorFromEigenArray(const PointList& array)
{
t_point_t res;
T_Point res;
for(int i =0;i<array.cols();++i)
res.push_back(array.col(i));
return res;
......@@ -52,17 +60,30 @@ t_point_t vectorFromEigenArray(const point_list_t& array)
bezier_t* wrapBezierConstructor(const point_list_t& array)
{
t_point_t asVector = vectorFromEigenArray(array);
t_point_t asVector = vectorFromEigenArray<point_list_t, t_point_t>(array);
return new bezier_t(asVector.begin(), asVector.end());
}
bezier_t* wrapBezierConstructorBounds(const point_list_t& array, const real lb, const real ub)
{
t_point_t asVector = vectorFromEigenArray(array);
t_point_t asVector = vectorFromEigenArray<point_list_t, t_point_t>(array);
return new bezier_t(asVector.begin(), asVector.end(), lb, ub);
}
bezier6_t* wrapBezierConstructor6(const point_list6_t& array)
{
t_point6_t asVector = vectorFromEigenArray<point_list6_t, t_point6_t>(array);
return new bezier6_t(asVector.begin(), asVector.end());
}
bezier6_t* wrapBezierConstructorBounds6(const point_list6_t& array, const real lb, const real ub)
{
t_point6_t asVector = vectorFromEigenArray<point_list6_t, t_point6_t>(array);
return new bezier6_t(asVector.begin(), asVector.end(), lb, ub);
}
spline_curve_t* wrapSplineConstructor(const coeff_t& array)
{
return new spline_curve_t(array, 0., 1.);
......@@ -146,11 +167,28 @@ BOOST_PYTHON_MODULE(spline)
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("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)
;
/** END bezier curve**/
/** BEGIN bezier curve**/
class_<bezier_t>
("bezier", no_init)
......
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