diff --git a/include/curves/curve_constraint.h b/include/curves/curve_constraint.h
index d43323faf4630ea5c1fa089212d7c98cd57c1dbd..f218f6f37c6dce9a4f19efebd95340c2b8822dde 100644
--- a/include/curves/curve_constraint.h
+++ b/include/curves/curve_constraint.h
@@ -19,12 +19,11 @@
namespace curves
{
- template <typename Point, std::size_t Dim=3>
+ template <typename Point>
struct curve_constraints
{
typedef Point point_t;
- curve_constraints():
- init_vel(point_t::Zero(Dim)),init_acc(init_vel),end_vel(init_vel),end_acc(init_vel){}
+ curve_constraints(){}
~curve_constraints(){}
diff --git a/include/curves/exact_cubic.h b/include/curves/exact_cubic.h
index 172f2b7f04459560b5e9c8abc2babbde71055455..fa76cd4bdde4a5faae0b8196832c4bf9ead12ddc 100644
--- a/include/curves/exact_cubic.h
+++ b/include/curves/exact_cubic.h
@@ -37,7 +37,7 @@ namespace curves
/// \brief Represents a set of cubic splines defining a continuous function
/// crossing each of the waypoint given in its initialization.
///
- template<typename Time= double, typename Numeric=Time, std::size_t Dim=3, bool Safe=false,
+ template<typename Time= double, typename Numeric=Time, bool Safe=false,
typename Point= Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
typename T_Point =std::vector<Point,Eigen::aligned_allocator<Point> >,
typename SplineBase=polynomial<Time, Numeric, Safe, Point, T_Point> >
@@ -53,9 +53,9 @@ namespace curves
typedef typename std::vector<spline_t> t_spline_t;
typedef typename t_spline_t::iterator it_spline_t;
typedef typename t_spline_t::const_iterator cit_spline_t;
- typedef curve_constraints<Point, Dim> spline_constraints;
+ typedef curve_constraints<Point> spline_constraints;
- typedef exact_cubic<Time, Numeric, Dim, Safe, Point, T_Point, SplineBase> exact_cubic_t;
+ typedef exact_cubic<Time, Numeric, Safe, Point, T_Point, SplineBase> exact_cubic_t;
typedef piecewise_curve<Time, Numeric, Safe, Point, T_Point, SplineBase> piecewise_curve_t;
/* Constructors - destructors */
@@ -119,7 +119,8 @@ namespace curves
template<typename In>
t_spline_t computeWayPoints(In wayPointsBegin, In wayPointsEnd) const
{
- std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd));
+ const std::size_t dim = wayPointsBegin->second.size();
+ const std::size_t size = std::distance(wayPointsBegin, wayPointsEnd);
if(Safe && size < 1)
{
throw std::length_error("size of waypoints must be superior to 0") ; // TODO
@@ -132,11 +133,11 @@ namespace curves
MatrixX h4 = MatrixX::Zero(size, size);
MatrixX h5 = MatrixX::Zero(size, size);
MatrixX h6 = MatrixX::Zero(size, size);
- MatrixX a = MatrixX::Zero(size, Dim);
- MatrixX b = MatrixX::Zero(size, Dim);
- MatrixX c = MatrixX::Zero(size, Dim);
- MatrixX d = MatrixX::Zero(size, Dim);
- MatrixX x = MatrixX::Zero(size, Dim);
+ MatrixX a = MatrixX::Zero(size, dim);
+ MatrixX b = MatrixX::Zero(size, dim);
+ MatrixX c = MatrixX::Zero(size, dim);
+ MatrixX d = MatrixX::Zero(size, dim);
+ MatrixX x = MatrixX::Zero(size, dim);
In it(wayPointsBegin), next(wayPointsBegin);
++next;
// Fill the matrices H as specified in the paper.
@@ -226,9 +227,10 @@ namespace curves
template<typename In>
void compute_end_spline(In wayPointsBegin, In wayPointsNext, spline_constraints& constraints, t_spline_t& subSplines) const
{
+ const std::size_t dim = wayPointsBegin->second.size();
const point_t& a0 = wayPointsBegin->second, a1 = wayPointsNext->second;
const point_t& b0 = constraints.init_vel, b1 = constraints.end_vel,
- c0 = constraints.init_acc / 2., c1 = constraints.end_acc;
+ c0 = constraints.init_acc / 2., c1 = constraints.end_acc;
const num_t& init_t = wayPointsBegin->first, end_t = wayPointsNext->first;
const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt, dt_4 = dt_3 * dt, dt_5 = dt_4 * dt;
//solving a system of four linear eq with 4 unknows: d0, e0
@@ -239,7 +241,7 @@ namespace curves
const num_t x_e_0 = dt_4, x_e_1 = 4*dt_3, x_e_2 = 12*dt_2;
const num_t x_f_0 = dt_5, x_f_1 = 5*dt_4, x_f_2 = 20*dt_3;
point_t d, e, f;
- MatrixX rhs = MatrixX::Zero(3,Dim);
+ MatrixX rhs = MatrixX::Zero(3,dim);
rhs.row(0) = alpha_0; rhs.row(1) = alpha_1; rhs.row(2) = alpha_2;
Matrix3 eq = Matrix3::Zero(3,3);
eq(0,0) = x_d_0; eq(0,1) = x_e_0; eq(0,2) = x_f_0;
diff --git a/include/curves/helpers/effector_spline.h b/include/curves/helpers/effector_spline.h
index 7cf61b2fdbb876bd519ea88a4d2b9a830fed9ae7..bc6bb44231b76750d90f5655709ecdaef56296e0 100644
--- a/include/curves/helpers/effector_spline.h
+++ b/include/curves/helpers/effector_spline.h
@@ -28,11 +28,11 @@ namespace curves
{
typedef double Numeric;
typedef double Time;
- typedef Eigen::Matrix<Numeric, 3, 1> Point;
+ typedef Eigen::Matrix<Numeric, Eigen::Dynamic, 1> Point;
typedef std::vector<Point,Eigen::aligned_allocator<Point> > T_Point;
typedef std::pair<double, Point> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
- typedef exact_cubic<Time, Numeric, 3, true, Point, T_Point> exact_cubic_t;
+ typedef exact_cubic<Time, Numeric, true, Point, T_Point> exact_cubic_t;
typedef exact_cubic_t::spline_constraints spline_constraints_t;
typedef exact_cubic_t::t_spline_t t_spline_t;
typedef exact_cubic_t::spline_t spline_t;
@@ -68,6 +68,8 @@ namespace curves
spline_constraints_t compute_required_offset_velocity_acceleration(const spline_t& end_spline, const Time /*time_offset*/)
{
spline_constraints_t constraints;
+ constraints.init_acc = Point::Zero(end_spline.dim());
+ constraints.init_vel = Point::Zero(end_spline.dim());
constraints.end_acc = end_spline.derivate(end_spline.min(),2);
constraints.end_vel = end_spline.derivate(end_spline.min(),1);
return constraints;
diff --git a/include/curves/helpers/effector_spline_rotation.h b/include/curves/helpers/effector_spline_rotation.h
index a733c4b610b8d8a9ee01ed78080fb5082fa75d78..1e34c79b4fe7d49e9991acfabc59132089a289ea 100644
--- a/include/curves/helpers/effector_spline_rotation.h
+++ b/include/curves/helpers/effector_spline_rotation.h
@@ -36,7 +36,7 @@ namespace curves
typedef std::pair<Numeric, quat_t > waypoint_quat_t;
typedef std::vector<waypoint_quat_t> t_waypoint_quat_t;
typedef Eigen::Matrix<Numeric, 1, 1> point_one_dim_t;
- typedef exact_cubic <Numeric, Numeric, 1, false, point_one_dim_t> exact_cubic_constraint_one_dim;
+ typedef exact_cubic <Numeric, Numeric, false, point_one_dim_t> exact_cubic_constraint_one_dim;
typedef std::pair<Numeric, point_one_dim_t > waypoint_one_dim_t;
typedef std::vector<waypoint_one_dim_t> t_waypoint_one_dim_t;
@@ -102,7 +102,7 @@ namespace curves
}; // End class rotation_spline
- typedef exact_cubic<Time, Numeric, 4, false, quat_t, std::vector<quat_t,Eigen::aligned_allocator<quat_t> >, rotation_spline> exact_cubic_quat_t;
+ typedef exact_cubic<Time, Numeric, false, quat_t, std::vector<quat_t,Eigen::aligned_allocator<quat_t> >, rotation_spline> exact_cubic_quat_t;
/// \class effector_spline_rotation.
/// \brief Represents a trajectory for and end effector.
diff --git a/python/curves_python.cpp b/python/curves_python.cpp
index d078eeb471645658fff288b07042360df29530f1..1e4412143674953e9f77c0800c0a41fa00b6432a 100644
--- a/python/curves_python.cpp
+++ b/python/curves_python.cpp
@@ -23,33 +23,18 @@
using namespace curves;
typedef double real;
typedef Eigen::VectorXd time_waypoints_t;
-// 3D
-typedef Eigen::Vector3d point3_t;
-typedef Eigen::Matrix<double, 3, 1, 0, 3, 1> ret_point3_t;
-typedef std::pair<point3_t, point3_t> pair_point3_tangent_t;
-typedef Eigen::Matrix<real, 3, Eigen::Dynamic> point3_list_t;
-typedef std::vector<point3_t,Eigen::aligned_allocator<point3_t> > t_point3_t;
-typedef std::vector<pair_point3_tangent_t,Eigen::aligned_allocator<pair_point3_tangent_t> > t_pair_point3_tangent_t;
-typedef curves::curve_constraints<point3_t> curve_constraints3_t;
-// XD
+
typedef Eigen::VectorXd pointX_t;
typedef Eigen::Matrix<double, Eigen::Dynamic, 1, 0, Eigen::Dynamic, 1> ret_pointX_t;
typedef std::pair<pointX_t, pointX_t> pair_pointX_tangent_t;
typedef Eigen::MatrixXd pointX_list_t;
-typedef std::vector<pointX_t,Eigen::aligned_allocator<point3_t> > t_pointX_t;
+typedef std::vector<pointX_t,Eigen::aligned_allocator<pointX_t> > t_pointX_t;
typedef std::vector<pair_pointX_tangent_t,Eigen::aligned_allocator<pair_pointX_tangent_t> > t_pair_pointX_tangent_t;
typedef curves::curve_constraints<pointX_t> curve_constraints_t;
-// Else
-typedef std::pair<real, point3_t> Waypoint;
-typedef std::vector<Waypoint> T_Waypoint;
-typedef std::vector<Waypoint6> T_Waypoint6;
-
-// 3D
-typedef curves::exact_cubic <real, real, 3, true, point3_t, t_point3_t> exact_cubic3_t;
-typedef std::pair<real, point3_t> waypoint3_t;
-typedef std::vector<waypoint3_t, Eigen::aligned_allocator<point3_t> > t_waypoint3_t;
+typedef std::pair<real, pointX_t> waypoint_t;
+typedef std::vector<waypoint_t> t_waypoint_t;
-// Dynamic dim
+// Curves
typedef curves::cubic_hermite_spline <real, real, true, pointX_t> cubic_hermite_spline_t;
typedef curves::bezier_curve <real, real, true, pointX_t> bezier_t;
typedef curves::polynomial <real, real, true, pointX_t, t_pointX_t> polynomial_t;
@@ -57,12 +42,12 @@ typedef polynomial_t::coeff_t coeff_t;
typedef curves::piecewise_curve <real, real, true, pointX_t, t_pointX_t, polynomial_t> piecewise_polynomial_curve_t;
typedef curves::piecewise_curve <real, real, true, pointX_t, t_pointX_t, bezier_t> piecewise_bezier_curve_t;
typedef curves::piecewise_curve <real, real, true, pointX_t, t_pointX_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
+typedef curves::exact_cubic <real, real, true, pointX_t, t_pointX_t> exact_cubic_t;
typedef curves::Bern<double> bernstein_t;
-/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
+/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bernstein_t)
-
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(cubic_hermite_spline_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(polynomial_t)
@@ -70,9 +55,7 @@ EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(piecewise_polynomial_curve_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(piecewise_bezier_curve_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(piecewise_cubic_hermite_curve_t)
-
-EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(exact_cubic3_t)
-EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curve_constraints3_t)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(exact_cubic_t)
namespace curves
{
@@ -183,9 +166,9 @@ namespace curves
/* end wrap piecewise polynomial curve */
/* Wrap exact cubic spline */
- t_waypoint3_t getWayPoints(const coeff_t& array, const time_waypoints_t& time_wp)
+ t_waypoint_t getWayPoints(const coeff_t& array, const time_waypoints_t& time_wp)
{
- t_waypoint3_t res;
+ t_waypoint_t res;
for(int i =0;i<array.cols();++i)
{
res.push_back(std::make_pair(time_wp(i), array.col(i)));
@@ -193,96 +176,56 @@ namespace curves
return res;
}
- exact_cubic3_t* wrapExactCubic3Constructor(const coeff_t& array, const time_waypoints_t& time_wp)
- {
- t_waypoint3_t wps = getWayPoints(array, time_wp);
- return new exact_cubic3_t(wps.begin(), wps.end());
- }
-
- exact_cubic3_t* wrapExactCubic3ConstructorConstraint(const coeff_t& array, const time_waypoints_t& time_wp,
- const curve_constraints3_t& constraints)
- {
- t_waypoint3_t wps = getWayPoints(array, time_wp);
- return new exact_cubic3_t(wps.begin(), wps.end(), constraints);
- }
-
- /// For constraints 3D
- point3_t get_init_vel(const curve_constraints3_t& c)
- {
- return c.init_vel;
- }
-
- point3_t get_init_acc(const curve_constraints3_t& c)
- {
- return c.init_acc;
- }
-
- point3_t get_end_vel(const curve_constraints3_t& c)
- {
- return c.end_vel;
- }
-
- point3_t get_end_acc(const curve_constraints3_t& c)
- {
- return c.end_acc;
- }
-
- void set_init_vel(curve_constraints3_t& c, const point3_t& val)
+ exact_cubic_t* wrapExactCubicConstructor(const coeff_t& array, const time_waypoints_t& time_wp)
{
- c.init_vel = val;
- }
-
- void set_init_acc(curve_constraints3_t& c, const point3_t& val)
- {
- c.init_acc = val;
+ t_waypoint_t wps = getWayPoints(array, time_wp);
+ return new exact_cubic_t(wps.begin(), wps.end());
}
- void set_end_vel(curve_constraints3_t& c, const point3_t& val)
+ exact_cubic_t* wrapExactCubicConstructorConstraint(const coeff_t& array, const time_waypoints_t& time_wp,
+ const curve_constraints_t& constraints)
{
- c.end_vel = val;
+ t_waypoint_t wps = getWayPoints(array, time_wp);
+ return new exact_cubic_t(wps.begin(), wps.end(), constraints);
}
- void set_end_acc(curve_constraints3_t& c, const point3_t& val)
- {
- c.end_acc = val;
- }
/// For constraints XD
- point3_t get_init_velX(const curve_constraints_t& c)
+ point_t get_init_vel(const curve_constraints_t& c)
{
return c.init_vel;
}
- point3_t get_init_accX(const curve_constraints_t& c)
+ point_t get_init_acc(const curve_constraints_t& c)
{
return c.init_acc;
}
- point3_t get_end_velX(const curve_constraints_t& c)
+ point_t get_end_vel(const curve_constraints_t& c)
{
return c.end_vel;
}
- point3_t get_end_accX(const curve_constraints_t& c)
+ point_t get_end_acc(const curve_constraints_t& c)
{
return c.end_acc;
}
- void set_init_velX(curve_constraints_t& c, const point_t& val)
+ void set_init_vel(curve_constraints_t& c, const point_t& val)
{
c.init_vel = val;
}
- void set_init_accX(curve_constraints_t& c, const point_t& val)
+ void set_init_acc(curve_constraints_t& c, const point_t& val)
{
c.init_acc = val;
}
- void set_end_velX(curve_constraints_t& c, const point_t& val)
+ void set_end_vel(curve_constraints_t& c, const point_t& val)
{
c.end_vel = val;
}
- void set_end_accX(curve_constraints_t& c, const point_t& val)
+ void set_end_acc(curve_constraints_t& c, const point_t& val)
{
c.end_acc = val;
}
@@ -297,8 +240,6 @@ namespace curves
eigenpy::enableEigenPy();
eigenpy::enableEigenPySpecific<pointX_t,pointX_t>();
eigenpy::enableEigenPySpecific<pointX_list_t,pointX_list_t>();
- eigenpy::enableEigenPySpecific<point3_t,point3_t>();
- eigenpy::enableEigenPySpecific<point3_list_t,point3_list_t>();
eigenpy::enableEigenPySpecific<coeff_t,coeff_t>();
/*eigenpy::exposeAngleAxis();
eigenpy::exposeQuaternion();*/
@@ -440,24 +381,24 @@ namespace curves
;
/** END piecewise curve function **/
/** BEGIN exact_cubic curve**/
- class_<exact_cubic3_t>
- ("exact_cubic3", init<>())
- .def("__init__", make_constructor(&wrapExactCubic3Constructor))
- .def("__init__", make_constructor(&wrapExactCubic3ConstructorConstraint))
- .def("min", &exact_cubic3_t::min)
- .def("max", &exact_cubic3_t::max)
- .def("dim", &exact_cubic3_t::dim)
- .def("__call__", &exact_cubic3_t::operator())
- .def("derivate", &exact_cubic3_t::derivate)
- .def("getNumberSplines", &exact_cubic3_t::getNumberSplines)
- .def("getSplineAt", &exact_cubic3_t::getSplineAt)
- .def("saveAsText", &exact_cubic3_t::saveAsText<exact_cubic3_t>,bp::args("filename"),"Saves *this inside a text file.")
- .def("loadFromText",&exact_cubic3_t::loadFromText<exact_cubic3_t>,bp::args("filename"),"Loads *this from a text file.")
- .def("saveAsXML",&exact_cubic3_t::saveAsXML<exact_cubic3_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
- .def("loadFromXML",&exact_cubic3_t::loadFromXML<exact_cubic3_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
- .def("saveAsBinary",&exact_cubic3_t::saveAsBinary<exact_cubic3_t>,bp::args("filename"),"Saves *this inside a binary file.")
- .def("loadFromBinary",&exact_cubic3_t::loadFromBinary<exact_cubic3_t>,bp::args("filename"),"Loads *this from a binary file.")
- //.def(SerializableVisitor<exact_cubic3_t>())
+ class_<exact_cubic_t>
+ ("exact_cubic", init<>())
+ .def("__init__", make_constructor(&wrapExactCubicConstructor))
+ .def("__init__", make_constructor(&wrapExactCubicConstructorConstraint))
+ .def("min", &exact_cubic_t::min)
+ .def("max", &exact_cubic_t::max)
+ .def("dim", &exact_cubic_t::dim)
+ .def("__call__", &exact_cubic_t::operator())
+ .def("derivate", &exact_cubic_t::derivate)
+ .def("getNumberSplines", &exact_cubic_t::getNumberSplines)
+ .def("getSplineAt", &exact_cubic_t::getSplineAt)
+ .def("saveAsText", &exact_cubic_t::saveAsText<exact_cubic_t>,bp::args("filename"),"Saves *this inside a text file.")
+ .def("loadFromText",&exact_cubic_t::loadFromText<exact_cubic_t>,bp::args("filename"),"Loads *this from a text file.")
+ .def("saveAsXML",&exact_cubic_t::saveAsXML<exact_cubic_t>,bp::args("filename","tag_name"),"Saves *this inside a XML file.")
+ .def("loadFromXML",&exact_cubic_t::loadFromXML<exact_cubic_t>,bp::args("filename","tag_name"),"Loads *this from a XML file.")
+ .def("saveAsBinary",&exact_cubic_t::saveAsBinary<exact_cubic_t>,bp::args("filename"),"Saves *this inside a binary file.")
+ .def("loadFromBinary",&exact_cubic_t::loadFromBinary<exact_cubic_t>,bp::args("filename"),"Loads *this from a binary file.")
+ //.def(SerializableVisitor<exact_cubic_t>())
;
/** END exact_cubic curve**/
/** BEGIN cubic_hermite_spline **/
@@ -479,23 +420,14 @@ namespace curves
;
/** END cubic_hermite_spline **/
/** BEGIN curve constraints**/
- class_<curve_constraints3_t>
- ("curve_constraints3", init<>())
+ 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 curve constraints**/
- class_<curve_constraints_t>
- ("curve_constraints", init<>())
- .add_property("init_vel", &get_init_velX, &set_init_velX)
- .add_property("init_acc", &get_init_accX, &set_init_accX)
- .add_property("end_vel", &get_end_velX, &set_end_velX)
- .add_property("end_acc", &get_end_accX, &set_end_accX)
- ;
- /** END curve constraints**/
/** BEGIN bernstein polynomial**/
class_<bernstein_t>
("bernstein", init<const unsigned int, const unsigned int>())
diff --git a/python/test/test.py b/python/test/test.py
index 4e54ae414d0319f6cf8ecec799685ffbf6184b6b..182984b629f9ec2b2ec7fa2c1ba29e6d161d1aa9 100644
--- a/python/test/test.py
+++ b/python/test/test.py
@@ -8,7 +8,7 @@ from numpy.linalg import norm
from curves import (bezier_from_hermite, bezier_from_polynomial, hermite_from_polynomial,
hermite_from_bezier, polynomial_from_hermite, polynomial_from_bezier,
- cubic_hermite_spline, curve_constraints3,curve_constraints, exact_cubic3, bezier,
+ cubic_hermite_spline, curve_constraints, exact_cubic, bezier,
piecewise_bezier_curve, piecewise_cubic_hermite_curve,
piecewise_polynomial_curve, polynomial
)
@@ -21,6 +21,7 @@ class TestCurves(unittest.TestCase):
# return
def test_bezier(self):
+ print("test_bezier")
# To test :
# - Functions : constructor, min, max, derivate,compute_derivate, compute_primitive
# - Variables : degree, nbWayPoints
@@ -103,6 +104,7 @@ class TestCurves(unittest.TestCase):
return
def test_polynomial(self):
+ print("test_polynomial")
# To test :
# - Functions : constructor, min, max, derivate, serialize, deserialize
waypoints_0 = matrix([[0.,0.,0.], [0.,0.,0.]]).transpose()
@@ -125,6 +127,7 @@ class TestCurves(unittest.TestCase):
return
def test_cubic_hermite_spline(self):
+ print("test_cubic_hermite_spline")
points = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
tangents = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
time_points = matrix([0., 1.]).transpose()
@@ -159,6 +162,7 @@ class TestCurves(unittest.TestCase):
return
def test_piecewise_polynomial_curve(self):
+ print("test_piecewise_polynomial_curve")
# To test :
# - Functions : constructor, min, max, derivate, add_curve, is_continuous, serialize, deserialize
waypoints0 = matrix([[0., 0., 0.]]).transpose()
@@ -185,6 +189,7 @@ class TestCurves(unittest.TestCase):
return
def test_piecewise_bezier_curve(self):
+ print("test_piecewise_bezier_curve")
# To test :
# - Functions : constructor, min, max, derivate, add_curve, is_continuous
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
@@ -208,6 +213,7 @@ class TestCurves(unittest.TestCase):
return
def test_piecewise_cubic_hermite_curve(self):
+ print("test_piecewise_cubic_hermite_curve")
# To test :
# - Functions : constructor, min, max, derivate, add_curve, is_continuous
points = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
@@ -234,12 +240,12 @@ class TestCurves(unittest.TestCase):
return
def test_exact_cubic(self):
- '''
+ print("test_exact_cubic")
# To test :
# - Functions : constructor, min, max, derivate, getNumberSplines, getSplineAt
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
time_waypoints = matrix([0., 1.]).transpose()
- a = exact_cubic3(waypoints, time_waypoints)
+ a = exact_cubic(waypoints, time_waypoints)
a.min()
a.max()
a(0.4)
@@ -249,32 +255,33 @@ class TestCurves(unittest.TestCase):
a.getSplineAt(0)
# Test serialization
a.saveAsText("serialization_pc.test")
- b = exact_cubic3()
+ b = exact_cubic()
b.loadFromText("serialization_pc.test")
self.assertTrue((a(0.4) == b(0.4)).all())
os.remove("serialization_pc.test")
- '''
return
def test_exact_cubic_constraint(self):
+ print("test_exact_cubic_constraint")
# To test :
# - Functions : constructor, min, max, derivate
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
time_waypoints = matrix([0., 1.]).transpose()
- c = curve_constraints3()
- c.init_vel
- c.end_vel
- c.init_acc
- c.end_acc
+ c = curve_constraints()
c.init_vel = matrix([0., 1., 1.]).transpose()
c.end_vel = matrix([0., 1., 1.]).transpose()
c.init_acc = matrix([0., 1., 1.]).transpose()
c.end_acc = matrix([0., 1., 1.]).transpose()
- a = exact_cubic3(waypoints, time_waypoints)
- a = exact_cubic3(waypoints, time_waypoints, c)
+ c.init_vel
+ c.end_vel
+ c.init_acc
+ c.end_acc
+ a = exact_cubic(waypoints, time_waypoints)
+ a = exact_cubic(waypoints, time_waypoints, c)
return
def test_conversion_curves(self):
+ print("test_conversion_curves")
__EPS = 1e-6
waypoints = matrix([[1., 2., 3.], [4., 5., 6.]]).transpose()
a = bezier(waypoints)
diff --git a/tests/Main.cpp b/tests/Main.cpp
index f128222ec5f6e978a99b8daeeeeb76da68e6d133..2bf56c4497ff53022875a81bc53c4e69c01bd32c 100644
--- a/tests/Main.cpp
+++ b/tests/Main.cpp
@@ -18,14 +18,13 @@ using namespace std;
namespace curves
{
typedef Eigen::Vector3d point_t;
- typedef Eigen::Vector3d tangent_t;
typedef std::vector<point_t,Eigen::aligned_allocator<point_t> > t_point_t;
typedef curve_abc <double, double, true, point_t> curve_abc_t;
typedef polynomial <double, double, true, point_t, t_point_t> polynomial_t;
- typedef exact_cubic <double, double, 3, true, point_t> exact_cubic_t;
+ typedef exact_cubic <double, double, true, point_t> exact_cubic_t;
+ typedef exact_cubic <double, double, true, Eigen::Matrix<double,1,1> > exact_cubic_one;
typedef bezier_curve <double, double, true, point_t> bezier_curve_t;
typedef cubic_hermite_spline <double, double, true, point_t> cubic_hermite_spline_t;
- typedef exact_cubic <double, double, 1, true, Eigen::Matrix<double,1,1> > exact_cubic_one;
typedef piecewise_curve <double, double, true, point_t, t_point_t, polynomial_t> piecewise_polynomial_curve_t;
typedef piecewise_curve <double, double, true, point_t, t_point_t, bezier_curve_t> piecewise_bezier_curve_t;
typedef piecewise_curve <double, double, true, point_t, t_point_t, cubic_hermite_spline_t> piecewise_cubic_hermite_curve_t;
@@ -35,7 +34,7 @@ namespace curves
typedef Eigen::Matrix<double,1,1> point_one;
typedef std::pair<double, point_one> WaypointOne;
typedef std::vector<WaypointOne> T_WaypointOne;
- typedef std::pair<point_t, tangent_t> pair_point_tangent_t;
+ typedef std::pair<point_t, point_t> pair_point_tangent_t;
typedef std::vector<pair_point_tangent_t,Eigen::aligned_allocator<pair_point_tangent_t> > t_pair_point_tangent_t;
const double margin = 1e-3;
@@ -630,12 +629,6 @@ void CheckWayPointConstraint(const std::string& errmsg, const double step, const
}
}
-void CheckDerivative(const std::string& errmsg, const double eval_point, const std::size_t order, const point_t& target, const exact_cubic_t* curve, bool& error )
-{
- point_t res1 = curve->derivate(eval_point,order);
- ComparePoints(target, res1, errmsg, error);
-}
-
void ExactCubicPointsCrossedTest(bool& error)
{
@@ -667,10 +660,10 @@ void ExactCubicVelocityConstraintsTest(bool& error)
CheckWayPointConstraint(errmsg, 0.2, waypoints, &exactCubic, error);
std::string errmsg3("Error in ExactCubicVelocityConstraintsTest (2); while checking derivative (expected / obtained)");
// now check derivatives
- CheckDerivative(errmsg3,0,1,constraints.init_vel,&exactCubic, error);
- CheckDerivative(errmsg3,1,1,constraints.end_vel,&exactCubic, error);
- CheckDerivative(errmsg3,0,2,constraints.init_acc,&exactCubic, error);
- CheckDerivative(errmsg3,1,2,constraints.end_acc,&exactCubic, error);
+ ComparePoints(constraints.init_vel, exactCubic.derivate(0,1), errmsg3, error);
+ ComparePoints(constraints.end_vel, exactCubic.derivate(1,1), errmsg3, error);
+ ComparePoints(constraints.init_acc, exactCubic.derivate(0,2), errmsg3, error);
+ ComparePoints(constraints.end_acc, exactCubic.derivate(1,2), errmsg3, error);
constraints.end_vel = point_t(1,2,3);
constraints.init_vel = point_t(-1,-2,-3);
constraints.end_acc = point_t(4,5,6);
@@ -680,13 +673,14 @@ void ExactCubicVelocityConstraintsTest(bool& error)
CheckWayPointConstraint(errmsg2, 0.2, waypoints, &exactCubic2, error);
std::string errmsg4("Error in ExactCubicVelocityConstraintsTest (4); while checking derivative (expected / obtained)");
// now check derivatives
- CheckDerivative(errmsg4,0,1,constraints.init_vel,&exactCubic2, error);
- CheckDerivative(errmsg4,1,1,constraints.end_vel ,&exactCubic2, error);
- CheckDerivative(errmsg4,0,2,constraints.init_acc,&exactCubic2, error);
- CheckDerivative(errmsg4,1,2,constraints.end_acc ,&exactCubic2, error);
+ ComparePoints(constraints.init_vel, exactCubic2.derivate(0,1), errmsg4, error);
+ ComparePoints(constraints.end_vel, exactCubic2.derivate(1,1), errmsg4, error);
+ ComparePoints(constraints.init_acc, exactCubic2.derivate(0,2), errmsg4, error);
+ ComparePoints(constraints.end_acc, exactCubic2.derivate(1,2), errmsg4, error);
}
-void CheckPointOnline(const std::string& errmsg, const point_t& A, const point_t& B, const double target, const exact_cubic_t* curve, bool& error )
+template<typename CurveType>
+void CheckPointOnline(const std::string& errmsg, const point_t& A, const point_t& B, const double target, const CurveType* curve, bool& error )
{
point_t res1 = curve->operator ()(target);
point_t ar =(res1-A); ar.normalize();
@@ -721,21 +715,20 @@ void EffectorTrajectoryTest(bool& error)
ComparePoints(off1, (*eff_traj)(1), errmsg, error);
ComparePoints(off2, (*eff_traj)(9.5), errmsg, error);
ComparePoints(end , (*eff_traj)(10), errmsg, error);
- //then check offset at start / goal positions
// now check derivatives
- CheckDerivative(errmsg2,0,1,zero,eff_traj, error);
- CheckDerivative(errmsg2,10,1,zero ,eff_traj, error);
- CheckDerivative(errmsg2,0,2,zero,eff_traj, error);
- CheckDerivative(errmsg2,10,2,zero ,eff_traj, error);
+ ComparePoints(zero, (*eff_traj).derivate(0,1), errmsg2, error);
+ ComparePoints(zero, (*eff_traj).derivate(10,1), errmsg2, error);
+ ComparePoints(zero, (*eff_traj).derivate(0,2), errmsg2, error);
+ ComparePoints(zero, (*eff_traj).derivate(10,2), errmsg2, error);
//check that end and init splines are line
std::string errmsg3("Error in EffectorTrajectoryTest; while checking that init/end splines are line (point A/ point B, time value / point obtained) \n");
for(double i = 0.1; i<1; i+=0.1)
{
- CheckPointOnline(errmsg3,(*eff_traj)(0),(*eff_traj)(1),i,eff_traj,error);
+ CheckPointOnline<helpers::exact_cubic_t>(errmsg3,(*eff_traj)(0),(*eff_traj)(1),i,eff_traj,error);
}
for(double i = 9.981; i<10; i+=0.002)
{
- CheckPointOnline(errmsg3,(*eff_traj)(9.5),(*eff_traj)(10),i,eff_traj,error);
+ CheckPointOnline<helpers::exact_cubic_t>(errmsg3,(*eff_traj)(9.5),(*eff_traj)(10),i,eff_traj,error);
}
delete eff_traj;
}
@@ -989,41 +982,41 @@ void BezierSplitCurve(bool& error)
/* cubic hermite spline function test */
void CubicHermitePairsPositionDerivativeTest(bool& error)
{
- std::string errmsg1("in Cubic Hermite 2 pairs (pos,vel), Error While checking that given wayPoints are crossed (expected / obtained) : ");
std::string errmsg2("in Cubic Hermite 2 points, Error While checking value of point on curve : ");
std::string errmsg3("in Cubic Hermite 2 points, Error While checking value of tangent on curve : ");
std::vector< pair_point_tangent_t > control_points;
point_t res1;
-
point_t p0(0.,0.,0.);
point_t p1(1.,2.,3.);
point_t p2(4.,4.,4.);
-
- tangent_t t0(0.5,0.5,0.5);
- tangent_t t1(0.1,0.2,-0.5);
- tangent_t t2(0.1,0.2,0.3);
-
- std::vector< double > time_control_points;
-
+ point_t t0(0.5,0.5,0.5);
+ point_t t1(0.1,0.2,-0.5);
+ point_t t2(0.1,0.2,0.3);
+ std::vector< double > time_control_points, time_control_points_test;
// Two pairs
control_points.clear();
control_points.push_back(pair_point_tangent_t(p0,t0));
control_points.push_back(pair_point_tangent_t(p1,t1));
- time_control_points.push_back(1.); // Time at P0
- time_control_points.push_back(3.); // Time at P1
+ time_control_points.push_back(0.); // Time at P0
+ time_control_points.push_back(1.); // Time at P1
// Create cubic hermite spline
cubic_hermite_spline_t cubic_hermite_spline_1Pair(control_points.begin(), control_points.end(), time_control_points);
+ // Dimension
+ if (cubic_hermite_spline_1Pair.dim() != 3)
+ {
+ error = true;
+ std::cout << "Cubic hermite spline test, Error : Dimension of curve is wrong\n";
+ }
//Check
- res1 = cubic_hermite_spline_1Pair(1.); // t=0
+ res1 = cubic_hermite_spline_1Pair(0.); // t=0
ComparePoints(p0, res1, errmsg1, error);
- res1 = cubic_hermite_spline_1Pair(3.); // t=1
+ res1 = cubic_hermite_spline_1Pair(1.); // t=1
ComparePoints(p1, res1, errmsg1, error);
// Test derivative : two pairs
- res1 = cubic_hermite_spline_1Pair.derivate(1.,1);
+ res1 = cubic_hermite_spline_1Pair.derivate(0.,1);
ComparePoints(t0, res1, errmsg3, error);
- res1 = cubic_hermite_spline_1Pair.derivate(3.,1);
+ res1 = cubic_hermite_spline_1Pair.derivate(1.,1);
ComparePoints(t1, res1, errmsg3, error);
-
// Three pairs
control_points.push_back(pair_point_tangent_t(p2,t2));
time_control_points.clear();
@@ -1047,11 +1040,11 @@ void CubicHermitePairsPositionDerivativeTest(bool& error)
ComparePoints(t2, res1, errmsg3, error);
// Test time control points by default [0,1] => with N control points :
// Time at P0= 0. | Time at P1= 1.0/(N-1) | Time at P2= 2.0/(N-1) | ... | Time at P_(N-1)= (N-1)/(N-1)= 1.0
- time_control_points.clear();
- time_control_points.push_back(0.); // Time at P0
- time_control_points.push_back(0.5); // Time at P1
- time_control_points.push_back(1.); // Time at P2
- cubic_hermite_spline_2Pairs.setTime(time_control_points);
+ time_control_points_test.clear();
+ time_control_points_test.push_back(0.); // Time at P0
+ time_control_points_test.push_back(0.5); // Time at P1
+ time_control_points_test.push_back(1.0); // Time at P2
+ cubic_hermite_spline_2Pairs.setTime(time_control_points_test);
res1 = cubic_hermite_spline_2Pairs(0.); // t=0
ComparePoints(p0, res1, errmsg1, error);
res1 = cubic_hermite_spline_2Pairs(0.5); // t=0.5
@@ -1145,9 +1138,9 @@ void piecewiseCurveTest(bool& error)
point_t p0(0.,0.,0.);
point_t p1(1.,2.,3.);
point_t p2(4.,4.,4.);
- tangent_t t0(0.5,0.5,0.5);
- tangent_t t1(0.1,0.2,-0.5);
- tangent_t t2(0.1,0.2,0.3);
+ point_t t0(0.5,0.5,0.5);
+ point_t t1(0.1,0.2,-0.5);
+ point_t t2(0.1,0.2,0.3);
std::vector< pair_point_tangent_t > control_points_0;
control_points_0.push_back(pair_point_tangent_t(p0,t0));
control_points_0.push_back(pair_point_tangent_t(p1,t1)); // control_points_0 = 1st piece of curve
@@ -1243,7 +1236,7 @@ void curveAbcDimDynamicTest(bool& error)
{
typedef curve_abc<double,double,true> curve_abc_test_t;
typedef polynomial <double, double, true> polynomial_test_t;
- typedef exact_cubic <double, double, 3, true> exact_cubic_test_t;
+ typedef exact_cubic <double, double, true> exact_cubic_test_t;
typedef exact_cubic_test_t::spline_constraints spline_constraints_test_t;
typedef bezier_curve <double, double, true> bezier_curve_test_t;
typedef cubic_hermite_spline <double, double, true> cubic_hermite_spline_test_t;