Commit 7a8e77f9 by stevet Committed by Pierre Fernbach

remove cubic and quintic classes

parent 2f385541
 ... ... @@ -58,9 +58,7 @@ SET(${PROJECT_NAME}_HEADERS include/${PROJECT_NAME}/MathDefs.h include/${PROJECT_NAME}/polynomial.h include/${PROJECT_NAME}/bezier_curve.h include/${PROJECT_NAME}/cubic_spline.h include/${PROJECT_NAME}/curve_constraint.h include/${PROJECT_NAME}/quintic_spline.h include/${PROJECT_NAME}/linear_variable.h include/${PROJECT_NAME}/quadratic_variable.h include/${PROJECT_NAME}/cubic_hermite_spline.h ... ...
 /** * \file cubic_spline.h * \brief Definition of a cubic spline. * \author Steve T. * \version 0.1 * \date 06/17/2013 * * This file contains definitions for the CubicFunction struct. * It allows the creation and evaluation of natural * smooth cubic splines of arbitrary dimension */ #ifndef _STRUCT_CUBICSPLINE #define _STRUCT_CUBICSPLINE #include "MathDefs.h" #include "polynomial.h" #include namespace curves { /// \brief Creates coefficient vector of a cubic spline defined on the interval /// \f$[t_{min}, t_{max}]\f$. It follows the equation :
/// \f$x(t) = a + b(t - t_{min}) + c(t - t_{min})^2 + d(t - t_{min})^3 \f$ where \f$t \in [t_{min}, t_{max}] \f$ /// with a, b, c and d the control points. /// template T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c, Point const& d) { T_Point res; res.push_back(a); res.push_back(b); res.push_back(c); res.push_back(d); return res; } template polynomial create_cubic(Point const& a, Point const& b, Point const& c, Point const& d, const Time t_min, const Time t_max) { T_Point coeffs = make_cubic_vector(a, b, c, d); return polynomial(coeffs.begin(), coeffs.end(), t_min, t_max); } } // namespace curves #endif //_STRUCT_CUBICSPLINE
 ... ... @@ -20,10 +20,9 @@ #define _CLASS_EXACTCUBIC #include "curve_abc.h" #include "cubic_spline.h" #include "quintic_spline.h" #include "curve_constraint.h" #include "piecewise_curve.h" #include "polynomial.h" #include "MathDefs.h" ... ... @@ -41,6 +40,7 @@ template > struct exact_cubic : public piecewise_curve { typedef Point point_t; typedef const Eigen::Ref point_ref_t; typedef T_Point t_point_t; typedef Eigen::Matrix MatrixX; typedef Eigen::Matrix Matrix3; ... ... @@ -52,9 +52,10 @@ struct exact_cubic : public piecewise_curve { typedef typename t_spline_t::const_iterator cit_spline_t; typedef curve_constraints spline_constraints; typedef exact_cubic exact_cubic_t; typedef exact_cubic exact_cubic_t; typedef curve_abc curve_abc_t; // parent class typedef piecewise_curve piecewise_curve_t; typedef piecewise_curve piecewise_curve_t; typedef polynomial polynomial_t; typedef typename piecewise_curve_t::t_curve_ptr_t t_curve_ptr_t; /* Constructors - destructors */ ... ... @@ -128,6 +129,29 @@ struct exact_cubic : public piecewise_curve { } private: static polynomial_t create_cubic(point_ref_t a,point_ref_t b, point_ref_t c, point_ref_t d, const time_t t_min, const time_t t_max){ typename polynomial_t::t_point_t coeffs; coeffs.push_back(a); coeffs.push_back(b); coeffs.push_back(c); coeffs.push_back(d); return polynomial_t(coeffs.begin(), coeffs.end(), t_min, t_max); } static polynomial_t create_quintic(point_ref_t a,point_ref_t b, point_ref_t c, point_ref_t d, point_ref_t e, point_ref_t f, const time_t t_min, const time_t t_max){ typename polynomial_t::t_point_t coeffs; coeffs.push_back(a); coeffs.push_back(b); coeffs.push_back(c); coeffs.push_back(d); coeffs.push_back(e); coeffs.push_back(f); return polynomial_t(coeffs.begin(), coeffs.end(), t_min, t_max); } /// \brief Compute polynom of exact cubic spline from waypoints. /// Compute the coefficients of polynom as in paper : "Task-Space Trajectories via Cubic Spline Optimization".
/// \f$x_i(t)=a_i+b_i(t-t_i)+c_i(t-t_i)^2\f$
... ... @@ -198,7 +222,7 @@ struct exact_cubic : public piecewise_curve { it = wayPointsBegin, next = wayPointsBegin; ++next; for (int i = 0; next != wayPointsEnd; ++i, ++it, ++next) { subSplines.push_back(create_cubic(a.row(i), b.row(i), c.row(i), d.row(i), subSplines.push_back(create_cubic(a.row(i), b.row(i), c.row(i), d.row(i), (*it).first, (*next).first)); } return subSplines; ... ... @@ -230,7 +254,7 @@ struct exact_cubic : public piecewise_curve { const num_t &init_t = wayPointsBegin->first, end_t = wayPointsNext->first; const num_t dt = end_t - init_t, dt_2 = dt * dt, dt_3 = dt_2 * dt; const point_t d0 = (a1 - a0 - b0 * dt - c0 * dt_2) / dt_3; subSplines.push_back(create_cubic(a0, b0, c0, d0, init_t, end_t)); subSplines.push_back(create_cubic(a0, b0, c0, d0, init_t, end_t)); constraints.init_vel = subSplines.back().derivate(end_t, 1); constraints.init_acc = subSplines.back().derivate(end_t, 2); } ... ... @@ -270,7 +294,7 @@ struct exact_cubic : public piecewise_curve { d = rhs.row(0); e = rhs.row(1); f = rhs.row(2); subSplines.push_back(create_quintic(a0, b0, c0, d, e, f, init_t, end_t)); subSplines.push_back(create_quintic(a0, b0, c0, d, e, f, init_t, end_t)); } public: ... ...