Commit 844e49b7 by Steve Tonneau

### externalized helper function to build cubic and quintic splines

parent 6ebf1138
 ... ... @@ -33,6 +33,14 @@ T_Point make_cubic_vector(Point const& a, Point const& b, Point const& c, Point res.push_back(a);res.push_back(b);res.push_back(c);res.push_back(d); return res; } template spline_curve create_cubic(Point const& a, Point const& b, Point const& c, Point const &d, const Time min, const Time max) { T_Point coeffs = make_cubic_vector(a,b,c,d); return spline_curve(coeffs.begin(),coeffs.end(), min, max); } } #endif //_STRUCT_CUBICSPLINE
 ... ... @@ -20,8 +20,7 @@ #ifndef _CLASS_CUBICZEROVELACC #define _CLASS_CUBICZEROVELACC #include "curve_abc.h" #include "cubic_spline.h" #include "exact_cubic.h" #include "MathDefs.h" ... ... @@ -30,23 +29,28 @@ namespace spline { /// \class ExactCubic /// \class cubic_zero_vel. /// \brief Represents a set of cubic splines defining a continuous function /// crossing each of the waypoint given in its initialization /// crossing each of the waypoint given in its initialization. Additional constraints /// are used to increase the order of the last and first splines, to start and finish /// trajectory with zero velocity and acceleration. Thus the first and last splines /// template > struct exact_cubic : public curve_abc /// template, typename T_Point =std::vector > > struct cubic_zero_vel : public exact_cubic { typedef Point point_t; typedef Eigen::Matrix MatrixX; typedef Time time_t; typedef Numeric num_t; typedef cubic_spline cubic_spline_t; typedef quintic_spline cubic_spline_t; typedef typename std::vector T_cubic; typedef typename T_cubic::iterator IT_cubic; typedef typename T_cubic::const_iterator CIT_cubic; typedef Point point_t; typedef T_Point t_point_t; typedef Eigen::Matrix MatrixX; typedef Time time_t; typedef Numeric num_t; typedef spline_curve spline_t; typedef exact_cubic exact_cubic_t; typedef typename std::vector t_spline_t; typedef typename t_spline_t::iterator it_spline_t; typedef typename t_spline_t::const_iterator cit_spline_t; /* Constructors - destructors */ public: ... ... @@ -54,119 +58,19 @@ struct exact_cubic : public curve_abc ///\param wayPointsBegin : an iterator pointing to the first element of a waypoint container ///\param wayPointsEns : an iterator pointing to the end of a waypoint container template exact_cubic(In wayPointsBegin, In wayPointsEnd) cubic_zero_vel(In wayPointsBegin, In wayPointsEnd) : exact_cubic_t(wayPointsBegin, wayPointsEnd) { std::size_t const size(std::distance(wayPointsBegin, wayPointsEnd)); if(Safe && size < 1) { throw; // TODO } // refer to the paper to understand all this. MatrixX h1 = MatrixX::Zero(size, size); MatrixX h2 = MatrixX::Zero(size, size); MatrixX h3 = MatrixX::Zero(size, size); MatrixX h4 = MatrixX::Zero(size, size); MatrixX h5 = MatrixX::Zero(size, size); MatrixX h6 = MatrixX::Zero(size, size); MatrixX a = MatrixX::Zero(size, Dim); MatrixX b = MatrixX::Zero(size, Dim); MatrixX c = MatrixX::Zero(size, Dim); MatrixX d = MatrixX::Zero(size, Dim); MatrixX x = MatrixX::Zero(size, Dim); In it(wayPointsBegin), next(wayPointsBegin); ++next; for(std::size_t i(0); next != wayPointsEnd; ++next, ++it, ++i) { num_t const dTi((*next).first - (*it).first); num_t const dTi_sqr(dTi * dTi); num_t const dTi_cube(dTi_sqr * dTi); // filling matrices values h3(i,i) = -3 / dTi_sqr; h3(i,i+1) = 3 / dTi_sqr; h4(i,i) = -2 / dTi; h4(i,i+1) = -1 / dTi; h5(i,i) = 2 / dTi_cube; h5(i,i+1) = -2 / dTi_cube; h6(i,i) = 1 / dTi_sqr; h6(i,i+1) = 1 / dTi_sqr; if( i+2 < size) { In it2(next); ++ it2; num_t const dTi_1((*it2).first - (*next).first); num_t const dTi_1sqr(dTi_1 * dTi_1); // this can be optimized but let's focus on clarity as long as not needed h1(i+1, i) = 2 / dTi; h1(i+1, i+1) = 4 / dTi + 4 / dTi_1; h1(i+1, i+2) = 2 / dTi_1; h2(i+1, i) = -6 / dTi_sqr; h2(i+1, i+1) = (6 / dTi_1sqr) - (6 / dTi_sqr); h2(i+1, i+2) = 6 / dTi_1sqr; } x.row(i)= (*it).second.transpose(); } // adding last x x.row(size-1)= (*it).second.transpose(); a= x; PseudoInverse(h1); b = h1 * h2 * x; //h1 * b = h2 * x => b = (h1)^-1 * h2 * x c = h3 * x + h4 * b; d = h5 * x + h6 * b; it= wayPointsBegin, next=wayPointsBegin; ++ next; for(int i=0; next != wayPointsEnd; ++i, ++it, ++next) { subSplines_.push_back(new cubic_spline_t(a.row(i), b.row(i), c.row(i), d.row(i), (*it).first, (*next).first)); } subSplines_.push_back(new cubic_spline_t(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1), (*it).first, (*it).first)); // NOTHING } ///\brief Destructor ~exact_cubic() { for(IT_cubic it = subSplines_.begin(); it != subSplines_.end(); ++ it) { delete(*it); } } private: exact_cubic(const exact_cubic&); exact_cubic& operator=(const exact_cubic&); /* Constructors - destructors */ /*Operations*/ public: /// \brief Evaluation of the cubic spline at time t. /// \param t : the time when to evaluate the spine /// \param return : the value x(t) virtual point_t operator()(time_t t) const { if(Safe && (t < subSplines_.front()->t_min_ || t > subSplines_.back()->t_max_)){throw std::out_of_range("TODO");} for(CIT_cubic it = subSplines_.begin(); it != subSplines_.end(); ++ it) { if(t >= ((*it)->t_min_) && t <= ((*it)->t_max_)) { return (*it)->operator()(t); } } } /*Operations*/ /*Helpers*/ public: num_t virtual min() const{return subSplines_.front()->t_min_;} num_t virtual max() const{return subSplines_.back()->t_max_;} /*Helpers*/ ~cubic_zero_vel(){} /*Attributes*/ private: T_cubic subSplines_; /*Attributes*/ cubic_zero_vel(const cubic_zero_vel&); cubic_zero_vel& operator=(const cubic_zero_vel&); /* Constructors - destructors */ }; } #endif //_CLASS_CUBICZEROVELACC ... ...
 ... ... @@ -48,6 +48,7 @@ struct exact_cubic : public curve_abc typedef typename std::vector t_spline_t; typedef typename t_spline_t::iterator it_spline_t; typedef typename t_spline_t::const_iterator cit_spline_t; typedef curve_abc curve_abc_t; /* Constructors - destructors */ public: ... ... @@ -56,7 +57,7 @@ struct exact_cubic : public curve_abc ///\param wayPointsEns : an iterator pointing to the end of a waypoint container template exact_cubic(In wayPointsBegin, In wayPointsEnd) : subSplines_(computeWayPoints(wayPointsBegin, wayPointsEnd)) : curve_abc_t(), subSplines_(computeWayPoints(wayPointsBegin, wayPointsEnd)) { } ... ... @@ -131,19 +132,14 @@ struct exact_cubic : public curve_abc it= wayPointsBegin, next=wayPointsBegin; ++ next; for(int i=0; next != wayPointsEnd; ++i, ++it, ++next) { add_cubic(a.row(i), b.row(i), c.row(i), d.row(i),(*it).first, (*next).first, subSplines); subSplines.push_back( create_cubic(a.row(i), b.row(i), c.row(i), d.row(i),(*it).first, (*next).first)); } add_cubic(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1),(*it).first, (*it).first, subSplines); subSplines.push_back( create_cubic(a.row(size-1), b.row(size-1), c.row(size-1), d.row(size-1), (*it).first, (*it).first)); return subSplines; } void add_cubic(point_t const& a, point_t const& b, point_t const& c, point_t const &d, const time_t min, const time_t max, t_spline_t& subSplines) const { t_point_t coeffs = make_cubic_vector(a,b,c,d); subSplines.push_back(spline_t(coeffs.begin(),coeffs.end(), min, max)); } private: exact_cubic(const exact_cubic&); exact_cubic& operator=(const exact_cubic&); ... ... @@ -156,10 +152,10 @@ struct exact_cubic : public curve_abc /// \param return : the value x(t) virtual point_t operator()(time_t t) const { if(Safe && (t < subSplines_.front().t_min_ || t > subSplines_.back().t_max_)){throw std::out_of_range("TODO");} if(Safe && (t < subSplines_.front().min() || t > subSplines_.back().max())){throw std::out_of_range("TODO");} for(cit_spline_t it = subSplines_.begin(); it != subSplines_.end(); ++ it) { if(t >= (it->t_min_) && t <= (it->t_max_)) if(t >= (it->min()) && t <= (it->max())) { return it->operator()(t); } ... ... @@ -169,8 +165,8 @@ struct exact_cubic : public curve_abc /*Helpers*/ public: num_t virtual min() const{return subSplines_.front().t_min_;} num_t virtual max() const{return subSplines_.back().t_max_;} num_t virtual min() const{return subSplines_.front().min();} num_t virtual max() const{return subSplines_.back().max();} /*Helpers*/ /*Attributes*/ ... ...
 ... ... @@ -35,6 +35,14 @@ T_Point make_quintic_vector(Point const& a, Point const& b, Point const& c, res.push_back(d);res.push_back(e);res.push_back(f); return res; } template spline_curve create_quintic(Point const& a, Point const& b, Point const& c, Point const &d, Point const &e, Point const &f, const Time min, const Time max) { T_Point coeffs = make_quintic_vector(a,b,c,d,e,f); return spline_curve(coeffs.begin(),coeffs.end(), min, max); } } #endif //_STRUCT_QUINTIC_SPLINE
 ... ... @@ -39,6 +39,7 @@ struct spline_curve : public curve_abc typedef typename t_point_t::const_iterator cit_point_t; typedef Time time_t; typedef Numeric num_t; typedef curve_abc curve_abc_t; /* Constructors - destructors */ public: ///\brief Constructor ... ... @@ -48,7 +49,8 @@ struct spline_curve : public curve_abc ///\param min: LOWER bound on interval definition of the spline ///\param max: UPPER bound on interval definition of the spline spline_curve(const T_Point& coefficients, const time_t min, const time_t max) :coefficients_(coefficients), t_min_(min), t_max_(max), dim_(Dim), order_(coefficients_.size()+1) : curve_abc_t(), coefficients_(coefficients), t_min_(min), t_max_(max), dim_(Dim), order_(coefficients_.size()+1) { if(Safe) { ... ... @@ -71,7 +73,8 @@ struct spline_curve : public curve_abc ///\param max: UPPER bound on interval definition of the spline template spline_curve(In zeroOrderCoefficient, In out, const time_t min, const time_t max) :coefficients_(init_coeffs(zeroOrderCoefficient, out)), t_min_(min), t_max_(max), dim_(Dim), order_(coefficients_.size()+1) :coefficients_(init_coeffs(zeroOrderCoefficient, out)), dim_(Dim), order_(coefficients_.size()+1), t_min_(min), t_max_(max) { if(Safe) { ... ... @@ -131,9 +134,11 @@ struct spline_curve : public curve_abc /*Attributes*/ public: t_point_t coefficients_; time_t t_min_, t_max_; std::size_t dim_; std::size_t order_; private: time_t t_min_, t_max_; /*Attributes*/ private: ... ...
 #include "spline/exact_cubic.h" #include "spline/cubic_zero_vel_acc.h" #include "spline/bezier_curve.h" #include "spline/spline_curve.h" ... ... @@ -14,7 +15,7 @@ namespace spline typedef Eigen::Vector3d point_t; typedef std::vector > t_point_t; typedef spline_curve cubic_function_t; typedef exact_cubic exact_cubic_t; typedef exact_cubic exact_cubic_t; typedef bezier_curve bezier_curve_t; typedef std::pair Waypoint; typedef std::vector T_Waypoint; ... ...
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment