diff --git a/include/curves/helpers/effector_spline_rotation.h b/include/curves/helpers/effector_spline_rotation.h
index e230e407617eb944f9e9ab7cc494d66b09f8ceac..ad503e7a763647069e3a71e13cef7a8f48ea1ebb 100644
--- a/include/curves/helpers/effector_spline_rotation.h
+++ b/include/curves/helpers/effector_spline_rotation.h
@@ -44,6 +44,7 @@ class rotation_spline : public curve_abc_quat_t {
: curve_abc_quat_t(),
quat_from_(quat_from.data()),
quat_to_(quat_to.data()),
+ dim_(4),
min_(min),
max_(max),
time_reparam_(computeWayPoints()) {}
@@ -54,6 +55,7 @@ class rotation_spline : public curve_abc_quat_t {
rotation_spline& operator=(const rotation_spline& from) {
quat_from_ = from.quat_from_;
quat_to_ = from.quat_to_;
+ dim_ = from.dim_;
min_ = from.min_;
max_ = from.max_;
time_reparam_ = exact_cubic_constraint_one_dim(from.time_reparam_);
diff --git a/include/curves/linear_variable.h b/include/curves/linear_variable.h
index 67348fa2df735b3ffa41292f8afce00d5e8aa326..ffcb27a73f578d36131a34a60fec49867bf6ba9a 100644
--- a/include/curves/linear_variable.h
+++ b/include/curves/linear_variable.h
@@ -81,6 +81,10 @@ struct linear_variable : public serialization::Serializable {
Numeric norm() const { return isZero() ? 0 : (B_.norm() + c_.norm()); }
+ bool isApprox(const linear_variable_t& other,const double prec = Eigen::NumTraits<Numeric>::dummy_precision()){
+ return (*this - other).norm() < prec;
+ }
+
const matrix_x_t& B() const { return B_; }
const vector_x_t& c() const { return c_; }
bool isZero() const { return zero; }
diff --git a/include/curves/piecewise_curve.h b/include/curves/piecewise_curve.h
index 6601846024c14066d9229b8a4ad30def105ad963..1557385142da2495b895a1b18f81b55ba23b2e49 100644
--- a/include/curves/piecewise_curve.h
+++ b/include/curves/piecewise_curve.h
@@ -24,10 +24,12 @@ namespace curves {
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 Curve = curve_abc<Time, Numeric, Safe, Point> >
-struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point> {
+ typename Curve = curve_abc<Time, Numeric, Safe, Point>,
+ typename Point_derivate = Point >
+struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_derivate> {
typedef Point point_t;
typedef T_Point t_point_t;
+ typedef Point_derivate point_derivate_t;
typedef Time time_t;
typedef Numeric num_t;
typedef Curve curve_t;
@@ -41,17 +43,17 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point> {
/// \brief Constructor.
/// Initialize a piecewise curve by giving the first curve.
- /// \param pol : a polynomial curve.
+ /// \param cf : a curve.
///
piecewise_curve(const curve_t& cf) {
- dim_ = cf(cf.min()).size();
+ dim_ = cf.dim();
size_ = 0;
add_curve(cf);
}
piecewise_curve(const t_curve_t list_curves) {
if (list_curves.size() != 0) {
- dim_ = list_curves[0](list_curves[0].min()).size();
+ dim_ = list_curves[0].dim();
}
size_ = 0;
for (std::size_t i = 0; i < list_curves.size(); i++) {
@@ -83,7 +85,7 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point> {
/// \param order : order of derivative.
/// \return \f$\frac{d^Np(t)}{dt^N}\f$ point corresponding on derivative spline of order N at time t.
///
- virtual Point derivate(const Time t, const std::size_t order) const {
+ virtual point_derivate_t derivate(const Time t, const std::size_t order) const {
check_if_not_empty();
if (Safe & !(T_min_ <= t && t <= T_max_)) {
throw std::invalid_argument("can't evaluate piecewise curve, out of range");
@@ -96,8 +98,8 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point> {
* @param order order of derivative
* @return
*/
- piecewise_curve<Time, Numeric, Safe, Point, T_Point, Curve> compute_derivate(const std::size_t order) const {
- piecewise_curve<Time, Numeric, Safe, Point, T_Point, Curve> res;
+ piecewise_curve<Time, Numeric, Safe, point_derivate_t, std::vector<point_derivate_t, Eigen::aligned_allocator<Point> >, Curve> compute_derivate(const std::size_t order) const {
+ piecewise_curve<Time, Numeric, Safe, point_derivate_t, std::vector<point_derivate_t, Eigen::aligned_allocator<Point> >, Curve> res;
for (typename t_curve_t::const_iterator itc = curves_.begin(); itc < curves_.end(); ++itc) {
res.add_curve(itc->compute_derivate(order));
}
@@ -111,23 +113,27 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point> {
/// \param cf : curve to add.
///
void add_curve(const curve_t& cf) {
- if (size_ == 0 && dim_ == 0) {
- dim_ = cf(cf.min()).size();
+ if (size_ == 0 && dim_ == 0) { // first curve added
+ dim_ = cf.dim();
}
- // Check time continuity : Beginning time of pol must be equal to T_max_ of actual piecewise curve.
+ // Check time continuity : Beginning time of cf must be equal to T_max_ of actual piecewise curve.
if (size_ != 0 && !(fabs(cf.min() - T_max_) < MARGIN)) {
throw std::invalid_argument(
"Can not add new Polynom to PiecewiseCurve : time discontinuity between T_max_ and pol.min()");
}
+ if(cf.dim() != dim_){
+ throw std::invalid_argument(
+ "All the curves in a piecewiseCurve should have the same dimension");
+ }
curves_.push_back(cf);
size_ = curves_.size();
T_max_ = cf.max();
- time_curves_.push_back(T_max_);
if (size_ == 1) {
// First curve added
time_curves_.push_back(cf.min());
T_min_ = cf.min();
}
+ time_curves_.push_back(T_max_);
}
/// \brief Check if the curve is continuous of order given.
@@ -138,21 +144,30 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point> {
check_if_not_empty();
bool isContinuous = true;
std::size_t i = 0;
- point_t value_end, value_start;
- while (isContinuous && i < (size_ - 1)) {
- curve_t current = curves_.at(i);
- curve_t next = curves_.at(i + 1);
- if (order == 0) {
+ if(order ==0){
+ point_t value_end, value_start;
+ while (isContinuous && i < (size_ - 1)) {
+ curve_t current = curves_.at(i);
+ curve_t next = curves_.at(i + 1);
value_end = current(current.max());
value_start = next(next.min());
- } else {
+ if (!value_end.isApprox(value_start,MARGIN)) {
+ isContinuous = false;
+ }
+ i++;
+ }
+ }else{
+ point_derivate_t value_end, value_start;
+ while (isContinuous && i < (size_ - 1)) {
+ curve_t current = curves_.at(i);
+ curve_t next = curves_.at(i + 1);
value_end = current.derivate(current.max(), order);
value_start = next.derivate(next.min(), order);
+ if (!value_end.isApprox(value_start,MARGIN)) {
+ isContinuous = false;
+ }
+ i++;
}
- if ((value_end - value_start).norm() > MARGIN) {
- isContinuous = false;
- }
- i++;
}
return isContinuous;
}
@@ -374,8 +389,8 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point> {
}
}; // End struct piecewise curve
-template <typename Time, typename Numeric, bool Safe, typename Point, typename T_Point, typename Curve>
-const double piecewise_curve<Time, Numeric, Safe, Point, T_Point, Curve>::MARGIN(0.001);
+template <typename Time, typename Numeric, bool Safe, typename Point, typename T_Point,typename Curve,typename Point_derivate>
+const double piecewise_curve<Time, Numeric, Safe, Point, T_Point, Curve,Point_derivate>::MARGIN(0.001);
} // namespace curves
diff --git a/python/curves_python.cpp b/python/curves_python.cpp
index 249c83948a68f0168e4c1e468ea42cb5cd2e7f55..33c7b08388d22d6bb28cf3084fb75a39335d440f 100644
--- a/python/curves_python.cpp
+++ b/python/curves_python.cpp
@@ -158,6 +158,14 @@ piecewise_bezier_linear_curve_t* wrapPiecewiseLinearBezierCurveConstructor(const
piecewise_cubic_hermite_curve_t* wrapPiecewiseCubicHermiteCurveConstructor(const cubic_hermite_spline_t& ch) {
return new piecewise_cubic_hermite_curve_t(ch);
}
+
+piecewise_SE3_curve_t* wrapPiecewiseSE3Constructor(const SE3Curve_t& curve) {
+ return new piecewise_SE3_curve_t(curve);
+}
+
+piecewise_SE3_curve_t* wrapPiecewiseSE3EmptyConstructor() {
+ return new piecewise_SE3_curve_t();
+}
static piecewise_polynomial_curve_t discretPointToPolynomialC0(const pointX_list_t& points,
const time_waypoints_t& time_points) {
t_pointX_t points_list = vectorFromEigenArray<pointX_list_t, t_pointX_t>(points);
@@ -186,16 +194,20 @@ static piecewise_polynomial_curve_t discretPointToPolynomialC2(const pointX_list
return piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(
points_list, points_derivative_list, points_second_derivative_list, time_points_list);
}
-void addFinalPointC0(piecewise_polynomial_curve_t self, const pointX_t& end, const real time) {
- if (self.is_continuous(1))
+void addFinalPointC0(piecewise_polynomial_curve_t& self, const pointX_t& end, const real time) {
+ if(self.num_curves() == 0)
+ throw std::runtime_error("Piecewise append : you need to add at least one curve before using append(finalPoint) method.");
+ if (self.is_continuous(1) && self.num_curves()>1 )
std::cout << "Warning: by adding this final point to the piecewise curve, you loose C1 continuity and only "
"guarantee C0 continuity."
<< std::endl;
polynomial_t pol(self(self.max()), end, self.max(), time);
self.add_curve(pol);
}
-void addFinalPointC1(piecewise_polynomial_curve_t self, const pointX_t& end, const pointX_t& d_end, const real time) {
- if (self.is_continuous(2))
+void addFinalPointC1(piecewise_polynomial_curve_t& self, const pointX_t& end, const pointX_t& d_end, const real time) {
+ if(self.num_curves() == 0)
+ throw std::runtime_error("Piecewise append : you need to add at least one curve before using append(finalPoint) method.");
+ if (self.is_continuous(2) && self.num_curves()>1 )
std::cout << "Warning: by adding this final point to the piecewise curve, you loose C2 continuity and only "
"guarantee C1 continuity."
<< std::endl;
@@ -203,9 +215,11 @@ void addFinalPointC1(piecewise_polynomial_curve_t self, const pointX_t& end, con
polynomial_t pol(self(self.max()), self.derivate(self.max(), 1), end, d_end, self.max(), time);
self.add_curve(pol);
}
-void addFinalPointC2(piecewise_polynomial_curve_t self, const pointX_t& end, const pointX_t& d_end,
+void addFinalPointC2(piecewise_polynomial_curve_t& self, const pointX_t& end, const pointX_t& d_end,
const pointX_t& dd_end, const real time) {
- if (self.is_continuous(3))
+ if(self.num_curves() == 0)
+ throw std::runtime_error("Piecewise append : you need to add at least one curve before using append(finalPoint) method.");
+ if (self.is_continuous(3) && self.num_curves()>1 )
std::cout << "Warning: by adding this final point to the piecewise curve, you loose C3 continuity and only "
"guarantee C2 continuity."
<< std::endl;
@@ -291,6 +305,10 @@ SE3Curve_t* wrapSE3CurveFromTransform(const matrix4_t& init_pose, const matrix4_
return new SE3Curve_t(transform_t(init_pose), transform_t(end_pose), min, max);
}
+SE3Curve_t* wrapSE3CurveFromPosAndRotation(const pointX_t& init_pos, const pointX_t& end_pos, const matrix3_t& init_rot, const matrix3_t& end_rot,const real& t_min, const real& t_max) {
+ return new SE3Curve_t(init_pos,end_pos,init_rot,end_rot, t_min, t_max);
+}
+
SE3Curve_t* wrapSE3CurveFromBezier3Translation(bezier3_t& translation_curve, const matrix3_t& init_rot,
const matrix3_t& end_rot) {
bezier_t* translation = new bezier_t(translation_curve.waypoints().begin(), translation_curve.waypoints().end(),
@@ -336,6 +354,50 @@ pointX_t se3returnTranslation(const SE3Curve_t& curve, const real t) { return po
/* End wrap SE3Curves */
+/* Wrap piecewiseSE3Curves */
+#ifdef CURVES_WITH_PINOCCHIO_SUPPORT
+typedef pinocchio::SE3Tpl<real, 0> SE3_t;
+typedef pinocchio::MotionTpl<real, 0> Motion_t;
+
+SE3_t piecewiseSE3ReturnPinocchio(const piecewise_SE3_curve_t& curve, const real t) { return SE3_t(curve(t).matrix()); }
+
+Motion_t piecewiseSE3ReturnDerivatePinocchio(const piecewise_SE3_curve_t& curve, const real t, const std::size_t order) {
+ return Motion_t(curve.derivate(t, order));
+}
+
+void addFinalSE3(piecewise_SE3_curve_t& self, const SE3_t& end, const real time) {
+ if(self.num_curves() == 0)
+ throw std::runtime_error("Piecewise append : you need to add at least one curve before using append(finalPoint) method.");
+ if (self.is_continuous(1) && self.num_curves()>1 )
+ std::cout << "Warning: by adding this final transform to the piecewise curve, you loose C1 continuity and only "
+ "guarantee C0 continuity."
+ << std::endl;
+ SE3Curve_t curve(self(self.max()), transform_t(end.toHomogeneousMatrix()), self.max(), time);
+ self.add_curve(curve);
+}
+
+#endif // CURVES_WITH_PINOCCHIO_SUPPORT
+
+matrix4_t piecewiseSE3Return(const piecewise_SE3_curve_t& curve, const real t) { return curve(t).matrix(); }
+
+
+matrix3_t piecewiseSE3returnRotation(const piecewise_SE3_curve_t& curve, const real t) { return curve(t).rotation(); }
+
+pointX_t piecewiseSE3returnTranslation(const piecewise_SE3_curve_t& curve, const real t) { return pointX_t(curve(t).translation()); }
+
+void addFinalTransform(piecewise_SE3_curve_t& self, const matrix4_t& end, const real time) {
+ if(self.num_curves() == 0)
+ throw std::runtime_error("Piecewise append : you need to add at least one curve before using append(finalPoint) method.");
+ if (self.is_continuous(1) && self.num_curves()>1 )
+ std::cout << "Warning: by adding this final transform to the piecewise curve, you loose C1 continuity and only "
+ "guarantee C0 continuity."
+ << std::endl;
+ SE3Curve_t curve(self(self.max()), transform_t(end), self.max(), time);
+ self.add_curve(curve);
+}
+
+/* End wrap piecewiseSE3Curves */
+
// TO DO : Replace all load and save function for serialization in class by using
// SerializableVisitor in archive_python_binding.
BOOST_PYTHON_MODULE(curves) {
@@ -571,7 +633,7 @@ BOOST_PYTHON_MODULE(curves) {
"Add a new curve to piecewise curve, which should be defined in T_{min},T_{max}] "
"where T_{min} is equal toT_{max} of the actual piecewise curve.")
.def("is_continuous", &piecewise_polynomial_curve_t::is_continuous,
- "Check if the curve is continuous at the given order.")
+ "Check if the curve is continuous at the given order.",args("self,order"))
.def("convert_piecewise_curve_to_bezier",
&piecewise_polynomial_curve_t::convert_piecewise_curve_to_bezier<bezier_t>,
"Convert a piecewise polynomial curve to to a piecewise bezier curve")
@@ -599,7 +661,7 @@ BOOST_PYTHON_MODULE(curves) {
.def("__init__", make_constructor(&wrapPiecewiseBezierCurveConstructor))
.def("compute_derivate", &piecewise_polynomial_curve_t::compute_derivate,
"Return a piecewise_polynomial curve which is the derivate of this.", args("self", "order"))
- .def("add_curve", &piecewise_bezier_curve_t::add_curve)
+ .def("append", &piecewise_bezier_curve_t::add_curve)
.def("is_continuous", &piecewise_bezier_curve_t::is_continuous)
.def("convert_piecewise_curve_to_polynomial",
&piecewise_bezier_curve_t::convert_piecewise_curve_to_polynomial<polynomial_t>,
@@ -625,7 +687,7 @@ BOOST_PYTHON_MODULE(curves) {
class_<piecewise_cubic_hermite_curve_t, bases<curve_abc_t> >("piecewise_cubic_hermite_curve", init<>())
.def("__init__", make_constructor(&wrapPiecewiseCubicHermiteCurveConstructor))
- .def("add_curve", &piecewise_cubic_hermite_curve_t::add_curve)
+ .def("append", &piecewise_cubic_hermite_curve_t::add_curve)
.def("is_continuous", &piecewise_cubic_hermite_curve_t::is_continuous)
.def("convert_piecewise_curve_to_polynomial",
&piecewise_cubic_hermite_curve_t::convert_piecewise_curve_to_polynomial<polynomial_t>,
@@ -653,7 +715,7 @@ BOOST_PYTHON_MODULE(curves) {
class_<piecewise_bezier_linear_curve_t, bases<curve_abc_t> >("piecewise_bezier_linear_curve_t", init<>())
.def("__init__", make_constructor(&wrapPiecewiseLinearBezierCurveConstructor))
- .def("add_curve", &piecewise_bezier_linear_curve_t::add_curve)
+ .def("append", &piecewise_bezier_linear_curve_t::add_curve)
.def("is_continuous", &piecewise_bezier_linear_curve_t::is_continuous,
"Check if the curve is continuous at the given order.")
.def("curve_at_index", &piecewise_bezier_linear_curve_t::curve_at_index,
@@ -674,6 +736,61 @@ BOOST_PYTHON_MODULE(curves) {
.def("loadFromBinary", &piecewise_bezier_linear_curve_t::loadFromBinary<piecewise_bezier_linear_curve_t>,
bp::args("filename"), "Loads *this from a binary file.");
+class_<piecewise_SE3_curve_t, bases<curve_abc_t> >("piecewise_SE3_curve", 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.")
+// .def("compute_derivate", &piecewise_SE3_curve_t::compute_derivate,
+// "Return a piecewise_polynomial curve which is the derivate of this.", args("self", "order"))
+ .def("append", &piecewise_SE3_curve_t::add_curve,
+ "Add a new curve to piecewise curve, which should be defined in T_{min},T_{max}] "
+ "where T_{min} is equal toT_{max} of the actual piecewise curve.",
+ args("self,curve"))
+ .def("is_continuous", &piecewise_SE3_curve_t::is_continuous, "Check if the curve is continuous at the given order.",args("self,order"))
+ .def("curve_at_index", &piecewise_SE3_curve_t::curve_at_index, return_value_policy<copy_const_reference>())
+ .def("curve_at_time", &piecewise_SE3_curve_t::curve_at_time, return_value_policy<copy_const_reference>())
+ .def("num_curves", &piecewise_SE3_curve_t::num_curves)
+ .def("rotation", &piecewiseSE3returnRotation, "Output the rotation (as a 3x3 matrix) at the given time.",
+ args("self", "t"))
+ .def("translation", &piecewiseSE3returnTranslation, "Output the translation (as a vector 3) at the given time.",
+ args("self", "t"))
+ .def("__call__", &piecewiseSE3Return, "Evaluate the curve at the given time. Return as an homogeneous matrix",
+ args("self", "t"))
+ .def("derivate", &piecewise_SE3_curve_t::derivate,
+ "Evaluate the derivative of order N of curve at time t. Return as a vector 6", args("self", "t", "N"))
+ .def("min", &piecewise_SE3_curve_t::min, "Get the LOWER bound on interval definition of the curve.")
+ .def("max", &piecewise_SE3_curve_t::max, "Get the HIGHER bound on interval definition of the curve.")
+ .def("dim", &piecewise_SE3_curve_t::dim, "Get the dimension of the curve.")
+ .def("append", &addFinalTransform,
+ "Append a new linear SE3 curve at the end of the piecewise curve, defined between self.max() "
+ "and time and connecting exactly self(self.max()) and end",
+ args("self", "end", "time"))
+// .def("saveAsText", &piecewise_bezier_curve_t::saveAsText<piecewise_bezier_curve_t>, bp::args("filename"),
+// "Saves *this inside a text file.")
+// .def("loadFromText", &piecewise_bezier_curve_t::loadFromText<piecewise_bezier_curve_t>, bp::args("filename"),
+// "Loads *this from a text file.")
+// .def("saveAsXML", &piecewise_bezier_curve_t::saveAsXML<piecewise_bezier_curve_t>,
+// bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
+// .def("loadFromXML", &piecewise_bezier_curve_t::loadFromXML<piecewise_bezier_curve_t>,
+// bp::args("filename", "tag_name"), "Loads *this from a XML file.")
+// .def("saveAsBinary", &piecewise_bezier_curve_t::saveAsBinary<piecewise_bezier_curve_t>, bp::args("filename"),
+// "Saves *this inside a binary file.")
+// .def("loadFromBinary", &piecewise_bezier_curve_t::loadFromBinary<piecewise_bezier_curve_t>, bp::args("filename"),
+// "Loads *this from a binary file.")
+ #ifdef CURVES_WITH_PINOCCHIO_SUPPORT
+ .def("evaluateAsSE3", &piecewiseSE3ReturnPinocchio, "Evaluate the curve at the given time. Return as a pinocchio.SE3 object",
+ args("self", "t"))
+ .def("derivateAsMotion", &piecewiseSE3ReturnDerivatePinocchio,
+ "Evaluate the derivative of order N of curve at time t. Return as a pinocchio.Motion",
+ args("self", "t", "N"))
+ .def("append", &addFinalSE3,
+ "Append a new linear SE3 curve at the end of the piecewise curve, defined between self.max() "
+ "and time and connecting exactly self(self.max()) and end",
+ args("self", "end", "time"))
+ #endif // CURVES_WITH_PINOCCHIO_SUPPORT
+ ;
+
/** END piecewise curve function **/
/** BEGIN exact_cubic curve**/
class_<exact_cubic_t, bases<curve_abc_t> >("exact_cubic", init<>())
@@ -763,6 +880,12 @@ BOOST_PYTHON_MODULE(curves) {
"Create a SE3 curve between two transform, defined for t \in [min,max]."
" Using linear interpolation for translation and slerp for rotation between init and end."
" The input transform are expressed as 4x4 matrix.")
+ .def("__init__",
+ make_constructor(&wrapSE3CurveFromPosAndRotation, default_call_policies(),
+ args("init_translation", "end_translation","init_rotation","end_rotation", "min", "max")),
+ "Create a SE3 curve between two transform, defined for t \in [min,max]."
+ " Using linear interpolation for translation and slerp for rotation between init and end."
+ " The input translations are expressed as 3d vector and the rotations as 3x3 matrix.")
.def("__init__",
make_constructor(&wrapSE3CurveFromTwoCurves, default_call_policies(),
args("translation_curve", "rotation_curve")),
@@ -802,9 +925,9 @@ BOOST_PYTHON_MODULE(curves) {
args("init_SE3", "end_SE3", "min", "max")),
"Create a SE3 curve between two SE3 objects from Pinocchio, defined for t \in [min,max]."
" Using linear interpolation for translation and slerp for rotation between init and end.")
- .def("evaluateAsSE3", &se3Return, "Evaluate the curve at the given time. Return as a pinocchio.SE3 object",
+ .def("evaluateAsSE3", &se3ReturnPinocchio, "Evaluate the curve at the given time. Return as a pinocchio.SE3 object",
args("self", "t"))
- .def("derivateAsMotion", &se3ReturnDerivate,
+ .def("derivateAsMotion", &se3ReturnDerivatePinocchio,
"Evaluate the derivative of order N of curve at time t. Return as a pinocchio.Motion",
args("self", "t", "N"))
#endif // CURVES_WITH_PINOCCHIO_SUPPORT
diff --git a/python/python_variables.h b/python/python_variables.h
index 59ae2fad01b81c619f6eeaac5823437f0517efd2..bc380f7abdd4878f411e5941ec615dd1ad9da6b1 100644
--- a/python/python_variables.h
+++ b/python/python_variables.h
@@ -99,6 +99,7 @@ typedef curves::piecewise_curve<real, real, true, linear_variable_t,
typedef curves::exact_cubic<real, real, true, pointX_t, t_pointX_t> exact_cubic_t;
typedef SO3Linear<double, double, true> SO3Linear_t;
typedef SE3Curve<double, double, true> SE3Curve_t;
+typedef curves::piecewise_curve<real, real, true, SE3Curve_t::point_t, t_pointX_t, SE3Curve_t,SE3Curve_t::point_derivate_t> piecewise_SE3_curve_t;
typedef curves::Bern<double> bernstein_t;
/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
@@ -117,6 +118,7 @@ EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::piecewise_bezier_linear_c
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::exact_cubic_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::SO3Linear_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::SE3Curve_t)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::piecewise_SE3_curve_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::matrix_x_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::pointX_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::linear_variable_t)
diff --git a/python/test/test.py b/python/test/test.py
index 2d3709567fe9113400beaf134682d980a05d4ddf..71be59d9f9390d4d622b9ce3867152d6a4fbd4d7 100644
--- a/python/test/test.py
+++ b/python/test/test.py
@@ -10,7 +10,7 @@ from numpy.linalg import norm
from curves import (CURVES_WITH_PINOCCHIO_SUPPORT, Quaternion, SE3Curve, SO3Linear, bezier, bezier3,
bezier_from_hermite, bezier_from_polynomial, cubic_hermite_spline, curve_constraints, exact_cubic,
hermite_from_bezier, hermite_from_polynomial, piecewise_bezier_curve,
- piecewise_cubic_hermite_curve, piecewise_polynomial_curve, polynomial, polynomial_from_bezier,
+ piecewise_cubic_hermite_curve, piecewise_polynomial_curve,piecewise_SE3_curve, polynomial, polynomial_from_bezier,
polynomial_from_hermite)
eigenpy.switchToNumpyArray()
@@ -339,6 +339,37 @@ class TestCurves(unittest.TestCase):
pc_test.loadFromText("serialization_pc.test")
self.assertTrue((pc(0.4) == pc_test(0.4)).all())
os.remove("serialization_pc.test")
+
+
+ waypoints3 = array([[1., 2., 3.,0.6,-9.], [-1., 1.6, 1.7,6.7,14]]).transpose()
+ c = polynomial(waypoints3, 3., 5.2)
+ with self.assertRaises(ValueError):# a and c doesn't have the same dimension
+ pc.append(c)
+
+ ### Test the different append methods :
+ pc = piecewise_polynomial_curve()
+ self.assertEqual(pc.num_curves(),0)
+ end_point1 = array([1.,3.,5.,6.5,-2.])
+ max1 = 2.5
+ with self.assertRaises(RuntimeError): # cannot add final point in an empty curve
+ pc.append(end_point1.reshape(-1,1),max1)
+ with self.assertRaises(ValueError):# a and end_point1 doesn't have the same dimension
+ pc.append(a)
+ pc.append(end_point1.reshape(-1,1),max1)
+
+ pc = piecewise_polynomial_curve()
+ d = polynomial(waypoints3, 0., 1.2)
+ self.assertEqual(pc.num_curves(),0)
+ pc.append(d)
+ self.assertEqual(pc.num_curves(),1)
+ pc.append(end_point1.reshape(-1,1),max1)
+ self.assertEqual(pc.num_curves(),2)
+ self.assertEqual(pc.min(),0.)
+ self.assertEqual(pc.max(),max1)
+ self.assertTrue(pc.is_continuous(0))
+ self.assertTrue(isclose(pc(0.),d(0.)).all())
+ self.assertTrue(isclose(pc(max1),end_point1).all())
+
return
def test_piecewise_from_points_list(self):
@@ -399,7 +430,7 @@ class TestCurves(unittest.TestCase):
a = bezier(waypoints, 0., 1.)
b = bezier(waypoints, 1., 2.)
pc = piecewise_bezier_curve(a)
- pc.add_curve(b)
+ pc.append(b)
pc.min()
pc.max()
pc(0.4)
@@ -427,7 +458,7 @@ class TestCurves(unittest.TestCase):
a = cubic_hermite_spline(points, tangents, time_points0)
b = cubic_hermite_spline(points, tangents, time_points1)
pc = piecewise_cubic_hermite_curve(a)
- pc.add_curve(b)
+ pc.append(b)
pc.min()
pc.max()
pc(0.4)
@@ -526,13 +557,178 @@ class TestCurves(unittest.TestCase):
self.assertTrue(norm(a(0.3) - a_bc(0.3)) < __EPS)
return
+ def test_piecewise_se3_curve(self):
+ init_quat = Quaternion.Identity()
+ end_quat = Quaternion(sqrt(2.) / 2., sqrt(2.) / 2., 0, 0)
+ init_rot = init_quat.matrix()
+ end_rot = end_quat.matrix()
+ waypoints = array([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.]]).transpose()
+ min = 0.2
+ max = 1.5
+ translation = bezier(waypoints, min, max)
+ # test with bezier
+ se3 = SE3Curve(translation, init_rot, end_rot)
+ pc = piecewise_SE3_curve(se3)
+ self.assertEqual(pc.num_curves(),1)
+ self.assertEqual(pc.min(), min)
+ self.assertEqual(pc.max(), max)
+ pmin = pc(min)
+ pmax = pc(max)
+ self.assertTrue(isclose(pmin[:3, :3], init_rot).all())
+ self.assertTrue(isclose(pmax[:3, :3], end_rot).all())
+ self.assertTrue(isclose(pmin[0:3, 3], translation(min)).all())
+ self.assertTrue(isclose(pmax[0:3, 3], translation(max)).all())
+ # add another curve :
+ end_pos2 = array([-2,0.2,1.6])
+ max2 = 2.7
+ se3_2 = SE3Curve(translation(max).reshape(-1,1),end_pos2.reshape(-1,1),end_rot,end_rot,max,max2)
+ pc.append(se3_2)
+ self.assertEqual(pc.num_curves(),2)
+ pmin2 = pc(max)
+ pmax2 = pc(max2)
+ self.assertTrue(isclose(pmin2[:3, :3], end_rot).all())
+ self.assertTrue(isclose(pmax2[:3, :3], end_rot).all())
+ self.assertTrue(isclose(pmin2[0:3, 3], se3_2.translation(max)).all())
+ self.assertTrue(isclose(pmax2[0:3, 3], se3_2.translation(max2)).all())
+ self.assertTrue(pc.is_continuous(0))
+ self.assertFalse(pc.is_continuous(1))
+
+ # check if error are correctly raised :
+ with self.assertRaises(ValueError): # time intervals do not match
+ se3_3 = SE3Curve(se3_2(max2),se3_2(max2-0.5),max2-0.5,max2+1.5)
+ pc.append(se3_3)
+ with self.assertRaises(ValueError):
+ se3_3 = SE3Curve(se3_2(max2),se3_2(max2-0.5),max2+0.1,max2+1.5)
+ pc.append(se3_3)
+
+ # TODO : serialization
+
+ se3_3 = SE3Curve(se3(max),se3_2(max2-0.5),max2,max2+1.5)
+ pc.append(se3_3)
+ self.assertFalse(pc.is_continuous(0))
+
+
+ ### test the different append methods :
+ init_quat = Quaternion.Identity()
+ end_quat = Quaternion(sqrt(2.) / 2., sqrt(2.) / 2., 0, 0)
+ init_rot = init_quat.matrix()
+ end_rot = end_quat.matrix()
+ waypoints = array([[1., 2., 3.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.], [4., 5., 6.]]).transpose()
+ min = 0.2
+ max = 1.5
+ translation = bezier(waypoints, min, max)
+ # test with bezier
+ se3 = SE3Curve(translation, init_rot, end_rot)
+ pc = piecewise_SE3_curve()
+ self.assertEqual(pc.num_curves(),0)
+ pc.append(se3)
+ self.assertEqual(pc.num_curves(),1)
+ self.assertEqual(pc.min(), min)
+ self.assertEqual(pc.max(), max)
+ pmin = pc(min)
+ pmax = pc(max)
+ self.assertTrue(isclose(pmin[:3, :3], init_rot).all())
+ self.assertTrue(isclose(pmax[:3, :3], end_rot).all())
+ self.assertTrue(isclose(pmin[0:3, 3], translation(min)).all())
+ self.assertTrue(isclose(pmax[0:3, 3], translation(max)).all())
+ # append a final tranform :
+ end_quat = Quaternion(sqrt(2.) / 2., 0., sqrt(2.) / 2., 0)
+ end_rot = end_quat.matrix()
+ end_translation = array([1.7, -0.8, 3.]).T
+ end_pose = array(np.identity(4))
+ end_pose[:3, :3] = end_rot
+ end_pose[:3, 3] = end_translation
+ max2 = 3.8
+ pc.append(end_pose,max2)
+ self.assertEqual(pc.num_curves(),2)
+ self.assertEqual(pc.min(), min)
+ self.assertEqual(pc.max(), max2)
+ pmin = pc(min)
+ pmax = pc(max2)
+ self.assertTrue(isclose(pmin[:3, :3], init_rot).all())
+ self.assertTrue(isclose(pmax[:3, :3], end_rot).all())
+ self.assertTrue(isclose(pmin[0:3, 3], translation(min)).all())
+ self.assertTrue(isclose(pmax[0:3, 3], end_translation).all())
+ self.assertTrue(pc.is_continuous(0))
+ if CURVES_WITH_PINOCCHIO_SUPPORT:
+ end_quat = Quaternion(sqrt(2.) / 2., 0., 0, sqrt(2.) / 2.)
+ end_rot = end_quat.matrix()
+ end_translation = array([-17., 3.7, 1.])
+ end_pose = SE3.Identity()
+ end_pose.rotation = end_rot
+ end_pose.translation = end_translation.reshape(-1,1)
+ max3 = 6.5
+ pc.append(end_pose,max3)
+ self.assertEqual(pc.num_curves(),3)
+ self.assertEqual(pc.min(), min)
+ self.assertEqual(pc.max(), max3)
+ pmin = pc(min)
+ pmax = pc(max3)
+ self.assertTrue(isclose(pmin[:3, :3], init_rot).all())
+ self.assertTrue(isclose(pmax[:3, :3], end_rot).all())
+ self.assertTrue(isclose(pmin[0:3, 3], translation(min)).all())
+ self.assertTrue(isclose(pmax[0:3, 3], end_translation).all())
+ self.assertTrue(pc.is_continuous(0))
+ pc = piecewise_SE3_curve()
+ with self.assertRaises(RuntimeError):
+ pc.append(end_pose,max)
+
+ if CURVES_WITH_PINOCCHIO_SUPPORT:
+
+ def test_piecewise_se3_curve_linear_pinocchio(self):
+ print("test piecewise SE3 pinocchio")
+ init_quat = Quaternion.Identity()
+ end_quat = Quaternion(sqrt(2.) / 2., sqrt(2.) / 2., 0, 0)
+ init_rot = init_quat.matrix()
+ end_rot = end_quat.matrix()
+ init_translation = array([0.2, -0.7, 0.6])
+ end_translation = array([3.6, -2.2, -0.9])
+ init_pose = SE3.Identity()
+ end_pose = SE3.Identity()
+ init_pose.rotation = init_rot
+ end_pose.rotation = end_rot
+ init_pose.translation = init_translation.reshape(-1,1)
+ end_pose.translation = end_translation.reshape(-1,1)
+ min = 0.7
+ max = 12.
+ se3 = SE3Curve(init_pose, end_pose, min, max)
+ pc = piecewise_SE3_curve(se3)
+ self.assertEqual(pc.num_curves(),1)
+ p = pc.evaluateAsSE3(min)
+ self.assertTrue(isinstance(p, SE3))
+ self.assertTrue(pc.evaluateAsSE3(min).isApprox(init_pose, 1e-6))
+ self.assertTrue(pc.evaluateAsSE3(max).isApprox(end_pose, 1e-6))
+ self.assertEqual(pc.min(), min)
+ self.assertEqual(pc.max(), max)
+ self.assertTrue(isclose(pc.rotation(min), init_rot).all())
+ self.assertTrue(isclose(pc.translation(min), init_translation).all())
+ self.assertTrue(isclose(pc.rotation(max), end_rot).all())
+ self.assertTrue(isclose(pc.translation(max), end_translation).all())
+ # add another curve :
+ end_translation2 = array([-2., 1.6, -14.])
+ end_pose2 = SE3.Identity()
+ end_pose2.rotation = end_rot
+ end_pose2.translation = end_translation2.reshape(-1,1)
+ max2 = 23.9
+ se3_2 = SE3Curve(end_pose, end_pose2, max, max2)
+ pc.append(se3_2)
+ self.assertEqual(pc.num_curves(),2)
+ p = pc.evaluateAsSE3(max2)
+ self.assertTrue(isinstance(p, SE3))
+ self.assertTrue(pc.evaluateAsSE3(max2).isApprox(end_pose2, 1e-6))
+ self.assertEqual(pc.min(), min)
+ self.assertEqual(pc.max(), max2)
+ self.assertTrue(isclose(pc.rotation(max2), end_rot).all())
+ self.assertTrue(isclose(pc.translation(max2), end_translation2).all())
+
+
def test_conversion_piecewise_curves(self):
__EPS = 1e-6
waypoints = array([[1., 2., 3.], [4., 5., 6.]]).transpose()
a = bezier(waypoints, 0., 1.)
b = bezier(waypoints, 1., 2.)
pc = piecewise_bezier_curve(a)
- pc.add_curve(b)
+ pc.append(b)
# Convert to piecewise polynomial
pc_pol = pc.convert_piecewise_curve_to_polynomial()
self.assertTrue(norm(pc_pol(0.3) - pc(0.3)) < __EPS)
@@ -600,6 +796,9 @@ class TestCurves(unittest.TestCase):
so3Rot.derivate(3., 1)
with self.assertRaises(ValueError):
so3Rot.derivate(1., 0)
+ with self.assertRaises(ValueError):
+ test = SO3Linear(init_rot,end_rot,max,min)
+
def test_se3_curve_linear(self):
print("test SE3 Linear")
@@ -621,10 +820,8 @@ class TestCurves(unittest.TestCase):
p = se3(min)
if CURVES_WITH_PINOCCHIO_SUPPORT:
self.assertTrue(isinstance(se3.evaluateAsSE3(min), SE3))
- init_pose = SE3(init_pose)
- end_pose = SE3(end_pose)
- self.assertTrue(se3.evaluateAsSE3(min).isApprox(init_pose, 1e-6))
- self.assertTrue(se3.evaluateAsSE3(max).isApprox(end_pose, 1e-6))
+ self.assertTrue(se3.evaluateAsSE3(min).isApprox(SE3(init_pose), 1e-6))
+ self.assertTrue(se3.evaluateAsSE3(max).isApprox(SE3(end_pose), 1e-6))
self.assertEqual(p.shape[0], 4)
self.assertEqual(p.shape[1], 4)
self.assertTrue(isclose(se3(min), init_pose).all())
@@ -643,8 +840,8 @@ class TestCurves(unittest.TestCase):
self.assertTrue(isclose(motion.linear, ((end_translation - init_translation) / (max - min))).all())
self.assertTrue(isclose(motion.angular[0], 1.20830487))
self.assertTrue(isclose(motion.angular[1:3], array([0, 0]).T).all())
- self.assertTrue(d.isApprox(se3.derivateAsMotion(0.5, 1), 1e-6))
- self.assertTrue(d.isApprox(se3.derivateAsMotion(max, 1), 1e-6))
+ self.assertTrue(motion.isApprox(se3.derivateAsMotion(0.5, 1), 1e-6))
+ self.assertTrue(motion.isApprox(se3.derivateAsMotion(max, 1), 1e-6))
self.assertTrue(se3.derivateAsMotion(min, 2).isApprox(Motion.Zero(), 1e-6))
self.assertTrue(se3.derivateAsMotion(min, 3).isApprox(Motion.Zero(), 1e-6))
self.assertEqual(d.shape[0], 6)
@@ -669,6 +866,8 @@ class TestCurves(unittest.TestCase):
se3.derivate(3., 1)
with self.assertRaises(ValueError):
se3.derivate(1., 0)
+ with self.assertRaises(ValueError):
+ test = SE3Curve(init_pose,end_pose,max,min)
def test_se3_from_translation_curve(self):
print("test SE3 From translation curves")
@@ -830,14 +1029,14 @@ class TestCurves(unittest.TestCase):
end_quat = Quaternion(sqrt(2.) / 2., sqrt(2.) / 2., 0, 0)
init_rot = init_quat.matrix()
end_rot = end_quat.matrix()
- init_translation = array([0.2, -0.7, 0.6]).T
- end_translation = array([3.6, -2.2, -0.9]).T
+ init_translation = array([0.2, -0.7, 0.6])
+ end_translation = array([3.6, -2.2, -0.9])
init_pose = SE3.Identity()
end_pose = SE3.Identity()
init_pose.rotation = init_rot
end_pose.rotation = end_rot
- init_pose.translation = init_translation
- end_pose.translation = end_translation
+ init_pose.translation = init_translation.reshape(-1,1)
+ end_pose.translation = end_translation.reshape(-1,1)
min = 0.7
max = 12.
se3 = SE3Curve(init_pose, end_pose, min, max)
@@ -867,11 +1066,11 @@ class TestCurves(unittest.TestCase):
with self.assertRaises(ValueError):
se3(-0.1)
with self.assertRaises(ValueError):
- se3(3)
+ se3(13.)
with self.assertRaises(ValueError):
se3.derivate(0, 1)
with self.assertRaises(ValueError):
- se3.derivate(3., 1)
+ se3.derivate(13., 1)
with self.assertRaises(ValueError):
se3.derivate(1., 0)