diff --git a/CMakeLists.txt b/CMakeLists.txt index c90b8af32fbfb6629f6439b2945b8ebb1c0d3f5d..775b2eff0cf22f9aa8507647435fe232a004460a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,11 +1,11 @@ cmake_minimum_required(VERSION 2.6) -project(spline) +project(curve) INCLUDE(cmake/base.cmake) INCLUDE(cmake/test.cmake) INCLUDE(cmake/python.cmake) INCLUDE(cmake/hpp.cmake) -SET(PROJECT_NAME hpp-spline) +SET(PROJECT_NAME curves) SET(PROJECT_DESCRIPTION "template based classes for creating and manipulating spline and bezier curves. Comes with extra options specific to end-effector trajectories in robotics." ) @@ -33,7 +33,7 @@ IF(BUILD_PYTHON_INTERFACE) ENDIF(BUILD_PYTHON_INTERFACE) -ADD_SUBDIRECTORY(include/hpp/spline) +ADD_SUBDIRECTORY(include/curve) ADD_SUBDIRECTORY(tests) SETUP_HPP_PROJECT_FINALIZE() diff --git a/include/hpp/spline/CMakeLists.txt b/include/curve/CMakeLists.txt similarity index 90% rename from include/hpp/spline/CMakeLists.txt rename to include/curve/CMakeLists.txt index b417e3d2cbfec4e0f3955998bd44a9d3fb09e0dc..3e1b1271a5dfa32ba7d97de8b637c12892e8551f 100644 --- a/include/hpp/spline/CMakeLists.txt +++ b/include/curve/CMakeLists.txt @@ -14,7 +14,7 @@ SET(${PROJECT_NAME}_HEADERS INSTALL(FILES ${${PROJECT_NAME}_HEADERS} - DESTINATION include/hpp/spline + DESTINATION include/curve ) ADD_SUBDIRECTORY(helpers) diff --git a/include/hpp/spline/MathDefs.h b/include/curve/MathDefs.h similarity index 93% rename from include/hpp/spline/MathDefs.h rename to include/curve/MathDefs.h index 4ded6adf6c0c0b3e71602f8e97ee68ee9d182144..94e8fa9f84a1c8777cb5f2118c976034d0a0d32e 100644 --- a/include/hpp/spline/MathDefs.h +++ b/include/curve/MathDefs.h @@ -20,8 +20,7 @@ #include <vector> #include <utility> - -namespace spline{ +namespace curve{ //REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels template<typename _Matrix_Type_> @@ -41,6 +40,6 @@ void PseudoInverse(_Matrix_Type_& pinvmat) pinvmat = (svd.matrixV()*m_sigma_inv*svd.matrixU().transpose()); } -} // namespace spline +} // namespace curve #endif //_SPLINEMATH diff --git a/include/hpp/spline/bernstein.h b/include/curve/bernstein.h similarity index 92% rename from include/hpp/spline/bernstein.h rename to include/curve/bernstein.h index ac93c24855635a0e82716da78a66f548755358a6..c96dd457ceade6eb871d6c7d50db340396d4ee2d 100644 --- a/include/hpp/spline/bernstein.h +++ b/include/curve/bernstein.h @@ -18,7 +18,7 @@ #include <vector> #include <stdexcept> -namespace spline +namespace curve { /// /// \brief Computes factorial of a number @@ -74,6 +74,6 @@ std::vector<Bern<Numeric> > makeBernstein(const unsigned int n) res.push_back(Bern<Numeric>(n, i)); return res; } -} // namespace spline +} // namespace curve #endif //_CLASS_BERNSTEIN diff --git a/include/hpp/spline/bezier_curve.h b/include/curve/bezier_curve.h similarity index 94% rename from include/hpp/spline/bezier_curve.h rename to include/curve/bezier_curve.h index c1e0e4b9460029c16cff57f5ecf3b1901d773ba2..8586230da90e796033bdfe98ebeee357953b6fac 100644 --- a/include/hpp/spline/bezier_curve.h +++ b/include/curve/bezier_curve.h @@ -21,7 +21,7 @@ #include <iostream> -namespace spline +namespace curve { /// \class BezierCurve /// \brief Represents a Bezier curve of arbitrary dimension and order. @@ -51,7 +51,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> , mult_T_(1.) , size_(std::distance(PointsBegin, PointsEnd)) , degree_(size_-1) - , bernstein_(spline::makeBernstein<num_t>((unsigned int)degree_)) + , bernstein_(curve::makeBernstein<num_t>((unsigned int)degree_)) { assert(bernstein_.size() == size_); In it(PointsBegin); @@ -70,7 +70,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> , mult_T_(1.) , size_(std::distance(PointsBegin, PointsEnd)) , degree_(size_-1) - , bernstein_(spline::makeBernstein<num_t>((unsigned int)degree_)) + , bernstein_(curve::makeBernstein<num_t>((unsigned int)degree_)) { assert(bernstein_.size() == size_); In it(PointsBegin); @@ -91,7 +91,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> , mult_T_(mult_T) , size_(std::distance(PointsBegin, PointsEnd)) , degree_(size_-1) - , bernstein_(spline::makeBernstein<num_t>((unsigned int)degree_)) + , bernstein_(curve::makeBernstein<num_t>((unsigned int)degree_)) { assert(bernstein_.size() == size_); In it(PointsBegin); @@ -113,7 +113,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> , mult_T_(1.) , size_(std::distance(PointsBegin, PointsEnd)+4) , degree_(size_-1) - , bernstein_(spline::makeBernstein<num_t>((unsigned int)degree_)) + , bernstein_(curve::makeBernstein<num_t>((unsigned int)degree_)) { if(Safe && (size_<1 || T_ <= 0.)) throw std::out_of_range("can't create bezier min bound is higher than max bound"); @@ -364,6 +364,6 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> return bezier_curve_t(ts.begin(), ts.end(),T); } }; -} +} // namespace curve #endif //_CLASS_BEZIERCURVE diff --git a/include/hpp/spline/bezier_polynom_conversion.h b/include/curve/bezier_polynom_conversion.h similarity index 94% rename from include/hpp/spline/bezier_polynom_conversion.h rename to include/curve/bezier_polynom_conversion.h index 2a4b5acc6991b98caac5ff5a2a8b317087408ac5..0a4910842718a5077fb94ee378ddd47524c2b59a 100644 --- a/include/hpp/spline/bezier_polynom_conversion.h +++ b/include/curve/bezier_polynom_conversion.h @@ -21,7 +21,7 @@ #include <iostream> -namespace spline +namespace curve { /// \brief Provides methods for converting a curve from a bernstein representation /// to a polynom representation @@ -67,6 +67,6 @@ Bezier from_polynom(const Polynom& polynom) typedef Bezier::cit_point_t cit_point_t; typedef Bezier::bezier_curve_t bezier_curve_t; }*/ -} +} // namespace curve #endif //_BEZIER_POLY_CONVERSION diff --git a/include/hpp/spline/cubic_spline.h b/include/curve/cubic_spline.h similarity index 94% rename from include/hpp/spline/cubic_spline.h rename to include/curve/cubic_spline.h index 016c54d7a7b16c561abd45bb93b7639dc668b1d1..9715f16fcd35e59a12819f7bd4600d41d139a48f 100644 --- a/include/hpp/spline/cubic_spline.h +++ b/include/curve/cubic_spline.h @@ -20,7 +20,7 @@ #include <stdexcept> -namespace spline +namespace curve { /// \brief Creates coefficient vector of a cubic spline defined on the interval /// [tBegin, tEnd]. It follows the equation @@ -41,6 +41,6 @@ polynom<Time,Numeric,Dim,Safe,Point,T_Point> create_cubic(Point const& a, Point T_Point coeffs = make_cubic_vector<Point, T_Point>(a,b,c,d); return polynom<Time,Numeric,Dim,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), min, max); } -} +} // namespace curve #endif //_STRUCT_CUBICSPLINE diff --git a/include/hpp/spline/curve_abc.h b/include/curve/curve_abc.h similarity index 94% rename from include/hpp/spline/curve_abc.h rename to include/curve/curve_abc.h index 7a8dccaf183075de4cde6032f475a26bad64342e..72aac2d98a917bad536c2eb129c853f32b5b61b4 100644 --- a/include/hpp/spline/curve_abc.h +++ b/include/curve/curve_abc.h @@ -16,7 +16,7 @@ #include <functional> -namespace spline +namespace curve { /// \struct curve_abc /// \brief Represents a curve of dimension Dim @@ -63,6 +63,6 @@ struct curve_abc : std::unary_function<Time, Point> /*Helpers*/ }; -} +} // namespace curve #endif //_STRUCT_CURVE_ABC diff --git a/include/hpp/spline/curve_constraint.h b/include/curve/curve_constraint.h similarity index 94% rename from include/hpp/spline/curve_constraint.h rename to include/curve/curve_constraint.h index af79b73129cb6a5dacc5d8b0eca27837925c7905..5b4e8efc9b82d2c3e097dd3675aa41bef8f286e8 100644 --- a/include/hpp/spline/curve_constraint.h +++ b/include/curve/curve_constraint.h @@ -17,7 +17,7 @@ #include <functional> #include <vector> -namespace spline +namespace curve { template <typename Point> struct curve_constraints @@ -32,5 +32,5 @@ struct curve_constraints point_t end_vel; point_t end_acc; }; -} +} // namespace curve #endif //_CLASS_CUBICZEROVELACC diff --git a/include/hpp/spline/exact_cubic.h b/include/curve/exact_cubic.h similarity index 96% rename from include/hpp/spline/exact_cubic.h rename to include/curve/exact_cubic.h index 550709c37371ac0b4ebb48eb39aa47ead8b8febe..d71cfa0ff6ce132c5ae4b8e5e6b9855ff2b97edb 100644 --- a/include/hpp/spline/exact_cubic.h +++ b/include/curve/exact_cubic.h @@ -29,7 +29,7 @@ #include <functional> #include <vector> -namespace spline +namespace curve { /// \class ExactCubic /// \brief Represents a set of cubic splines defining a continuous function @@ -198,6 +198,6 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point> t_spline_t subSplines_; // const /*Attributes*/ }; -} +} // namespace curve #endif //_CLASS_EXACTCUBIC diff --git a/include/hpp/spline/helpers/CMakeLists.txt b/include/curve/helpers/CMakeLists.txt similarity index 77% rename from include/hpp/spline/helpers/CMakeLists.txt rename to include/curve/helpers/CMakeLists.txt index 737295b2636e4d6f74d54ec1f660ffefb75286a4..7a849f3810093da39a1fb5d2bf1a2ac92500aa1f 100644 --- a/include/hpp/spline/helpers/CMakeLists.txt +++ b/include/curve/helpers/CMakeLists.txt @@ -5,5 +5,5 @@ SET(${PROJECT_NAME}_HELPERS_HEADERS INSTALL(FILES ${${PROJECT_NAME}_HELPERS_HEADERS} - DESTINATION include/hpp/spline/helpers + DESTINATION include/curve/helpers ) diff --git a/include/hpp/spline/helpers/effector_spline.h b/include/curve/helpers/effector_spline.h similarity index 98% rename from include/hpp/spline/helpers/effector_spline.h rename to include/curve/helpers/effector_spline.h index 0f286d3bc5b86f62da8d4a266ad2fe8a0812bc51..97dae8c57a700d1ce9fe0808b29fe479a96afc98 100644 --- a/include/hpp/spline/helpers/effector_spline.h +++ b/include/curve/helpers/effector_spline.h @@ -20,9 +20,9 @@ #ifndef _CLASS_EFFECTORSPLINE #define _CLASS_EFFECTORSPLINE -#include "hpp/spline/spline_deriv_constraint.h" +#include "curve/spline_deriv_constraint.h" -namespace spline +namespace curve { namespace helpers { diff --git a/include/hpp/spline/helpers/effector_spline_rotation.h b/include/curve/helpers/effector_spline_rotation.h similarity index 98% rename from include/hpp/spline/helpers/effector_spline_rotation.h rename to include/curve/helpers/effector_spline_rotation.h index 508716228558c283c70c7f515a2bf40126531d2c..42efa1cb960026db1f01ad5b4af3dd30f88922f4 100644 --- a/include/hpp/spline/helpers/effector_spline_rotation.h +++ b/include/curve/helpers/effector_spline_rotation.h @@ -20,11 +20,11 @@ #ifndef _CLASS_EFFECTOR_SPLINE_ROTATION #define _CLASS_EFFECTOR_SPLINE_ROTATION -#include "hpp/spline/helpers/effector_spline.h" -#include "hpp/spline/curve_abc.h" +#include "curve/helpers/effector_spline.h" +#include "curve/curve_abc.h" #include <Eigen/Geometry> -namespace spline +namespace curve { namespace helpers { @@ -252,5 +252,5 @@ class effector_spline_rotation }; } // namespace helpers -} // namespace spline +} // namespace curve #endif //_CLASS_EFFECTOR_SPLINE_ROTATION diff --git a/include/hpp/spline/optimization/OptimizeSpline.h b/include/curve/optimization/OptimizeSpline.h similarity index 99% rename from include/hpp/spline/optimization/OptimizeSpline.h rename to include/curve/optimization/OptimizeSpline.h index a5425b10bc92c6497814c89228fcf58eac38f019..a7d9e161993cbaa69f1e769474bea4d757d26eb3 100644 --- a/include/hpp/spline/optimization/OptimizeSpline.h +++ b/include/curve/optimization/OptimizeSpline.h @@ -12,15 +12,15 @@ #ifndef _CLASS_SPLINEOPTIMIZER #define _CLASS_SPLINEOPTIMIZER -#include "spline/MathDefs.h" -#include "spline/exact_cubic.h" +#include "curve/MathDefs.h" +#include "curve/exact_cubic.h" #include "mosek/mosek.h" #include <Eigen/SparseCore> #include <utility> -namespace spline +namespace curve { /// \class SplineOptimizer /// \brief Mosek connection to produce optimized splines @@ -516,5 +516,5 @@ inline exact_cubic<Time, Numeric, Dim, Safe, Point>* return res; } -} // namespace spline +} // namespace curve #endif //_CLASS_SPLINEOPTIMIZER diff --git a/include/hpp/spline/polynom.h b/include/curve/polynom.h similarity index 96% rename from include/hpp/spline/polynom.h rename to include/curve/polynom.h index 32c8aad06b92769015b660ed1e85402423561d1e..2aca2fed9d47429c5e3c221343985bbdd283dc67 100644 --- a/include/hpp/spline/polynom.h +++ b/include/curve/polynom.h @@ -23,7 +23,7 @@ #include <functional> #include <stdexcept> -namespace spline +namespace curve { /// \class polynom /// \brief Represents a polynomf arbitrary order defined on the interval @@ -199,6 +199,6 @@ struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point> return res; } }; //class polynom -} +} // namespace curve #endif //_STRUCT_POLYNOM diff --git a/include/hpp/spline/quintic_spline.h b/include/curve/quintic_spline.h similarity index 94% rename from include/hpp/spline/quintic_spline.h rename to include/curve/quintic_spline.h index ae14e4d4d62f5a2467ec9160cbfb307532265b69..31f6bf98303ccfa27e10a102070c4d98f7c7f079 100644 --- a/include/hpp/spline/quintic_spline.h +++ b/include/curve/quintic_spline.h @@ -20,7 +20,7 @@ #include <stdexcept> -namespace spline +namespace curve { /// \brief Creates coefficient vector of a quintic spline defined on the interval /// [tBegin, tEnd]. It follows the equation @@ -43,6 +43,6 @@ polynom<Time,Numeric,Dim,Safe,Point,T_Point> create_quintic(Point const& a, Poin T_Point coeffs = make_quintic_vector<Point, T_Point>(a,b,c,d,e,f); return polynom<Time,Numeric,Dim,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), min, max); } -} +} // namespace curve #endif //_STRUCT_QUINTIC_SPLINE diff --git a/include/hpp/spline/spline_deriv_constraint.h b/include/curve/spline_deriv_constraint.h similarity index 97% rename from include/hpp/spline/spline_deriv_constraint.h rename to include/curve/spline_deriv_constraint.h index 398b6973df439c672c611be3f014e424684f2812..9c2ed3ecefbb00366dde13767fe5081c064b4981 100644 --- a/include/hpp/spline/spline_deriv_constraint.h +++ b/include/curve/spline_deriv_constraint.h @@ -28,7 +28,7 @@ #include <functional> #include <vector> -namespace spline +namespace curve { /// \class spline_deriv_constraint. /// \brief Represents a set of cubic splines defining a continuous function @@ -137,6 +137,6 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po private: /* Constructors - destructors */ }; -} +} // namespace curve #endif //_CLASS_CUBICZEROVELACC diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 499561c0144a27e1daad52e197d5568264dfa848..e149c14e7cf8e9bb1b1fb89b8ae3445daf1b2e45 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -3,7 +3,7 @@ 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 curve_python.cpp ) #~ target_link_libraries( centroidal_dynamics ${Boost_LIBRARIES} ${PROJECT_NAME} ) # don't prepend wrapper library name with lib set_target_properties( ${PY_NAME} PROPERTIES PREFIX "" ) @@ -18,4 +18,4 @@ INSTALL( TARGETS ${PY_NAME} DESTINATION ${PYTHON_SITELIB} ) -ADD_PYTHON_UNIT_TEST("python-spline" "python/test/test.py" "python") +ADD_PYTHON_UNIT_TEST("python-curve" "python/test/test.py" "python") diff --git a/python/spline_python.cpp b/python/curve_python.cpp similarity index 80% rename from python/spline_python.cpp rename to python/curve_python.cpp index 764b5cc78e360fd6bbc0e216569e0da3edfa8574..aa3f1914730506ff578a0612d6a1f290c4c7cfde 100644 --- a/python/spline_python.cpp +++ b/python/curve_python.cpp @@ -1,10 +1,10 @@ -#include "hpp/spline/bezier_curve.h" -#include "hpp/spline/polynom.h" -#include "hpp/spline/exact_cubic.h" -#include "hpp/spline/spline_deriv_constraint.h" -#include "hpp/spline/curve_constraint.h" -#include "hpp/spline/bezier_polynom_conversion.h" -#include "hpp/spline/bernstein.h" +#include "curve/bezier_curve.h" +#include "curve/polynom.h" +#include "curve/exact_cubic.h" +#include "curve/spline_deriv_constraint.h" +#include "curve/curve_constraint.h" +#include "curve/bezier_polynom_conversion.h" +#include "curve/bernstein.h" #include <vector> @@ -30,31 +30,31 @@ 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::polynom <real, real, 3, true, point_t, t_point_t> polynom_t; -typedef spline::exact_cubic <real, real, 3, true, point_t, t_point_t> exact_cubic_t; +typedef curve::bezier_curve <real, real, 3, true, point_t> bezier3_t; +typedef curve::bezier_curve <real, real, 6, true, point6_t> bezier6_t; +typedef curve::polynom <real, real, 3, true, point_t, t_point_t> polynom_t; +typedef curve::exact_cubic <real, real, 3, true, point_t, t_point_t> exact_cubic_t; typedef polynom_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 spline::Bern<double> bernstein_t; +typedef curve::Bern<double> bernstein_t; -typedef spline::spline_deriv_constraint <real, real, 3, true, point_t, t_point_t> spline_deriv_constraint_t; -typedef spline::curve_constraints<point_t> curve_constraints_t; -typedef spline::curve_constraints<point6_t> curve_constraints6_t; +typedef curve::spline_deriv_constraint <real, real, 3, true, point_t, t_point_t> spline_deriv_constraint_t; +typedef curve::curve_constraints<point_t> curve_constraints_t; +typedef curve::curve_constraints<point6_t> curve_constraints6_t; /*** TEMPLATE SPECIALIZATION FOR PYTHON ****/ EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bernstein_t) -EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_t) +EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier3_t) EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier6_t) EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(polynom_t) EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(exact_cubic_t) EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints_t) EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_deriv_constraint_t) -namespace spline +namespace curve { using namespace boost::python; template <typename PointList, typename T_Point> @@ -81,21 +81,21 @@ Bezier* wrapBezierConstructorConstraintsTemplate(const PointList& array, const C } /*3D constructors */ -bezier_t* wrapBezierConstructor(const point_list_t& array) +bezier3_t* wrapBezierConstructor(const point_list_t& array) { - return wrapBezierConstructorTemplate<bezier_t, point_list_t, t_point_t>(array) ; + return wrapBezierConstructorTemplate<bezier3_t, point_list_t, t_point_t>(array) ; } -bezier_t* wrapBezierConstructorBounds(const point_list_t& array, const real ub) +bezier3_t* wrapBezierConstructorBounds(const point_list_t& array, const real ub) { - return wrapBezierConstructorTemplate<bezier_t, point_list_t, t_point_t>(array, ub) ; + return wrapBezierConstructorTemplate<bezier3_t, point_list_t, t_point_t>(array, ub) ; } -bezier_t* wrapBezierConstructorConstraints(const point_list_t& array, const curve_constraints_t& constraints) +bezier3_t* wrapBezierConstructorConstraints(const point_list_t& array, const curve_constraints_t& constraints) { - return wrapBezierConstructorConstraintsTemplate<bezier_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints) ; + return wrapBezierConstructorConstraintsTemplate<bezier3_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints) ; } -bezier_t* wrapBezierConstructorBoundsConstraints(const point_list_t& array, const curve_constraints_t& constraints, const real ub) +bezier3_t* wrapBezierConstructorBoundsConstraints(const point_list_t& array, const curve_constraints_t& constraints, const real ub) { - return wrapBezierConstructorConstraintsTemplate<bezier_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints, ub) ; + return wrapBezierConstructorConstraintsTemplate<bezier3_t, point_list_t, t_point_t, curve_constraints_t>(array, constraints, ub) ; } /*END 3D constructors */ /*6D constructors */ @@ -205,7 +205,7 @@ void set_end_acc(curve_constraints_t& c, const point_t& val) -BOOST_PYTHON_MODULE(hpp_spline) +BOOST_PYTHON_MODULE(curves) { /** BEGIN eigenpy init**/ eigenpy::enableEigenPy(); @@ -241,21 +241,21 @@ BOOST_PYTHON_MODULE(hpp_spline) /** END bezier curve**/ /** BEGIN bezier curve**/ - class_<bezier_t> - ("bezier", no_init) + class_<bezier3_t> + ("bezier3", no_init) .def("__init__", make_constructor(&wrapBezierConstructor)) .def("__init__", make_constructor(&wrapBezierConstructorBounds)) .def("__init__", make_constructor(&wrapBezierConstructorConstraints)) .def("__init__", make_constructor(&wrapBezierConstructorBoundsConstraints)) - .def("min", &bezier_t::min) - .def("max", &bezier_t::max) - .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<bezier_t,3>) - .def_readonly("degree", &bezier_t::degree_) - .def_readonly("nbWaypoints", &bezier_t::size_) + .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**/ @@ -314,10 +314,10 @@ BOOST_PYTHON_MODULE(hpp_spline) /** END bernstein polynom**/ /** BEGIN Bezier to polynom conversion**/ - def("from_bezier", from_bezier<bezier_t,polynom_t>); + def("from_bezier", from_bezier<bezier3_t,polynom_t>); /** END Bezier to polynom conversion**/ } -} // namespace spline +} // namespace curve diff --git a/python/test/test.py b/python/test/test.py index 38e775957d5b9e18c958878e2982f9887d8f1e6f..c35a8788c3b41c3ee2cbf9d937e037b096e109fa 100644 --- a/python/test/test.py +++ b/python/test/test.py @@ -6,19 +6,26 @@ from numpy import matrix from numpy.linalg import norm from numpy.testing import assert_allclose -from hpp_spline import bezier, bezier6, curve_constraints, exact_cubic, from_bezier, polynom, spline_deriv_constraint +from curves import bezier3, bezier6, curve_constraints, exact_cubic, from_bezier, polynom, spline_deriv_constraint -class TestSpline(unittest.TestCase): - def test_spline(self): +class TestCurve(unittest.TestCase): + def test_curve(self): waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).T waypoints6 = matrix([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 4., 5., 6.]]).T time_waypoints = matrix([0., 1.]).T - # testing bezier curve - a = bezier6(waypoints6) - a = bezier(waypoints, 3.) + # Test : Bezier3/Bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraints, bernstein + # Done : Bezier3/Bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraints + # To do ? Bernstein + # TESTING BEZIER CURVE + # - Functions : constructor, min, max, derivate,compute_derivate, compute_primitive + # - Variables : degree, nbWayPoints + # Create bezier6 and bezier3 + a = bezier6(waypoints6) + a = bezier3(waypoints, 3.) + # Test : Degree, min, max, derivate self.assertEqual(a.degree, a.nbWaypoints - 1) a.min() a.max() @@ -26,26 +33,25 @@ class TestSpline(unittest.TestCase): assert_allclose(a.derivate(0.4, 0), a(0.4)) a.derivate(0.4, 2) a = a.compute_derivate(100) - prim = a.compute_primitive(1) - + # Check primitive and derivate - order 1 for i in range(10): t = float(i) / 10. assert_allclose(a(t), prim.derivate(t, 1)) assert_allclose(prim(0), matrix([0., 0., 0.]).T) - + # Check primitive and derivate - order 2 prim = a.compute_primitive(2) for i in range(10): t = float(i) / 10. assert_allclose(a(t), prim.derivate(t, 2), atol=1e-20) assert_allclose(prim(0), matrix([0., 0., 0.]).T) - + # Create new bezier3 curve waypoints = matrix([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.]]).T - a0 = bezier(waypoints) - a1 = bezier(waypoints, 3.) + a0 = bezier3(waypoints) + a1 = bezier3(waypoints, 3.) prim0 = a0.compute_primitive(1) prim1 = a1.compute_primitive(1) - + # Check change in argument time_t of bezier3 for i in range(10): t = float(i) / 10. assert_allclose(a0(t), a1(3 * t)) @@ -55,20 +61,20 @@ class TestSpline(unittest.TestCase): assert_allclose(prim(0), matrix([0., 0., 0.]).T) with self.assertRaises(AssertionError): assert_allclose(prim(0), matrix([0., 0., 0.])) - # testing bezier with constraints c = curve_constraints() c.init_vel = matrix([0., 1., 1.]).T c.end_vel = matrix([0., 1., 1.]).T c.init_acc = matrix([0., 1., -1.]).T c.end_acc = matrix([0., 100., 1.]).T - + #Check derivate with constraints waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).T - a = bezier(waypoints, c) + a = bezier3(waypoints, c) assert_allclose(a.derivate(0, 1), c.init_vel) assert_allclose(a.derivate(1, 2), c.end_acc) - # testing polynom function + # TESTING POLYNOM FUNCTION + # - Functions : constructor, min, max, derivate a = polynom(waypoints) a = polynom(waypoints, -1., 3.) a.min() @@ -77,7 +83,8 @@ class TestSpline(unittest.TestCase): assert_allclose(a.derivate(0.4, 0), a(0.4)) a.derivate(0.4, 2) - # testing exact_cubic function + # TESTING EXACT_CUBIC FUNCTION + # - Functions : constructor, min, max, derivate a = exact_cubic(waypoints, time_waypoints) a.min() a.max() @@ -85,24 +92,22 @@ class TestSpline(unittest.TestCase): assert_allclose(a.derivate(0.4, 0), a(0.4)) a.derivate(0.4, 2) - # testing spline_deriv_constraints + # TESTING SPLINE_DERIV_CONSTRAINTS + # - Functions : constructor, min, max, derivate c = curve_constraints() c.init_vel c.end_vel c.init_acc c.end_acc - c.init_vel = matrix([0., 1., 1.]).T c.end_vel = matrix([0., 1., 1.]).T c.init_acc = matrix([0., 1., 1.]).T c.end_acc = matrix([0., 1., 1.]).T - a = spline_deriv_constraint(waypoints, time_waypoints) a = spline_deriv_constraint(waypoints, time_waypoints, c) - # converting bezier to polynom - - a = bezier(waypoints) + # CONVERTING BEZIER TO POLYNOM + a = bezier3(waypoints) a_pol = from_bezier(a) assert_allclose(a(0.3), a_pol(0.3)) diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 70960bbbfd3df38d9fe09ab0138fc8c6e1ef02d9..e03c2b0434e161f67df8e1c89f78049b7f9eda65 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,3 +1,3 @@ ADD_UNIT_TEST( - spline_tests Main.cpp + curve_tests Main.cpp ) diff --git a/tests/Main.cpp b/tests/Main.cpp index 5046ad1c85f21378c10893be9f0dbc49b06ebb97..00065db7c600e863db3d76d56e92f7dcbe853cc1 100644 --- a/tests/Main.cpp +++ b/tests/Main.cpp @@ -1,11 +1,11 @@ -#include "hpp/spline/exact_cubic.h" -#include "hpp/spline/bezier_curve.h" -#include "hpp/spline/polynom.h" -#include "hpp/spline/spline_deriv_constraint.h" -#include "hpp/spline/helpers/effector_spline.h" -#include "hpp/spline/helpers/effector_spline_rotation.h" -#include "hpp/spline/bezier_polynom_conversion.h" +#include "curve/exact_cubic.h" +#include "curve/bezier_curve.h" +#include "curve/polynom.h" +#include "curve/spline_deriv_constraint.h" +#include "curve/helpers/effector_spline.h" +#include "curve/helpers/effector_spline_rotation.h" +#include "curve/bezier_polynom_conversion.h" #include <string> #include <iostream> @@ -13,7 +13,7 @@ using namespace std; -namespace spline +namespace curve { typedef Eigen::Vector3d point_t; typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t; @@ -46,9 +46,9 @@ bool QuasiEqual(const double a, const double b, const float margin) const double margin = 0.001; -} // namespace spline +} // namespace curve -using namespace spline; +using namespace curve; ostream& operator<<(ostream& os, const point_t& pt) { @@ -456,7 +456,7 @@ void BezierToPolynomConversionTest(bool& error) /*Exact Cubic Function tests*/ void ExactCubicNoErrorTest(bool& error) { - spline::T_Waypoint waypoints; + curve::T_Waypoint waypoints; for(double i = 0; i <= 1; i = i + 0.2) { waypoints.push_back(std::make_pair(i,point_t(i,i,i))); @@ -502,7 +502,7 @@ void ExactCubicNoErrorTest(bool& error) /*Exact Cubic Function tests*/ void ExactCubicTwoPointsTest(bool& error) { - spline::T_Waypoint waypoints; + curve::T_Waypoint waypoints; for(double i = 0; i < 2; ++i) { waypoints.push_back(std::make_pair(i,point_t(i,i,i))); @@ -519,7 +519,7 @@ void ExactCubicTwoPointsTest(bool& error) void ExactCubicOneDimTest(bool& error) { - spline::T_WaypointOne waypoints; + curve::T_WaypointOne waypoints; point_one zero; zero(0,0) = 9; point_one one; one(0,0) = 14; point_one two; two(0,0) = 25; @@ -536,7 +536,7 @@ void ExactCubicOneDimTest(bool& error) ComparePoints(one, res1, errmsg, error); } -void CheckWayPointConstraint(const std::string& errmsg, const double step, const spline::T_Waypoint& /*wayPoints*/, const exact_cubic_t* curve, bool& error ) +void CheckWayPointConstraint(const std::string& errmsg, const double step, const curve::T_Waypoint& /*wayPoints*/, const exact_cubic_t* curve, bool& error ) { point_t res1; for(double i = 0; i <= 1; i = i + step) @@ -555,7 +555,7 @@ void CheckDerivative(const std::string& errmsg, const double eval_point, const s void ExactCubicPointsCrossedTest(bool& error) { - spline::T_Waypoint waypoints; + curve::T_Waypoint waypoints; for(double i = 0; i <= 1; i = i + 0.2) { waypoints.push_back(std::make_pair(i,point_t(i,i,i))); @@ -568,7 +568,7 @@ void ExactCubicPointsCrossedTest(bool& error) void ExactCubicVelocityConstraintsTest(bool& error) { - spline::T_Waypoint waypoints; + curve::T_Waypoint waypoints; for(double i = 0; i <= 1; i = i + 0.2) { waypoints.push_back(std::make_pair(i,point_t(i,i,i))); @@ -617,7 +617,7 @@ void CheckPointOnline(const std::string& errmsg, const point_t& A, const point_t void EffectorTrajectoryTest(bool& error) { // create arbitrary trajectory - spline::T_Waypoint waypoints; + curve::T_Waypoint waypoints; for(double i = 0; i <= 10; i = i + 2) { waypoints.push_back(std::make_pair(i,point_t(i,i,i))); @@ -674,7 +674,7 @@ double GetXRotFromQuat(helpers::quat_ref_const_t q) void EffectorSplineRotationNoRotationTest(bool& error) { // create arbitrary trajectory - spline::T_Waypoint waypoints; + curve::T_Waypoint waypoints; for(double i = 0; i <= 10; i = i + 2) { waypoints.push_back(std::make_pair(i,point_t(i,i,i))); @@ -696,7 +696,7 @@ void EffectorSplineRotationNoRotationTest(bool& error) void EffectorSplineRotationRotationTest(bool& error) { // create arbitrary trajectory - spline::T_Waypoint waypoints; + curve::T_Waypoint waypoints; for(double i = 0; i <= 10; i = i + 2) { waypoints.push_back(std::make_pair(i,point_t(i,i,i))); @@ -719,7 +719,7 @@ void EffectorSplineRotationRotationTest(bool& error) void EffectorSplineRotationWayPointRotationTest(bool& error) { // create arbitrary trajectory - spline::T_Waypoint waypoints; + curve::T_Waypoint waypoints; for(double i = 0; i <= 10; i = i + 2) { waypoints.push_back(std::make_pair(i,point_t(i,i,i)));