Commit c76e5052 by Fernbach Pierre Committed by GitHub

### Merge pull request #14 from pFernbach/topic/piecewise_generic

Topic/piecewise generic
parents 596a5d2c 6b64aaf9
 ... ... @@ -39,8 +39,10 @@ struct bezier_curve : public curve_abc { typedef std::vector > t_point_t; typedef typename t_point_t::const_iterator cit_point_t; typedef bezier_curve bezier_curve_t; typedef piecewise_curve piecewise_bezier_curve_t; typedef boost::shared_ptr bezier_curve_ptr_t; typedef piecewise_curve piecewise_curve_t; typedef curve_abc 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 { 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$.
/// 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 { /// \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 { } /// \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 curves; bezier_curve_t current = *this; for (int i = 0; i < times.rows(); ++i) { std::pair pairsplit = current.split(times[i]); ... ... @@ -338,7 +346,12 @@ struct bezier_curve : public curve_abc { current = pairsplit.second; } curves.push_back(current); return piecewise_bezier_curve_t(curves); piecewise_curve_t res; for(typename std::vector::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 { /// \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 */ ... ...
 ... ... @@ -39,6 +39,7 @@ struct cubic_hermite_spline : public curve_abc { typedef std::vector
 ... ... @@ -15,6 +15,7 @@ #include "serialization/archive.hpp" #include "serialization/eigen-matrix.hpp" #include #include #include ... ... @@ -28,6 +29,8 @@ struct curve_abc : std::unary_function, public serialization::Seria typedef Point point_t; typedef Point_derivate point_derivate_t; typedef Time time_t; typedef curve_abc curve_t; // parent class typedef boost::shared_ptr curve_ptr_t; /* Constructors - destructors */ public: ... ... @@ -44,12 +47,19 @@ struct curve_abc : std::unary_function, 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, 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 timeRange() { return std::make_pair(min(), max()); } /*Helpers*/ ... ...
 ... ... @@ -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 Polynomial polynomial_from_curve(const curveTypeToConvert& curve) { template 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 Bezier bezier_from_curve(const curveTypeToConvert& curve) { template 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 Hermite hermite_from_curve(const curveTypeToConvert& curve) { template 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); ... ...
 ... ... @@ -39,7 +39,7 @@ template , typename T_Point = std::vector >, typename SplineBase = polynomial > struct exact_cubic : public piecewise_curve { struct exact_cubic : public piecewise_curve { typedef Point point_t; typedef T_Point t_point_t; typedef Eigen::Matrix MatrixX; ... ... @@ -53,8 +53,9 @@ struct exact_cubic : public piecewise_curve spline_constraints; typedef exact_cubic exact_cubic_t; typedef piecewise_curve piecewise_curve_t; typedef curve_abc curve_abc_t; // parent class typedef piecewise_curve 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 exact_cubic(In wayPointsBegin, In wayPointsEnd) : piecewise_curve_t(computeWayPoints(wayPointsBegin, wayPointsEnd)) {} : piecewise_curve_t() { t_spline_t subSplines = computeWayPoints(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 exact_cubic(In wayPointsBegin, In wayPointsEnd, const spline_constraints& constraints) : piecewise_curve_t(computeWayPoints(wayPointsBegin, wayPointsEnd, constraints)) {} : piecewise_curve_t() { t_spline_t subSplines = computeWayPoints(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_curvegetNumberCurves(); } spline_t getSplineAt(std::size_t index) { return this->curves_.at(index); } spline_t getSplineAt(std::size_t index) { boost::shared_ptr s_ptr = boost::dynamic_pointer_cast(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. ... ...
 ... ... @@ -9,6 +9,9 @@ #ifndef CURVES_FWD_H #define CURVES_FWD_H #include #include #include namespace curves { ... ... @@ -21,12 +24,12 @@ template template struct cubic_hermite_spline; template template struct exact_cubic; template typename Point_derivate> struct piecewise_curve; template ... ... @@ -49,6 +52,50 @@ template template struct quadratic_variable; // typedef of the commonly used templates arguments : // eigen types : typedef Eigen::Vector3d point3_t; typedef Eigen::Matrix point6_t; typedef Eigen::VectorXd pointX_t; typedef Eigen::Matrix matrix3_t; typedef Eigen::Matrix matrix4_t; typedef Eigen::Quaternion quaternion_t; typedef Eigen::Transform transform_t; typedef std::vector > t_point3_t; typedef std::vector > t_pointX_t; // abstract curves types: typedef curve_abc curve_abc_t; // base abstract class typedef curve_abc curve_3_t; // generic class of curve of size 3 typedef curve_abc curve_rotation_t; // templated class used for the rotation (return dimension are fixed) typedef curve_abc 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_ptr_t; typedef boost::shared_ptr curve3_ptr_t; typedef boost::shared_ptr curve_rotation_ptr_t; typedef boost::shared_ptr curve_SE3_ptr_t; // definition of all curves class with pointX as return type: typedef polynomial polynomial_t; typedef exact_cubic exact_cubic_t; typedef bezier_curve bezier_t; typedef cubic_hermite_spline cubic_hermite_spline_t; typedef piecewise_curve piecewise_t; // definition of all curves class with point3 as return type: typedef polynomial polynomial3_t; typedef exact_cubic exact_cubic3_t; typedef bezier_curve bezier3_t; typedef cubic_hermite_spline cubic_hermite_spline3_t; typedef piecewise_curve piecewise3_t; // special curves with return type fixed: typedef SO3Linear SO3Linear_t; typedef SE3Curve SE3Curve_t; typedef piecewise_curve piecewise_SE3_t; } #endif // CURVES_FWD_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 ... ...
 ... ... @@ -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 ... ...
 ... ... @@ -34,7 +34,8 @@ quadratic_variable compute_integral_cost_internal(const problem_datacompute_derivate(num_derivate); const t_point_t& wps = acc.waypoints(); return bezier_product(wps.begin(), wps.end(), wps.begin(), wps.end(), pData.dim_); quadratic_variable res(bezier_product(wps.begin(), wps.end(), wps.begin(), wps.end(), pData.dim_)); return res; } template ... ...
 ... ... @@ -10,6 +10,8 @@ #include "curve_abc.h" #include "curve_conversion.h" #include #include namespace curves { /// \class PiecewiseCurve. ... ... @@ -23,21 +25,19 @@ namespace curves { /// template , typename T_Point = std::vector >, typename Curve = curve_abc, typename Point_derivate = Point > struct piecewise_curve : public curve_abc { 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 > t_point_t; typedef std::vector > t_point_derivate_t; typedef Time time_t; typedef Numeric num_t; typedef Curve curve_t; typedef typename std::vector t_curve_t; typedef curve_abc curve_t; // parent class typedef boost::shared_ptr curve_ptr_t; typedef typename std::vector t_curve_ptr_t; typedef typename std::vector