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
...@@ -25,9 +25,6 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric ...@@ -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 Eigen::Transform<Numeric, 3, Eigen::Affine> transform_t;
typedef transform_t point_t; typedef transform_t point_t;
typedef Eigen::Matrix<Scalar, 6, 1> point_derivate_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 Eigen::Quaternion<Scalar> Quaternion;
typedef Time time_t; typedef Time time_t;
typedef curve_abc<Time, Numeric, Safe, point_t, point_derivate_t> curve_abc_t; // parent class 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 ...@@ -215,6 +212,10 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
/// \brief Get the degree of the curve. /// \brief Get the degree of the curve.
/// \return \f$degree\f$, the degree of the curve. /// \return \f$degree\f$, the degree of the curve.
virtual std::size_t degree() const { return translation_curve_->degree(); } 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*/ /*Helpers*/
/*Attributes*/ /*Attributes*/
......
...@@ -17,10 +17,8 @@ namespace curves { ...@@ -17,10 +17,8 @@ namespace curves {
/// ///
/// ///
template <typename Time = double, typename Numeric = Time, bool Safe = false> 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 Numeric Scalar;
typedef Eigen::Matrix<Scalar, 3, 1> point3_t;
typedef Eigen::Matrix<Scalar, 3, 3> matrix3_t;
typedef matrix3_t point_t; typedef matrix3_t point_t;
typedef point3_t point_derivate_t; typedef point3_t point_derivate_t;
typedef Eigen::Quaternion<Scalar> quaternion_t; typedef Eigen::Quaternion<Scalar> quaternion_t;
...@@ -76,7 +74,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric, ...@@ -76,7 +74,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
dim_(3), dim_(3),
init_rot_(quaternion_t(init_rot)), init_rot_(quaternion_t(init_rot)),
end_rot_(quaternion_t(end_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_min_(0.),
T_max_(1.) { T_max_(1.) {
safe_check(); safe_check();
...@@ -108,7 +106,7 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric, ...@@ -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. /// \brief Evaluation of the SO3Linear at time t using Eigen slerp.
/// \param t : time when to evaluate the spline. /// \param t : time when to evaluate the spline.
/// \return \f$x(t)\f$ point corresponding on spline at time t. /// \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. * @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, ...@@ -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 t : the time when to evaluate the spline.
/// \param order : order of derivative. /// \param order : order of derivative.
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derivative spline at time t. /// \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) { if ((t < T_min_ || t > T_max_) && Safe) {
throw std::invalid_argument( throw std::invalid_argument(
"error in SO3_linear : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve"); "error in SO3_linear : time t to evaluate derivative should be in range [Tmin, Tmax] of the curve");
......
...@@ -24,8 +24,8 @@ struct CurveWrapper : curve_abc_t, wrapper<curve_abc_t> { ...@@ -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) 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> { struct Curve3Wrapper : curve_3_t, wrapper<curve_3_t> {
point_t operator()(const real) { return this->get_override("operator()")(); } point3_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 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")(); } curve_t* compute_derivate_ptr(const real) { return this->get_override("compute_derivate")(); }
std::size_t dim() { return this->get_override("dim")(); } std::size_t dim() { return this->get_override("dim")(); }
real min() { return this->get_override("min")(); } real min() { return this->get_override("min")(); }
...@@ -34,8 +34,8 @@ struct Curve3Wrapper : curve_3_t, wrapper<curve_3_t> { ...@@ -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) 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> { struct CurveRotationWrapper : curve_rotation_t, wrapper<curve_rotation_t> {
point_t operator()(const real) { return this->get_override("operator()")(); } 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_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")(); } curve_t* compute_derivate_ptr(const real) { return this->get_override("compute_derivate")(); }
std::size_t dim() { return this->get_override("dim")(); } std::size_t dim() { return this->get_override("dim")(); }
real min() { return this->get_override("min")(); } real min() { return this->get_override("min")(); }
...@@ -44,8 +44,8 @@ struct CurveRotationWrapper : curve_rotation_t, wrapper<curve_rotation_t> { ...@@ -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) 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> { struct CurveSE3Wrapper : curve_SE3_t, wrapper<curve_SE3_t> {
point_t operator()(const real) { return this->get_override("operator()")(); } 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_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")(); } curve_t* compute_derivate_ptr(const real) { return this->get_override("compute_derivate")(); }
std::size_t dim() { return this->get_override("dim")(); } std::size_t dim() { return this->get_override("dim")(); }
real min() { return this->get_override("min")(); } real min() { return this->get_override("min")(); }
...@@ -96,6 +96,10 @@ bezier3_t* wrapBezier3ConstructorBoundsConstraints(const pointX_list_t& array, c ...@@ -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>( return wrapBezierConstructorConstraintsTemplate<bezier3_t, pointX_list_t, t_point3_t, curve_constraints3_t>(
array, convertToConstraints3(constraints), T_min, T_max); 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 */ /*END 3D constructors bezier */
/*constructors bezier */ /*constructors bezier */
...@@ -114,6 +118,9 @@ bezier_t* wrapBezierConstructorBoundsConstraints(const pointX_list_t& array, con ...@@ -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>( return wrapBezierConstructorConstraintsTemplate<bezier_t, pointX_list_t, t_pointX_t, curve_constraints_t>(
array, constraints, T_min, T_max); 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 */ /*END constructors bezier */
/* Wrap Cubic hermite spline */ /* Wrap Cubic hermite spline */
...@@ -349,6 +356,14 @@ SE3Curve_t* wrapSE3CurveFromTwoCurves(const curve_ptr_t& translation_curve, ...@@ -349,6 +356,14 @@ SE3Curve_t* wrapSE3CurveFromTwoCurves(const curve_ptr_t& translation_curve,
return new SE3Curve_t(translation_curve, rotation_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 #ifdef CURVES_WITH_PINOCCHIO_SUPPORT
typedef pinocchio::SE3Tpl<real, 0> SE3_t; typedef pinocchio::SE3Tpl<real, 0> SE3_t;
typedef pinocchio::MotionTpl<real, 0> Motion_t; typedef pinocchio::MotionTpl<real, 0> Motion_t;
...@@ -417,7 +432,7 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -417,7 +432,7 @@ BOOST_PYTHON_MODULE(curves) {
/*eigenpy::exposeAngleAxis(); /*eigenpy::exposeAngleAxis();
eigenpy::exposeQuaternion();*/ eigenpy::exposeQuaternion();*/
/** END eigenpy init**/ /** 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.", .def("__call__", pure_virtual(&curve_abc_t::operator()), "Evaluate the curve at the given time.",
args("self", "t")) args("self", "t"))
.def("derivate", pure_virtual(&curve_abc_t::derivate), "Evaluate the derivative of order N of curve at time 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) { ...@@ -445,7 +460,7 @@ BOOST_PYTHON_MODULE(curves) {
.def("loadFromBinary", pure_virtual(&curve_abc_t::loadFromBinary<curve_abc_t>), bp::args("filename"), .def("loadFromBinary", pure_virtual(&curve_abc_t::loadFromBinary<curve_abc_t>), bp::args("filename"),
"Loads *this from a binary file."); "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.", .def("__call__", pure_virtual(&curve_3_t::operator()), "Evaluate the curve at the given time.",
args("self", "t")) args("self", "t"))
.def("derivate", pure_virtual(&curve_3_t::derivate), "Evaluate the derivative of order N of curve at time 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) { ...@@ -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("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."); .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.", .def("__call__", pure_virtual(&curve_rotation_t::operator()), "Evaluate the curve at the given time.",
args("self", "t")) args("self", "t"))
.def("derivate", pure_virtual(&curve_rotation_t::derivate), .def("derivate", pure_virtual(&curve_rotation_t::derivate),
...@@ -477,7 +492,7 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -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("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."); .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.", .def("__call__", &se3Return, "Evaluate the curve at the given time. Return as an homogeneous matrix.",
args("self", "t")) args("self", "t"))
.def("derivate", pure_virtual(&curve_SE3_t::derivate), .def("derivate", pure_virtual(&curve_SE3_t::derivate),
...@@ -513,6 +528,7 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -513,6 +528,7 @@ BOOST_PYTHON_MODULE(curves) {
.def("__init__", make_constructor(&wrapBezier3ConstructorBoundsConstraints)) .def("__init__", make_constructor(&wrapBezier3ConstructorBoundsConstraints))
.def("compute_primitive", &bezier3_t::compute_primitive) .def("compute_primitive", &bezier3_t::compute_primitive)
.def("waypointAtIndex", &bezier3_t::waypointAtIndex) .def("waypointAtIndex", &bezier3_t::waypointAtIndex)
.def("waypoints",&wrapBezier3Waypoints)
.def_readonly("degree", &bezier3_t::degree_) .def_readonly("degree", &bezier3_t::degree_)
.def_readonly("nbWaypoints", &bezier3_t::size_) .def_readonly("nbWaypoints", &bezier3_t::size_)
.def("saveAsText", &bezier3_t::saveAsText<bezier3_t>, bp::args("filename"), "Saves *this inside a text file.") .def("saveAsText", &bezier3_t::saveAsText<bezier3_t>, bp::args("filename"), "Saves *this inside a text file.")
...@@ -537,6 +553,7 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -537,6 +553,7 @@ BOOST_PYTHON_MODULE(curves) {
.def("__init__", make_constructor(&wrapBezierConstructorBoundsConstraints)) .def("__init__", make_constructor(&wrapBezierConstructorBoundsConstraints))
.def("compute_primitive", &bezier_t::compute_primitive) .def("compute_primitive", &bezier_t::compute_primitive)
.def("waypointAtIndex", &bezier_t::waypointAtIndex) .def("waypointAtIndex", &bezier_t::waypointAtIndex)
.def("waypoints",&wrapBezierWaypoints)
.def_readonly("degree", &bezier_t::degree_) .def_readonly("degree", &bezier_t::degree_)
.def_readonly("nbWaypoints", &bezier_t::size_) .def_readonly("nbWaypoints", &bezier_t::size_)
.def("split", splitspe) .def("split", splitspe)
...@@ -605,7 +622,7 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -605,7 +622,7 @@ BOOST_PYTHON_MODULE(curves) {
/** END variable points bezier curve**/ /** END variable points bezier curve**/
/** BEGIN polynomial curve function**/ /** 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__", .def("__init__",
make_constructor(&wrapPolynomialConstructor1, default_call_policies(), args("coeffs", "min", "max")), 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]." "Create polynomial spline from an Eigen matrix of coefficient defined for t in [min,max]."
...@@ -654,7 +671,7 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -654,7 +671,7 @@ BOOST_PYTHON_MODULE(curves) {
/** END polynomial function**/ /** END polynomial function**/
/** BEGIN piecewise curve 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")), .def("__init__", make_constructor(&wrapPiecewiseCurveConstructor, default_call_policies(), arg("curve")),
"Create a peicewise curve containing the given curve.") "Create a peicewise curve containing the given curve.")
.def("FromPointsList", &discretPointToPolynomialC0, .def("FromPointsList", &discretPointToPolynomialC0,
...@@ -714,7 +731,7 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -714,7 +731,7 @@ BOOST_PYTHON_MODULE(curves) {
.def(bp::self == bp::self) .def(bp::self == bp::self)
.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")), .def("__init__", make_constructor(&wrapPiecewiseBezierConstructor, default_call_policies(), arg("curve")),
"Create a peicewise Bezier curve containing the given curve.") "Create a peicewise Bezier curve containing the given curve.")
.def("__init__", make_constructor(&wrapPiecewiseBezierEmptyConstructor), .def("__init__", make_constructor(&wrapPiecewiseBezierEmptyConstructor),
...@@ -742,7 +759,7 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -742,7 +759,7 @@ BOOST_PYTHON_MODULE(curves) {
.def(bp::self == bp::self) .def(bp::self == bp::self)
.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")), .def("__init__", make_constructor(&wrapPiecewiseBezierLinearConstructor, default_call_policies(), arg("curve")),
"Create a peicewise Bezier curve containing the given curve.") "Create a peicewise Bezier curve containing the given curve.")
.def("__init__", make_constructor(&wrapPiecewiseBezierLinearEmptyConstructor), .def("__init__", make_constructor(&wrapPiecewiseBezierLinearEmptyConstructor),
...@@ -770,7 +787,7 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -770,7 +787,7 @@ BOOST_PYTHON_MODULE(curves) {
.def(bp::self == bp::self) .def(bp::self == bp::self)
.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")), .def("__init__", make_constructor(&wrapPiecewiseSE3Constructor, default_call_policies(), arg("curve")),
"Create a piecewise-se3 curve containing the given se3 curve.") "Create a piecewise-se3 curve containing the given se3 curve.")
.def("__init__", make_constructor(&wrapPiecewiseSE3EmptyConstructor), "Create an empty piecewise-se3 curve.") .def("__init__", make_constructor(&wrapPiecewiseSE3EmptyConstructor), "Create an empty piecewise-se3 curve.")
...@@ -811,7 +828,7 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -811,7 +828,7 @@ BOOST_PYTHON_MODULE(curves) {
/** END piecewise curve function **/ /** END piecewise curve function **/
/** BEGIN exact_cubic curve**/ /** 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(&wrapExactCubicConstructor))
.def("__init__", make_constructor(&wrapExactCubicConstructorConstraint)) .def("__init__", make_constructor(&wrapExactCubicConstructorConstraint))
.def("getNumberSplines", &exact_cubic_t::getNumberSplines) .def("getNumberSplines", &exact_cubic_t::getNumberSplines)
...@@ -833,7 +850,7 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -833,7 +850,7 @@ BOOST_PYTHON_MODULE(curves) {
/** END exact_cubic curve**/ /** END exact_cubic curve**/
/** BEGIN cubic_hermite_spline **/ /** 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("__init__", make_constructor(&wrapCubicHermiteSplineConstructor))
.def("saveAsText", &cubic_hermite_spline_t::saveAsText<cubic_hermite_spline_t>, bp::args("filename"), .def("saveAsText", &cubic_hermite_spline_t::saveAsText<cubic_hermite_spline_t>, bp::args("filename"),
"Saves *this inside a text file.") "Saves *this inside a text file.")
...@@ -868,7 +885,7 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -868,7 +885,7 @@ BOOST_PYTHON_MODULE(curves) {
/** END bernstein polynomial**/ /** END bernstein polynomial**/
/** BEGIN SO3 Linear**/ /** 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__", .def("__init__",
make_constructor(&wrapSO3LinearConstructorFromMatrix, default_call_policies(), make_constructor(&wrapSO3LinearConstructorFromMatrix, default_call_policies(),
args("init_rotation", "end_rotation", "min", "max")), args("init_rotation", "end_rotation", "min", "max")),
...@@ -899,7 +916,7 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -899,7 +916,7 @@ BOOST_PYTHON_MODULE(curves) {
/** END SO3 Linear**/ /** END SO3 Linear**/
/** BEGIN SE3 Curve**/ /** 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__", .def("__init__",
make_constructor(&wrapSE3CurveFromTransform, default_call_policies(), make_constructor(&wrapSE3CurveFromTransform, default_call_policies(),
args("init_transform", "end_transform", "min", "max")), args("init_transform", "end_transform", "min", "max")),
...@@ -935,6 +952,10 @@ BOOST_PYTHON_MODULE(curves) { ...@@ -935,6 +952,10 @@ BOOST_PYTHON_MODULE(curves) {
"translation curve." "translation curve."
"The orientation along the SE3Curve will be a slerp between the two given rotations." "The orientation along the SE3Curve will be a slerp between the two given rotations."
"The orientations should be represented as 3x3 rotation matrix") "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("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"), .def("loadFromText", &SE3Curve_t::loadFromText<SE3Curve_t>, bp::args("filename"),
"Loads *this from a text file.") "Loads *this from a text file.")
......
...@@ -45,5 +45,16 @@ T_Point vectorFromEigenVector(const PointList& vector) { ...@@ -45,5 +45,16 @@ T_Point vectorFromEigenVector(const PointList& vector) {
} }
return res; 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 } // namespace curves
#endif //_DEFINITION_PYTHON_BINDINGS #endif //_DEFINITION_PYTHON_BINDINGS
...@@ -98,6 +98,14 @@ class TestCurves(unittest.TestCase): ...@@ -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(a0.derivate(t, 2) - a1.derivate(3 * t, 2) * 9.) < __EPS)
self.assertTrue(norm(prim0(t) - prim1(t * 3) / 3.) < __EPS) self.assertTrue(norm(prim0(t) - prim1(t * 3) / 3.) < __EPS)
self.assertTrue((prim(0) == array([0., 0., 0.])).all()) 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 # testing bezier with constraints
c = curve_constraints(3) c = curve_constraints(3)
c.init_vel = array([[0., 1., 1.]]).transpose() c.init_vel = array([[0., 1., 1.]]).transpose()
...@@ -189,6 +197,14 @@ class TestCurves(unittest.TestCase): ...@@ -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(a0.derivate(t, 2) - a1.derivate(3 * t, 2) * 9.) < __EPS)
self.assertTrue(norm(prim0(t) - prim1(t * 3) / 3.) < __EPS) self.assertTrue(norm(prim0(t) - prim1(t * 3) / 3.) < __EPS)
self.assertTrue((prim(0) == array([0., 0., 0.])).all()) 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 # testing bezier with constraints
c = curve_constraints(3) c = curve_constraints(3)
c.init_vel = array([[0., 1., 1.]]).transpose() c.init_vel = array([[0., 1., 1.]]).transpose()
...@@ -448,6 +464,14 @@ class TestCurves(unittest.TestCase): ...@@ -448,6 +464,14 @@ class TestCurves(unittest.TestCase):
pc.derivate(0.4, 2) pc.derivate(0.4, 2)
pc.is_continuous(0) pc.is_continuous(0)
pc.is_continuous(1) 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 # Test serialization
pc.saveAsText("serialization_pc.test") pc.saveAsText("serialization_pc.test")
pc_test = piecewise() pc_test = piecewise()
...@@ -897,6 +921,18 @@ class TestCurves(unittest.TestCase): ...@@ -897,6 +921,18 @@ class TestCurves(unittest.TestCase):
self.assertTrue(isclose(d, se3.derivate(max, 1)).all()) 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, 2), zeros((6, 1))).all())
self.assertTrue(isclose(se3.derivate(min, 3), 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 : # check that errors are correctly raised when necessary :
with self.assertRaises(ValueError): with self.assertRaises(ValueError):
se3(0.) se3(0.)
...@@ -949,6 +985,21 @@ class TestCurves(unittest.TestCase): ...@@ -949,6 +985,21 @@ class TestCurves(unittest.TestCase):
self.assertTrue(isclose(se3.derivate(t, 1)[0:3], translation.derivate(t, 1)).all()) self.assertTrue(isclose(se3.derivate(t, 1)[0:3], translation.derivate(t, 1)).all())
t += 0.02 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 # test with bezier3
translation = bezier3(waypoints, min, max) translation = bezier3(waypoints, min, max)
se3 = SE3Curve(translation, init_rot, end_rot) se3 = SE3Curve(translation, init_rot, end_rot)
...@@ -1051,6 +1102,20 @@ class TestCurves(unittest.TestCase): ...@@ -1051,6 +1102,20 @@ class TestCurves(unittest.TestCase):
self.assertTrue(isclose(se3.derivate(t, 1)[3:6], rotation.derivate(t, 1)).all()) self.assertTrue(isclose(se3.derivate(t, 1)[3:6], rotation.derivate(t, 1)).all())
t += 0.02 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 : # check if errors are correctly raised :
rotation = SO3Linear(init_rot, end_rot, min + 0.2, max) rotation = SO3Linear(init_rot, end_rot, min + 0.2, max)
with self.assertRaises(ValueError): with self.assertRaises(ValueError):
......
...@@ -1855,6 +1855,36 @@ void se3CurveTest(bool& error) { ...@@ -1855,6 +1855,36 @@ void se3CurveTest(bool& error) {
std::cout << "SE3 curve : second order derivative for rotation should be null" << std::endl; 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;
}