Commit d0f0841d by Pierre Fernbach

### Merge branch 'topic/piecewise_polynomial' into 'devel'

Add new methods in polynomial and piecewise polynomial

See merge request loco-3d/curves!19
parents 70c4ecba 4eb2b375
 ... ... @@ -100,6 +100,19 @@ namespace curves return (curves_.at(find_interval(t))).derivate(t, order); } /** * @brief compute_derivate return a piecewise_curve which is the derivative of this at given order * @param order order of derivative * @return */ piecewise_curve compute_derivate(const std::size_t order) const{ piecewise_curve res; for(typename t_curve_t::const_iterator itc = curves_.begin() ; itc < curves_.end() ; ++itc){ res.add_curve(itc->compute_derivate(order)); } return res; } /// \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. ... ... @@ -179,6 +192,7 @@ namespace curves return pc_res; } template piecewise_curve convert_piecewise_curve_to_cubic_hermite() { ... ... @@ -216,53 +230,75 @@ namespace curves } template static piecewise_curve convert_discrete_points_to_polynomial(T_Point points, Time T_min, Time T_max) static piecewise_curve convert_discrete_points_to_polynomial(T_Point points, t_time_t time_points) { if(Safe &! (points.size()>1)) { //std::cout<<"[Min,Max]=["< convert_discrete_points_to_polynomial, Error, less than 2 discrete points"); throw std::invalid_argument("piecewise_curve::convert_discrete_points_to_polynomial: Error, less than 2 discrete points"); } typedef piecewise_curve piecewise_curve_out_t; Time discretization_step = (T_max-T_min)/Time(points.size()-1); Time time_actual = T_min; // Initialization at first points point_t actual_point = points[0]; point_t next_point = points[1]; point_t coeff_order_zero(actual_point); point_t coeff_order_one((next_point-actual_point)/discretization_step); t_point_t coeffs; coeffs.push_back(coeff_order_zero); coeffs.push_back(coeff_order_one); Polynomial pol(coeffs,time_actual,time_actual+discretization_step); piecewise_curve_out_t ppc(pol); time_actual += discretization_step; // Other points for (std::size_t i=1; i 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])); } return piecewise_res; } template static piecewise_curve convert_discrete_points_to_polynomial(T_Point points,T_Point points_derivative, t_time_t time_points) { if(Safe &! (points.size()>1)) { //std::cout<<"[Min,Max]=["< 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],time_points[i-1],time_points[i])); } return piecewise_res; } template static piecewise_curve convert_discrete_points_to_polynomial(T_Point points,T_Point points_derivative, T_Point points_second_derivative, t_time_t time_points) { if(Safe &! (points.size()>1)) { coeffs.clear(); actual_point = points[i]; next_point = points[i+1]; coeff_order_zero = actual_point; coeff_order_one = (next_point-actual_point)/discretization_step; coeffs.push_back(coeff_order_zero); coeffs.push_back(coeff_order_one); ppc.add_curve(Polynomial(coeffs,time_actual,time_actual+discretization_step)); time_actual += discretization_step; //std::cout<<"[Min,Max]=["< 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],points[i],points_derivative[i],points_second_derivative[i],time_points[i-1],time_points[i])); } return piecewise_res; } private: /// \brief Get index of the interval corresponding to time t for the interpolation. ... ... @@ -357,4 +393,4 @@ namespace curves } // end namespace #endif // _CLASS_PIECEWISE_CURVE \ No newline at end of file #endif // _CLASS_PIECEWISE_CURVE
 ... ... @@ -84,6 +84,7 @@ namespace curves safe_check(); } /// \brief Constructor. /// \param zeroOrderCoefficient : an iterator pointing to the first element of a structure containing the coefficients /// it corresponds to the zero degree coefficient. ... ... @@ -101,6 +102,134 @@ namespace curves safe_check(); } /// /// \brief Constructor from boundary condition with C0 : create a polynomial that connect exactly init and end (order 1) /// \param init the initial point of the curve /// \param end the final point of the curve /// \param min : LOWER bound on interval definition of the spline. /// \param max : UPPER bound on interval definition of the spline. /// polynomial(const Point& init, const Point& end, const time_t min, const time_t max ): dim_(init.size()), degree_(1), T_min_(min), T_max_(max) { if(init.size() != end.size()) throw std::invalid_argument("init and end points must have the same dimensions."); t_point_t coeffs; coeffs.push_back(init); coeffs.push_back((end-init)/(max-min)); coefficients_ = init_coeffs(coeffs.begin(), coeffs.end()); safe_check(); } /// /// \brief Constructor from boundary condition with C1 : /// create a polynomial that connect exactly init and end and thier first order derivatives(order 3) /// \param init the initial point of the curve /// \param d_init the initial value of the derivative of the curve /// \param end the final point of the curve /// \param d_end the final value of the derivative of the curve /// \param min : LOWER bound on interval definition of the spline. /// \param max : UPPER bound on interval definition of the spline. /// polynomial(const Point& init,const Point& d_init, const Point& end, const Point& d_end,const time_t min, const time_t max ): dim_(init.size()), degree_(3), T_min_(min), T_max_(max) { if(init.size() != end.size()) throw std::invalid_argument("init and end points must have the same dimensions."); if(init.size() != d_init.size()) throw std::invalid_argument("init and d_init points must have the same dimensions."); if(init.size() != d_end.size()) throw std::invalid_argument("init and d_end points must have the same dimensions."); /* the coefficients [c0 c1 c2 c3] are found by solving the following system of equation (found from the boundary conditions) : [1 0 0 0 ] [c0] [ init ] [1 T T^2 T^3 ] x [c1] = [ end ] [0 1 0 0 ] [c2] [d_init] [0 1 2T 3T^2] [c3] [d_end ] */ double T = max-min; Eigen::Matrix m; m << 1.,0,0,0, 1.,T,T*T,T*T*T, 0,1.,0,0, 0,1.,2.*T,3.*T*T; Eigen::Matrix m_inv = m.inverse(); Eigen::Matrix bc; // boundary condition vector coefficients_ = coeff_t::Zero(dim_,degree_+1); // init coefficient matrix with the right size for(size_t i = 0 ;i < dim_ ; ++i){ // for each dimension, solve the boundary condition problem : bc[0] = init[i]; bc[1] = end[i]; bc[2] = d_init[i]; bc[3] = d_end[i]; coefficients_.row(i) = (m_inv*bc).transpose(); } safe_check(); } /// /// \brief Constructor from boundary condition with C2 : /// create a polynomial that connect exactly init and end and thier first and second order derivatives(order 5) /// \param init the initial point of the curve /// \param d_init the initial value of the derivative of the curve /// \param d_init the initial value of the second derivative of the curve /// \param end the final point of the curve /// \param d_end the final value of the derivative of the curve /// \param d_end the final value of the second derivative of the curve /// \param min : LOWER bound on interval definition of the spline. /// \param max : UPPER bound on interval definition of the spline. /// polynomial(const Point& init,const Point& d_init,const Point& dd_init, const Point& end, const Point& d_end,const Point& dd_end,const time_t min, const time_t max ): dim_(init.size()), degree_(5), T_min_(min), T_max_(max) { if(init.size() != end.size()) throw std::invalid_argument("init and end points must have the same dimensions."); if(init.size() != d_init.size()) throw std::invalid_argument("init and d_init points must have the same dimensions."); if(init.size() != d_end.size()) throw std::invalid_argument("init and d_end points must have the same dimensions."); if(init.size() != dd_init.size()) throw std::invalid_argument("init and dd_init points must have the same dimensions."); if(init.size() != dd_end.size()) throw std::invalid_argument("init and dd_end points must have the same dimensions."); /* the coefficients [c0 c1 c2 c3 c4 c5] are found by solving the following system of equation (found from the boundary conditions) : [1 0 0 0 0 0 ] [c0] [ init ] [1 T T^2 T^3 T^4 T^5 ] [c1] [ end ] [0 1 0 0 0 0 ] [c2] [d_init ] [0 1 2T 3T^2 4T^3 5T^4 ] x [c3] = [d_end ] [0 0 2 0 0 0 ] [c4] [dd_init] [0 0 2 6T 12T^2 20T^3] [c5] [dd_end ] */ double T = max-min; Eigen::Matrix m; m << 1.,0,0,0,0,0, 1.,T,T*T,pow(T,3),pow(T,4),pow(T,5), 0,1.,0,0,0,0, 0,1.,2.*T,3.*T*T,4.*pow(T,3),5.*pow(T,4), 0,0,2,0,0,0, 0,0,2,6.*T,12.*T*T,20.*pow(T,3); Eigen::Matrix m_inv = m.inverse(); Eigen::Matrix bc; // boundary condition vector coefficients_ = coeff_t::Zero(dim_,degree_+1); // init coefficient matrix with the right size for(size_t i = 0 ;i < dim_ ; ++i){ // for each dimension, solve the boundary condition problem : bc[0] = init[i]; bc[1] = end[i]; bc[2] = d_init[i]; bc[3] = d_end[i]; bc[4] = dd_init[i]; bc[5] = dd_end[i]; coefficients_.row(i) = (m_inv*bc).transpose(); } safe_check(); } /// \brief Destructor ~polynomial() { ... ... @@ -123,11 +252,11 @@ namespace curves { if(T_min_ > T_max_) { std::invalid_argument("Tmin should be inferior to Tmax"); throw std::invalid_argument("Tmin should be inferior to Tmax"); } if(coefficients_.size() != int(degree_+1)) if(coefficients_.cols() != int(degree_+1)) { std::runtime_error("Spline order and coefficients do not match"); throw std::runtime_error("Spline order and coefficients do not match"); } } } ... ... @@ -203,6 +332,8 @@ namespace curves coeff_t deriv_coeff(coeff_t coeff) const { if(coeff.cols() == 1) // only the constant part is left, fill with 0 return coeff_t::Zero(coeff.rows(),1); coeff_t coeff_derivated(coeff.rows(), coeff.cols()-1); for (std::size_t i=0; i