Commit d1ee66a4 authored by Guilhem Saurel's avatar Guilhem Saurel

Merge branch 'refactoring' into 'master'

Refactoring

See merge request loco-3d/curves!1
parents dc7ad399 f4fd73e6
cmake_minimum_required(VERSION 2.6) cmake_minimum_required(VERSION 2.6)
project(spline) project(curve)
INCLUDE(cmake/base.cmake) INCLUDE(cmake/base.cmake)
INCLUDE(cmake/test.cmake) INCLUDE(cmake/test.cmake)
INCLUDE(cmake/python.cmake) INCLUDE(cmake/python.cmake)
INCLUDE(cmake/hpp.cmake) INCLUDE(cmake/hpp.cmake)
SET(PROJECT_NAME hpp-spline) SET(PROJECT_NAME curves)
SET(PROJECT_DESCRIPTION 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." "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) ...@@ -33,7 +33,7 @@ IF(BUILD_PYTHON_INTERFACE)
ENDIF(BUILD_PYTHON_INTERFACE) ENDIF(BUILD_PYTHON_INTERFACE)
ADD_SUBDIRECTORY(include/hpp/spline) ADD_SUBDIRECTORY(include/curve)
ADD_SUBDIRECTORY(tests) ADD_SUBDIRECTORY(tests)
SETUP_HPP_PROJECT_FINALIZE() SETUP_HPP_PROJECT_FINALIZE()
...@@ -14,7 +14,7 @@ SET(${PROJECT_NAME}_HEADERS ...@@ -14,7 +14,7 @@ SET(${PROJECT_NAME}_HEADERS
INSTALL(FILES INSTALL(FILES
${${PROJECT_NAME}_HEADERS} ${${PROJECT_NAME}_HEADERS}
DESTINATION include/hpp/spline DESTINATION include/curve
) )
ADD_SUBDIRECTORY(helpers) ADD_SUBDIRECTORY(helpers)
...@@ -20,8 +20,7 @@ ...@@ -20,8 +20,7 @@
#include <vector> #include <vector>
#include <utility> #include <utility>
namespace curve{
namespace spline{
//REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels //REF: boulic et al An inverse kinematics architecture enforcing an arbitrary number of strict priority levels
template<typename _Matrix_Type_> template<typename _Matrix_Type_>
...@@ -41,6 +40,6 @@ void PseudoInverse(_Matrix_Type_& pinvmat) ...@@ -41,6 +40,6 @@ void PseudoInverse(_Matrix_Type_& pinvmat)
pinvmat = (svd.matrixV()*m_sigma_inv*svd.matrixU().transpose()); pinvmat = (svd.matrixV()*m_sigma_inv*svd.matrixU().transpose());
} }
} // namespace spline } // namespace curve
#endif //_SPLINEMATH #endif //_SPLINEMATH
...@@ -18,7 +18,7 @@ ...@@ -18,7 +18,7 @@
#include <vector> #include <vector>
#include <stdexcept> #include <stdexcept>
namespace spline namespace curve
{ {
/// ///
/// \brief Computes factorial of a number /// \brief Computes factorial of a number
...@@ -74,6 +74,6 @@ std::vector<Bern<Numeric> > makeBernstein(const unsigned int n) ...@@ -74,6 +74,6 @@ std::vector<Bern<Numeric> > makeBernstein(const unsigned int n)
res.push_back(Bern<Numeric>(n, i)); res.push_back(Bern<Numeric>(n, i));
return res; return res;
} }
} // namespace spline } // namespace curve
#endif //_CLASS_BERNSTEIN #endif //_CLASS_BERNSTEIN
...@@ -21,7 +21,7 @@ ...@@ -21,7 +21,7 @@
#include <iostream> #include <iostream>
namespace spline namespace curve
{ {
/// \class BezierCurve /// \class BezierCurve
/// \brief Represents a Bezier curve of arbitrary dimension and order. /// \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> ...@@ -51,7 +51,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
, mult_T_(1.) , mult_T_(1.)
, size_(std::distance(PointsBegin, PointsEnd)) , size_(std::distance(PointsBegin, PointsEnd))
, degree_(size_-1) , degree_(size_-1)
, bernstein_(spline::makeBernstein<num_t>((unsigned int)degree_)) , bernstein_(curve::makeBernstein<num_t>((unsigned int)degree_))
{ {
assert(bernstein_.size() == size_); assert(bernstein_.size() == size_);
In it(PointsBegin); In it(PointsBegin);
...@@ -70,7 +70,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> ...@@ -70,7 +70,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
, mult_T_(1.) , mult_T_(1.)
, size_(std::distance(PointsBegin, PointsEnd)) , size_(std::distance(PointsBegin, PointsEnd))
, degree_(size_-1) , degree_(size_-1)
, bernstein_(spline::makeBernstein<num_t>((unsigned int)degree_)) , bernstein_(curve::makeBernstein<num_t>((unsigned int)degree_))
{ {
assert(bernstein_.size() == size_); assert(bernstein_.size() == size_);
In it(PointsBegin); In it(PointsBegin);
...@@ -91,7 +91,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> ...@@ -91,7 +91,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
, mult_T_(mult_T) , mult_T_(mult_T)
, size_(std::distance(PointsBegin, PointsEnd)) , size_(std::distance(PointsBegin, PointsEnd))
, degree_(size_-1) , degree_(size_-1)
, bernstein_(spline::makeBernstein<num_t>((unsigned int)degree_)) , bernstein_(curve::makeBernstein<num_t>((unsigned int)degree_))
{ {
assert(bernstein_.size() == size_); assert(bernstein_.size() == size_);
In it(PointsBegin); In it(PointsBegin);
...@@ -113,7 +113,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point> ...@@ -113,7 +113,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
, mult_T_(1.) , mult_T_(1.)
, size_(std::distance(PointsBegin, PointsEnd)+4) , size_(std::distance(PointsBegin, PointsEnd)+4)
, degree_(size_-1) , 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.)) if(Safe && (size_<1 || T_ <= 0.))
throw std::out_of_range("can't create bezier min bound is higher than max bound"); 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> ...@@ -364,6 +364,6 @@ struct bezier_curve : public curve_abc<Time, Numeric, Dim, Safe, Point>
return bezier_curve_t(ts.begin(), ts.end(),T); return bezier_curve_t(ts.begin(), ts.end(),T);
} }
}; };
} } // namespace curve
#endif //_CLASS_BEZIERCURVE #endif //_CLASS_BEZIERCURVE
...@@ -21,7 +21,7 @@ ...@@ -21,7 +21,7 @@
#include <iostream> #include <iostream>
namespace spline namespace curve
{ {
/// \brief Provides methods for converting a curve from a bernstein representation /// \brief Provides methods for converting a curve from a bernstein representation
/// to a polynom representation /// to a polynom representation
...@@ -67,6 +67,6 @@ Bezier from_polynom(const Polynom& polynom) ...@@ -67,6 +67,6 @@ Bezier from_polynom(const Polynom& polynom)
typedef Bezier::cit_point_t cit_point_t; typedef Bezier::cit_point_t cit_point_t;
typedef Bezier::bezier_curve_t bezier_curve_t; typedef Bezier::bezier_curve_t bezier_curve_t;
}*/ }*/
} } // namespace curve
#endif //_BEZIER_POLY_CONVERSION #endif //_BEZIER_POLY_CONVERSION
...@@ -20,7 +20,7 @@ ...@@ -20,7 +20,7 @@
#include <stdexcept> #include <stdexcept>
namespace spline namespace curve
{ {
/// \brief Creates coefficient vector of a cubic spline defined on the interval /// \brief Creates coefficient vector of a cubic spline defined on the interval
/// [tBegin, tEnd]. It follows the equation /// [tBegin, tEnd]. It follows the equation
...@@ -41,6 +41,6 @@ polynom<Time,Numeric,Dim,Safe,Point,T_Point> create_cubic(Point const& a, Point ...@@ -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); 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); return polynom<Time,Numeric,Dim,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), min, max);
} }
} } // namespace curve
#endif //_STRUCT_CUBICSPLINE #endif //_STRUCT_CUBICSPLINE
...@@ -16,7 +16,7 @@ ...@@ -16,7 +16,7 @@
#include <functional> #include <functional>
namespace spline namespace curve
{ {
/// \struct curve_abc /// \struct curve_abc
/// \brief Represents a curve of dimension Dim /// \brief Represents a curve of dimension Dim
...@@ -63,6 +63,6 @@ struct curve_abc : std::unary_function<Time, Point> ...@@ -63,6 +63,6 @@ struct curve_abc : std::unary_function<Time, Point>
/*Helpers*/ /*Helpers*/
}; };
} } // namespace curve
#endif //_STRUCT_CURVE_ABC #endif //_STRUCT_CURVE_ABC
...@@ -17,7 +17,7 @@ ...@@ -17,7 +17,7 @@
#include <functional> #include <functional>
#include <vector> #include <vector>
namespace spline namespace curve
{ {
template <typename Point> template <typename Point>
struct curve_constraints struct curve_constraints
...@@ -32,5 +32,5 @@ struct curve_constraints ...@@ -32,5 +32,5 @@ struct curve_constraints
point_t end_vel; point_t end_vel;
point_t end_acc; point_t end_acc;
}; };
} } // namespace curve
#endif //_CLASS_CUBICZEROVELACC #endif //_CLASS_CUBICZEROVELACC
...@@ -29,7 +29,7 @@ ...@@ -29,7 +29,7 @@
#include <functional> #include <functional>
#include <vector> #include <vector>
namespace spline namespace curve
{ {
/// \class ExactCubic /// \class ExactCubic
/// \brief Represents a set of cubic splines defining a continuous function /// \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> ...@@ -198,6 +198,6 @@ struct exact_cubic : public curve_abc<Time, Numeric, Dim, Safe, Point>
t_spline_t subSplines_; // const t_spline_t subSplines_; // const
/*Attributes*/ /*Attributes*/
}; };
} } // namespace curve
#endif //_CLASS_EXACTCUBIC #endif //_CLASS_EXACTCUBIC
...@@ -5,5 +5,5 @@ SET(${PROJECT_NAME}_HELPERS_HEADERS ...@@ -5,5 +5,5 @@ SET(${PROJECT_NAME}_HELPERS_HEADERS
INSTALL(FILES INSTALL(FILES
${${PROJECT_NAME}_HELPERS_HEADERS} ${${PROJECT_NAME}_HELPERS_HEADERS}
DESTINATION include/hpp/spline/helpers DESTINATION include/curve/helpers
) )
...@@ -20,9 +20,9 @@ ...@@ -20,9 +20,9 @@
#ifndef _CLASS_EFFECTORSPLINE #ifndef _CLASS_EFFECTORSPLINE
#define _CLASS_EFFECTORSPLINE #define _CLASS_EFFECTORSPLINE
#include "hpp/spline/spline_deriv_constraint.h" #include "curve/spline_deriv_constraint.h"
namespace spline namespace curve
{ {
namespace helpers namespace helpers
{ {
......
...@@ -20,11 +20,11 @@ ...@@ -20,11 +20,11 @@
#ifndef _CLASS_EFFECTOR_SPLINE_ROTATION #ifndef _CLASS_EFFECTOR_SPLINE_ROTATION
#define _CLASS_EFFECTOR_SPLINE_ROTATION #define _CLASS_EFFECTOR_SPLINE_ROTATION
#include "hpp/spline/helpers/effector_spline.h" #include "curve/helpers/effector_spline.h"
#include "hpp/spline/curve_abc.h" #include "curve/curve_abc.h"
#include <Eigen/Geometry> #include <Eigen/Geometry>
namespace spline namespace curve
{ {
namespace helpers namespace helpers
{ {
...@@ -252,5 +252,5 @@ class effector_spline_rotation ...@@ -252,5 +252,5 @@ class effector_spline_rotation
}; };
} // namespace helpers } // namespace helpers
} // namespace spline } // namespace curve
#endif //_CLASS_EFFECTOR_SPLINE_ROTATION #endif //_CLASS_EFFECTOR_SPLINE_ROTATION
...@@ -12,15 +12,15 @@ ...@@ -12,15 +12,15 @@
#ifndef _CLASS_SPLINEOPTIMIZER #ifndef _CLASS_SPLINEOPTIMIZER
#define _CLASS_SPLINEOPTIMIZER #define _CLASS_SPLINEOPTIMIZER
#include "spline/MathDefs.h" #include "curve/MathDefs.h"
#include "spline/exact_cubic.h" #include "curve/exact_cubic.h"
#include "mosek/mosek.h" #include "mosek/mosek.h"
#include <Eigen/SparseCore> #include <Eigen/SparseCore>
#include <utility> #include <utility>
namespace spline namespace curve
{ {
/// \class SplineOptimizer /// \class SplineOptimizer
/// \brief Mosek connection to produce optimized splines /// \brief Mosek connection to produce optimized splines
...@@ -516,5 +516,5 @@ inline exact_cubic<Time, Numeric, Dim, Safe, Point>* ...@@ -516,5 +516,5 @@ inline exact_cubic<Time, Numeric, Dim, Safe, Point>*
return res; return res;
} }
} // namespace spline } // namespace curve
#endif //_CLASS_SPLINEOPTIMIZER #endif //_CLASS_SPLINEOPTIMIZER
...@@ -23,7 +23,7 @@ ...@@ -23,7 +23,7 @@
#include <functional> #include <functional>
#include <stdexcept> #include <stdexcept>
namespace spline namespace curve
{ {
/// \class polynom /// \class polynom
/// \brief Represents a polynomf arbitrary order defined on the interval /// \brief Represents a polynomf arbitrary order defined on the interval
...@@ -199,6 +199,6 @@ struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point> ...@@ -199,6 +199,6 @@ struct polynom : public curve_abc<Time, Numeric, Dim, Safe, Point>
return res; return res;
} }
}; //class polynom }; //class polynom
} } // namespace curve
#endif //_STRUCT_POLYNOM #endif //_STRUCT_POLYNOM
...@@ -20,7 +20,7 @@ ...@@ -20,7 +20,7 @@
#include <stdexcept> #include <stdexcept>
namespace spline namespace curve
{ {
/// \brief Creates coefficient vector of a quintic spline defined on the interval /// \brief Creates coefficient vector of a quintic spline defined on the interval
/// [tBegin, tEnd]. It follows the equation /// [tBegin, tEnd]. It follows the equation
...@@ -43,6 +43,6 @@ polynom<Time,Numeric,Dim,Safe,Point,T_Point> create_quintic(Point const& a, Poin ...@@ -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); 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); return polynom<Time,Numeric,Dim,Safe,Point,T_Point>(coeffs.begin(),coeffs.end(), min, max);
} }
} } // namespace curve
#endif //_STRUCT_QUINTIC_SPLINE #endif //_STRUCT_QUINTIC_SPLINE
...@@ -28,7 +28,7 @@ ...@@ -28,7 +28,7 @@
#include <functional> #include <functional>
#include <vector> #include <vector>
namespace spline namespace curve
{ {
/// \class spline_deriv_constraint. /// \class spline_deriv_constraint.
/// \brief Represents a set of cubic splines defining a continuous function /// \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 ...@@ -137,6 +137,6 @@ struct spline_deriv_constraint : public exact_cubic<Time, Numeric, Dim, Safe, Po
private: private:
/* Constructors - destructors */ /* Constructors - destructors */
}; };
} } // namespace curve
#endif //_CLASS_CUBICZEROVELACC #endif //_CLASS_CUBICZEROVELACC
...@@ -3,7 +3,7 @@ STRING(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME}) ...@@ -3,7 +3,7 @@ STRING(REGEX REPLACE "-" "_" PY_NAME ${PROJECT_NAME})
ADD_REQUIRED_DEPENDENCY("eigenpy") ADD_REQUIRED_DEPENDENCY("eigenpy")
# Define the wrapper library that wraps our library # 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} ) #~ target_link_libraries( centroidal_dynamics ${Boost_LIBRARIES} ${PROJECT_NAME} )
# don't prepend wrapper library name with lib # don't prepend wrapper library name with lib
set_target_properties( ${PY_NAME} PROPERTIES PREFIX "" ) set_target_properties( ${PY_NAME} PROPERTIES PREFIX "" )
...@@ -18,4 +18,4 @@ INSTALL( ...@@ -18,4 +18,4 @@ INSTALL(
TARGETS ${PY_NAME} DESTINATION ${PYTHON_SITELIB} 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")
#include "hpp/spline/bezier_curve.h" #include "curve/bezier_curve.h"
#include "hpp/spline/polynom.h" #include "curve/polynom.h"
#include "hpp/spline/exact_cubic.h" #include "curve/exact_cubic.h"
#include "hpp/spline/spline_deriv_constraint.h" #include "curve/spline_deriv_constraint.h"
#include "hpp/spline/curve_constraint.h" #include "curve/curve_constraint.h"
#include "hpp/spline/bezier_polynom_conversion.h" #include "curve/bezier_polynom_conversion.h"
#include "hpp/spline/bernstein.h" #include "curve/bernstein.h"
#include <vector> #include <vector>
...@@ -30,31 +30,31 @@ typedef std::vector<Waypoint> T_Waypoint; ...@@ -30,31 +30,31 @@ typedef std::vector<Waypoint> T_Waypoint;
typedef std::pair<real, point6_t> Waypoint6; typedef std::pair<real, point6_t> Waypoint6;
typedef std::vector<Waypoint6> T_Waypoint6; typedef std::vector<Waypoint6> T_Waypoint6;
typedef spline::bezier_curve <real, real, 3, true, point_t> bezier_t; typedef curve::bezier_curve <real, real, 3, true, point_t> bezier3_t;
typedef spline::bezier_curve <real, real, 6, true, point6_t> bezier6_t; typedef curve::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 curve::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::exact_cubic <real, real, 3, true, point_t, t_point_t> exact_cubic_t;
typedef polynom_t::coeff_t coeff_t; typedef polynom_t::coeff_t coeff_t;
typedef std::pair<real, point_t> waypoint_t; typedef std::pair<real, point_t> waypoint_t;
typedef std::vector<waypoint_t, Eigen::aligned_allocator<point_t> > 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 curve::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 curve::curve_constraints<point_t> curve_constraints_t;
typedef spline::curve_constraints<point6_t> curve_constraints6_t; typedef curve::curve_constraints<point6_t> curve_constraints6_t;
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/ /*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bernstein_t) 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(bezier6_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(polynom_t) EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(polynom_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(exact_cubic_t) EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(exact_cubic_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints_t) EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_deriv_constraint_t) EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(spline_deriv_constraint_t)
namespace spline namespace curve
{ {
using namespace boost::python; using namespace boost::python;
template <typename PointList, typename T_Point> template <typename PointList, typename T_Point>
...@@ -81,21 +81,21 @@ Bezier* wrapBezierConstructorConstraintsTemplate(const PointList& array, const C ...@@ -81,21 +81,21 @@ Bezier* wrapBezierConstructorConstraintsTemplate(const PointList& array, const C
} }
/*3D constructors */ /*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 */ /*END 3D constructors */
/*6D constructors */ /*6D constructors */
...@@ -205,7 +205,7 @@ void set_end_acc(curve_constraints_t& c, const point_t& val) ...@@ -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**/ /** BEGIN eigenpy init**/
eigenpy::enableEigenPy(); eigenpy::enableEigenPy();
...@@ -241,21 +241,21 @@ BOOST_PYTHON_MODULE(hpp_spline) ...@@ -241,21 +241,21 @@ BOOST_PYTHON_MODULE(hpp_spline)
/** END bezier curve**/ /** END bezier curve**/
/** BEGIN bezier curve**/ /** BEGIN bezier curve**/
class_<bezier_t> class_<bezier3_t>
("bezier", no_init) ("bezier3", no_init)
.def("__init__", make_constructor(&wrapBezierConstructor)) .def("__init__", make_constructor(&wrapBezierConstructor))
.def("__init__", make_constructor(&wrapBezierConstructorBounds)) .def("__init__", make_constructor(&wrapBezierConstructorBounds))
.def("__init__", make_constructor(&wrapBezierConstructorConstraints)) .def("__init__", make_constructor(&wrapBezierConstructorConstraints))
.def("__init__", make_constructor(&wrapBezierConstructorBoundsConstraints)) .def("__init__", make_constructor(&wrapBezierConstructorBoundsConstraints))
.def("min", &bezier_t::min) .def("min", &bezier3_t::min)
.def("max", &bezier_t::max) .def("max", &bezier3_t::max)
.def("__call__", &bezier_t::operator()) .def("__call__", &bezier3_t::operator())
.def("derivate", &bezier_t::derivate) .def("derivate", &bezier3_t::derivate)
.def("compute_derivate", &bezier_t::compute_derivate) .def("compute_derivate", &bezier3_t::compute_derivate)
.def("compute_primitive", &bezier_t::compute_primitive) .def("compute_primitive", &bezier3_t::compute_primitive)
.def("waypoints", &wayPointsToList<bezier_t,3>) .def("waypoints", &wayPointsToList<bezier3_t,3>)
.def_readonly("degree", &bezier_t::degree_) .def_readonly("degree", &bezier3_t::degree_)
.def_readonly("nbWaypoints", &bezier_t::size_) .def_readonly("nbWaypoints", &bezier3_t::size_)
; ;
/** END bezier curve**/ /** END bezier curve**/
...@@ -314,10 +314,10 @@ BOOST_PYTHON_MODULE(hpp_spline) ...@@ -314,10 +314,10 @@ BOOST_PYTHON_MODULE(hpp_spline)
/** END bernstein polynom**/ /** END bernstein polynom**/
/** BEGIN Bezier to polynom conversion**/ /** 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**/ /** END Bezier to polynom conversion**/
} }
} // namespace spline } // namespace curve
...@@ -6,19 +6,26 @@ from numpy import matrix ...@@ -6,19 +6,26 @@ from numpy import matrix
from numpy.linalg import norm from numpy.linalg import norm
from numpy.testing import assert_allclose 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): class TestCurve(unittest.TestCase):
def test_spline(self): def test_curve(self):
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).T waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).T
waypoints6 = matrix([[1., 2., 3., 7., 5., 5.], [4., 5., 6., 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 time_waypoints = matrix([0., 1.]).T
# testing bezier curve # Test : Bezier3/Bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraints, bernstein
a = bezier6(waypoints6) # Done : Bezier3/Bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraints
a = bezier(waypoints, 3.) # To do ? Bernstein
# TESTING BEZIER CURVE
# - Functions : constructor, min, max, derivate,compute_derivate, compute_primitive
# - Variables : degree, nbWayPoints
# Create bezier6 and bezier3