diff --git a/include/curves/bezier_curve.h b/include/curves/bezier_curve.h
index 36eb7e1a63f54a35576b1e6795b13b24c41ac0ff..ee15b7df95578a3f63efefa9e9ddad0f120f2862 100644
--- a/include/curves/bezier_curve.h
+++ b/include/curves/bezier_curve.h
@@ -39,8 +39,10 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
typedef typename t_point_t::const_iterator cit_point_t;
typedef bezier_curve<Time, Numeric, Safe, Point> bezier_curve_t;
- typedef piecewise_curve<Time, Numeric, Safe, point_t, t_point_t, bezier_curve_t> piecewise_bezier_curve_t;
+ typedef boost::shared_ptr<bezier_curve_t> bezier_curve_ptr_t;
+ typedef piecewise_curve<Time, Numeric, Safe, point_t> piecewise_curve_t;
typedef curve_abc<Time, Numeric, Safe, point_t> curve_abc_t; // parent class
+ typedef typename curve_abc_t::curve_ptr_t curve_ptr_t;
/* Constructors - destructors */
public:
@@ -158,6 +160,13 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
return deriv.compute_derivate(order - 1);
}
+ /// \brief Compute the derived curve at order N.
+ /// \param order : order of derivative.
+ /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
+ bezier_curve_t* compute_derivate_ptr(const std::size_t order) const {
+ return new bezier_curve_t(compute_derivate(order));
+ }
+
/// \brief Compute the primitive of the curve at order N.
/// Computes the primitive at order N of bezier curve of parametric equation \f$x(t)\f$. <br>
/// At order \f$N=1\f$, the primitve \f$X(t)\f$ of \f$x(t)\f$ is such as \f$\frac{dX(t)}{dt} = x(t)\f$.
@@ -190,8 +199,7 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$ point corresponding on derived curve of order N at time t.
///
virtual point_t derivate(const time_t t, const std::size_t order) const {
- bezier_curve_t deriv = compute_derivate(order);
- return deriv(t);
+ return compute_derivate(order)(t);
}
/// \brief Evaluate all Bernstein polynomes for a certain degree.
@@ -325,12 +333,12 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
}
/// \brief Split the bezier curve in several curves, all accessible
- /// within a piecewise_bezier_curve_t.
+ /// within a piecewise_curve_t.
/// \param times : list of times of size n.
- /// \return a piecewise_bezier_curve_t comprising n+1 curves
+ /// \return a piecewise_curve_t comprising n+1 curves
///
- piecewise_bezier_curve_t split(const vector_x_t& times) const {
- typename piecewise_bezier_curve_t::t_curve_t curves;
+ piecewise_curve_t split(const vector_x_t& times) const {
+ std::vector<bezier_curve_t> curves;
bezier_curve_t current = *this;
for (int i = 0; i < times.rows(); ++i) {
std::pair<bezier_curve_t, bezier_curve_t> pairsplit = current.split(times[i]);
@@ -338,7 +346,12 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
current = pairsplit.second;
}
curves.push_back(current);
- return piecewise_bezier_curve_t(curves);
+ piecewise_curve_t res;
+ for(typename std::vector<bezier_curve_t>::const_iterator cit = curves.begin(); cit != curves.end() ; ++cit){
+ typename piecewise_curve_t::curve_ptr_t ptr(new bezier_curve_t(*cit));
+ res.add_curve_ptr(ptr);
+ }
+ return res;
}
/// \brief Extract a bezier curve defined between \f$[t_1,t_2]\f$ from the actual bezier curve
@@ -414,6 +427,9 @@ struct bezier_curve : public curve_abc<Time, Numeric, Safe, Point> {
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$, upper bound of time range.
virtual time_t max() const { return T_max_; }
+ /// \brief Get the degree of the curve.
+ /// \return \f$degree\f$, the degree of the curve.
+ virtual std::size_t degree() const {return degree_;}
/*Helpers*/
/* Attributes */
diff --git a/include/curves/cubic_hermite_spline.h b/include/curves/cubic_hermite_spline.h
index 79a7a9b76643d931bec3e141832d47b53a58efcf..e7c7efe0d668dbccb6d7f450936bcb38db3f7eb3 100644
--- a/include/curves/cubic_hermite_spline.h
+++ b/include/curves/cubic_hermite_spline.h
@@ -39,6 +39,7 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
typedef std::vector<Time> vector_time_t;
typedef Numeric num_t;
typedef curve_abc<Time, Numeric, Safe, point_t> curve_abc_t; // parent class
+ typedef cubic_hermite_spline<Time, Numeric, Safe, point_t> cubic_hermite_spline_t;
public:
@@ -112,6 +113,19 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
return evalCubicHermiteSpline(t, order);
}
+ cubic_hermite_spline_t compute_derivate(const std::size_t /*order*/) const {
+ throw std::logic_error("Compute derivate for cubic hermite spline is not implemented yet.");
+ }
+
+ /// \brief Compute the derived curve at order N.
+ /// \param order : order of derivative.
+ /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
+ cubic_hermite_spline_t* compute_derivate_ptr(const std::size_t order) const {
+ return new cubic_hermite_spline_t(compute_derivate(order));
+ }
+
+
+
/// \brief Set time of each control point of cubic hermite spline.
/// Set duration of each spline, Exemple : \f$( 0., 0.5, 0.9, ..., 4.5 )\f$ with
/// values corresponding to times for \f$P_0, P_1, P_2, ..., P_N\f$ respectively.<br>
@@ -325,6 +339,9 @@ struct cubic_hermite_spline : public curve_abc<Time, Numeric, Safe, Point> {
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$, upper bound of time range.
Time virtual max() const { return time_control_points_.back(); }
+ /// \brief Get the degree of the curve.
+ /// \return \f$degree\f$, the degree of the curve.
+ virtual std::size_t degree() const {return degree_;}
/*Helpers*/
/*Attributes*/
diff --git a/include/curves/curve_abc.h b/include/curves/curve_abc.h
index acfa3c78dbe59c79098af133a1a366fa7a608cb2..72a2e31a8061439ba75db285532c3e8dcb6454cd 100644
--- a/include/curves/curve_abc.h
+++ b/include/curves/curve_abc.h
@@ -15,6 +15,7 @@
#include "serialization/archive.hpp"
#include "serialization/eigen-matrix.hpp"
#include <boost/serialization/shared_ptr.hpp>
+#include <boost/smart_ptr/shared_ptr.hpp>
#include <functional>
@@ -28,6 +29,8 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
typedef Point point_t;
typedef Point_derivate point_derivate_t;
typedef Time time_t;
+ typedef curve_abc<Time, Numeric, Safe, point_t,point_derivate_t> curve_t; // parent class
+ typedef boost::shared_ptr<curve_t> curve_ptr_t;
/* Constructors - destructors */
public:
@@ -44,12 +47,19 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
/// \return \f$x(t)\f$, point corresponding on curve at time t.
virtual point_t operator()(const time_t t) const = 0;
+
+ /// \brief Compute the derived curve at order N.
+ /// \param order : order of derivative.
+ /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
+ virtual curve_t* compute_derivate_ptr(const std::size_t order) const = 0;
+
/// \brief Evaluate the derivative of order N of curve at time t.
/// \param t : time when to evaluate the spline.
/// \param order : order of derivative.
/// \return \f$\frac{d^Nx(t)}{dt^N}\f$, point corresponding on derivative curve of order N at time t.
virtual point_derivate_t derivate(const time_t t, const std::size_t order) const = 0;
+
/*Operations*/
/*Helpers*/
@@ -62,6 +72,10 @@ struct curve_abc : std::unary_function<Time, Point>, public serialization::Seria
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$, upper bound of time range.
virtual time_t max() const = 0;
+ /// \brief Get the degree of the curve.
+ /// \return \f$degree\f$, the degree of the curve.
+ virtual std::size_t degree() const =0;
+
std::pair<time_t, time_t> timeRange() { return std::make_pair(min(), max()); }
/*Helpers*/
diff --git a/include/curves/curve_conversion.h b/include/curves/curve_conversion.h
index 3bb3a434bb313053bf8399dad4fb10455f8e14f7..194eac1a156a2ee2ad23ffa2c16b431c4ba49914 100644
--- a/include/curves/curve_conversion.h
+++ b/include/curves/curve_conversion.h
@@ -16,38 +16,38 @@ namespace curves {
/// \brief Converts a cubic hermite spline or a bezier curve to a polynomial.
/// \param curve : the bezier curve/cubic hermite spline defined between [Tmin,Tmax] to convert.
/// \return the equivalent polynomial.
-template <typename Polynomial, typename curveTypeToConvert>
-Polynomial polynomial_from_curve(const curveTypeToConvert& curve) {
+template <typename Polynomial>
+Polynomial polynomial_from_curve(const typename Polynomial::curve_abc_t& curve) {
typedef typename Polynomial::t_point_t t_point_t;
typedef typename Polynomial::num_t num_t;
t_point_t coefficients;
- curveTypeToConvert current(curve);
coefficients.push_back(curve(curve.min()));
num_t fact = 1;
- for (std::size_t i = 1; i <= curve.degree_; ++i) {
+ for (std::size_t i = 1; i <= curve.degree(); ++i) {
fact *= (num_t)i;
- coefficients.push_back(current.derivate(current.min(), i) / fact);
+ coefficients.push_back(curve.derivate(curve.min(), i) / fact);
}
+
+
return Polynomial(coefficients, curve.min(), curve.max());
}
/// \brief Converts a cubic hermite spline or polynomial of order 3 or less to a cubic bezier curve.
/// \param curve : the polynomial of order 3 or less/cubic hermite spline defined between [Tmin,Tmax] to convert.
/// \return the equivalent cubic bezier curve.
-template <typename Bezier, typename curveTypeToConvert>
-Bezier bezier_from_curve(const curveTypeToConvert& curve) {
+template <typename Bezier>
+Bezier bezier_from_curve(const typename Bezier::curve_abc_t& curve) {
typedef typename Bezier::point_t point_t;
typedef typename Bezier::t_point_t t_point_t;
typedef typename Bezier::num_t num_t;
- curveTypeToConvert current(curve);
- num_t T_min = current.min();
- num_t T_max = current.max();
+ num_t T_min = curve.min();
+ num_t T_max = curve.max();
num_t T = T_max - T_min;
// Positions and derivatives
- point_t p0 = current(T_min);
- point_t p1 = current(T_max);
- point_t m0 = current.derivate(T_min, 1);
- point_t m1 = current.derivate(T_max, 1);
+ point_t p0 = curve(T_min);
+ point_t p1 = curve(T_max);
+ point_t m0 = curve.derivate(T_min, 1);
+ point_t m1 = curve.derivate(T_max, 1);
// Convert to bezier control points
// for t in [Tmin,Tmax] and T=Tmax-Tmin : x'(0)=3(b_p1-b_p0)/T and x'(1)=3(b_p3-b_p2)/T
// so : m0=3(b_p1-b_p0)/T and m1=3(b_p3-b_p2)/T
@@ -61,26 +61,25 @@ Bezier bezier_from_curve(const curveTypeToConvert& curve) {
control_points.push_back(b_p1);
control_points.push_back(b_p2);
control_points.push_back(b_p3);
- return Bezier(control_points.begin(), control_points.end(), current.min(), current.max());
+ return Bezier(control_points.begin(), control_points.end(), curve.min(), curve.max());
}
/// \brief Converts a polynomial of order 3 or less/cubic bezier curve to a cubic hermite spline.
/// \param curve : the polynomial of order 3 or less/cubic bezier curve defined between [Tmin,Tmax] to convert.
/// \return the equivalent cubic hermite spline.
-template <typename Hermite, typename curveTypeToConvert>
-Hermite hermite_from_curve(const curveTypeToConvert& curve) {
+template <typename Hermite>
+Hermite hermite_from_curve(const typename Hermite::curve_abc_t& curve) {
typedef typename Hermite::pair_point_tangent_t pair_point_tangent_t;
typedef typename Hermite::t_pair_point_tangent_t t_pair_point_tangent_t;
typedef typename Hermite::point_t point_t;
typedef typename Hermite::num_t num_t;
- curveTypeToConvert current(curve);
- num_t T_min = current.min();
- num_t T_max = current.max();
+ num_t T_min = curve.min();
+ num_t T_max = curve.max();
// Positions and derivatives
- point_t p0 = current(T_min);
- point_t p1 = current(T_max);
- point_t m0 = current.derivate(T_min, 1);
- point_t m1 = current.derivate(T_max, 1);
+ point_t p0 = curve(T_min);
+ point_t p1 = curve(T_max);
+ point_t m0 = curve.derivate(T_min, 1);
+ point_t m1 = curve.derivate(T_max, 1);
// Create pairs pos/vel
pair_point_tangent_t pair0(p0, m0);
pair_point_tangent_t pair1(p1, m1);
diff --git a/include/curves/exact_cubic.h b/include/curves/exact_cubic.h
index 4991067286d775d4fbd7bf10389b125fb48b7096..994c9604625a580c6b04394315a0bf0e3d4975bf 100644
--- a/include/curves/exact_cubic.h
+++ b/include/curves/exact_cubic.h
@@ -39,7 +39,7 @@ template <typename Time = double, typename Numeric = Time, bool Safe = false,
typename Point = Eigen::Matrix<Numeric, Eigen::Dynamic, 1>,
typename T_Point = std::vector<Point, Eigen::aligned_allocator<Point> >,
typename SplineBase = polynomial<Time, Numeric, Safe, Point, T_Point> >
-struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point, T_Point, SplineBase> {
+struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point> {
typedef Point point_t;
typedef T_Point t_point_t;
typedef Eigen::Matrix<Numeric, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
@@ -53,8 +53,9 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point, T_Point,
typedef curve_constraints<Point> spline_constraints;
typedef exact_cubic<Time, Numeric, Safe, Point, T_Point, SplineBase> exact_cubic_t;
- typedef piecewise_curve<Time, Numeric, Safe, Point, T_Point, SplineBase> piecewise_curve_t;
typedef curve_abc<Time, Numeric, Safe, point_t> curve_abc_t; // parent class
+ typedef piecewise_curve<Time, Numeric, Safe, Point> piecewise_curve_t;
+ typedef typename piecewise_curve_t::t_curve_ptr_t t_curve_ptr_t;
/* Constructors - destructors */
public:
@@ -68,7 +69,13 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point, T_Point,
///
template <typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd)
- : piecewise_curve_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd)) {}
+ : piecewise_curve_t()
+ {
+ t_spline_t subSplines = computeWayPoints<In>(wayPointsBegin, wayPointsEnd);
+ for (cit_spline_t it = subSplines.begin() ; it != subSplines.end() ; ++it){
+ this->add_curve(*it);
+ }
+ }
/// \brief Constructor.
/// \param wayPointsBegin : an iterator pointing to the first element of a waypoint container.
@@ -77,11 +84,24 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point, T_Point,
///
template <typename In>
exact_cubic(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints)
- : piecewise_curve_t(computeWayPoints<In>(wayPointsBegin, wayPointsEnd, constraints)) {}
+ : piecewise_curve_t()
+ {
+ t_spline_t subSplines = computeWayPoints<In>(wayPointsBegin, wayPointsEnd,constraints);
+ for (cit_spline_t it = subSplines.begin() ; it != subSplines.end() ; ++it){
+ this->add_curve(*it);
+ }
+ }
/// \brief Constructor.
/// \param subSplines: vector of subSplines.
- exact_cubic(const t_spline_t& subSplines) : piecewise_curve_t(subSplines) {}
+ exact_cubic(const t_spline_t& subSplines) : piecewise_curve_t()
+ {
+ for (cit_spline_t it = subSplines.begin() ; it != subSplines.end() ; ++it){
+ this->add_curve(*it);
+ }
+ }
+
+ exact_cubic(const t_curve_ptr_t& subSplines) : piecewise_curve_t(subSplines) { }
/// \brief Copy Constructor.
exact_cubic(const exact_cubic& other) : piecewise_curve_t(other) {}
@@ -91,7 +111,13 @@ struct exact_cubic : public piecewise_curve<Time, Numeric, Safe, Point, T_Point,
std::size_t getNumberSplines() { return this->getNumberCurves(); }
- spline_t getSplineAt(std::size_t index) { return this->curves_.at(index); }
+ spline_t getSplineAt(std::size_t index) {
+ boost::shared_ptr<spline_t> s_ptr = boost::dynamic_pointer_cast<spline_t>(this->curves_.at(index));
+ if(s_ptr)
+ return *s_ptr;
+ else
+ throw std::runtime_error("Parent piecewise curve do not contain only curves created from exact_cubic class methods");
+ }
private:
/// \brief Compute polynom of exact cubic spline from waypoints.
diff --git a/include/curves/fwd.h b/include/curves/fwd.h
index 78098e0882f7d165b90c32cd1dd0fdf810a78041..da951b88b335843b00a1e537a59f7008faeef3d9 100644
--- a/include/curves/fwd.h
+++ b/include/curves/fwd.h
@@ -9,6 +9,9 @@
#ifndef CURVES_FWD_H
#define CURVES_FWD_H
+#include <Eigen/Dense>
+#include <vector>
+#include <boost/smart_ptr/shared_ptr.hpp>
namespace curves {
@@ -21,12 +24,12 @@ template <typename Time, typename Numeric, bool Safe,typename Point >
template <typename Time, typename Numeric, bool Safe,typename Point >
struct cubic_hermite_spline;
-template <typename Time, typename Numeric, bool Safe, typename Point ,
- typename T_Point , typename SplineBase >
+template <typename Time, typename Numeric, bool Safe, typename Point,
+ typename T_Point,typename SplineBase >
struct exact_cubic;
template <typename Time, typename Numeric, bool Safe, typename Point,
- typename T_Point, typename Curve, typename Point_derivate>
+ typename Point_derivate>
struct piecewise_curve;
template <typename Time, typename Numeric, bool Safe,typename Point, typename T_Point>
@@ -49,6 +52,50 @@ template <typename Numeric, bool Safe>
template <typename Numeric>
struct quadratic_variable;
+
+// typedef of the commonly used templates arguments :
+// eigen types :
+typedef Eigen::Vector3d point3_t;
+typedef Eigen::Matrix<double, 6, 1> point6_t;
+typedef Eigen::VectorXd pointX_t;
+typedef Eigen::Matrix<double, 3, 3> matrix3_t;
+typedef Eigen::Matrix<double, 4, 4> matrix4_t;
+typedef Eigen::Quaternion<double> quaternion_t;
+typedef Eigen::Transform<double, 3, Eigen::Affine> transform_t;
+typedef std::vector<pointX_t, Eigen::aligned_allocator<point3_t> > t_point3_t;
+typedef std::vector<pointX_t, Eigen::aligned_allocator<pointX_t> > t_pointX_t;
+
+// abstract curves types:
+typedef curve_abc<double, double, true, pointX_t,pointX_t> curve_abc_t; // base abstract class
+typedef curve_abc<double, double, true, point3_t,point3_t> curve_3_t; // generic class of curve of size 3
+typedef curve_abc<double, double, true, matrix3_t, point3_t> curve_rotation_t; // templated class used for the rotation (return dimension are fixed)
+typedef curve_abc<double, double, true, transform_t, point6_t> curve_SE3_t; // templated abstract class used for all the se3 curves (return dimension are fixed)
+
+// shared pointer to abstract types:
+typedef boost::shared_ptr<curve_abc_t> curve_ptr_t;
+typedef boost::shared_ptr<curve_3_t> curve3_ptr_t;
+typedef boost::shared_ptr<curve_rotation_t> curve_rotation_ptr_t;
+typedef boost::shared_ptr<curve_SE3_t> curve_SE3_ptr_t;
+
+// definition of all curves class with pointX as return type:
+typedef polynomial<double, double, true, pointX_t, t_pointX_t> polynomial_t;
+typedef exact_cubic<double, double, true, pointX_t,t_pointX_t, polynomial_t> exact_cubic_t;
+typedef bezier_curve<double, double, true, pointX_t> bezier_t;
+typedef cubic_hermite_spline<double, double, true, pointX_t> cubic_hermite_spline_t;
+typedef piecewise_curve <double, double, true, pointX_t,pointX_t> piecewise_t;
+
+// definition of all curves class with point3 as return type:
+typedef polynomial<double, double, true, point3_t, t_point3_t> polynomial3_t;
+typedef exact_cubic<double, double, true, point3_t,t_point3_t, polynomial_t> exact_cubic3_t;
+typedef bezier_curve<double, double, true, point3_t> bezier3_t;
+typedef cubic_hermite_spline<double, double, true, point3_t> cubic_hermite_spline3_t;
+typedef piecewise_curve <double, double, true, point3_t,point3_t> piecewise3_t;
+
+// special curves with return type fixed:
+typedef SO3Linear<double, double, true> SO3Linear_t;
+typedef SE3Curve<double, double, true> SE3Curve_t;
+typedef piecewise_curve <double, double, true, transform_t, point6_t> piecewise_SE3_t;
+
}
#endif // CURVES_FWD_H
diff --git a/include/curves/helpers/effector_spline.h b/include/curves/helpers/effector_spline.h
index 383398d8e7e949911285c6b01076a61356d7f465..764d99f1150bb5139be6be48727a3baed781823e 100644
--- a/include/curves/helpers/effector_spline.h
+++ b/include/curves/helpers/effector_spline.h
@@ -102,9 +102,8 @@ exact_cubic_t* effector_spline(In wayPointsBegin, In wayPointsEnd, const Point&
spline_t end_spline =
make_end_spline(land_normal, landWaypoint.second, land_offset, landWaypoint.first, land_offset_duration);
spline_constraints_t constraints = compute_required_offset_velocity_acceleration(end_spline, land_offset_duration);
- exact_cubic_t all_but_end(waypoints.begin(), waypoints.end(), constraints);
- t_spline_t splines = all_but_end.curves_;
- splines.push_back(end_spline);
+ exact_cubic_t splines(waypoints.begin(), waypoints.end(), constraints);
+ splines.add_curve(end_spline);
return new exact_cubic_t(splines);
}
} // namespace helpers
diff --git a/include/curves/helpers/effector_spline_rotation.h b/include/curves/helpers/effector_spline_rotation.h
index aaf06d6649f0a4b88aa7d8f152e1506aae56c191..fed827998439c6e93ff9bcbd4ee02ce826168773 100644
--- a/include/curves/helpers/effector_spline_rotation.h
+++ b/include/curves/helpers/effector_spline_rotation.h
@@ -76,6 +76,14 @@ class rotation_spline : public curve_abc_quat_t {
throw std::runtime_error("TODO quaternion spline does not implement derivate");
}
+ /// \brief Compute the derived curve at order N.
+ /// \param order : order of derivative.
+ /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
+ curve_abc_quat_t* compute_derivate_ptr(const std::size_t /*order*/) const {
+ throw std::logic_error("Compute derivate for quaternion spline is not implemented yet.");
+ }
+
+
/// \brief Initialize time reparametrization for spline.
exact_cubic_constraint_one_dim computeWayPoints() const {
t_waypoint_one_dim_t waypoints;
@@ -87,6 +95,9 @@ class rotation_spline : public curve_abc_quat_t {
virtual std::size_t dim() const { return dim_; }
virtual time_t min() const { return min_; }
virtual time_t max() const { return max_; }
+ /// \brief Get the degree of the curve.
+ /// \return \f$degree\f$, the degree of the curve.
+ virtual std::size_t degree() const {return 1;}
/*Attributes*/
Eigen::Quaterniond quat_from_; // const
diff --git a/include/curves/optimization/integral_cost.h b/include/curves/optimization/integral_cost.h
index 28f92079046e74a977d0e0d534754d826e084004..dc5c5ce4cf9cea3357398c95c48bc0fb1c5dc6a8 100644
--- a/include/curves/optimization/integral_cost.h
+++ b/include/curves/optimization/integral_cost.h
@@ -34,7 +34,8 @@ quadratic_variable<Numeric> compute_integral_cost_internal(const problem_data<Po
typedef typename t_point_t::const_iterator cit_point_t;
bezier_t acc = pData.bezier->compute_derivate(num_derivate);
const t_point_t& wps = acc.waypoints();
- return bezier_product<Point, Numeric, cit_point_t>(wps.begin(), wps.end(), wps.begin(), wps.end(), pData.dim_);
+ quadratic_variable<Numeric> res(bezier_product<Point, Numeric, cit_point_t>(wps.begin(), wps.end(), wps.begin(), wps.end(), pData.dim_));
+ return res;
}
template <typename Point, typename Numeric>
diff --git a/include/curves/piecewise_curve.h b/include/curves/piecewise_curve.h
index c6b75d969c3123aaca8ac86943448ee2727a4bf3..359d21819fe5f97ade311dd4ecaf49ceeebbbb0c 100644
--- a/include/curves/piecewise_curve.h
+++ b/include/curves/piecewise_curve.h
@@ -10,6 +10,8 @@
#include "curve_abc.h"
#include "curve_conversion.h"
+#include <boost/smart_ptr/shared_ptr.hpp>
+#include <boost/serialization/vector.hpp>
namespace curves {
/// \class PiecewiseCurve.
@@ -23,21 +25,19 @@ 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>,
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 Point_derivate point_derivate_t;
+ typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
+ typedef std::vector<point_derivate_t, Eigen::aligned_allocator<point_derivate_t> > t_point_derivate_t;
typedef Time time_t;
typedef Numeric num_t;
- typedef Curve curve_t;
- typedef typename std::vector<curve_t> t_curve_t;
+ typedef curve_abc<Time, Numeric, Safe, point_t,point_derivate_t> curve_t; // parent class
+ typedef boost::shared_ptr<curve_t> curve_ptr_t;
+ typedef typename std::vector<curve_ptr_t> t_curve_ptr_t;
typedef typename std::vector<Time> t_time_t;
- typedef curve_abc<Time, Numeric, Safe, point_t,point_derivate_t> curve_abc_t; // parent class
-
-
+ typedef piecewise_curve<Time, Numeric, Safe, Point,Point_derivate> piecewise_curve_t;
public:
/// \brief Empty constructor. Add at least one curve to call other class functions.
///
@@ -47,19 +47,17 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
/// Initialize a piecewise curve by giving the first curve.
/// \param cf : a curve.
///
- piecewise_curve(const curve_t& cf) {
- dim_ = cf.dim();
- size_ = 0;
- add_curve(cf);
+ piecewise_curve(const curve_ptr_t& cf):
+ dim_(0), size_(0), T_min_(0), T_max_(0)
+ {
+ add_curve_ptr(cf);
}
- piecewise_curve(const t_curve_t list_curves) {
- if (list_curves.size() != 0) {
- dim_ = list_curves[0].dim();
- }
- size_ = 0;
- for (std::size_t i = 0; i < list_curves.size(); i++) {
- add_curve(list_curves[i]);
+ piecewise_curve(const t_curve_ptr_t& curves_list):
+ dim_(0), size_(0), T_min_(0), T_max_(0)
+ {
+ for(typename t_curve_ptr_t::const_iterator it = curves_list.begin() ; it != curves_list.end() ; ++it){
+ add_curve_ptr(*it);
}
}
@@ -73,13 +71,13 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
virtual ~piecewise_curve() {}
- virtual Point operator()(const Time t) const {
+ virtual point_t operator()(const Time t) const {
check_if_not_empty();
if (Safe & !(T_min_ <= t && t <= T_max_)) {
// std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
throw std::out_of_range("can't evaluate piecewise curve, out of range");
}
- return curves_.at(find_interval(t))(t);
+ return (*curves_.at(find_interval(t)))(t);
}
/// \brief Evaluate the derivative of order N of curve at time t.
@@ -92,7 +90,7 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
if (Safe & !(T_min_ <= t && t <= T_max_)) {
throw std::invalid_argument("can't evaluate piecewise curve, out of range");
}
- return (curves_.at(find_interval(t))).derivate(t, order);
+ return (*curves_.at(find_interval(t))).derivate(t, order);
}
/**
@@ -100,44 +98,54 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
* @param order order of derivative
* @return
*/
- 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));
+ piecewise_curve_t* compute_derivate_ptr(const std::size_t order) const {
+ piecewise_curve_t* res(new piecewise_curve_t());
+ for (typename t_curve_ptr_t::const_iterator itc = curves_.begin(); itc < curves_.end(); ++itc) {
+ curve_ptr_t ptr((*itc)->compute_derivate_ptr(order));
+ res->add_curve_ptr(ptr);
}
return res;
}
+
+ template <typename Curve>
+ void add_curve(const Curve& curve) {
+ curve_ptr_t curve_ptr = boost::make_shared<Curve>(curve);
+ add_curve_ptr(curve_ptr);
+ }
+
/// \brief Add a new curve to piecewise curve, which should be defined in \f$[T_{min},T_{max}]\f$ where
/// \f$T_{min}\f$
/// is equal to \f$T_{max}\f$ of the actual piecewise curve. The curve added should be of type Curve as
/// defined in the template.
/// \param cf : curve to add.
///
- void add_curve(const curve_t& cf) {
- if (size_ == 0 && dim_ == 0) { // first curve added
- dim_ = cf.dim();
+ void add_curve_ptr(const curve_ptr_t& cf) {
+ if (size_ == 0) { // first curve added
+ dim_ = cf->dim();
}
// 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 (size_ != 0 && !(fabs(cf->min() - T_max_) < MARGIN)) {
+ std::stringstream ss; ss << "Can not add new Polynom to PiecewiseCurve : time discontinuity between T_max_ and pol.min(). Current T_max is "<<T_max_<<" new curve min is "<<cf->min();
+ throw std::invalid_argument(ss.str().c_str());
}
- if(cf.dim() != dim_){
- throw std::invalid_argument(
- "All the curves in a piecewiseCurve should have the same dimension");
+ if(cf->dim() != dim_){
+ std::stringstream ss; ss << "All the curves in a piecewiseCurve should have the same dimension. Current dim is "<<dim_<<" dim of the new curve is "<<cf->dim();
+ throw std::invalid_argument(ss.str().c_str());
}
curves_.push_back(cf);
size_ = curves_.size();
- T_max_ = cf.max();
+ T_max_ = cf->max();
if (size_ == 1) {
// First curve added
- time_curves_.push_back(cf.min());
- T_min_ = cf.min();
+ 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.
/// \param order : order of continuity we want to check.
/// \return True if the curve is continuous of order given.
@@ -149,10 +157,10 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
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());
+ curve_ptr_t current = curves_.at(i);
+ curve_ptr_t next = curves_.at(i + 1);
+ value_end = (*current)(current->max());
+ value_start = (*next)(next->min());
if (!value_end.isApprox(value_start,MARGIN)) {
isContinuous = false;
}
@@ -161,10 +169,10 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
}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);
+ curve_ptr_t current = curves_.at(i);
+ curve_ptr_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;
}
@@ -176,9 +184,9 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
std::size_t num_curves() const { return curves_.size(); }
- const curve_t& curve_at_time(const time_t t) const { return curves_[find_interval(t)]; }
+ const curve_ptr_t& curve_at_time(const time_t t) const { return curves_[find_interval(t)]; }
- const curve_t& curve_at_index(const std::size_t idx) const {
+ const curve_ptr_t& curve_at_index(const std::size_t idx) const {
if (Safe && idx >= num_curves()) {
throw std::length_error(
"curve_at_index: requested index greater than number of curves in piecewise_curve instance");
@@ -187,56 +195,54 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
}
template <typename Bezier>
- piecewise_curve<Time, Numeric, Safe, Point, T_Point, Bezier> convert_piecewise_curve_to_bezier() {
+ piecewise_curve_t convert_piecewise_curve_to_bezier() {
check_if_not_empty();
- typedef piecewise_curve<Time, Numeric, Safe, Point, T_Point, Bezier> piecewise_curve_out_t;
- // Get first curve (segment)
- curve_t first_curve = curves_.at(0);
- Bezier first_curve_output = bezier_from_curve<Bezier, curve_t>(first_curve);
+ // check if given Bezier curve have the correct dimension :
+ BOOST_STATIC_ASSERT(boost::is_same<typename Bezier::point_t, point_t>::value);
+ BOOST_STATIC_ASSERT(boost::is_same<typename Bezier::point_derivate_t, point_derivate_t>::value);
// Create piecewise curve
- piecewise_curve_out_t pc_res(first_curve_output);
+ piecewise_curve_t pc_res;
// Convert and add all other curves (segments)
- for (std::size_t i = 1; i < size_; i++) {
- pc_res.add_curve(bezier_from_curve<Bezier, curve_t>(curves_.at(i)));
+ for (std::size_t i = 0; i < size_; i++) {
+ pc_res.add_curve(bezier_from_curve<Bezier>(*curves_.at(i)));
}
return pc_res;
}
- template <typename Hermite>
- piecewise_curve<Time, Numeric, Safe, Point, T_Point, Hermite> convert_piecewise_curve_to_cubic_hermite() {
+ template <typename Hermite>
+ piecewise_curve_t convert_piecewise_curve_to_cubic_hermite() {
check_if_not_empty();
- typedef piecewise_curve<Time, Numeric, Safe, Point, T_Point, Hermite> piecewise_curve_out_t;
- // Get first curve (segment)
- curve_t first_curve = curves_.at(0);
- Hermite first_curve_output = hermite_from_curve<Hermite, curve_t>(first_curve);
+ // check if given Hermite curve have the correct dimension :
+ BOOST_STATIC_ASSERT(boost::is_same<typename Hermite::point_t, point_t>::value);
+ BOOST_STATIC_ASSERT(boost::is_same<typename Hermite::point_derivate_t, point_derivate_t>::value);
// Create piecewise curve
- piecewise_curve_out_t pc_res(first_curve_output);
+ piecewise_curve_t pc_res;
// Convert and add all other curves (segments)
- for (std::size_t i = 1; i < size_; i++) {
- pc_res.add_curve(hermite_from_curve<Hermite, curve_t>(curves_.at(i)));
+ for (std::size_t i = 0; i < size_; i++) {
+ pc_res.add_curve(hermite_from_curve<Hermite>(*curves_.at(i)));
}
return pc_res;
}
template <typename Polynomial>
- piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> convert_piecewise_curve_to_polynomial() {
+ piecewise_curve_t convert_piecewise_curve_to_polynomial() {
check_if_not_empty();
- typedef piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> piecewise_curve_out_t;
- // Get first curve (segment)
- curve_t first_curve = curves_.at(0);
- Polynomial first_curve_output = polynomial_from_curve<Polynomial, curve_t>(first_curve);
+ // check if given Polynomial curve have the correct dimension :
+ BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_t, point_t>::value);
+ BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_derivate_t, point_derivate_t>::value);
// Create piecewise curve
- piecewise_curve_out_t pc_res(first_curve_output);
+ piecewise_curve_t pc_res;
// Convert and add all other curves (segments)
- for (std::size_t i = 1; i < size_; i++) {
- pc_res.add_curve(polynomial_from_curve<Polynomial, curve_t>(curves_.at(i)));
+ for (std::size_t i = 0; i < size_; i++) {
+ pc_res.add_curve(polynomial_from_curve<Polynomial>(*curves_.at(i)));
}
return pc_res;
}
+
template <typename Polynomial>
- static piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> convert_discrete_points_to_polynomial(
- T_Point points, t_time_t time_points) {
+ static piecewise_curve_t convert_discrete_points_to_polynomial(
+ t_point_t points, t_time_t time_points) {
if (Safe & !(points.size() > 1)) {
// std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
throw std::invalid_argument(
@@ -247,7 +253,10 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
"piecewise_curve::convert_discrete_points_to_polynomial: Error, points and time_points must have the same "
"size.");
}
- piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> piecewise_res;
+ // check if given Polynomial curve have the correct dimension :
+ BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_t, point_t>::value);
+ BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_derivate_t, point_derivate_t>::value);
+ piecewise_curve_t piecewise_res;
for (size_t i = 1; i < points.size(); ++i) {
piecewise_res.add_curve(Polynomial(points[i - 1], points[i], time_points[i - 1], time_points[i]));
@@ -256,8 +265,8 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
}
template <typename Polynomial>
- static piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> convert_discrete_points_to_polynomial(
- T_Point points, T_Point points_derivative, t_time_t time_points) {
+ static piecewise_curve_t convert_discrete_points_to_polynomial(
+ t_point_t points, t_point_derivate_t points_derivative, t_time_t time_points) {
if (Safe & !(points.size() > 1)) {
// std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
throw std::invalid_argument(
@@ -273,7 +282,10 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
"piecewise_curve::convert_discrete_points_to_polynomial: Error, points and points_derivative must have the "
"same size.");
}
- piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> piecewise_res;
+ // check if given Polynomial curve have the correct dimension :
+ BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_t, point_t>::value);
+ BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_derivate_t, point_derivate_t>::value);
+ piecewise_curve_t piecewise_res;
for (size_t i = 1; i < points.size(); ++i) {
piecewise_res.add_curve(Polynomial(points[i - 1], points_derivative[i - 1], points[i], points_derivative[i],
@@ -283,8 +295,8 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
}
template <typename Polynomial>
- static piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> convert_discrete_points_to_polynomial(
- T_Point points, T_Point points_derivative, T_Point points_second_derivative, t_time_t time_points) {
+ static piecewise_curve_t convert_discrete_points_to_polynomial(
+ t_point_t points, t_point_derivate_t points_derivative, t_point_derivate_t points_second_derivative, t_time_t time_points) {
if (Safe & !(points.size() > 1)) {
// std::cout<<"[Min,Max]=["<<T_min_<<","<<T_max_<<"]"<<" t="<<t<<std::endl;
throw std::invalid_argument(
@@ -305,7 +317,10 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
"piecewise_curve::convert_discrete_points_to_polynomial: Error, points and points_second_derivative must "
"have the same size.");
}
- piecewise_curve<Time, Numeric, Safe, Point, T_Point, Polynomial> piecewise_res;
+ // check if given Polynomial curve have the correct dimension :
+ BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_t, point_t>::value);
+ BOOST_STATIC_ASSERT(boost::is_same<typename Polynomial::point_derivate_t, point_derivate_t>::value);
+ piecewise_curve_t piecewise_res;
for (size_t i = 1; i < points.size(); ++i) {
piecewise_res.add_curve(Polynomial(points[i - 1], points_derivative[i - 1], points_second_derivative[i - 1],
@@ -362,12 +377,17 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$, upper bound of time range.
Time virtual max() const { return T_max_; }
+ /// \brief Get the degree of the curve.
+ /// \return \f$degree\f$, the degree of the curve.
+ virtual std::size_t degree() const {
+ throw std::runtime_error("degree() method is not implemented for this type of curve.");
+ }
std::size_t getNumberCurves() { return curves_.size(); }
/*Helpers*/
/* Attributes */
std::size_t dim_; // Dim of curve
- t_curve_t curves_; // for curves 0/1/2 : [ curve0, curve1, curve2 ]
+ t_curve_ptr_t curves_; // for curves 0/1/2 : [ curve0, curve1, curve2 ]
t_time_t time_curves_; // for curves 0/1/2 : [ Tmin0, Tmax0,Tmax1,Tmax2 ]
std::size_t size_; // Number of segments in piecewise curve = size of curves_
Time T_min_, T_max_;
@@ -382,7 +402,7 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
if (version) {
// Do something depending on version ?
}
- ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_abc_t);
+ ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(curve_t);
ar& boost::serialization::make_nvp("dim", dim_);
ar& boost::serialization::make_nvp("curves", curves_);
ar& boost::serialization::make_nvp("time_curves", time_curves_);
@@ -392,8 +412,9 @@ struct piecewise_curve : public curve_abc<Time, Numeric, Safe, Point,Point_deriv
}
}; // End struct piecewise curve
-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);
+
+template <typename Time, typename Numeric, bool Safe, typename Point, typename Point_derivate>
+const double piecewise_curve<Time, Numeric, Safe, Point,Point_derivate >::MARGIN(0.001);
} // namespace curves
diff --git a/include/curves/polynomial.h b/include/curves/polynomial.h
index 757c13c00a3b042ce245756f750f7c0684aa7c49..2e75d6b6e75a7a45853c8c8e40b40a35be3efabc 100644
--- a/include/curves/polynomial.h
+++ b/include/curves/polynomial.h
@@ -41,6 +41,7 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
typedef Eigen::MatrixXd coeff_t;
typedef Eigen::Ref<coeff_t> coeff_t_ref;
typedef polynomial<Time, Numeric, Safe, Point, T_Point> polynomial_t;
+ typedef typename curve_abc_t::curve_ptr_t curve_ptr_t;
/* Constructors - destructors */
public:
@@ -282,6 +283,13 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
return deriv.compute_derivate(order - 1);
}
+ /// \brief Compute the derived curve at order N.
+ /// \param order : order of derivative.
+ /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
+ polynomial_t* compute_derivate_ptr(const std::size_t order) const {
+ return new polynomial_t(compute_derivate(order));
+ }
+
Eigen::MatrixXd coeff() const { return coefficients_; }
point_t coeffAtDegree(const std::size_t degree) const {
@@ -329,6 +337,9 @@ struct polynomial : public curve_abc<Time, Numeric, Safe, Point> {
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$ upper bound of time range.
num_t virtual max() const { return T_max_; }
+ /// \brief Get the degree of the curve.
+ /// \return \f$degree\f$, the degree of the curve.
+ virtual std::size_t degree() const {return degree_;}
/*Helpers*/
/*Attributes*/
diff --git a/include/curves/se3_curve.h b/include/curves/se3_curve.h
index 2a4678650841cc18305187741fd022541b1db15f..c929771f290a0bf7af68b89c541d9395a27f6d21 100644
--- a/include/curves/se3_curve.h
+++ b/include/curves/se3_curve.h
@@ -164,6 +164,17 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
return res;
}
+ SE3Curve_t compute_derivate(const std::size_t /*order*/) const {
+ throw std::logic_error("Compute derivate for SE3 is not implemented yet.");
+ }
+
+ /// \brief Compute the derived curve at order N.
+ /// \param order : order of derivative.
+ /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
+ SE3Curve_t* compute_derivate_ptr(const std::size_t order) const {
+ return new SE3Curve_t(compute_derivate(order));
+ }
+
/*Helpers*/
/// \brief Get dimension of curve.
/// \return dimension of curve.
@@ -174,6 +185,9 @@ struct SE3Curve : public curve_abc<Time, Numeric, Safe, Eigen::Transform<Numeric
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$ upper bound of time range.
time_t max() const { return T_max_; }
+ /// \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();}
/*Helpers*/
/*Attributes*/
diff --git a/include/curves/serialization/registeration.hpp b/include/curves/serialization/registeration.hpp
index 3a4a5e64b3f20c1d304c6430cb8733695577a12f..ec4f7a0f2b7f83e51431eb76e1a3a37008e8d0e4 100644
--- a/include/curves/serialization/registeration.hpp
+++ b/include/curves/serialization/registeration.hpp
@@ -26,23 +26,22 @@ namespace serialization {
template <class Archive>
void register_types(Archive& ar){
- typedef double Scalar;
- typedef Eigen::Matrix<Scalar, -1, 1> pointX_t;
- typedef std::vector<pointX_t, Eigen::aligned_allocator<pointX_t> > t_point_t;
//register derived class
- ar.template register_type<bezier_curve<Scalar, Scalar, true, pointX_t> >();
- ar.template register_type<cubic_hermite_spline<Scalar, Scalar, true, pointX_t> >();
- ar.template register_type<exact_cubic<Scalar, Scalar, true, pointX_t,
- t_point_t, polynomial<Scalar, Scalar, true, pointX_t,t_point_t> > >();
- ar.template register_type<piecewise_curve<Scalar, Scalar, true, pointX_t,
- t_point_t, polynomial<Scalar, Scalar, true, pointX_t, t_point_t> ,pointX_t> >();
- ar.template register_type<piecewise_curve<Scalar, Scalar, true, pointX_t,
- t_point_t, bezier_curve<Scalar, Scalar, true, pointX_t> ,pointX_t> >();
- ar.template register_type<piecewise_curve<Scalar, Scalar, true, pointX_t,
- t_point_t, cubic_hermite_spline<Scalar, Scalar, true, pointX_t> ,pointX_t> >();
- ar.template register_type<polynomial<Scalar, Scalar, true, pointX_t, t_point_t> >();
- ar.template register_type<SO3Linear<Scalar, Scalar, true> >();
+ ar.template register_type<polynomial_t >();
+ ar.template register_type<exact_cubic_t >();
+ ar.template register_type<bezier_t >();
+ ar.template register_type<cubic_hermite_spline_t >();
+ ar.template register_type<piecewise_t >();
+ ar.template register_type<polynomial3_t >();
+ ar.template register_type<exact_cubic3_t >();
+ ar.template register_type<bezier3_t >();
+ ar.template register_type<cubic_hermite_spline3_t >();
+ ar.template register_type<piecewise3_t >();
+ ar.template register_type<SO3Linear_t >();
+ ar.template register_type<SE3Curve_t >();
+ ar.template register_type<piecewise_SE3_t >();
+
}
}
diff --git a/include/curves/so3_linear.h b/include/curves/so3_linear.h
index bd8ecee748c29035386293d161837e269e46e379..61e076e531ba8f547638ba992926c3715bddaeec 100644
--- a/include/curves/so3_linear.h
+++ b/include/curves/so3_linear.h
@@ -125,6 +125,18 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
}
}
+ SO3Linear_t compute_derivate(const std::size_t /*order*/) const {
+ throw std::logic_error("Compute derivate for SO3Linear is not implemented yet.");
+ }
+
+ /// \brief Compute the derived curve at order N.
+ /// \param order : order of derivative.
+ /// \return A pointer to \f$\frac{d^Nx(t)}{dt^N}\f$ derivative order N of the curve.
+ SO3Linear_t* compute_derivate_ptr(const std::size_t order) const {
+ return new SO3Linear_t(compute_derivate(order));
+ }
+
+
/*Helpers*/
/// \brief Get dimension of curve.
/// \return dimension of curve.
@@ -135,6 +147,9 @@ struct SO3Linear : public curve_abc<Time, Numeric, Safe, Eigen::Matrix<Numeric,
/// \brief Get the maximum time for which the curve is defined.
/// \return \f$t_{max}\f$ upper bound of time range.
time_t max() const { return T_max_; }
+ /// \brief Get the degree of the curve.
+ /// \return \f$degree\f$, the degree of the curve.
+ virtual std::size_t degree() const {return 1;}
matrix3_t getInitRotation()const {return init_rot_.toRotationMatrix();}
matrix3_t getEndRotation()const {return end_rot_.toRotationMatrix();}
matrix3_t getInitRotation() {return init_rot_.toRotationMatrix();}
diff --git a/python/curves_python.cpp b/python/curves_python.cpp
index 1b6556e4ec50101918b556fde9554c1777988996..d8eebec8e0fcaf9f514a861efdf4065eb1ce77d2 100644
--- a/python/curves_python.cpp
+++ b/python/curves_python.cpp
@@ -17,6 +17,7 @@ using namespace boost::python;
struct CurveWrapper : curve_abc_t, wrapper<curve_abc_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_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")(); }
real max() { return this->get_override("max")(); }
@@ -24,6 +25,7 @@ struct CurveWrapper : curve_abc_t, wrapper<curve_abc_t> {
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")(); }
+ 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")(); }
real max() { return this->get_override("max")(); }
@@ -31,6 +33,15 @@ struct Curve3Wrapper : curve_3_t, wrapper<curve_3_t> {
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_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")(); }
+ real max() { return this->get_override("max")(); }
+};
+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_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")(); }
real max() { return this->get_override("max")(); }
@@ -137,53 +148,42 @@ polynomial_t* wrapPolynomialConstructorFromBoundaryConditionsDegree3(const point
}
polynomial_t* wrapPolynomialConstructorFromBoundaryConditionsDegree5(const pointX_t& init, const pointX_t& d_init,
const pointX_t& dd_init, const pointX_t& end,
- const point_t& d_end, const point_t& dd_end,
+ const pointX_t& d_end, const pointX_t& dd_end,
const real min, const real max) {
return new polynomial_t(init, d_init, dd_init, end, d_end, dd_end, min, max);
}
/* End wrap polynomial */
/* Wrap piecewise curve */
-piecewise_polynomial_curve_t* wrapPiecewisePolynomialCurveConstructor(const polynomial_t& pol) {
- return new piecewise_polynomial_curve_t(pol);
+piecewise_t* wrapPiecewiseCurveConstructor(const curve_ptr_t& curve) {
+ return new piecewise_t(curve);
}
-piecewise_polynomial_curve_t* wrapPiecewisePolynomialCurveEmptyConstructor() {
- return new piecewise_polynomial_curve_t();
+piecewise_t* wrapPiecewisePolynomialCurveEmptyConstructor() {
+ return new piecewise_t();
}
-piecewise_bezier_curve_t* wrapPiecewiseBezierCurveConstructor(const bezier_t& bc) {
- return new piecewise_bezier_curve_t(bc);
+piecewise_SE3_t* wrapPiecewiseSE3Constructor(const curve_SE3_ptr_t& curve) {
+ return new piecewise_SE3_t(curve);
}
-piecewise_bezier_linear_curve_t* wrapPiecewiseLinearBezierCurveConstructor(const bezier_linear_variable_t& bc) {
- return new piecewise_bezier_linear_curve_t(bc);
-}
-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_t* wrapPiecewiseSE3EmptyConstructor() {
+ return new piecewise_SE3_t();
}
-piecewise_SE3_curve_t* wrapPiecewiseSE3EmptyConstructor() {
- return new piecewise_SE3_curve_t();
-}
-static piecewise_polynomial_curve_t discretPointToPolynomialC0(const pointX_list_t& points,
+static piecewise_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);
t_time_t time_points_list = vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points);
- return piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(points_list,
- time_points_list);
+ return piecewise_t::convert_discrete_points_to_polynomial<polynomial_t>(points_list,time_points_list);
}
-static piecewise_polynomial_curve_t discretPointToPolynomialC1(const pointX_list_t& points,
+static piecewise_t discretPointToPolynomialC1(const pointX_list_t& points,
const pointX_list_t& points_derivative,
const time_waypoints_t& time_points) {
t_pointX_t points_list = vectorFromEigenArray<pointX_list_t, t_pointX_t>(points);
t_pointX_t points_derivative_list = vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_derivative);
t_time_t time_points_list = vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points);
- return piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(
+ return piecewise_t::convert_discrete_points_to_polynomial<polynomial_t>(
points_list, points_derivative_list, time_points_list);
}
-static piecewise_polynomial_curve_t discretPointToPolynomialC2(const pointX_list_t& points,
+static piecewise_t discretPointToPolynomialC2(const pointX_list_t& points,
const pointX_list_t& points_derivative,
const pointX_list_t& points_second_derivative,
const time_waypoints_t& time_points) {
@@ -192,20 +192,21 @@ static piecewise_polynomial_curve_t discretPointToPolynomialC2(const pointX_list
t_pointX_t points_second_derivative_list = vectorFromEigenArray<pointX_list_t, t_pointX_t>(points_second_derivative);
t_time_t time_points_list = vectorFromEigenVector<time_waypoints_t, t_time_t>(time_points);
- return piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(
+ return piecewise_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) {
+
+void addFinalPointC0(piecewise_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);
+ curve_ptr_t pol(new polynomial_t(self(self.max()), end, self.max(), time));
+ self.add_curve_ptr(pol);
}
-void addFinalPointC1(piecewise_polynomial_curve_t& self, const pointX_t& end, const pointX_t& d_end, const real time) {
+void addFinalPointC1(piecewise_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 )
@@ -213,10 +214,10 @@ void addFinalPointC1(piecewise_polynomial_curve_t& self, const pointX_t& end, co
"guarantee C1 continuity."
<< std::endl;
if (!self.is_continuous(1)) std::cout << "Warning: the current piecewise curve is not C1 continuous." << std::endl;
- polynomial_t pol(self(self.max()), self.derivate(self.max(), 1), end, d_end, self.max(), time);
- self.add_curve(pol);
+ curve_ptr_t pol(new polynomial_t(self(self.max()), self.derivate(self.max(), 1), end, d_end, self.max(), time));
+ self.add_curve_ptr(pol);
}
-void addFinalPointC2(piecewise_polynomial_curve_t& self, const pointX_t& end, const pointX_t& d_end,
+void addFinalPointC2(piecewise_t& self, const pointX_t& end, const pointX_t& d_end,
const pointX_t& dd_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.");
@@ -225,9 +226,9 @@ void addFinalPointC2(piecewise_polynomial_curve_t& self, const pointX_t& end, co
"guarantee C2 continuity."
<< std::endl;
if (!self.is_continuous(2)) std::cout << "Warning: the current piecewise curve is not C2 continuous." << std::endl;
- polynomial_t pol(self(self.max()), self.derivate(self.max(), 1), self.derivate(self.max(), 2), end, d_end, dd_end,
- self.max(), time);
- self.add_curve(pol);
+ curve_ptr_t pol(new polynomial_t(self(self.max()), self.derivate(self.max(), 1), self.derivate(self.max(), 2), end, d_end, dd_end,
+ self.max(), time));
+ self.add_curve_ptr(pol);
}
/* end wrap piecewise polynomial curve */
@@ -281,8 +282,8 @@ bezier_t* bezier_linear_variable_t_evaluate(const bezier_linear_variable_t* b, c
return new bezier_t(evaluateLinear<bezier_t, bezier_linear_variable_t>(*b, x));
}
-bezier_t::piecewise_bezier_curve_t (bezier_t::*splitspe)(const bezier_t::vector_x_t&) const = &bezier_t::split;
-bezier_linear_variable_t::piecewise_bezier_curve_t (bezier_linear_variable_t::*split_py)(
+bezier_t::piecewise_curve_t (bezier_t::*splitspe)(const bezier_t::vector_x_t&) const = &bezier_t::split;
+bezier_linear_variable_t::piecewise_curve_t (bezier_linear_variable_t::*split_py)(
const bezier_linear_variable_t::vector_x_t&) const = &bezier_linear_variable_t::split;
/* End wrap exact cubic spline */
@@ -301,6 +302,13 @@ SO3Linear_t* wrapSO3LinearConstructorFromMatrix(const matrix3_t& init_rot, const
/* End wrap SO3Linear */
/* Wrap SE3Curves */
+
+matrix4_t se3Return(const curve_SE3_t& curve, const real t) { return curve(t).matrix(); }
+
+matrix3_t se3returnRotation(const curve_SE3_t& curve, const real t) { return curve(t).rotation(); }
+
+pointX_t se3returnTranslation(const curve_SE3_t& curve, const real t) { return pointX_t(curve(t).translation()); }
+
SE3Curve_t* wrapSE3CurveFromTransform(const matrix4_t& init_pose, const matrix4_t& end_pose, const real min,
const real max) {
return new SE3Curve_t(transform_t(init_pose), transform_t(end_pose), min, max);
@@ -336,23 +344,12 @@ SE3Curve_t* wrapSE3CurveFromSE3Pinocchio(const SE3_t& init_pose, const SE3_t& en
max);
}
-SE3_t se3ReturnPinocchio(const SE3Curve_t& curve, const real t) { return SE3_t(curve(t).matrix()); }
+SE3_t se3ReturnPinocchio(const curve_SE3_t& curve, const real t) { return SE3_t(curve(t).matrix()); }
-Motion_t se3ReturnDerivatePinocchio(const SE3Curve_t& curve, const real t, const std::size_t order) {
+Motion_t se3ReturnDerivatePinocchio(const curve_SE3_t& curve, const real t, const std::size_t order) {
return Motion_t(curve.derivate(t, order));
}
#endif // CURVES_WITH_PINOCCHIO_SUPPORT
-
-matrix4_t se3Return(const SE3Curve_t& curve, const real t) { return curve(t).matrix(); }
-
-pointX_t se3ReturnDerivate(const SE3Curve_t& curve, const real t, const std::size_t order) {
- return curve.derivate(t, order);
-}
-
-matrix3_t se3returnRotation(const SE3Curve_t& curve, const real t) { return curve(t).rotation(); }
-
-pointX_t se3returnTranslation(const SE3Curve_t& curve, const real t) { return pointX_t(curve(t).translation()); }
-
/* End wrap SE3Curves */
/* Wrap piecewiseSE3Curves */
@@ -360,13 +357,7 @@ pointX_t se3returnTranslation(const SE3Curve_t& curve, const real t) { return po
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) {
+void addFinalSE3(piecewise_SE3_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 )
@@ -379,14 +370,7 @@ void addFinalSE3(piecewise_SE3_curve_t& self, const SE3_t& end, const real time)
#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) {
+void addFinalTransform(piecewise_SE3_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 )
@@ -409,7 +393,6 @@ BOOST_PYTHON_MODULE(curves) {
ENABLE_SPECIFIC_MATRIX_TYPE(point6_t);
ENABLE_SPECIFIC_MATRIX_TYPE(pointX_list_t);
ENABLE_SPECIFIC_MATRIX_TYPE(coeff_t);
- ENABLE_SPECIFIC_MATRIX_TYPE(point_list_t);
ENABLE_SPECIFIC_MATRIX_TYPE(matrix3_t);
ENABLE_SPECIFIC_MATRIX_TYPE(matrix4_t);
// ENABLE_SPECIFIC_MATRIX_TYPE(quaternion_t);
@@ -422,6 +405,7 @@ BOOST_PYTHON_MODULE(curves) {
args("self", "t"))
.def("derivate", pure_virtual(&curve_abc_t::derivate), "Evaluate the derivative of order N of curve at time t.",
args("self", "t", "N"))
+ .def("compute_derivate", pure_virtual(&curve_abc_t::compute_derivate_ptr),return_value_policy<manage_new_object>(), "Return the derivative of *this at the order N.", args("self", "N"))
.def("min", pure_virtual(&curve_abc_t::min), "Get the LOWER bound on interval definition of the curve.")
.def("max", pure_virtual(&curve_abc_t::max), "Get the HIGHER bound on interval definition of the curve.")
.def("dim", pure_virtual(&curve_abc_t::dim), "Get the dimension of the curve.")
@@ -443,6 +427,7 @@ BOOST_PYTHON_MODULE(curves) {
args("self", "t"))
.def("derivate", pure_virtual(&curve_3_t::derivate), "Evaluate the derivative of order N of curve at time t.",
args("self", "t", "N"))
+ .def("compute_derivate", pure_virtual(&curve_3_t::compute_derivate_ptr),return_value_policy<manage_new_object>(), "Return the derivative of *this at the order N.", args("self", "N"))
.def("min", pure_virtual(&curve_3_t::min), "Get the LOWER 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.");
@@ -452,17 +437,40 @@ BOOST_PYTHON_MODULE(curves) {
args("self", "t"))
.def("derivate", pure_virtual(&curve_rotation_t::derivate),
"Evaluate the derivative of order N of curve at time t.", args("self", "t", "N"))
+ .def("compute_derivate", pure_virtual(&curve_rotation_t::compute_derivate_ptr),return_value_policy<manage_new_object>(), "Return the derivative of *this at the order N.", args("self", "N"))
.def("min", pure_virtual(&curve_rotation_t::min), "Get the LOWER 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.");
+ class_<CurveSE3Wrapper, boost::noncopyable, bases<curve_abc_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),
+ "Evaluate the derivative of order N of curve at time t. Return as a vector 6.", args("self", "t", "N"))
+ .def("compute_derivate", pure_virtual(&curve_rotation_t::compute_derivate_ptr),return_value_policy<manage_new_object>(), "Return the derivative of *this at the order N.", args("self", "N"))
+ .def("min", pure_virtual(&curve_SE3_t::min), "Get the LOWER bound on interval definition of the curve.")
+ .def("max", pure_virtual(&curve_SE3_t::max), "Get the HIGHER bound on interval definition of the curve.")
+ .def("dim", pure_virtual(&curve_SE3_t::dim), "Get the dimension of the curve.")
+ .def("rotation", &se3returnRotation, "Output the rotation (as a 3x3 matrix) at the given time.",
+ args("self", "time"))
+ .def("translation", &se3returnTranslation, "Output the rotation (as a vector 3) at the given time.",
+ args("self", "time"))
+ #ifdef CURVES_WITH_PINOCCHIO_SUPPORT
+ .def("evaluateAsSE3", &se3ReturnPinocchio, "Evaluate the curve at the given time. Return as a pinocchio.SE3 object",
+ args("self", "t"))
+ .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
+ ;
+
+
/** BEGIN bezier3 curve**/
class_<bezier3_t, bases<curve_3_t> >("bezier3", init<>())
.def("__init__", make_constructor(&wrapBezier3Constructor))
.def("__init__", make_constructor(&wrapBezier3ConstructorBounds))
.def("__init__", make_constructor(&wrapBezier3ConstructorConstraints))
.def("__init__", make_constructor(&wrapBezier3ConstructorBoundsConstraints))
- .def("compute_derivate", &bezier3_t::compute_derivate)
.def("compute_primitive", &bezier3_t::compute_primitive)
.def("waypointAtIndex", &bezier3_t::waypointAtIndex)
.def_readonly("degree", &bezier3_t::degree_)
@@ -486,7 +494,6 @@ BOOST_PYTHON_MODULE(curves) {
.def("__init__", make_constructor(&wrapBezierConstructorBounds))
.def("__init__", make_constructor(&wrapBezierConstructorConstraints))
.def("__init__", make_constructor(&wrapBezierConstructorBoundsConstraints))
- .def("compute_derivate", &bezier_t::compute_derivate)
.def("compute_primitive", &bezier_t::compute_primitive)
.def("waypointAtIndex", &bezier_t::waypointAtIndex)
.def_readonly("degree", &bezier_t::degree_)
@@ -536,7 +543,7 @@ BOOST_PYTHON_MODULE(curves) {
.def("__call__", &bezier_linear_variable_t::operator())
.def("evaluate", &bezier_linear_variable_t_evaluate, bp::return_value_policy<bp::manage_new_object>())
.def("derivate", &bezier_linear_variable_t::derivate)
- .def("compute_derivate", &bezier_linear_variable_t::compute_derivate)
+ .def("compute_derivate", &bezier_linear_variable_t::compute_derivate_ptr, return_value_policy<manage_new_object>())
.def("compute_primitive", &bezier_linear_variable_t::compute_primitive)
.def("split", split_py)
.def("waypoints", &wayPointsToLists, return_value_policy<manage_new_object>())
@@ -583,7 +590,6 @@ BOOST_PYTHON_MODULE(curves) {
" ddc(min) == dd_init and ddc(max) == dd_end")
.def("coeffAtDegree", &polynomial_t::coeffAtDegree)
.def("coeff", &polynomial_t::coeff)
- .def("compute_derivate", &polynomial_t::compute_derivate, "Compute derivative of order N of curve at time t.")
.def("saveAsText", &polynomial_t::saveAsText<polynomial_t>, bp::args("filename"),
"Saves *this inside a text file.")
.def("loadFromText", &polynomial_t::loadFromText<polynomial_t>, bp::args("filename"),
@@ -599,10 +605,10 @@ BOOST_PYTHON_MODULE(curves) {
/** END polynomial function**/
/** BEGIN piecewise curve function **/
- class_<piecewise_polynomial_curve_t, bases<curve_abc_t> >("piecewise_polynomial_curve", init<>())
+ class_<piecewise_t, bases<curve_abc_t> >("piecewise", init<>())
.def("__init__",
- make_constructor(&wrapPiecewisePolynomialCurveConstructor, default_call_policies(), arg("curve")),
- "Create a peicewise-polynomial curve containing the given polynomial curve.")
+ make_constructor(&wrapPiecewiseCurveConstructor, default_call_policies(), arg("curve")),
+ "Create a peicewise curve containing the given curve.")
.def("FromPointsList", &discretPointToPolynomialC0,
"Create a piecewise-polynomial connecting exactly all the given points at the given time. The created "
"piecewise is C0 continuous.",
@@ -630,163 +636,67 @@ BOOST_PYTHON_MODULE(curves) {
"and time and connecting exactly self(self.max()) and end. It guarantee C2 continuity and guarantee that "
"self.derivate(time,1) == d_end and self.derivate(time,2) == dd_end",
args("self", "end", "d_end", "d_end", "time"))
- .def("compute_derivate", &piecewise_polynomial_curve_t::compute_derivate,
- "Return a piecewise_polynomial curve which is the derivate of this.", args("self", "order"))
- .def("append", &piecewise_polynomial_curve_t::add_curve,
+ .def("append", &piecewise_t::add_curve_ptr,
"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,
+ .def("is_continuous", &piecewise_t::is_continuous,
"Check if the curve is continuous at the given order.",args("self,order"))
+ .def("convert_piecewise_curve_to_polynomial",
+ &piecewise_t::convert_piecewise_curve_to_polynomial<polynomial_t>,
+ "Convert a piecewise curve to to a piecewise polynomial curve")
.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")
+ &piecewise_t::convert_piecewise_curve_to_bezier<bezier_t>,
+ "Convert a piecewise curve to to a piecewise bezier curve")
.def("convert_piecewise_curve_to_cubic_hermite",
- &piecewise_polynomial_curve_t::convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>,
- "Convert a piecewise polynomial curve to to a piecewise cubic hermite spline")
- .def("curve_at_index", &piecewise_polynomial_curve_t::curve_at_index,
+ &piecewise_t::convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>,
+ "Convert a piecewise curve to to a piecewise cubic hermite spline")
+ .def("curve_at_index", &piecewise_t::curve_at_index,
return_value_policy<copy_const_reference>())
- .def("curve_at_time", &piecewise_polynomial_curve_t::curve_at_time, return_value_policy<copy_const_reference>())
- .def("num_curves", &piecewise_polynomial_curve_t::num_curves)
- .def("saveAsText", &piecewise_polynomial_curve_t::saveAsText<piecewise_polynomial_curve_t>, bp::args("filename"),
+ .def("curve_at_time", &piecewise_t::curve_at_time, return_value_policy<copy_const_reference>())
+ .def("num_curves", &piecewise_t::num_curves)
+ .def("saveAsText", &piecewise_t::saveAsText<piecewise_t>, bp::args("filename"),
"Saves *this inside a text file.")
- .def("loadFromText", &piecewise_polynomial_curve_t::loadFromText<piecewise_polynomial_curve_t>,
+ .def("loadFromText", &piecewise_t::loadFromText<piecewise_t>,
bp::args("filename"), "Loads *this from a text file.")
- .def("saveAsXML", &piecewise_polynomial_curve_t::saveAsXML<piecewise_polynomial_curve_t>,
+ .def("saveAsXML", &piecewise_t::saveAsXML<piecewise_t>,
bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
- .def("loadFromXML", &piecewise_polynomial_curve_t::loadFromXML<piecewise_polynomial_curve_t>,
+ .def("loadFromXML", &piecewise_t::loadFromXML<piecewise_t>,
bp::args("filename", "tag_name"), "Loads *this from a XML file.")
- .def("saveAsBinary", &piecewise_polynomial_curve_t::saveAsBinary<piecewise_polynomial_curve_t>,
+ .def("saveAsBinary", &piecewise_t::saveAsBinary<piecewise_t>,
bp::args("filename"), "Saves *this inside a binary file.")
- .def("loadFromBinary", &piecewise_polynomial_curve_t::loadFromBinary<piecewise_polynomial_curve_t>,
+ .def("loadFromBinary", &piecewise_t::loadFromBinary<piecewise_t>,
bp::args("filename"), "Loads *this from a binary file.");
- class_<piecewise_bezier_curve_t, bases<curve_abc_t> >("piecewise_bezier_curve", init<>())
- .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("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>,
- "Convert a piecewise bezier curve to to a piecewise polynomial curve")
- .def("convert_piecewise_curve_to_cubic_hermite",
- &piecewise_bezier_curve_t::convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>,
- "Convert a piecewise bezier curve to to a piecewise cubic hermite spline")
- .def("curve_at_index", &piecewise_bezier_curve_t::curve_at_index, return_value_policy<copy_const_reference>())
- .def("curve_at_time", &piecewise_bezier_curve_t::curve_at_time, return_value_policy<copy_const_reference>())
- .def("num_curves", &piecewise_bezier_curve_t::num_curves)
- .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.");
-
- class_<piecewise_cubic_hermite_curve_t, bases<curve_abc_t> >("piecewise_cubic_hermite_curve", init<>())
- .def("__init__", make_constructor(&wrapPiecewiseCubicHermiteCurveConstructor))
- .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>,
- "Convert a piecewise cubic hermite spline to to a piecewise polynomial curve")
- .def("convert_piecewise_curve_to_bezier",
- &piecewise_cubic_hermite_curve_t::convert_piecewise_curve_to_bezier<bezier_t>,
- "Convert a piecewise cubic hermite spline to to a piecewise bezier curve")
- .def("curve_at_index", &piecewise_cubic_hermite_curve_t::curve_at_index,
- return_value_policy<copy_const_reference>())
- .def("curve_at_time", &piecewise_cubic_hermite_curve_t::curve_at_time,
- return_value_policy<copy_const_reference>())
- .def("num_curves", &piecewise_cubic_hermite_curve_t::num_curves)
- .def("saveAsText", &piecewise_cubic_hermite_curve_t::saveAsText<piecewise_cubic_hermite_curve_t>,
- bp::args("filename"), "Saves *this inside a text file.")
- .def("loadFromText", &piecewise_cubic_hermite_curve_t::loadFromText<piecewise_cubic_hermite_curve_t>,
- bp::args("filename"), "Loads *this from a text file.")
- .def("saveAsXML", &piecewise_cubic_hermite_curve_t::saveAsXML<piecewise_cubic_hermite_curve_t>,
- bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
- .def("loadFromXML", &piecewise_cubic_hermite_curve_t::loadFromXML<piecewise_cubic_hermite_curve_t>,
- bp::args("filename", "tag_name"), "Loads *this from a XML file.")
- .def("saveAsBinary", &piecewise_cubic_hermite_curve_t::saveAsBinary<piecewise_cubic_hermite_curve_t>,
- bp::args("filename"), "Saves *this inside a binary file.")
- .def("loadFromBinary", &piecewise_cubic_hermite_curve_t::loadFromBinary<piecewise_cubic_hermite_curve_t>,
- bp::args("filename"), "Loads *this from a binary file.");
-
- class_<piecewise_bezier_linear_curve_t, bases<curve_abc_t> >("piecewise_bezier_linear_curve_t", init<>())
- .def("__init__", make_constructor(&wrapPiecewiseLinearBezierCurveConstructor))
- .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,
- return_value_policy<copy_const_reference>())
- .def("curve_at_time", &piecewise_bezier_linear_curve_t::curve_at_time,
- return_value_policy<copy_const_reference>())
- .def("num_curves", &piecewise_bezier_linear_curve_t::num_curves)
- .def("saveAsText", &piecewise_bezier_linear_curve_t::saveAsText<piecewise_bezier_linear_curve_t>,
- bp::args("filename"), "Saves *this inside a text file.")
- .def("loadFromText", &piecewise_bezier_linear_curve_t::loadFromText<piecewise_bezier_linear_curve_t>,
- bp::args("filename"), "Loads *this from a text file.")
- .def("saveAsXML", &piecewise_bezier_linear_curve_t::saveAsXML<piecewise_bezier_linear_curve_t>,
- bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
- .def("loadFromXML", &piecewise_bezier_linear_curve_t::loadFromXML<piecewise_bezier_linear_curve_t>,
- bp::args("filename", "tag_name"), "Loads *this from a XML file.")
- .def("saveAsBinary", &piecewise_bezier_linear_curve_t::saveAsBinary<piecewise_bezier_linear_curve_t>,
- bp::args("filename"), "Saves *this inside a binary file.")
- .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<>())
+ class_<piecewise_SE3_t, bases<curve_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.")
-// .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,
+ .def("append", &piecewise_SE3_t::add_curve_ptr,
"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("is_continuous", &piecewise_SE3_t::is_continuous, "Check if the curve is continuous at the given order.",args("self,order"))
+ .def("curve_at_index", &piecewise_SE3_t::curve_at_index, return_value_policy<copy_const_reference>())
+ .def("curve_at_time", &piecewise_SE3_t::curve_at_time, return_value_policy<copy_const_reference>())
+ .def("num_curves", &piecewise_SE3_t::num_curves)
.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_SE3_curve_t::saveAsText<piecewise_SE3_curve_t>, bp::args("filename"),
+ .def("saveAsText", &piecewise_SE3_t::saveAsText<piecewise_SE3_t>, bp::args("filename"),
"Saves *this inside a text file.")
- .def("loadFromText", &piecewise_SE3_curve_t::loadFromText<piecewise_SE3_curve_t>, bp::args("filename"),
+ .def("loadFromText", &piecewise_SE3_t::loadFromText<piecewise_SE3_t>, bp::args("filename"),
"Loads *this from a text file.")
- .def("saveAsXML", &piecewise_SE3_curve_t::saveAsXML<piecewise_SE3_curve_t>,
+ .def("saveAsXML", &piecewise_SE3_t::saveAsXML<piecewise_SE3_t>,
bp::args("filename", "tag_name"), "Saves *this inside a XML file.")
- .def("loadFromXML", &piecewise_SE3_curve_t::loadFromXML<piecewise_SE3_curve_t>,
+ .def("loadFromXML", &piecewise_SE3_t::loadFromXML<piecewise_SE3_t>,
bp::args("filename", "tag_name"), "Loads *this from a XML file.")
- .def("saveAsBinary", &piecewise_SE3_curve_t::saveAsBinary<piecewise_SE3_curve_t>, bp::args("filename"),
+ .def("saveAsBinary", &piecewise_SE3_t::saveAsBinary<piecewise_SE3_t>, bp::args("filename"),
"Saves *this inside a binary file.")
- .def("loadFromBinary", &piecewise_SE3_curve_t::loadFromBinary<piecewise_SE3_curve_t>, bp::args("filename"),
+ .def("loadFromBinary", &piecewise_SE3_t::loadFromBinary<piecewise_SE3_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",
@@ -877,7 +787,7 @@ class_<piecewise_SE3_curve_t, bases<curve_abc_t> >("piecewise_SE3_curve", init<>
/** END SO3 Linear**/
/** BEGIN SE3 Curve**/
- class_<SE3Curve_t, bases<curve_abc_t> >("SE3Curve", init<>())
+ class_<SE3Curve_t, bases<curve_SE3_t> >("SE3Curve", init<>())
.def("__init__",
make_constructor(&wrapSE3CurveFromTransform, default_call_policies(),
args("init_transform", "end_transform", "min", "max")),
@@ -912,17 +822,6 @@ class_<piecewise_SE3_curve_t, bases<curve_abc_t> >("piecewise_SE3_curve", init<>
"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("rotation", &se3returnRotation, "Output the rotation (as a 3x3 matrix) at the given time.",
- args("self", "time"))
- .def("translation", &se3returnTranslation, "Output the rotation (as a vector 3) at the given time.",
- args("self", "time"))
- .def("__call__", &se3Return, "Evaluate the curve at the given time. Return as an homogeneous matrix",
- args("self", "t"))
- .def("derivate", &se3ReturnDerivate,
- "Evaluate the derivative of order N of curve at time t. Return as a vector 6", args("self", "t", "N"))
- .def("min", &SE3Curve_t::min, "Get the LOWER bound on interval definition of the curve.")
- .def("max", &SE3Curve_t::max, "Get the HIGHER bound on interval definition of the curve.")
- .def("dim", &SE3Curve_t::dim, "Get the dimension of the curve.")
.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"),
@@ -941,22 +840,14 @@ class_<piecewise_SE3_curve_t, bases<curve_abc_t> >("piecewise_SE3_curve", init<>
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", &se3ReturnPinocchio, "Evaluate the curve at the given time. Return as a pinocchio.SE3 object",
- args("self", "t"))
- .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
;
/** END SE3 Curve**/
/** BEGIN curves conversion**/
- def("polynomial_from_bezier", polynomial_from_curve<polynomial_t, bezier_t>);
- def("polynomial_from_hermite", polynomial_from_curve<polynomial_t, cubic_hermite_spline_t>);
- def("bezier_from_hermite", bezier_from_curve<bezier_t, cubic_hermite_spline_t>);
- def("bezier_from_polynomial", bezier_from_curve<bezier_t, polynomial_t>);
- def("hermite_from_bezier", hermite_from_curve<cubic_hermite_spline_t, bezier_t>);
- def("hermite_from_polynomial", hermite_from_curve<cubic_hermite_spline_t, polynomial_t>);
+ def("convert_to_polynomial", polynomial_from_curve<polynomial_t>);
+ def("convert_to_bezier", bezier_from_curve<bezier_t>);
+ def("convert_to_hermite", hermite_from_curve<cubic_hermite_spline_t>);
/** END curves conversion**/
optimization::python::exposeOptimization();
diff --git a/python/python_definitions.h b/python/python_definitions.h
index d33b96380ee5d4719c8ca9d631338193b25ebf9f..02131a4ad25b0d927f4e52e93a052fec181bbe66 100644
--- a/python/python_definitions.h
+++ b/python/python_definitions.h
@@ -1,3 +1,4 @@
+#include "curves/fwd.h"
#include "curves/bezier_curve.h"
#include "curves/linear_variable.h"
#include "curves/quadratic_variable.h"
@@ -8,24 +9,25 @@
#define _DEFINITION_PYTHON_BINDINGS
namespace curves {
+/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
typedef double real;
-typedef Eigen::Vector3d point_t;
-typedef Eigen::Vector3d tangent_t;
-typedef Eigen::VectorXd vectorX_t;
-typedef std::pair<point_t, tangent_t> pair_point_tangent_t;
-typedef Eigen::Matrix<double, 6, 1, 0, 6, 1> point6_t;
-typedef Eigen::Matrix<double, 3, 1, 0, 3, 1> ret_point_t;
-typedef Eigen::Matrix<double, 6, 1, 0, 6, 1> ret_point6_t;
+typedef std::vector<real> t_time_t;
typedef Eigen::VectorXd time_waypoints_t;
-typedef Eigen::Matrix<real, 3, Eigen::Dynamic> point_list_t;
+
+typedef Eigen::Matrix<double, Eigen::Dynamic, 1, 0, Eigen::Dynamic, 1> ret_pointX_t;
+typedef std::pair<pointX_t, pointX_t> pair_pointX_tangent_t;
+typedef Eigen::MatrixXd pointX_list_t;
+typedef std::vector<pair_pointX_tangent_t, Eigen::aligned_allocator<pair_pointX_tangent_t> > t_pair_pointX_tangent_t;
+typedef curves::curve_constraints<pointX_t> curve_constraints_t;
+typedef curves::curve_constraints<point3_t> curve_constraints3_t;
+typedef std::pair<real, pointX_t> waypoint_t;
+typedef std::vector<waypoint_t> t_waypoint_t;
+typedef Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> point_listX_t;
+typedef Eigen::Matrix<real, 3, Eigen::Dynamic> point_list3_t;
typedef Eigen::Matrix<real, 6, Eigen::Dynamic> point_list6_t;
-typedef std::vector<real> t_time_t;
-typedef std::vector<point_t, Eigen::aligned_allocator<point_t> > t_point_t;
-typedef std::vector<point6_t, Eigen::aligned_allocator<point6_t> > t_point6_t;
-typedef std::pair<real, point_t> Waypoint;
-typedef std::vector<Waypoint> T_Waypoint;
-typedef std::pair<real, point6_t> Waypoint6;
-typedef std::vector<Waypoint6> T_Waypoint6;
+
+typedef polynomial_t::coeff_t coeff_t;
+typedef curves::Bern<double> bernstein_t;
template <typename PointList, typename T_Point>
T_Point vectorFromEigenArray(const PointList& array) {
diff --git a/python/python_variables.cpp b/python/python_variables.cpp
index 13e66c90d5ae1e9e4ed2a2bb9a4fcd47380458d9..c4fdc500a3b60e0bf42c579a03309961711351ec 100644
--- a/python/python_variables.cpp
+++ b/python/python_variables.cpp
@@ -4,7 +4,7 @@
#include <Eigen/Core>
namespace curves {
-std::vector<linear_variable_t> matrix3DFromEigenArray(const point_list_t& matrices, const point_list_t& vectors) {
+std::vector<linear_variable_t> matrix3DFromEigenArray(const point_list3_t& matrices, const point_list3_t& vectors) {
assert(vectors.cols() * 3 == matrices.cols());
std::vector<linear_variable_t> res;
for (int i = 0; i < vectors.cols(); ++i) {
@@ -19,7 +19,7 @@ linear_variable_t fillWithZeros(const linear_variable_t& var, const std::size_t
return linear_variable_t(B, var.c());
}
-std::vector<linear_variable_t> computeLinearControlPoints(const point_list_t& matrices, const point_list_t& vectors) {
+std::vector<linear_variable_t> computeLinearControlPoints(const point_list3_t& matrices, const point_list3_t& vectors) {
std::vector<linear_variable_t> res;
std::vector<linear_variable_t> variables = matrix3DFromEigenArray(matrices, vectors);
// now need to fill all this with zeros...
@@ -29,12 +29,12 @@ std::vector<linear_variable_t> computeLinearControlPoints(const point_list_t& ma
}
/*linear variable control points*/
-bezier_linear_variable_t* wrapBezierLinearConstructor(const point_list_t& matrices, const point_list_t& vectors) {
+bezier_linear_variable_t* wrapBezierLinearConstructor(const point_list3_t& matrices, const point_list3_t& vectors) {
std::vector<linear_variable_t> asVector = computeLinearControlPoints(matrices, vectors);
return new bezier_linear_variable_t(asVector.begin(), asVector.end());
}
-bezier_linear_variable_t* wrapBezierLinearConstructorBounds(const point_list_t& matrices, const point_list_t& vectors,
+bezier_linear_variable_t* wrapBezierLinearConstructorBounds(const point_list3_t& matrices, const point_list3_t& vectors,
const real T_min, const real T_max) {
std::vector<linear_variable_t> asVector = computeLinearControlPoints(matrices, vectors);
return new bezier_linear_variable_t(asVector.begin(), asVector.end(), T_min, T_max);
diff --git a/python/python_variables.h b/python/python_variables.h
index 70ebfcbe6bcf94ef3e830dde7950262e60a188d3..9e4d2906debce8a76415560cc5ec6a38c1e37e8d 100644
--- a/python/python_variables.h
+++ b/python/python_variables.h
@@ -1,3 +1,4 @@
+#include "curves/fwd.h"
#include "curves/linear_variable.h"
#include "curves/bezier_curve.h"
#include "curves/polynomial.h"
@@ -27,8 +28,8 @@ typedef quadratic_variable<real> quadratic_variable_t;
typedef bezier_curve<real, real, true, linear_variable_t> bezier_linear_variable_t;
/*linear variable control points*/
-bezier_linear_variable_t* wrapBezierLinearConstructor(const point_list_t& matrices, const point_list_t& vectors);
-bezier_linear_variable_t* wrapBezierLinearConstructorBounds(const point_list_t& matrices, const point_list_t& vectors,
+bezier_linear_variable_t* wrapBezierLinearConstructor(const point_list3_t& matrices, const point_list3_t& vectors);
+bezier_linear_variable_t* wrapBezierLinearConstructorBounds(const point_list3_t& matrices, const point_list3_t& vectors,
const real T_min, const real T_max);
typedef Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> matrix_x_t;
@@ -57,74 +58,29 @@ struct LinearBezierVector {
}
};
-/*** TEMPLATE SPECIALIZATION FOR PYTHON ****/
-typedef double real;
-typedef Eigen::VectorXd time_waypoints_t;
-
-typedef Eigen::VectorXd pointX_t;
-typedef Eigen::Matrix<double, 3, 1> point3_t;
-typedef Eigen::Matrix<double, Eigen::Dynamic, 1, 0, Eigen::Dynamic, 1> ret_pointX_t;
-typedef std::pair<pointX_t, pointX_t> pair_pointX_tangent_t;
-typedef Eigen::MatrixXd pointX_list_t;
-typedef std::vector<pointX_t, Eigen::aligned_allocator<pointX_t> > t_pointX_t;
-typedef std::vector<pointX_t, Eigen::aligned_allocator<point3_t> > t_point3_t;
-typedef std::vector<pair_pointX_tangent_t, Eigen::aligned_allocator<pair_pointX_tangent_t> > t_pair_pointX_tangent_t;
-typedef curves::curve_constraints<pointX_t> curve_constraints_t;
-typedef curves::curve_constraints<point3_t> curve_constraints3_t;
-typedef std::pair<real, pointX_t> waypoint_t;
-typedef std::vector<waypoint_t> t_waypoint_t;
-typedef Eigen::Matrix<real, 3, 3> matrix3_t;
-typedef Eigen::Matrix<real, 4, 4> matrix4_t;
-typedef Eigen::Transform<double, 3, Eigen::Affine> transform_t;
-typedef Eigen::Quaternion<real> quaternion_t;
-
-// Curves
-typedef curve_abc<real, real, true, pointX_t> curve_abc_t; // generic class of curve
-typedef curve_abc<real, real, true, point3_t> curve_3_t; // generic class of curve of size 3
-typedef curve_abc<real, real, true, matrix3_t, point3_t>
- curve_rotation_t; // templated class used for the rotation (return dimension are fixed)
-typedef boost::shared_ptr<curve_abc_t> curve_ptr_t;
-typedef boost::shared_ptr<curve_rotation_t> curve_rotation_ptr_t;
-typedef curves::cubic_hermite_spline<real, real, true, pointX_t> cubic_hermite_spline_t;
-typedef curves::bezier_curve<real, real, true, pointX_t> bezier_t;
-typedef curves::bezier_curve<real, real, true, Eigen::Vector3d> bezier3_t;
-typedef curves::polynomial<real, real, true, pointX_t, t_pointX_t> polynomial_t;
-typedef polynomial_t::coeff_t coeff_t;
-typedef curves::piecewise_curve<real, real, true, pointX_t, t_pointX_t, polynomial_t> piecewise_polynomial_curve_t;
-typedef curves::piecewise_curve<real, real, true, pointX_t, t_pointX_t, bezier_t> piecewise_bezier_curve_t;
-typedef curves::piecewise_curve<real, real, true, pointX_t, t_pointX_t, cubic_hermite_spline_t>
- piecewise_cubic_hermite_curve_t;
-typedef curves::piecewise_curve<real, real, true, linear_variable_t,
- std::vector<linear_variable_t, Eigen::aligned_allocator<linear_variable_t> >,
- bezier_linear_variable_t>
- piecewise_bezier_linear_curve_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 ****/
} // namespace curves
-
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::bernstein_t)
-EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::cubic_hermite_spline_t)
-EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::bezier_t)
-EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::bezier3_t)
-EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::polynomial_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::curve_constraints_t)
-EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::piecewise_polynomial_curve_t)
-EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::piecewise_bezier_curve_t)
-EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::piecewise_cubic_hermite_curve_t)
-EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::piecewise_bezier_linear_curve_t)
-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)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::bezier_linear_variable_t)
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::matrix_pair)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::polynomial_t)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::exact_cubic_t)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::bezier_t)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::cubic_hermite_spline_t)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::piecewise_t)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::polynomial3_t)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::exact_cubic3_t)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::bezier3_t)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::cubic_hermite_spline3_t)
+EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(curves::piecewise3_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_t)
+
#endif //_VARIABLES_PYTHON_BINDINGS
diff --git a/python/test/test.py b/python/test/test.py
index a7dc24149a95ed57dcfaab71117fd19ae245c536..a9de8c253e60d5d92d21ebcf16126e570b39a2fe 100644
--- a/python/test/test.py
+++ b/python/test/test.py
@@ -8,10 +8,8 @@ from numpy import array, array_equal, isclose, random, zeros
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,piecewise_SE3_curve, polynomial, polynomial_from_bezier,
- polynomial_from_hermite)
+ cubic_hermite_spline, curve_constraints, exact_cubic,polynomial,
+ piecewise, piecewise_SE3, convert_to_polynomial,convert_to_bezier,convert_to_hermite)
eigenpy.switchToNumpyArray()
@@ -28,13 +26,15 @@ class TestCurves(unittest.TestCase):
def compareCurves(self,c1,c2):
t_min = c1.min()
t_max = c1.max()
- self.assertEqual(t_min,c2.min())
- self.assertEqual(t_max,c2.max())
- self.assertTrue(norm(c1.derivate(t_min, 1) - c2.derivate(t_min, 1)) < 1e-10)
- self.assertTrue(norm(c1.derivate(t_max, 1) - c2.derivate(t_max, 1)) < 1e-10)
+ self.assertEqual(t_min,c2.min(),"c1 min : "+str(t_min)+" ; c2 min : "+str(c2.min()))
+ self.assertEqual(t_max,c2.max(),"c1 max : "+str(t_max)+" ; c2 max : "+str(c2.max()))
+ self.assertTrue(norm(c1.derivate(t_min, 1) - c2.derivate(t_min, 1)) < 1e-10,
+ "dc1(tmin) = "+str(c1.derivate(t_min, 1))+" ; dc2(tmin) = "+str(c2.derivate(t_min, 1)))
+ self.assertTrue(norm(c1.derivate(t_max, 1) - c2.derivate(t_max, 1)) < 1e-10,
+ "dc1(tmax) = "+str(c1.derivate(t_max, 1))+" ; dc2(tmax) = "+str(c2.derivate(t_max, 1)))
t = t_min
while t < t_max:
- self.assertTrue(norm(c1(t) - c2(t)) < 1e-10)
+ self.assertTrue(norm(c1(t) - c2(t)) < 1e-10," at t = "+str(t)+" c1 = "+str(c1(t))+" ; c2 = "+str(c2(t)))
t = t+0.01
@@ -69,7 +69,6 @@ class TestCurves(unittest.TestCase):
a.max()
a(0.4)
self.assertTrue((a(a.min()) == array([1., 2., 3.])).all())
- self.assertTrue((a.derivate(0.4, 0) == a(0.4)).all())
a.derivate(0.4, 2)
a = a.compute_derivate(100)
prim = a.compute_primitive(1)
@@ -161,7 +160,6 @@ class TestCurves(unittest.TestCase):
a.max()
a(0.4)
self.assertTrue((a(a.min()) == array([1., 2., 3.])).all())
- self.assertTrue((a.derivate(0.4, 0) == a(0.4)).all())
a.derivate(0.4, 2)
a = a.compute_derivate(100)
prim = a.compute_primitive(1)
@@ -337,7 +335,7 @@ class TestCurves(unittest.TestCase):
polynomial(waypoints0, 0., 0.1)
a = polynomial(waypoints1, 0., 1.)
b = polynomial(waypoints2, 1., 3.)
- pc = piecewise_polynomial_curve(a)
+ pc = piecewise(a)
pc.append(b)
pc.min()
pc.max()
@@ -349,7 +347,7 @@ class TestCurves(unittest.TestCase):
pc.is_continuous(1)
# Test serialization
pc.saveAsText("serialization_pc.test")
- pc_test = piecewise_polynomial_curve()
+ pc_test = piecewise()
pc_test.loadFromText("serialization_pc.test")
self.assertTrue((pc(0.4) == pc_test(0.4)).all())
os.remove("serialization_pc.test")
@@ -361,7 +359,7 @@ class TestCurves(unittest.TestCase):
pc.append(c)
### Test the different append methods :
- pc = piecewise_polynomial_curve()
+ pc = piecewise()
self.assertEqual(pc.num_curves(),0)
end_point1 = array([1.,3.,5.,6.5,-2.])
max1 = 2.5
@@ -371,7 +369,7 @@ class TestCurves(unittest.TestCase):
pc.append(a)
pc.append(end_point1,max1)
- pc = piecewise_polynomial_curve()
+ pc = piecewise()
d = polynomial(waypoints3, 0., 1.2)
self.assertEqual(pc.num_curves(),0)
pc.append(d)
@@ -393,7 +391,7 @@ class TestCurves(unittest.TestCase):
points_second_derivative = array(random.rand(3, N))
time_points = array(random.rand(1, N)).T
time_points.sort(0)
- polC0 = piecewise_polynomial_curve.FromPointsList(points, time_points)
+ polC0 = piecewise.FromPointsList(points, time_points)
self.assertEqual(polC0.min(), time_points[0, 0])
self.assertEqual(polC0.max(), time_points[-1, 0])
self.assertTrue(polC0.is_continuous(0))
@@ -401,7 +399,7 @@ class TestCurves(unittest.TestCase):
for i in range(N):
self.assertTrue(isclose(polC0(time_points[i, 0]), points[:, i]).all())
- polC1 = piecewise_polynomial_curve.FromPointsList(points, points_derivative, time_points)
+ polC1 = piecewise.FromPointsList(points, points_derivative, time_points)
self.assertEqual(polC1.min(), time_points[0, 0])
self.assertEqual(polC1.max(), time_points[-1, 0])
self.assertTrue(polC1.is_continuous(0))
@@ -411,7 +409,7 @@ class TestCurves(unittest.TestCase):
self.assertTrue(isclose(polC1(time_points[i, 0]), points[:, i]).all())
self.assertTrue(isclose(polC1.derivate(time_points[i, 0], 1), points_derivative[:, i]).all())
- polC2 = piecewise_polynomial_curve.FromPointsList(points, points_derivative, points_second_derivative,
+ polC2 = piecewise.FromPointsList(points, points_derivative, points_second_derivative,
time_points)
self.assertEqual(polC2.min(), time_points[0, 0])
self.assertEqual(polC2.max(), time_points[-1, 0])
@@ -428,13 +426,13 @@ class TestCurves(unittest.TestCase):
time_points[0, 0] = 1
time_points[1, 0] = 0.5
with self.assertRaises(ValueError):
- polC0 = piecewise_polynomial_curve.FromPointsList(points, time_points)
+ polC0 = piecewise.FromPointsList(points, time_points)
with self.assertRaises(ValueError):
- polC1 = piecewise_polynomial_curve.FromPointsList(points, points_derivative, time_points)
+ polC1 = piecewise.FromPointsList(points, points_derivative, time_points)
with self.assertRaises(ValueError):
- polC2 = piecewise_polynomial_curve.FromPointsList(points, points_derivative, points_second_derivative,
+ polC2 = piecewise.FromPointsList(points, points_derivative, points_second_derivative,
time_points)
def test_piecewise_bezier_curve(self):
@@ -443,7 +441,7 @@ class TestCurves(unittest.TestCase):
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 = piecewise(a)
pc.append(b)
pc.min()
pc.max()
@@ -455,7 +453,7 @@ class TestCurves(unittest.TestCase):
pc.is_continuous(1)
# Test serialization
pc.saveAsText("serialization_pc.test")
- pc_test = piecewise_bezier_curve()
+ pc_test = piecewise()
pc_test.loadFromText("serialization_pc.test")
self.assertTrue((pc(0.4) == pc_test(0.4)).all())
os.remove("serialization_pc.test")
@@ -471,7 +469,7 @@ class TestCurves(unittest.TestCase):
time_points1 = array([[1., 2.]]).transpose()
a = cubic_hermite_spline(points, tangents, time_points0)
b = cubic_hermite_spline(points, tangents, time_points1)
- pc = piecewise_cubic_hermite_curve(a)
+ pc = piecewise(a)
pc.append(b)
pc.min()
pc.max()
@@ -483,7 +481,7 @@ class TestCurves(unittest.TestCase):
pc.is_continuous(1)
# Test serialization
pc.saveAsText("serialization_pc.test")
- pc_test = piecewise_cubic_hermite_curve()
+ pc_test = piecewise()
pc_test.loadFromText("serialization_pc.test")
self.assertTrue((pc(0.4) == pc_test(0.4)).all())
os.remove("serialization_pc.test")
@@ -550,23 +548,23 @@ class TestCurves(unittest.TestCase):
waypoints = array([[1., 2., 3.], [4., 5., 6.]]).transpose()
a = bezier(waypoints)
# converting bezier to polynomial
- a_pol = polynomial_from_bezier(a)
+ a_pol = convert_to_polynomial(a)
self.assertTrue(norm(a(0.3) - a_pol(0.3)) < __EPS)
# converting polynomial to hermite
- a_chs = hermite_from_polynomial(a_pol)
+ a_chs = convert_to_hermite(a_pol)
self.assertTrue(norm(a_chs(0.3) - a_pol(0.3)) < __EPS)
# converting hermite to bezier
- a_bc = bezier_from_hermite(a_chs)
+ a_bc = convert_to_bezier(a_chs)
self.assertTrue(norm(a_chs(0.3) - a_bc(0.3)) < __EPS)
self.assertTrue(norm(a(0.3) - a_bc(0.3)) < __EPS)
# converting bezier to hermite
- a_chs = hermite_from_bezier(a)
+ a_chs = convert_to_hermite(a)
self.assertTrue(norm(a(0.3) - a_chs(0.3)) < __EPS)
# converting hermite to polynomial
- a_pol = polynomial_from_hermite(a_chs)
+ a_pol = convert_to_polynomial(a_chs)
self.assertTrue(norm(a_pol(0.3) - a_chs(0.3)) < __EPS)
# converting polynomial to bezier
- a_bc = bezier_from_polynomial(a_pol)
+ a_bc = convert_to_bezier(a_pol)
self.assertTrue(norm(a_bc(0.3) - a_pol(0.3)) < __EPS)
self.assertTrue(norm(a(0.3) - a_bc(0.3)) < __EPS)
return
@@ -582,7 +580,7 @@ class TestCurves(unittest.TestCase):
translation = bezier(waypoints, min, max)
# test with bezier
se3 = SE3Curve(translation, init_rot, end_rot)
- pc = piecewise_SE3_curve(se3)
+ pc = piecewise_SE3(se3)
self.assertEqual(pc.num_curves(),1)
self.assertEqual(pc.min(), min)
self.assertEqual(pc.max(), max)
@@ -616,17 +614,17 @@ class TestCurves(unittest.TestCase):
pc.append(se3_3)
pc.saveAsText("serialization_curve.txt")
- pc_txt = piecewise_SE3_curve()
+ pc_txt = piecewise_SE3()
pc_txt.loadFromText("serialization_curve.txt")
self.compareCurves(pc,pc_txt)
pc.saveAsXML("serialization_curve.xml","pc")
- pc_xml = piecewise_SE3_curve()
+ pc_xml = piecewise_SE3()
pc_xml.loadFromXML("serialization_curve.xml","pc")
self.compareCurves(pc,pc_xml)
pc.saveAsBinary("serialization_curve")
- pc_bin = piecewise_SE3_curve()
+ pc_bin = piecewise_SE3()
pc_bin.loadFromBinary("serialization_curve")
self.compareCurves(pc,pc_bin)
@@ -646,7 +644,7 @@ class TestCurves(unittest.TestCase):
translation = bezier(waypoints, min, max)
# test with bezier
se3 = SE3Curve(translation, init_rot, end_rot)
- pc = piecewise_SE3_curve()
+ pc = piecewise_SE3()
self.assertEqual(pc.num_curves(),0)
pc.append(se3)
self.assertEqual(pc.num_curves(),1)
@@ -696,7 +694,7 @@ class TestCurves(unittest.TestCase):
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()
+ pc = piecewise_SE3()
with self.assertRaises(RuntimeError):
pc.append(end_pose,max)
@@ -719,7 +717,7 @@ class TestCurves(unittest.TestCase):
min = 0.7
max = 12.
se3 = SE3Curve(init_pose, end_pose, min, max)
- pc = piecewise_SE3_curve(se3)
+ pc = piecewise_SE3(se3)
self.assertEqual(pc.num_curves(),1)
p = pc.evaluateAsSE3(min)
self.assertTrue(isinstance(p, SE3))
@@ -754,17 +752,17 @@ class TestCurves(unittest.TestCase):
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 = piecewise(a)
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)
+ self.compareCurves(pc_pol, pc)
# Convert to piecewise cubic hermite
pc_chs = pc.convert_piecewise_curve_to_cubic_hermite()
- self.assertTrue(norm(pc_chs(0.3) - pc(0.3)) < __EPS)
+ self.compareCurves(pc_chs, pc)
# Convert to piecewise bezier
pc_bc = pc_chs.convert_piecewise_curve_to_bezier()
- self.assertTrue(norm(pc_bc(0.3) - pc(0.3)) < __EPS)
+ self.compareCurves(pc_bc, pc)
return
def test_so3_linear(self):
@@ -992,7 +990,7 @@ class TestCurves(unittest.TestCase):
# points_second_derivative = array(random.rand(3, N))
time_points = array(random.rand(1, N)).T
time_points.sort(0)
- translation = piecewise_polynomial_curve.FromPointsList(points, time_points)
+ translation = piecewise.FromPointsList(points, time_points)
min = translation.min()
max = translation.max()
se3 = SE3Curve(translation, init_rot, end_rot)
diff --git a/tests/Main.cpp b/tests/Main.cpp
index 5c513e755dc0a02235ea33a59829b85cfd8dcb13..5f2621949c640b91130bd9b14d7a672a07e2a8d7 100644
--- a/tests/Main.cpp
+++ b/tests/Main.cpp
@@ -1,4 +1,4 @@
-
+#include "curves/fwd.h"
#include "curves/exact_cubic.h"
#include "curves/bezier_curve.h"
#include "curves/polynomial.h"
@@ -15,29 +15,14 @@
#include <iostream>
#include <cmath>
#include <ctime>
+#include <boost/smart_ptr/shared_ptr.hpp>
using namespace std;
namespace curves {
-typedef Eigen::Vector3d point3_t;
-typedef Eigen::VectorXd pointX_t;
-typedef Eigen::Matrix<double, 3, 3> matrix3_t;
-typedef Eigen::Quaternion<double> quaternion_t;
-typedef std::vector<pointX_t, Eigen::aligned_allocator<pointX_t> > t_pointX_t;
-typedef curve_abc<double, double, true, pointX_t> curve_abc_t;
-typedef curve_abc<double, double, true, matrix3_t, point3_t> curve_rotation_t; // templated class used for the rotation (return dimension are fixed)
-typedef boost::shared_ptr<curve_abc_t> curve_ptr_t;
-typedef boost::shared_ptr<curve_rotation_t> curve_rotation_ptr_t;
-typedef polynomial<double, double, true, pointX_t, t_pointX_t> polynomial_t;
-typedef exact_cubic<double, double, true, pointX_t> exact_cubic_t;
typedef exact_cubic<double, double, true, Eigen::Matrix<double, 1, 1> > exact_cubic_one;
-typedef bezier_curve<double, double, true, pointX_t> bezier_curve_t;
-typedef cubic_hermite_spline<double, double, true, pointX_t> cubic_hermite_spline_t;
-typedef piecewise_curve<double, double, true, pointX_t, t_pointX_t, polynomial_t> piecewise_polynomial_curve_t;
-typedef piecewise_curve<double, double, true, pointX_t, t_pointX_t, bezier_curve_t> piecewise_bezier_curve_t;
-typedef piecewise_curve<double, double, true, pointX_t, t_pointX_t, cubic_hermite_spline_t>
- piecewise_cubic_hermite_curve_t;
typedef exact_cubic_t::spline_constraints spline_constraints_t;
+
typedef std::pair<double, pointX_t> Waypoint;
typedef std::vector<Waypoint> T_Waypoint;
typedef Eigen::Matrix<double, 1, 1> point_one;
@@ -45,9 +30,7 @@ typedef std::pair<double, point_one> WaypointOne;
typedef std::vector<WaypointOne> T_WaypointOne;
typedef std::pair<pointX_t, pointX_t> pair_point_tangent_t;
typedef std::vector<pair_point_tangent_t, Eigen::aligned_allocator<pair_point_tangent_t> > t_pair_point_tangent_t;
-typedef SO3Linear<double, double, true> SO3Linear_t;
-typedef SE3Curve<double, double, true> SE3Curve_t;
-typedef Eigen::Transform<double, 3, Eigen::Affine> transform_t;
+
const double margin = 1e-3;
@@ -85,11 +68,11 @@ void ComparePoints(const Eigen::MatrixXd& pt1, const Eigen::MatrixXd& pt2, const
}
template <typename curve1, typename curve2>
-void CompareCurves(curve1 c1, curve2 c2, const std::string& errMsg, bool& error ,double prec = Eigen::NumTraits<double>::dummy_precision()) {
+void CompareCurves(const curve1& c1,const curve2& c2, const std::string& errMsg, bool& error ,double prec = Eigen::NumTraits<double>::dummy_precision()) {
double T_min = c1.min();
double T_max = c1.max();
if (!QuasiEqual(T_min, c2.min()) || !QuasiEqual(T_max, c2.max())) {
- std::cout << "CompareCurves, ERROR, time min and max of curves do not match [" << T_min << "," << T_max << "] "
+ std::cout << errMsg<<"CompareCurves, ERROR, time min and max of curves do not match [" << T_min << "," << T_max << "] "
<< " and [" << c2.min() << "," << c2.max() << "] " << std::endl;
error = true;
} else {
@@ -162,17 +145,17 @@ void PolynomialCubicFunctionTest(bool& error) {
}
// Test derivate and compute_derivative
// Order 1
- polynomial_t cf_derivated = cf.compute_derivate(1);
- ComparePoints(cf.derivate(0, 1), cf_derivated(0), errMsg + " - derivate order 1 : ", error);
- ComparePoints(cf.derivate(0.3, 1), cf_derivated(0.3), errMsg + " - derivate order 1 : ", error);
- ComparePoints(cf.derivate(0.5, 1), cf_derivated(0.5), errMsg + " - derivate order 1 : ", error);
- ComparePoints(cf.derivate(1, 1), cf_derivated(1), errMsg + " - derivate order 1 : ", error);
+ curve_abc_t* cf_derivated = cf.compute_derivate_ptr(1);
+ ComparePoints(cf.derivate(0, 1), (*cf_derivated)(0), errMsg + " - derivate order 1 : ", error);
+ ComparePoints(cf.derivate(0.3, 1), (*cf_derivated)(0.3), errMsg + " - derivate order 1 : ", error);
+ ComparePoints(cf.derivate(0.5, 1), (*cf_derivated)(0.5), errMsg + " - derivate order 1 : ", error);
+ ComparePoints(cf.derivate(1, 1), (*cf_derivated)(1), errMsg + " - derivate order 1 : ", error);
// Order 2
polynomial_t cf_derivated_2 = cf.compute_derivate(2);
- ComparePoints(cf.derivate(0, 2), cf_derivated_2(0), errMsg + " - derivate order 1 : ", error);
- ComparePoints(cf.derivate(0.3, 2), cf_derivated_2(0.3), errMsg + " - derivate order 1 : ", error);
- ComparePoints(cf.derivate(0.5, 2), cf_derivated_2(0.5), errMsg + " - derivate order 1 : ", error);
- ComparePoints(cf.derivate(1, 2), cf_derivated_2(1), errMsg + " - derivate order 1 : ", error);
+ ComparePoints(cf.derivate(0, 2), (cf_derivated_2)(0), errMsg + " - derivate order 1 : ", error);
+ ComparePoints(cf.derivate(0.3, 2), (cf_derivated_2)(0.3), errMsg + " - derivate order 1 : ", error);
+ ComparePoints(cf.derivate(0.5, 2), (cf_derivated_2)(0.5), errMsg + " - derivate order 1 : ", error);
+ ComparePoints(cf.derivate(1, 2), (cf_derivated_2)(1), errMsg + " - derivate order 1 : ", error);
}
/*bezier_curve Function tests*/
@@ -185,7 +168,7 @@ void BezierCurveTest(bool& error) {
std::vector<point3_t> params;
params.push_back(a);
// 1d curve in [0,1]
- bezier_curve_t cf1(params.begin(), params.end());
+ bezier_t cf1(params.begin(), params.end());
point3_t res1;
res1 = cf1(0);
point3_t x10 = a;
@@ -194,7 +177,7 @@ void BezierCurveTest(bool& error) {
ComparePoints(x10, res1, errMsg + "1(1) ", error);
// 2d curve in [0,1]
params.push_back(b);
- bezier_curve_t cf(params.begin(), params.end());
+ bezier_t cf(params.begin(), params.end());
res1 = cf(0);
point3_t x20 = a;
ComparePoints(x20, res1, errMsg + "2(0) ", error);
@@ -203,22 +186,26 @@ void BezierCurveTest(bool& error) {
ComparePoints(x21, res1, errMsg + "2(1) ", error);
// 3d curve in [0,1]
params.push_back(c);
- bezier_curve_t cf3(params.begin(), params.end());
+ bezier_t cf3(params.begin(), params.end());
res1 = cf3(0);
ComparePoints(a, res1, errMsg + "3(0) ", error);
res1 = cf3(1);
ComparePoints(c, res1, errMsg + "3(1) ", error);
// 4d curve in [1,2]
params.push_back(d);
- bezier_curve_t cf4(params.begin(), params.end(), 1., 2.);
+ bezier_t cf4(params.begin(), params.end(), 1., 2.);
// testing bernstein polynomials
- bezier_curve_t cf5(params.begin(), params.end(), 1., 2.);
+ bezier_t cf5(params.begin(), params.end(), 1., 2.);
std::string errMsg2("In test BezierCurveTest ; Bernstein polynomials do not evaluate as analytical evaluation");
+ bezier_t cf5_derivated = cf5.compute_derivate(1);
+
for (double d = 1.; d < 2.; d += 0.1) {
ComparePoints(cf5.evalBernstein(d), cf5(d), errMsg2, error);
ComparePoints(cf5.evalHorner(d), cf5(d), errMsg2, error);
- ComparePoints(cf5.compute_derivate(1).evalBernstein(d), cf5.compute_derivate(1)(d), errMsg2, error);
- ComparePoints(cf5.compute_derivate(1).evalHorner(d), cf5.compute_derivate(1)(d), errMsg2, error);
+ ComparePoints(cf5_derivated.evalBernstein(d), cf5_derivated(d), errMsg2, error);
+ ComparePoints(cf5_derivated.evalHorner(d), cf5_derivated(d), errMsg2, error);
+ ComparePoints(cf5.derivate(d,1), cf5_derivated(d), errMsg2, error);
+
}
bool error_in(true);
try {
@@ -272,7 +259,7 @@ void BezierCurveTestCompareHornerAndBernstein(bool&) // error
params.push_back(b);
params.push_back(c);
// 3d curve
- bezier_curve_t cf(params.begin(), params.end()); // defined in [0,1]
+ bezier_t cf(params.begin(), params.end()); // defined in [0,1]
// Check all evaluation of bezier curve
clock_t s0, e0, s1, e1, s2, e2, s3, e3;
s0 = clock();
@@ -307,7 +294,7 @@ void BezierCurveTestCompareHornerAndBernstein(bool&) // error
params.push_back(g);
params.push_back(h);
params.push_back(i);
- bezier_curve_t cf2(params.begin(), params.end());
+ bezier_t cf2(params.begin(), params.end());
s1 = clock();
for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
cf2.evalBernstein(*cit);
@@ -343,8 +330,7 @@ void BezierDerivativeCurveTest(bool& error) {
params.push_back(a);
params.push_back(b);
params.push_back(c);
- bezier_curve_t cf3(params.begin(), params.end());
- ComparePoints(cf3(0), cf3.derivate(0., 0), errMsg, error);
+ bezier_t cf3(params.begin(), params.end());
ComparePoints(cf3(0), cf3.derivate(0., 1), errMsg, error, true);
ComparePoints(point3_t::Zero(), cf3.derivate(0., 100), errMsg, error);
}
@@ -368,8 +354,8 @@ void BezierDerivativeCurveTimeReparametrizationTest(bool& error) {
double Tmin = 0.;
double Tmax = 2.;
double diffT = Tmax - Tmin;
- bezier_curve_t cf(params.begin(), params.end());
- bezier_curve_t cfT(params.begin(), params.end(), Tmin, Tmax);
+ bezier_t cf(params.begin(), params.end());
+ bezier_t cfT(params.begin(), params.end(), Tmin, Tmax);
ComparePoints(cf(0.5), cfT(1), errMsg, error);
ComparePoints(cf.derivate(0.5, 1), cfT.derivate(1, 1) * (diffT), errMsg, error);
ComparePoints(cf.derivate(0.5, 2), cfT.derivate(1, 2) * diffT * diffT, errMsg, error);
@@ -380,7 +366,7 @@ void BezierDerivativeCurveConstraintTest(bool& error) {
point3_t a(1, 2, 3);
point3_t b(2, 3, 4);
point3_t c(3, 4, 5);
- bezier_curve_t::curve_constraints_t constraints(3);
+ bezier_t::curve_constraints_t constraints(3);
constraints.init_vel = point3_t(-1, -1, -1);
constraints.init_acc = point3_t(-2, -2, -2);
constraints.end_vel = point3_t(-10, -10, -10);
@@ -389,9 +375,9 @@ void BezierDerivativeCurveConstraintTest(bool& error) {
params.push_back(a);
params.push_back(b);
params.push_back(c);
- bezier_curve_t::num_t T_min = 1.0;
- bezier_curve_t::num_t T_max = 3.0;
- bezier_curve_t cf(params.begin(), params.end(), constraints, T_min, T_max);
+ bezier_t::num_t T_min = 1.0;
+ bezier_t::num_t T_max = 3.0;
+ bezier_t cf(params.begin(), params.end(), constraints, T_min, T_max);
ComparePoints(a, cf(T_min), errMsg0, error);
ComparePoints(c, cf(T_max), errMsg0, error);
ComparePoints(constraints.init_vel, cf.derivate(T_min, 1), errMsg0, error);
@@ -435,11 +421,11 @@ void toPolynomialConversionTest(bool& error) {
control_points.push_back(g);
control_points.push_back(h);
control_points.push_back(i);
- bezier_curve_t::num_t T_min = 1.0;
- bezier_curve_t::num_t T_max = 3.0;
- bezier_curve_t bc(control_points.begin(), control_points.end(), T_min, T_max);
- polynomial_t pol = polynomial_from_curve<polynomial_t, bezier_curve_t>(bc);
- CompareCurves<polynomial_t, bezier_curve_t>(pol, bc, errMsg, error);
+ bezier_t::num_t T_min = 1.0;
+ bezier_t::num_t T_max = 3.0;
+ bezier_t bc(control_points.begin(), control_points.end(), T_min, T_max);
+ polynomial_t pol = polynomial_from_curve<polynomial_t>(bc);
+ CompareCurves<polynomial_t, bezier_t>(pol, bc, errMsg, error);
}
void cubicConversionTest(bool& error) {
@@ -468,38 +454,38 @@ void cubicConversionTest(bool& error) {
// hermite to bezier
// std::cout<<"======================= \n";
// std::cout<<"hermite to bezier \n";
- bezier_curve_t bc0 = bezier_from_curve<bezier_curve_t, cubic_hermite_spline_t>(chs0);
- CompareCurves<cubic_hermite_spline_t, bezier_curve_t>(chs0, bc0, errMsg0, error);
+ bezier_t bc0 = bezier_from_curve<bezier_t>(chs0);
+ CompareCurves<cubic_hermite_spline_t, bezier_t>(chs0, bc0, errMsg0, error);
// hermite to pol
// std::cout<<"======================= \n";
// std::cout<<"hermite to polynomial \n";
- polynomial_t pol0 = polynomial_from_curve<polynomial_t, cubic_hermite_spline_t>(chs0);
+ polynomial_t pol0 = polynomial_from_curve<polynomial_t>(chs0);
CompareCurves<cubic_hermite_spline_t, polynomial_t>(chs0, pol0, errMsg0, error);
// pol to hermite
// std::cout<<"======================= \n";
// std::cout<<"polynomial to hermite \n";
- cubic_hermite_spline_t chs1 = hermite_from_curve<cubic_hermite_spline_t, polynomial_t>(pol0);
+ cubic_hermite_spline_t chs1 = hermite_from_curve<cubic_hermite_spline_t>(pol0);
CompareCurves<polynomial_t, cubic_hermite_spline_t>(pol0, chs1, errMsg2, error);
// pol to bezier
// std::cout<<"======================= \n";
// std::cout<<"polynomial to bezier \n";
- bezier_curve_t bc1 = bezier_from_curve<bezier_curve_t, polynomial_t>(pol0);
- CompareCurves<bezier_curve_t, polynomial_t>(bc1, pol0, errMsg2, error);
+ bezier_t bc1 = bezier_from_curve<bezier_t>(pol0);
+ CompareCurves<bezier_t, polynomial_t>(bc1, pol0, errMsg2, error);
// Bezier to pol
// std::cout<<"======================= \n";
// std::cout<<"bezier to polynomial \n";
- polynomial_t pol1 = polynomial_from_curve<polynomial_t, bezier_curve_t>(bc0);
- CompareCurves<bezier_curve_t, polynomial_t>(bc0, pol1, errMsg1, error);
+ polynomial_t pol1 = polynomial_from_curve<polynomial_t>(bc0);
+ CompareCurves<bezier_t, polynomial_t>(bc0, pol1, errMsg1, error);
// bezier => hermite
// std::cout<<"======================= \n";
// std::cout<<"bezier to hermite \n";
- cubic_hermite_spline_t chs2 = hermite_from_curve<cubic_hermite_spline_t, bezier_curve_t>(bc0);
- CompareCurves<bezier_curve_t, cubic_hermite_spline_t>(bc0, chs2, errMsg1, error);
+ cubic_hermite_spline_t chs2 = hermite_from_curve<cubic_hermite_spline_t>(bc0);
+ CompareCurves<bezier_t, cubic_hermite_spline_t>(bc0, chs2, errMsg1, error);
// Test : compute derivative of bezier => Convert it to polynomial
- bezier_curve_t bc_der = bc0.compute_derivate(1);
- polynomial_t pol_test = polynomial_from_curve<polynomial_t, bezier_curve_t>(bc_der);
- CompareCurves<bezier_curve_t, polynomial_t>(bc_der, pol_test, errMsg1, error);
+ curve_abc_t* bc_der = bc0.compute_derivate_ptr(1);
+ polynomial_t pol_test = polynomial_from_curve<polynomial_t>(*bc_der);
+ CompareCurves<curve_abc_t, polynomial_t>(*bc_der, pol_test, errMsg1, error);
}
/*Exact Cubic Function tests*/
@@ -861,7 +847,7 @@ void BezierEvalDeCasteljau(bool& error) {
params.push_back(b);
params.push_back(c);
// 3d curve
- bezier_curve_t cf(params.begin(), params.end());
+ bezier_t cf(params.begin(), params.end());
std::string errmsg("Error in BezierEvalDeCasteljau; while comparing actual bezier evaluation and de Casteljau : ");
for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
ComparePoints(cf.evalDeCasteljau(*cit), cf(*cit), errmsg, error);
@@ -872,7 +858,7 @@ void BezierEvalDeCasteljau(bool& error) {
params.push_back(g);
params.push_back(h);
params.push_back(i);
- bezier_curve_t cf2(params.begin(), params.end());
+ bezier_t cf2(params.begin(), params.end());
for (std::vector<double>::const_iterator cit = values.begin(); cit != values.end(); ++cit) {
ComparePoints(cf.evalDeCasteljau(*cit), cf(*cit), errmsg, error);
}
@@ -908,8 +894,8 @@ void BezierSplitCurve(bool& error) {
double t0 = (rand() / (double)RAND_MAX) * (t_max - t_min) + t_min;
double t1 = (rand() / (double)RAND_MAX) * (t_max - t0) + t0;
double ts = (rand() / (double)RAND_MAX) * (t1 - t0) + t0;
- bezier_curve_t c(wps.begin(), wps.end(), t0, t1);
- std::pair<bezier_curve_t, bezier_curve_t> cs = c.split(ts);
+ bezier_t c(wps.begin(), wps.end(), t0, t1);
+ std::pair<bezier_t, bezier_t> cs = c.split(ts);
// test on splitted curves :
if (!((c.degree_ == cs.first.degree_) && (c.degree_ == cs.second.degree_))) {
error = true;
@@ -939,19 +925,19 @@ void BezierSplitCurve(bool& error) {
ti += 0.01;
}
// Test extract function
- bezier_curve_t bezier_extracted0 = c.extract(t0 + 0.01, t1 - 0.01); // T_min < t0 < t1 < T_max
+ bezier_t bezier_extracted0 = c.extract(t0 + 0.01, t1 - 0.01); // T_min < t0 < t1 < T_max
for (double t = bezier_extracted0.min(); t < bezier_extracted0.max(); t += 0.01) {
ComparePoints(bezier_extracted0(t), c(t), errMsg6, error);
}
- bezier_curve_t bezier_extracted1 = c.extract(t0, t1 - 0.01); // T_min = t0 < t1 < T_max
+ bezier_t bezier_extracted1 = c.extract(t0, t1 - 0.01); // T_min = t0 < t1 < T_max
for (double t = bezier_extracted1.min(); t < bezier_extracted1.max(); t += 0.01) {
ComparePoints(bezier_extracted1(t), c(t), errMsg6, error);
}
- bezier_curve_t bezier_extracted2 = c.extract(t0 + 0.01, t1); // T_min < t0 < t1 = T_max
+ bezier_t bezier_extracted2 = c.extract(t0 + 0.01, t1); // T_min < t0 < t1 = T_max
for (double t = bezier_extracted2.min(); t < bezier_extracted2.max(); t += 0.01) {
ComparePoints(bezier_extracted2(t), c(t), errMsg6, error);
}
- bezier_curve_t bezier_extracted3 = c.extract(t0, t1); // T_min = t0 < t1 = T_max
+ bezier_t bezier_extracted3 = c.extract(t0, t1); // T_min = t0 < t1 = T_max
for (double t = bezier_extracted3.min(); t < bezier_extracted3.max(); t += 0.01) {
ComparePoints(bezier_extracted3(t), c(t), errMsg6, error);
}
@@ -1069,16 +1055,16 @@ void piecewiseCurveTest(bool& error) {
vec2.push_back(b); // x=2, y=1, z=1
vec3.push_back(c); // x=3, y=1, z=1
// Create three polynomials of constant value in the interval of definition
- polynomial_t pol1(vec1.begin(), vec1.end(), 0, 1);
- polynomial_t pol2(vec2.begin(), vec2.end(), 1, 2);
- polynomial_t pol3(vec3.begin(), vec3.end(), 2, 3);
+ boost::shared_ptr<polynomial_t> pol1_ptr (new polynomial_t(vec1.begin(), vec1.end(), 0, 1));
+ boost::shared_ptr<polynomial_t> pol2_ptr (new polynomial_t(vec2.begin(), vec2.end(), 1, 2));
+ boost::shared_ptr<polynomial_t> pol3_ptr (new polynomial_t(vec3.begin(), vec3.end(), 2, 3));
// 1 polynomial in curve
- piecewise_polynomial_curve_t pc(pol1);
+ piecewise_t pc(pol1_ptr);
res = pc(0.5);
ComparePoints(a, res, errmsg1, error);
// 3 polynomials in curve
- pc.add_curve(pol2);
- pc.add_curve(pol3);
+ pc.add_curve_ptr(pol2_ptr);
+ pc.add_curve_ptr(pol3_ptr);
// Check values on piecewise curve
// t in [0,1[ -> res=a
res = pc(0.);
@@ -1110,10 +1096,10 @@ void piecewiseCurveTest(bool& error) {
params1.push_back(c0);
params1.push_back(b0);
params1.push_back(a0);
- bezier_curve_t bc0(params0.begin(), params0.end(), 0., 1.);
- bezier_curve_t bc1(params1.begin(), params1.end(), 1., 2.);
- piecewise_bezier_curve_t pc_C0(bc0);
- pc_C0.add_curve(bc1);
+ boost::shared_ptr<bezier_t> bc0_ptr(new bezier_t(params0.begin(), params0.end(), 0., 1.));
+ boost::shared_ptr<bezier_t> bc1_ptr(new bezier_t(params1.begin(), params1.end(), 1., 2.));
+ piecewise_t pc_C0(bc0_ptr);
+ pc_C0.add_curve_ptr(bc1_ptr);
// Check value in t=0.5 and t=1.5
res = pc_C0(0.0);
ComparePoints(a0, res, errmsg1, error);
@@ -1139,10 +1125,10 @@ void piecewiseCurveTest(bool& error) {
time_control_points0.push_back(1.); // hermite 0 between [0,1]
time_control_points1.push_back(1.);
time_control_points1.push_back(3.); // hermite 1 between [1,3]
- cubic_hermite_spline_t chs0(control_points_0.begin(), control_points_0.end(), time_control_points0);
- cubic_hermite_spline_t chs1(control_points_1.begin(), control_points_1.end(), time_control_points1);
- piecewise_cubic_hermite_curve_t pc_C1(chs0);
- pc_C1.add_curve(chs1);
+ boost::shared_ptr<cubic_hermite_spline_t> chs0_ptr(new cubic_hermite_spline_t(control_points_0.begin(), control_points_0.end(), time_control_points0));
+ boost::shared_ptr<cubic_hermite_spline_t> chs1_ptr(new cubic_hermite_spline_t(control_points_1.begin(), control_points_1.end(), time_control_points1));
+ piecewise_t pc_C1(chs0_ptr);
+ pc_C1.add_curve_ptr(chs1_ptr);
// Create piecewise curve C2
point3_t a1(0, 0, 0);
point3_t b1(1, 1, 1);
@@ -1153,10 +1139,10 @@ void piecewiseCurveTest(bool& error) {
// in [1,2]
vecb.push_back(b1);
vecb.push_back(b1); // x=(t-1)+1, y=(t-1)+1, z=(t-1)+1
- polynomial_t pola(veca.begin(), veca.end(), 0, 1);
- polynomial_t polb(vecb.begin(), vecb.end(), 1, 2);
- piecewise_polynomial_curve_t pc_C2(pola);
- pc_C2.add_curve(polb);
+ boost::shared_ptr<polynomial_t> pola_ptr(new polynomial_t(veca.begin(), veca.end(), 0, 1));
+ boost::shared_ptr<polynomial_t> polb_ptr(new polynomial_t(vecb.begin(), vecb.end(), 1, 2));
+ piecewise_t pc_C2(pola_ptr);
+ pc_C2.add_curve_ptr(polb_ptr);
// check C0 continuity
std::string errmsg2("in piecewise polynomial curve test, Error while checking continuity C0 on ");
std::string errmsg3("in piecewise polynomial curve test, Error while checking continuity C1 on ");
@@ -1199,40 +1185,49 @@ void piecewiseCurveTest(bool& error) {
}
// CONVERT PIECEWISE POLYNOMIAL CURVES TO BEZIER AND HERMITE
std::string errmsg5("in piecewise polynomial curve test, Error while checking piecewise curve conversion");
- piecewise_bezier_curve_t pc_bezier = pc.convert_piecewise_curve_to_bezier<bezier_curve_t>();
- CompareCurves<piecewise_polynomial_curve_t, piecewise_bezier_curve_t>(pc, pc_bezier, errmsg5, error);
- piecewise_cubic_hermite_curve_t pc_hermite = pc.convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>();
- CompareCurves<piecewise_polynomial_curve_t, piecewise_cubic_hermite_curve_t>(pc, pc_hermite, errmsg5, error);
- piecewise_polynomial_curve_t pc_polynomial_same = pc.convert_piecewise_curve_to_polynomial<polynomial_t>();
- CompareCurves<piecewise_polynomial_curve_t, piecewise_polynomial_curve_t>(pc, pc_polynomial_same, errmsg5, error);
+ piecewise_t pc_bezier = pc.convert_piecewise_curve_to_bezier<bezier_t>();
+ CompareCurves<piecewise_t, piecewise_t>(pc, pc_bezier, errmsg5, error);
+ piecewise_t pc_hermite = pc.convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>();
+ CompareCurves<piecewise_t, piecewise_t>(pc, pc_hermite, errmsg5, error);
+ piecewise_t pc_polynomial_same = pc.convert_piecewise_curve_to_polynomial<polynomial_t>();
+ CompareCurves<piecewise_t, piecewise_t>(pc, pc_polynomial_same, errmsg5, error);
+ // CONVERT PIECEWISE BEZIER TO POLYNOMIAL AND HERMITE
+
+ std::string errmsg6("in piecewise bezier curve test, Error while checking piecewise curve conversion");
+ piecewise_t pc_bezier1 = pc_C0.convert_piecewise_curve_to_bezier<bezier_t>();
+ CompareCurves<piecewise_t, piecewise_t>(pc_C0, pc_bezier1, errmsg6, error);
+ piecewise_t pc_hermite1 = pc_C0.convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>();
+ CompareCurves<piecewise_t, piecewise_t>(pc_C0, pc_hermite1, errmsg6, error);
+ piecewise_t pc_polynomial1 = pc_C0.convert_piecewise_curve_to_polynomial<polynomial_t>();
+ CompareCurves<piecewise_t, piecewise_t>(pc_C0, pc_polynomial1, errmsg6, error);
// compare compute_derivate and derivate results :
- piecewise_polynomial_curve_t pc_C2_derivate = pc_C2.compute_derivate(1);
- piecewise_polynomial_curve_t pc_C2_derivate2 = pc_C2.compute_derivate(2);
- if (pc_C2.min() != pc_C2_derivate.min()) {
+ curve_abc_t* pc_C2_derivate = pc_C2.compute_derivate_ptr(1);
+ curve_abc_t* pc_C2_derivate2 = pc_C2.compute_derivate_ptr(2);
+ if (pc_C2.min() != pc_C2_derivate->min()) {
error = true;
std::cout << "min bounds for curve and it's derivate are not equals." << std::endl;
}
- if (pc_C2.min() != pc_C2_derivate2.min()) {
+ if (pc_C2.min() != pc_C2_derivate2->min()) {
error = true;
std::cout << "min bounds for curve and it's second derivate are not equals." << std::endl;
}
- if (pc_C2.max() != pc_C2_derivate.max()) {
+ if (pc_C2.max() != pc_C2_derivate->max()) {
error = true;
std::cout << "max bounds for curve and it's derivate are not equals." << std::endl;
}
- if (pc_C2.max() != pc_C2_derivate2.max()) {
+ if (pc_C2.max() != pc_C2_derivate2->max()) {
error = true;
std::cout << "max bounds for curve and it's second derivate are not equals." << std::endl;
}
double t = 0.;
while (t < pc_C2.max()) {
- if (!QuasiEqual(pc_C2.derivate(t, 1), pc_C2_derivate(t))) {
+ if (!QuasiEqual(pc_C2.derivate(t, 1), (*pc_C2_derivate)(t))) {
error = true;
std::cout << "value not equal between derivate and compute_derivate (order 1) at t = " << t << std::endl;
}
- if (!QuasiEqual(pc_C2.derivate(t, 2), pc_C2_derivate2(t))) {
+ if (!QuasiEqual(pc_C2.derivate(t, 2), (*pc_C2_derivate2)(t))) {
error = true;
std::cout << "value not equal between derivate and compute_derivate (order 2) at t = " << t << std::endl;
}
@@ -1250,7 +1245,7 @@ void curveAbcDimDynamicTest(bool& error) {
typedef polynomial<double, double, true> polynomial_test_t;
typedef exact_cubic<double, double, true> exact_cubic_test_t;
typedef exact_cubic_test_t::spline_constraints spline_constraints_test_t;
- typedef bezier_curve<double, double, true> bezier_curve_test_t;
+ typedef bezier_curve<double, double, true> bezier_test_t;
typedef cubic_hermite_spline<double, double, true> cubic_hermite_spline_test_t;
curve_abc_test_t* pt_curve_abc;
// POLYNOMIAL
@@ -1267,7 +1262,7 @@ void curveAbcDimDynamicTest(bool& error) {
error = false;
}
// BEZIER
- bezier_curve_test_t bc = bezier_from_curve<bezier_curve_test_t, polynomial_test_t>(pol);
+ bezier_test_t bc = bezier_from_curve<bezier_test_t>(pol);
try {
bc(0);
bc(1);
@@ -1275,7 +1270,7 @@ void curveAbcDimDynamicTest(bool& error) {
error = false;
}
// CUBIC HERMITE
- cubic_hermite_spline_test_t chs = hermite_from_curve<cubic_hermite_spline_test_t, polynomial_test_t>(pol);
+ cubic_hermite_spline_test_t chs = hermite_from_curve<cubic_hermite_spline_test_t>(pol);
try {
chs(0);
chs(1);
@@ -1361,10 +1356,11 @@ void PiecewisePolynomialCurveFromDiscretePoints(bool& error) {
time_points.push_back(t3);
// Piecewise polynomial curve C0 => Linear interpolation between points
- piecewise_polynomial_curve_t ppc_C0 =
- piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(points, time_points);
- if (!ppc_C0.is_continuous(0)) {
- std::cout << "PiecewisePolynomialCurveFromDiscretePoints, Error, piecewise curve is not C0" << std::endl;
+ piecewise_t ppc_C0 = piecewise_t::
+ convert_discrete_points_to_polynomial<polynomial_t>(points,time_points);
+ if (!ppc_C0.is_continuous(0))
+ {
+ std::cout<<"PiecewisePolynomialCurveFromDiscretePoints, Error, piecewise curve is not C0"<<std::endl;
error = true;
}
for (std::size_t i = 0; i < points.size(); i++) {
@@ -1375,11 +1371,13 @@ void PiecewisePolynomialCurveFromDiscretePoints(bool& error) {
ComparePoints(pos_between_po_and_p1, ppc_C0(time_between_po_and_p1), errMsg, error);
// Piecewise polynomial curve C1
- piecewise_polynomial_curve_t ppc_C1 =
- piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(points, points_derivative,
- time_points);
- if (!ppc_C1.is_continuous(1)) {
- std::cout << "PiecewisePolynomialCurveFromDiscretePoints, Error, piecewise curve is not C1" << std::endl;
+ piecewise_t ppc_C1 = piecewise_t::
+ convert_discrete_points_to_polynomial<polynomial_t>(points,
+ points_derivative,
+ time_points);
+ if (!ppc_C1.is_continuous(1))
+ {
+ std::cout<<"PiecewisePolynomialCurveFromDiscretePoints, Error, piecewise curve is not C1"<<std::endl;
error = true;
}
for (std::size_t i = 0; i < points.size(); i++) {
@@ -1388,14 +1386,18 @@ void PiecewisePolynomialCurveFromDiscretePoints(bool& error) {
}
// Piecewise polynomial curve C2
- piecewise_polynomial_curve_t ppc_C2 =
- piecewise_polynomial_curve_t::convert_discrete_points_to_polynomial<polynomial_t>(
- points, points_derivative, points_second_derivative, time_points);
- if (!ppc_C2.is_continuous(2)) {
- std::cout << "PiecewisePolynomialCurveFromDiscretePoints, Error, piecewise curve is not C1" << std::endl;
- error = true;
- }
- for (std::size_t i = 0; i < points.size(); i++) {
+ piecewise_t ppc_C2 = piecewise_t::
+ convert_discrete_points_to_polynomial<polynomial_t>(points,
+ points_derivative,
+ points_second_derivative,
+ time_points);
+ if (!ppc_C2.is_continuous(2))
+ {
+ std::cout<<"PiecewisePolynomialCurveFromDiscretePoints, Error, piecewise curve is not C1"<<std::endl;
+ error = true;
+ }
+ for (std::size_t i=0; i<points.size(); i++)
+ {
ComparePoints(points[i], ppc_C2(time_points[i]), errMsg, error);
ComparePoints(points_derivative[i], ppc_C2.derivate(time_points[i], 1), errMsg, error);
ComparePoints(points_second_derivative[i], ppc_C2.derivate(time_points[i], 2), errMsg, error);
@@ -1421,9 +1423,10 @@ void serializationCurvesTest(bool& error) {
polynomial_t pol1(vec1.begin(), vec1.end(), 0, 1);
polynomial_t pol2(vec2.begin(), vec2.end(), 1, 2);
polynomial_t pol3(vec3.begin(), vec3.end(), 2, 3);
- piecewise_polynomial_curve_t ppc(pol1);
- ppc.add_curve(pol2);
- ppc.add_curve(pol3);
+ piecewise_t ppc;
+ ppc.add_curve<polynomial_t>(pol1);
+ ppc.add_curve<polynomial_t>(pol2);
+ ppc.add_curve<polynomial_t>(pol3);
std::string fileName("fileTest.test");
std::string fileName1("fileTest1.test");
// Simple curves
@@ -1433,35 +1436,39 @@ void serializationCurvesTest(bool& error) {
pol_test.loadFromText<polynomial_t>(fileName1);
CompareCurves<polynomial_t, polynomial_t>(pol1, pol_test, errMsg1, error);
// Test serialization on Bezier
- bezier_curve_t bc = bezier_from_curve<bezier_curve_t, polynomial_t>(pol1);
- bc.saveAsText<bezier_curve_t>(fileName);
- bezier_curve_t bc_test;
- bc_test.loadFromText<bezier_curve_t>(fileName);
- CompareCurves<polynomial_t, bezier_curve_t>(pol1, bc_test, errMsg2, error);
+ bezier_t bc = bezier_from_curve<bezier_t>(pol1);
+ bc.saveAsText<bezier_t>(fileName);
+ bezier_t bc_test;
+ bc_test.loadFromText<bezier_t>(fileName);
+ CompareCurves<polynomial_t, bezier_t>(pol1, bc_test, errMsg2, error);
// Test serialization on Cubic Hermite
- cubic_hermite_spline_t chs = hermite_from_curve<cubic_hermite_spline_t, polynomial_t>(pol1);
+ cubic_hermite_spline_t chs = hermite_from_curve<cubic_hermite_spline_t>(pol1);
chs.saveAsText<cubic_hermite_spline_t>(fileName);
cubic_hermite_spline_t chs_test;
chs_test.loadFromText<cubic_hermite_spline_t>(fileName);
CompareCurves<polynomial_t, cubic_hermite_spline_t>(pol1, chs_test, errMsg3, error);
// Piecewise curves
// Test serialization on Piecewise Polynomial curve
- ppc.saveAsText<piecewise_polynomial_curve_t>(fileName);
- piecewise_polynomial_curve_t ppc_test;
- ppc_test.loadFromText<piecewise_polynomial_curve_t>(fileName);
- CompareCurves<piecewise_polynomial_curve_t, piecewise_polynomial_curve_t>(ppc, ppc_test, errMsg4, error);
+ ppc.saveAsText<piecewise_t>(fileName);
+ piecewise_t ppc_test,ppc_test_binary;
+ ppc_test.loadFromText<piecewise_t>(fileName);
+ CompareCurves<piecewise_t, piecewise_t>(ppc, ppc_test, errMsg4, error);
+ ppc.saveAsBinary<piecewise_t>(fileName);
+ ppc_test_binary.loadFromBinary<piecewise_t>(fileName);
+ CompareCurves<piecewise_t, piecewise_t>(ppc, ppc_test_binary, errMsg4, error);
+
// Test serialization on Piecewise Bezier curve
- piecewise_bezier_curve_t pbc = ppc.convert_piecewise_curve_to_bezier<bezier_curve_t>();
- pbc.saveAsText<piecewise_bezier_curve_t>(fileName);
- piecewise_bezier_curve_t pbc_test;
- pbc_test.loadFromText<piecewise_bezier_curve_t>(fileName);
- CompareCurves<piecewise_polynomial_curve_t, piecewise_bezier_curve_t>(ppc, pbc_test, errMsg4, error);
+ piecewise_t pbc = ppc.convert_piecewise_curve_to_bezier<bezier_t>();
+ pbc.saveAsText<piecewise_t>(fileName);
+ piecewise_t pbc_test;
+ pbc_test.loadFromText<piecewise_t>(fileName);
+ CompareCurves<piecewise_t, piecewise_t>(ppc, pbc_test, errMsg4, error);
// Test serialization on Piecewise Cubic Hermite curve
- piecewise_cubic_hermite_curve_t pchc = ppc.convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>();
- pchc.saveAsText<piecewise_cubic_hermite_curve_t>(fileName);
- piecewise_cubic_hermite_curve_t pchc_test;
- pchc_test.loadFromText<piecewise_cubic_hermite_curve_t>(fileName);
- CompareCurves<piecewise_polynomial_curve_t, piecewise_cubic_hermite_curve_t>(ppc, pchc_test, errMsg4, error);
+ piecewise_t pchc = ppc.convert_piecewise_curve_to_cubic_hermite<cubic_hermite_spline_t>();
+ pchc.saveAsText<piecewise_t>(fileName);
+ piecewise_t pchc_test;
+ pchc_test.loadFromText<piecewise_t>(fileName);
+ CompareCurves<piecewise_t, piecewise_t>(ppc, pchc_test, errMsg4, error);
// Test serialization on exact cubic
curves::T_Waypoint waypoints;
for (double i = 0; i <= 1; i = i + 0.2) {
@@ -1490,13 +1497,13 @@ void serializationCurvesTest(bool& error) {
// Piecewise Polynomial
pt_0 = NULL;
pt_1 = NULL;
- ppc_test = piecewise_polynomial_curve_t();
+ ppc_test = piecewise_t();
pt_0 = &ppc;
pt_1 = &ppc_test;
- (*pt_0).saveAsText<piecewise_polynomial_curve_t>(fileName);
- (*pt_1).loadFromText<piecewise_polynomial_curve_t>(fileName);
- CompareCurves<piecewise_polynomial_curve_t, piecewise_polynomial_curve_t>(
- ppc, (*dynamic_cast<piecewise_polynomial_curve_t*>(pt_1)), errMsg6, error);
+ (*pt_0).saveAsText<piecewise_t>(fileName);
+ (*pt_1).loadFromText<piecewise_t>(fileName);
+ CompareCurves<piecewise_t, piecewise_t>(
+ ppc, (*dynamic_cast<piecewise_t*>(pt_1)), errMsg6, error);
} catch (...) {
error = true;
std::cout << "Error in serializationCurvesTest" << std::endl;
@@ -1780,7 +1787,7 @@ void se3CurveTest(bool& error) {
params.push_back(b);
params.push_back(c);
params.push_back(d);
- boost::shared_ptr<bezier_curve_t> translation_bezier(new bezier_curve_t(params.begin(), params.end(), min, max));
+ boost::shared_ptr<bezier_t> translation_bezier(new bezier_t(params.begin(), params.end(), min, max));
cBezier = SE3Curve_t(translation_bezier, q0.toRotationMatrix(), q1.toRotationMatrix());
p0 = (*translation_bezier)(min);
p1 = (*translation_bezier)(max);
@@ -1918,7 +1925,7 @@ void Se3serializationTest(bool &error){
params.push_back(b);
params.push_back(c);
params.push_back(d);
- boost::shared_ptr<bezier_curve_t> translation_bezier(new bezier_curve_t(params.begin(), params.end(), min, max));
+ boost::shared_ptr<bezier_t> translation_bezier(new bezier_t(params.begin(), params.end(), min, max));
cBezier = SE3Curve_t(translation_bezier, q0, q1);
}