Skip to content
GitLab
Unverified Commit 8d201862 authored by Fernbach Pierre's avatar Fernbach Pierre Committed by GitHub
Browse files

Merge pull request #30 from pFernbach/devel 

Add API to get bezier waypoints and SE3 translation/rotation curves
parents 3092691d 6cd9a639
Hide whitespace changes
Inline Side-by-side
include/curves/se3_curve.h
View file @ 8d201862
......@@ -25,9 +25,6 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
typedef Eigen::Transform<Numeric, 3, Eigen::Affine> transform_t;
typedef transform_t point_t;
typedef Eigen::Matrix<Scalar, 6, 1> point_derivate_t;
typedef Eigen::Matrix<Scalar, 3, 1> point3_t;
typedef Eigen::Matrix<Scalar, -1, 1> pointX_t;
typedef Eigen::Matrix<Scalar, 3, 3> matrix3_t;
typedef Eigen::Quaternion<Scalar> Quaternion;
typedef Time time_t;
typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t> curve_abc_t; // parent class
......@@ -215,6 +212,10 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
/// \brief Get the degree of the curve.
/// \return \f$degree\f$, the degree of the curve.
virtual std::size_t degree() const { return translation_curve_->degree(); }
/// \brief const accessor to the translation curve
const curve_ptr_t translation_curve() const {return translation_curve_;}
/// \brief const accessor to the rotation curve
const curve_rotation_ptr_t rotation_curve() const {return rotation_curve_;}
/*Helpers*/
/*Attributes*/
......
include/curves/so3_linear.h
View file @ 8d201862
......@@ -17,10 +17,8 @@ namespace curves {
///
///
template <typename Time = double, typename Numeric = Time, bool Safe = false>
struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric, 3, 3>, Eigen::Matrix<Numeric, 3, 1> > {
struct SO3Linear : public curve_abc<Time, Numeric, Safe, matrix3_t, point3_t > {
typedef Numeric Scalar;
typedef Eigen::Matrix<Scalar, 3, 1> point3_t;
typedef Eigen::Matrix<Scalar, 3, 3> matrix3_t;
typedef matrix3_t point_t;
typedef point3_t point_derivate_t;
typedef Eigen::Quaternion<Scalar> quaternion_t;
......@@ -76,7 +74,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
dim_(3),
init_rot_(quaternion_t(init_rot)),
end_rot_(quaternion_t(end_rot)),
angular_vel_(log3(init_rot.toRotationMatrix().transpose() * end_rot.toRotationMatrix())),
angular_vel_(log3(init_rot.transpose() * end_rot)),
T_min_(0.),
T_max_(1.) {
safe_check();
......@@ -108,7 +106,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
/// \brief Evaluation of the SO3Linear at time t using Eigen slerp.
/// \param t : time when to evaluate the spline.
/// \return \f$x(t)\f$ point corresponding on spline at time t.
virtual matrix3_t operator()(const time_t t) const { return computeAsQuaternion(t).toRotationMatrix(); }
virtual point_t operator()(const time_t t) const { return computeAsQuaternion(t).toRotationMatrix(); }
/**
* @brief isApprox check if other and *this are approximately equals.
......@@ -141,7 +139,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
/// \param t : the time when to evaluate the spline.
/// \param order : order of derivative.
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derivative spline at time t.
virtual point3_t derivate(const time_t t, const std::size_t order) const {
virtual point_derivate_t derivate(const time_t t, const std::size_t order) const {
if ((t < T_min_ || t > T_max_) && Safe) {
throw std::invalid_argument(
"error in SO3_linear : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve");
......
python/curves/curves_python.cpp
View file @ 8d201862
......@@ -24,8 +24,8 @@ struct CurveWrapper : curve_abc_t, wrapper<curve_abc_t> {
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(curve_abc_t_isEquivalent_overloads, curve_abc_t::isEquivalent, 1, 3)
struct Curve3Wrapper : curve_3_t, wrapper<curve_3_t> {
point_t operator()(const real) { return this->get_override("operator()")(); }
point_t derivate(const real, const std::size_t) { return this->get_override("derivate")(); }
point3_t operator()(const real) { return this->get_override("operator()")(); }
point3_t derivate(const real, const std::size_t) { return this->get_override("derivate")(); }
curve_t* compute_derivate_ptr(const real) { return this->get_override("compute_derivate")(); }
std::size_t dim() { return this->get_override("dim")(); }
real min() { return this->get_override("min")(); }
......@@ -34,8 +34,8 @@ struct Curve3Wrapper : curve_3_t, wrapper<curve_3_t> {
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(curve_3_t_isEquivalent_overloads, curve_3_t::isEquivalent, 1, 3)
struct CurveRotationWrapper : curve_rotation_t, wrapper<curve_rotation_t> {
point_t operator()(const real) { return this->get_override("operator()")(); }
point_t derivate(const real, const std::size_t) { return this->get_override("derivate")(); }
curve_rotation_t::point_t operator()(const real) { return this->get_override("operator()")(); }
curve_rotation_t::point_derivate_t derivate(const real, const std::size_t) { return this->get_override("derivate")(); }
curve_t* compute_derivate_ptr(const real) { return this->get_override("compute_derivate")(); }
std::size_t dim() { return this->get_override("dim")(); }
real min() { return this->get_override("min")(); }
......@@ -44,8 +44,8 @@ struct CurveRotationWrapper : curve_rotation_t, wrapper<curve_rotation_t> {
BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(curve_rotation_t_isEquivalent_overloads, curve_rotation_t::isEquivalent, 1, 3)
struct CurveSE3Wrapper : curve_SE3_t, wrapper<curve_SE3_t> {
point_t operator()(const real) { return this->get_override("operator()")(); }
point_t derivate(const real, const std::size_t) { return this->get_override("derivate")(); }
curve_SE3_t::point_t operator()(const real) { return this->get_override("operator()")(); }
curve_SE3_t::point_derivate_t derivate(const real, const std::size_t) { return this->get_override("derivate")(); }
curve_t* compute_derivate_ptr(const real) { return this->get_override("compute_derivate")(); }
std::size_t dim() { return this->get_override("dim")(); }
real min() { return this->get_override("min")(); }
......@@ -96,6 +96,10 @@ bezier3_t* wrapBezier3ConstructorBoundsConstraints(const pointX_list_t& array, c
return wrapBezierConstructorConstraintsTemplate<bezier3_t, pointX_list_t, t_point3_t, curve_constraints3_t>(
array, convertToConstraints3(constraints), T_min, T_max);
}
pointX_list_t wrapBezier3Waypoints(bezier3_t& self){
return vectorToEigenArray<bezier3_t::t_point_t,pointX_list_t>(self.waypoints());
}
/*END 3D constructors bezier */
/*constructors bezier */
......@@ -114,6 +118,9 @@ bezier_t* wrapBezierConstructorBoundsConstraints(const pointX_list_t& array, con
return wrapBezierConstructorConstraintsTemplate<bezier_t, pointX_list_t, t_pointX_t, curve_constraints_t>(
array, constraints, T_min, T_max);
}
pointX_list_t wrapBezierWaypoints(bezier_t& self){
return vectorToEigenArray<bezier_t::t_point_t,pointX_list_t>(self.waypoints());
}
/*END constructors bezier */
/* Wrap Cubic hermite spline */
......@@ -349,6 +356,14 @@ SE3Curve_t* wrapSE3CurveFromTwoCurves(const curve_ptr_t& translation_curve,
return new SE3Curve_t(translation_curve, rotation_curve);
}
curve_abc_t* SE3getTranslationCurve(SE3Curve_t& self){
return self.translation_curve().get();
}
curve_rotation_t* SE3getRotationCurve(SE3Curve_t& self){
return self.rotation_curve().get();
}
#ifdef CURVES_WITH_PINOCCHIO_SUPPORT
typedef pinocchio::SE3Tpl<real, 0> SE3_t;
typedef pinocchio::MotionTpl<real, 0> Motion_t;
......@@ -417,7 +432,7 @@ BOOST_PYTHON_MODULE(curves) {
/*eigenpy::exposeAngleAxis();
eigenpy::exposeQuaternion();*/
/** END eigenpy init**/
class_<CurveWrapper, boost::noncopyable>("curve", no_init)
class_<CurveWrapper, boost::noncopyable, boost::shared_ptr<curve_abc_t> >("curve", no_init)
.def("__call__", pure_virtual(&curve_abc_t::operator()), "Evaluate the curve at the given time.",
args("self", "t"))
.def("derivate", pure_virtual(&curve_abc_t::derivate), "Evaluate the derivative of order N of curve at time t.",
......@@ -445,7 +460,7 @@ BOOST_PYTHON_MODULE(curves) {
.def("loadFromBinary", pure_virtual(&curve_abc_t::loadFromBinary<curve_abc_t>), bp::args("filename"),
"Loads *this from a binary file.");
class_<Curve3Wrapper, boost::noncopyable, bases<curve_abc_t> >("curve3", no_init)
class_<Curve3Wrapper, boost::noncopyable, bases<curve_abc_t>, boost::shared_ptr<curve_3_t> >("curve3", no_init)
.def("__call__", pure_virtual(&curve_3_t::operator()), "Evaluate the curve at the given time.",
args("self", "t"))
.def("derivate", pure_virtual(&curve_3_t::derivate), "Evaluate the derivative of order N of curve at time t.",
......@@ -461,7 +476,7 @@ BOOST_PYTHON_MODULE(curves) {
.def("max", pure_virtual(&curve_3_t::max), "Get the HIGHER bound on interval definition of the curve.")
.def("dim", pure_virtual(&curve_3_t::dim), "Get the dimension of the curve.");
class_<CurveRotationWrapper, boost::noncopyable, bases<curve_abc_t> >("curve_rotation", no_init)
class_<CurveRotationWrapper, boost::noncopyable, bases<curve_abc_t>, boost::shared_ptr<curve_rotation_t> >("curve_rotation", no_init)
.def("__call__", pure_virtual(&curve_rotation_t::operator()), "Evaluate the curve at the given time.",
args("self", "t"))
.def("derivate", pure_virtual(&curve_rotation_t::derivate),
......@@ -477,7 +492,7 @@ BOOST_PYTHON_MODULE(curves) {
.def("max", pure_virtual(&curve_rotation_t::max), "Get the HIGHER bound on interval definition of the curve.")
.def("dim", pure_virtual(&curve_rotation_t::dim), "Get the dimension of the curve.");
class_<CurveSE3Wrapper, boost::noncopyable, bases<curve_abc_t> >("curve_SE3", no_init)
class_<CurveSE3Wrapper, boost::noncopyable, bases<curve_abc_t>, boost::shared_ptr<curve_SE3_t> >("curve_SE3", no_init)
.def("__call__", &se3Return, "Evaluate the curve at the given time. Return as an homogeneous matrix.",
args("self", "t"))
.def("derivate", pure_virtual(&curve_SE3_t::derivate),
......@@ -513,6 +528,7 @@ BOOST_PYTHON_MODULE(curves) {
.def("__init__", make_constructor(&wrapBezier3ConstructorBoundsConstraints))
.def("compute_primitive", &bezier3_t::compute_primitive)
.def("waypointAtIndex", &bezier3_t::waypointAtIndex)
.def("waypoints",&wrapBezier3Waypoints)
.def_readonly("degree", &bezier3_t::degree_)
.def_readonly("nbWaypoints", &bezier3_t::size_)
.def("saveAsText", &bezier3_t::saveAsText<bezier3_t>, bp::args("filename"), "Saves *this inside a text file.")
......@@ -537,6 +553,7 @@ BOOST_PYTHON_MODULE(curves) {
.def("__init__", make_constructor(&wrapBezierConstructorBoundsConstraints))
.def("compute_primitive", &bezier_t::compute_primitive)
.def("waypointAtIndex", &bezier_t::waypointAtIndex)
.def("waypoints",&wrapBezierWaypoints)
.def_readonly("degree", &bezier_t::degree_)
.def_readonly("nbWaypoints", &bezier_t::size_)
.def("split", splitspe)
......@@ -605,7 +622,7 @@ BOOST_PYTHON_MODULE(curves) {
/** END variable points bezier curve**/
/** BEGIN polynomial curve function**/
class_<polynomial_t, bases<curve_abc_t> >("polynomial", init<>())
class_<polynomial_t, bases<curve_abc_t>, boost::shared_ptr<polynomial_t> >("polynomial", init<>())
.def("__init__",
make_constructor(&wrapPolynomialConstructor1, default_call_policies(), args("coeffs", "min", "max")),
"Create polynomial spline from an Eigen matrix of coefficient defined for t in [min,max]."
......@@ -654,7 +671,7 @@ BOOST_PYTHON_MODULE(curves) {
/** END polynomial function**/
/** BEGIN piecewise curve function **/
class_<piecewise_t, bases<curve_abc_t> >("piecewise", init<>())
class_<piecewise_t, bases<curve_abc_t>, boost::shared_ptr<piecewise_t> >("piecewise", init<>())
.def("__init__", make_constructor(&wrapPiecewiseCurveConstructor, default_call_policies(), arg("curve")),
"Create a peicewise curve containing the given curve.")
.def("FromPointsList", &discretPointToPolynomialC0,
......@@ -714,7 +731,7 @@ BOOST_PYTHON_MODULE(curves) {
.def(bp::self == bp::self)
.def(bp::self != bp::self);
class_<piecewise_bezier_t, bases<curve_abc_t> >("piecewise_bezier", init<>())
class_<piecewise_bezier_t, bases<curve_abc_t>, boost::shared_ptr<piecewise_bezier_t> >("piecewise_bezier", init<>())
.def("__init__", make_constructor(&wrapPiecewiseBezierConstructor, default_call_policies(), arg("curve")),
"Create a peicewise Bezier curve containing the given curve.")
.def("__init__", make_constructor(&wrapPiecewiseBezierEmptyConstructor),
......@@ -742,7 +759,7 @@ BOOST_PYTHON_MODULE(curves) {
.def(bp::self == bp::self)
.def(bp::self != bp::self);
class_<piecewise_linear_bezier_t, bases<curve_abc_t> >("piecewise_bezier_linear", init<>())
class_<piecewise_linear_bezier_t, bases<curve_abc_t>, boost::shared_ptr<piecewise_linear_bezier_t> >("piecewise_bezier_linear", init<>())
.def("__init__", make_constructor(&wrapPiecewiseBezierLinearConstructor, default_call_policies(), arg("curve")),
"Create a peicewise Bezier curve containing the given curve.")
.def("__init__", make_constructor(&wrapPiecewiseBezierLinearEmptyConstructor),
......@@ -770,7 +787,7 @@ BOOST_PYTHON_MODULE(curves) {
.def(bp::self == bp::self)
.def(bp::self != bp::self);
class_<piecewise_SE3_t, bases<curve_SE3_t> >("piecewise_SE3", init<>())
class_<piecewise_SE3_t, bases<curve_SE3_t>, boost::shared_ptr<piecewise_SE3_t> >("piecewise_SE3", init<>())
.def("__init__", make_constructor(&wrapPiecewiseSE3Constructor, default_call_policies(), arg("curve")),
"Create a piecewise-se3 curve containing the given se3 curve.")
.def("__init__", make_constructor(&wrapPiecewiseSE3EmptyConstructor), "Create an empty piecewise-se3 curve.")
......@@ -811,7 +828,7 @@ BOOST_PYTHON_MODULE(curves) {
/** END piecewise curve function **/
/** BEGIN exact_cubic curve**/
class_<exact_cubic_t, bases<curve_abc_t> >("exact_cubic", init<>())
class_<exact_cubic_t, bases<curve_abc_t>, boost::shared_ptr<exact_cubic_t> >("exact_cubic", init<>())
.def("__init__", make_constructor(&wrapExactCubicConstructor))
.def("__init__", make_constructor(&wrapExactCubicConstructorConstraint))
.def("getNumberSplines", &exact_cubic_t::getNumberSplines)
......@@ -833,7 +850,7 @@ BOOST_PYTHON_MODULE(curves) {
/** END exact_cubic curve**/
/** BEGIN cubic_hermite_spline **/
class_<cubic_hermite_spline_t, bases<curve_abc_t> >("cubic_hermite_spline", init<>())
class_<cubic_hermite_spline_t, bases<curve_abc_t>, boost::shared_ptr<cubic_hermite_spline_t> >("cubic_hermite_spline", init<>())
.def("__init__", make_constructor(&wrapCubicHermiteSplineConstructor))
.def("saveAsText", &cubic_hermite_spline_t::saveAsText<cubic_hermite_spline_t>, bp::args("filename"),
"Saves *this inside a text file.")
......@@ -868,7 +885,7 @@ BOOST_PYTHON_MODULE(curves) {
/** END bernstein polynomial**/
/** BEGIN SO3 Linear**/
class_<SO3Linear_t, bases<curve_rotation_t> >("SO3Linear", init<>())
class_<SO3Linear_t, bases<curve_rotation_t>, boost::shared_ptr<SO3Linear_t> >("SO3Linear", init<>())
.def("__init__",
make_constructor(&wrapSO3LinearConstructorFromMatrix, default_call_policies(),
args("init_rotation", "end_rotation", "min", "max")),
......@@ -899,7 +916,7 @@ BOOST_PYTHON_MODULE(curves) {
/** END SO3 Linear**/
/** BEGIN SE3 Curve**/
class_<SE3Curve_t, bases<curve_SE3_t> >("SE3Curve", init<>())
class_<SE3Curve_t, bases<curve_SE3_t>, boost::shared_ptr<SE3Curve_t> >("SE3Curve", init<>())
.def("__init__",
make_constructor(&wrapSE3CurveFromTransform, default_call_policies(),
args("init_transform", "end_transform", "min", "max")),
......@@ -935,6 +952,10 @@ BOOST_PYTHON_MODULE(curves) {
"translation curve."
"The orientation along the SE3Curve will be a slerp between the two given rotations."
"The orientations should be represented as 3x3 rotation matrix")
.def("translation_curve",&SE3getTranslationCurve,return_internal_reference<>(),
"Return a curve corresponding to the translation part of self.")
.def("rotation_curve",&SE3getRotationCurve,return_internal_reference<>(),
"Return a curve corresponding to the rotation part of self.")
.def("saveAsText", &SE3Curve_t::saveAsText<SE3Curve_t>, bp::args("filename"), "Saves *this inside a text file.")
.def("loadFromText", &SE3Curve_t::loadFromText<SE3Curve_t>, bp::args("filename"),
"Loads *this from a text file.")
......
python/curves/python_definitions.h
View file @ 8d201862
......@@ -45,5 +45,16 @@ T_Point vectorFromEigenVector(const PointList& vector) {
}
return res;
}
template <typename T_point, typename PointList>
PointList vectorToEigenArray(const T_point& vect) {
const size_t nCols = vect.size();
const size_t nRows = vect[0].rows();
PointList res(nRows,nCols);
for (size_t i = 0; i < vect.size(); ++i) {
res.block(0,i,nRows,1) = vect[i];
}
return res;
}
} // namespace curves
#endif //_DEFINITION_PYTHON_BINDINGS
python/test/test.py
View file @ 8d201862
......@@ -98,6 +98,14 @@ class TestCurves(unittest.TestCase):
self.assertTrue(norm(a0.derivate(t, 2) - a1.derivate(3 * t, 2) * 9.) < __EPS)
self.assertTrue(norm(prim0(t) - prim1(t * 3) / 3.) < __EPS)
self.assertTrue((prim(0) == array([0., 0., 0.])).all())
# testing accessor to waypoints :
wp_getter = a0.waypoints()
self.assertEqual(wp_getter.shape[0],waypoints.shape[0])
self.assertEqual(wp_getter.shape[1],waypoints.shape[1])
self.assertTrue(array_equal(wp_getter,waypoints))
# check that it return a copy:
a0.waypoints()[1,1] = -15.
self.assertEqual(a0.waypoints()[1,1],waypoints[1,1])
# testing bezier with constraints
c = curve_constraints(3)
c.init_vel = array([[0., 1., 1.]]).transpose()
......@@ -189,6 +197,14 @@ class TestCurves(unittest.TestCase):
self.assertTrue(norm(a0.derivate(t, 2) - a1.derivate(3 * t, 2) * 9.) < __EPS)
self.assertTrue(norm(prim0(t) - prim1(t * 3) / 3.) < __EPS)
self.assertTrue((prim(0) == array([0., 0., 0.])).all())
# testing accessor to waypoints :
wp_getter = a0.waypoints()
self.assertEqual(wp_getter.shape[0],waypoints.shape[0])
self.assertEqual(wp_getter.shape[1],waypoints.shape[1])
self.assertTrue(array_equal(wp_getter,waypoints))
# check that it return a copy:
a0.waypoints()[1,1] = -15.
self.assertEqual(a0.waypoints()[1,1],waypoints[1,1])
# testing bezier with constraints
c = curve_constraints(3)
c.init_vel = array([[0., 1., 1.]]).transpose()
......@@ -448,6 +464,14 @@ class TestCurves(unittest.TestCase):
pc.derivate(0.4, 2)
pc.is_continuous(0)
pc.is_continuous(1)
# test access to curves :
self.assertTrue(array_equal(pc.curve_at_index(0)(0.5), a(0.5)))
waypoints = array([[3., 4., -3.], [5., 1., 2.]]).transpose()
c = bezier(waypoints, 1.5, 2.)
c0 = pc.curve_at_index(0)
c0 = c # should not have any effect
self.assertTrue(array_equal(pc.curve_at_index(0)(0.5), a(0.5)))
# Test serialization
pc.saveAsText("serialization_pc.test")
pc_test = piecewise()
......@@ -897,6 +921,18 @@ class TestCurves(unittest.TestCase):
self.assertTrue(isclose(d, se3.derivate(max, 1)).all())
self.assertTrue(isclose(se3.derivate(min, 2), zeros((6, 1))).all())
self.assertTrue(isclose(se3.derivate(min, 3), zeros((6, 1))).all())
# test accessor to translation_curve :
tr_se3 = se3.translation_curve()
self.assertTrue(array_equal(tr_se3((max+min)/2.), se3.translation((max+min)/2.)))
# test accessor to rotation :
rot_se3 = se3.rotation_curve()
rot_se3((max+min)/2.)
self.assertTrue(isclose(rot_se3((max+min)/2.),(se3.rotation((max+min)/2.))).all())
# check that it return a CONST reference :
waypoints2 = array([[1., -2., 3.5], [5.6, 5., -6.], [4.,1.2, 0.5]]).transpose()
tr_se3 = bezier3(waypoints2, min, max) # should not have any effect
self.assertFalse(array_equal(tr_se3((max+min)/2.), se3.translation((max+min)/2.)))
# check that errors are correctly raised when necessary :
with self.assertRaises(ValueError):
se3(0.)
......@@ -949,6 +985,21 @@ class TestCurves(unittest.TestCase):
self.assertTrue(isclose(se3.derivate(t, 1)[0:3], translation.derivate(t, 1)).all())
t += 0.02
# test accessor to translation_curve :
tr_se3 = se3.translation_curve()
self.assertTrue(tr_se3 == translation)
self.assertTrue(array_equal(tr_se3((max+min)/2.), se3.translation((max+min)/2.)))
# test accessor to rotation :
rot_se3 = se3.rotation_curve()
rot_se3((max+min)/2.)
se3.rotation((max+min)/2.)
self.assertTrue(isclose(rot_se3((max+min)/2.), se3.rotation((max+min)/2.)).all())
# check that it return a CONST reference :
waypoints2 = array([[1., -2., 3.5], [5.6, 5., -6.], [4.,1.2, 0.5]]).transpose()
tr_se3 = bezier3(waypoints2, min, max) # should not have any effect
self.assertFalse(array_equal(tr_se3((max+min)/2.), se3.translation((max+min)/2.)))
# test with bezier3
translation = bezier3(waypoints, min, max)
se3 = SE3Curve(translation, init_rot, end_rot)
......@@ -1051,6 +1102,20 @@ class TestCurves(unittest.TestCase):
self.assertTrue(isclose(se3.derivate(t, 1)[3:6], rotation.derivate(t, 1)).all())
t += 0.02
# test accessor to translation_curve :
tr_se3 = se3.translation_curve()
self.assertTrue(tr_se3 == translation)
self.assertTrue(array_equal(tr_se3((max+min)/2.), se3.translation((max+min)/2.)))
# test accessor to rotation :
rot_se3 = se3.rotation_curve()
rot_se3((max+min)/2.)
se3.rotation((max+min)/2.)
self.assertTrue(array_equal(rot_se3((max+min)/2.), se3.rotation((max+min)/2.)))
# check that it return a CONST reference :
waypoints2 = array([[1., -2., 3.5], [5.6, 5., -6.], [4.,1.2, 0.5]]).transpose()
tr_se3 = bezier3(waypoints2, min, max) # should not have any effect
self.assertFalse(array_equal(tr_se3((max+min)/2.), se3.translation((max+min)/2.)))
# check if errors are correctly raised :
rotation = SO3Linear(init_rot, end_rot, min + 0.2, max)
with self.assertRaises(ValueError):
......
tests/Main.cpp
View file @ 8d201862
......@@ -1855,6 +1855,36 @@ void se3CurveTest(bool& error) {
std::cout << "SE3 curve : second order derivative for rotation should be null" << std::endl;
}
// check accessor to translation curves :
curve_ptr_t translation = cBezier.translation_curve();
if (translation->operator()(min) != cBezier(min).translation()) {
error = true;
std::cout << "SE3 curve : translation curve not equal to se3.translation" << std::endl;
}
if (translation->operator()(max) != cBezier(max).translation()) {
error = true;
std::cout << "SE3 curve : translation curve not equal to se3.translation" << std::endl;
}
if (translation->operator()((max+min)/2.) != cBezier((max+min)/2.).translation()) {
error = true;
std::cout << "SE3 curve : translation curve not equal to se3.translation" << std::endl;
}
// check accessor to rotation curves :
curve_rotation_ptr_t rotation = cBezier.rotation_curve();
if (! rotation->operator()(min).isApprox(cBezier(min).rotation())) {
error = true;
std::cout << "SE3 curve : rotation curve not equal to se3.rotation" << std::endl;
}
if (! rotation->operator()(max).isApprox(cBezier(max).rotation())) {
error = true;
std::cout << "SE3 curve : rotation curve not equal to se3.rotation" << std::endl;
}
if (! rotation->operator()((max+min)/2.).isApprox(cBezier((max+min)/2.).rotation())) {
error = true;
std::cout << "SE3 curve : rotation curve not equal to se3.rotation" << std::endl;
}
// check if errors are correctly raised
try {
cBezier(0.1);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment