Skip to content
GitLab
Commit 5e0243f7 authored by JasonChmn's avatar JasonChmn Committed by Pierre Fernbach
Browse files

[dynamic dim - exact cubic/constraints/helpers] Remove dim from template + binding python

parent 88ff7cce
Hide whitespace changes
Inline Side-by-side
include/curves/curve_constraint.h
View file @ 5e0243f7
......@@ -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(){}
......
include/curves/exact_cubic.h
View file @ 5e0243f7
......@@ -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;
......
include/curves/helpers/effector_spline.h
View file @ 5e0243f7
......@@ -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;
......
include/curves/helpers/effector_spline_rotation.h
View file @ 5e0243f7
......@@ -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.
......
python/curves_python.cpp
View file @ 5e0243f7
......@@ -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>())
......
python/test/test.py
View file @ 5e0243f7
......@@ -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)
......
tests/Main.cpp
View file @ 5e0243f7
......@@ -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);