Commit d88c3393 by Steve Tonneau

Merge branch 'packaging'

parents 62165aed 86554a37
 ... ... @@ -18,6 +18,8 @@ #include #include #include namespace spline { /// \class BezierCurve ... ... @@ -45,7 +47,8 @@ struct bezier_curve : public curve_abc : minBound_(minBound) , maxBound_(maxBound) , size_(std::distance(PointsBegin, PointsEnd)) , bernstein_(spline::makeBernstein(size_-1)) , degree_(size_-1) , bernstein_(spline::makeBernstein(degree_)) { assert(bernstein_.size() == size_); In it(PointsBegin); ... ... @@ -117,13 +120,34 @@ struct bezier_curve : public curve_abc if(order == 0) return *this; t_point_t derived_wp; for(typename t_point_t::const_iterator pit = pts_.begin(); pit != pts_.end()-1; ++pit) derived_wp.push_back(*(pit+1) - (*pit)); derived_wp.push_back(degree_ * (*(pit+1) - (*pit))); if(derived_wp.empty()) derived_wp.push_back(point_t::Zero()); bezier_curve_t deriv(derived_wp.begin(), derived_wp.end(),minBound_,maxBound_); return deriv.compute_derivate(order-1); } /// \brief Computes the primitive of the curve at order N. /// \param constant : value of the primitive at t = 0 /// \param return : the curve x_1(t) such that d/dt(x_1(t)) = x_1(t) bezier_curve_t compute_primitive(const std::size_t order) const { if(order == 0) return *this; num_t new_degree = (num_t)(degree_+1); t_point_t n_wp; point_t current_sum = point_t::Zero(); // recomputing waypoints q_i from derivative waypoints p_i. q_0 is the given constant. // then q_i = (sum( j = 0 -> j = i-1) p_j) /n+1 n_wp.push_back(current_sum); for(typename t_point_t::const_iterator pit = pts_.begin(); pit != pts_.end(); ++pit) { current_sum += *pit; n_wp.push_back(current_sum / new_degree); } bezier_curve_t integ(n_wp.begin(), n_wp.end(),minBound_,maxBound_); return integ.compute_primitive(order-1); } /// \brief Evaluates the derivative at order N of the curve. /// If the derivative is to be evaluated several times, it is /// rather recommended to compute the derivative curve using compute_derivate ... ... @@ -151,6 +175,8 @@ struct bezier_curve : public curve_abc return res; } const t_point_t& waypoints() const {return pts_;} /*Operations*/ /*Helpers*/ ... ... @@ -161,6 +187,7 @@ struct bezier_curve : public curve_abc public: const time_t minBound_, maxBound_; const std::size_t size_; const std::size_t degree_; const std::vector > bernstein_; private: ... ...
 ... ... @@ -13,14 +13,21 @@ /*** TEMPLATE SPECIALIZATION FOR PYTHON ****/ typedef double real; typedef Eigen::Vector3d point_t; typedef Eigen::Matrix point6_t; typedef Eigen::Matrix ret_point_t; typedef Eigen::Matrix ret_point6_t; typedef Eigen::VectorXd time_waypoints_t; typedef Eigen::Matrix point_list_t; typedef Eigen::Matrix point_list6_t; typedef std::vector > t_point_t; typedef std::vector > t_point6_t; typedef std::pair Waypoint; typedef std::vector T_Waypoint; typedef std::pair Waypoint6; typedef std::vector T_Waypoint6; typedef spline::bezier_curve bezier_t; typedef spline::bezier_curve bezier6_t; typedef spline::spline_curve spline_curve_t; typedef spline::exact_cubic 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 T_Point vectorFromEigenArray(const PointList& array) { t_point_t res; T_Point res; for(int i =0;i(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(array); return new bezier_t(asVector.begin(), asVector.end(), lb, ub); } bezier6_t* wrapBezierConstructor6(const point_list6_t& array) { t_point6_t asVector = vectorFromEigenArray(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(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.); ... ... @@ -77,6 +98,19 @@ t_waypoint_t getWayPoints(const coeff_t& array, const time_waypoints_t& time_wp) return res; } template Eigen::Matrix 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 res (dim, wps.size()); int col = 0; for(cit_point cit = wps.begin(); cit != wps.end(); ++cit, ++col) res.block(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); ... ... @@ -146,11 +180,31 @@ BOOST_PYTHON_MODULE(spline) eigenpy::enableEigenPySpecific(); eigenpy::enableEigenPySpecific(); eigenpy::enableEigenPySpecific(); eigenpy::enableEigenPySpecific(); eigenpy::enableEigenPySpecific(); eigenpy::enableEigenPySpecific(); eigenpy::enableEigenPySpecific(); /*eigenpy::exposeAngleAxis(); eigenpy::exposeQuaternion();*/ /** END eigenpy init**/ /** BEGIN bezier curve 6**/ class_ ("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) .def("waypoints", &wayPointsToList) .def_readonly("degree", &bezier6_t::degree_) .def_readonly("nbWaypoints", &bezier6_t::size_) ; /** END bezier curve**/ /** BEGIN bezier curve**/ class_ ("bezier", no_init) ... ... @@ -161,6 +215,10 @@ BOOST_PYTHON_MODULE(spline) .def("__call__", &bezier_t::operator()) .def("derivate", &bezier_t::derivate) .def("compute_derivate", &bezier_t::compute_derivate) .def("compute_primitive", &bezier_t::compute_primitive) .def("waypoints", &wayPointsToList) .def_readonly("degree", &bezier_t::degree_) .def_readonly("nbWaypoints", &bezier_t::size_) ; /** END bezier curve**/ ... ...
 ... ... @@ -6,16 +6,31 @@ waypoints = matrix([[1.,2.,3.],[4.,5.,6.]]).transpose() time_waypoints = matrix([0.,1.]) #testing bezier curve a = bezier(waypoints) a = bezier(waypoints, -1., 3.) a = bezier(waypoints) assert(a.degree == a.nbWaypoints -1) a.min() a.max() a(0.4) assert((a.derivate(0.4,0) == a(0.4)).all()) a.derivate(0.4,2) a = a.compute_derivate(100) prim = a.compute_primitive(1) for i in range(10): t = float(i) / 10. assert(a(t) == prim.derivate(t,1)).all() assert(prim(0) == matrix([0.,0.,0.])).all() prim = a.compute_primitive(2) for i in range(10): t = float(i) / 10. assert(a(t) == prim.derivate(t,2)).all() assert(prim(0) == matrix([0.,0.,0.])).all() #testing spline function a = spline(waypoints) ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!