spline.hpp

Go to the documentation of this file.

 
#ifndef _SQUIGGLES_SPLINE_HPP_
#define _SQUIGGLES_SPLINE_HPP_
 
#include <initializer_list>
#include <memory>
#include <vector>
 
#include "constraints.hpp"
#include "geometry/controlvector.hpp"
#include "geometry/profilepoint.hpp"
#include "math/quinticpolynomial.hpp"
#include "physicalmodel/passthroughmodel.hpp"
#include "physicalmodel/physicalmodel.hpp"
 
namespace squiggles {
class SplineGenerator {
  public:
  SplineGenerator(Constraints iconstraints,
                  std::shared_ptr<PhysicalModel> imodel =
                    std::make_shared<PassthroughModel>(),
                  double idt = 0.1);
 
  std::vector<ProfilePoint> generate(std::vector<Pose> iwaypoints,
                                     bool fast = false);
  std::vector<ProfilePoint> generate(std::initializer_list<Pose> iwaypoints,
                                     bool fast = false);
 
  std::vector<ProfilePoint> generate(std::vector<ControlVector> iwaypoints);
  std::vector<ProfilePoint>
  generate(std::initializer_list<ControlVector> iwaypoints);
 
  protected:
  Constraints constraints;
 
  std::shared_ptr<PhysicalModel> model;
 
  double dt;
 
  const int T_MIN = 2;
  const int T_MAX = 15;
  const int MAX_GRAD_DESCENT_ITERATIONS = 10;
 
  public:
  const double K_DEFAULT_VEL = 1.0;
 
  struct GeneratedPoint {
    GeneratedPoint(Pose ipose, double icurvature = 0.0)
      : pose(ipose), curvature(icurvature) {}
 
    std::string to_string() const {
      return "GeneratedPoint: {" + pose.to_string() +
             ", curvature: " + std::to_string(curvature) + "}";
    }
 
    Pose pose;
    double curvature;
  };
 
  struct GeneratedVector {
    GeneratedVector(GeneratedPoint ipoint,
                    double ivel,
                    double iaccel,
                    double ijerk)
      : point(ipoint), vel(ivel), accel(iaccel), jerk(ijerk) {}
 
    GeneratedPoint point;
    double vel;
    double accel;
    double jerk;
 
    std::string to_string() const {
      return "GeneratedVector: {" + point.to_string() +
             ", vel: " + std::to_string(vel) +
             ", accel: " + std::to_string(accel) +
             ", jerk: " + std::to_string(jerk) + "}";
    }
  };
 
  std::vector<GeneratedVector> gen_single_raw_path(ControlVector start,
                                                   ControlVector end,
                                                   int duration,
                                                   double start_vel,
                                                   double end_vel);
  std::vector<GeneratedPoint>
  gradient_descent(ControlVector& start, ControlVector& end, bool fast);
 
  struct ConstrainedState {
    ConstrainedState(Pose ipose,
                     double icurvature,
                     double idistance,
                     double imax_vel,
                     double imin_accel,
                     double imax_accel)
      : pose(ipose),
        curvature(icurvature),
        distance(idistance),
        max_vel(imax_vel),
        min_accel(imin_accel),
        max_accel(imax_accel) {}
 
    ConstrainedState() = default;
 
    Pose pose = Pose();
    double curvature = 0;
    double distance = 0;
    double max_vel = 0;
    double min_accel = 0;
    double max_accel = 0;
 
    std::string to_string() const {
      return "ConstrainedState: {x: " + std::to_string(pose.x) +
             ", y: " + std::to_string(pose.y) +
             ", yaw: " + std::to_string(pose.yaw) +
             ", k: " + std::to_string(curvature) +
             ", dist: " + std::to_string(distance) +
             ", v: " + std::to_string(max_vel) +
             ", min_a: " + std::to_string(min_accel) +
             ", max_a: " + std::to_string(max_accel) + "}";
    }
  };
 
  template <class Iter>
  std::vector<ProfilePoint> _generate(Iter start, Iter end, bool fast);
 
  public:
  std::vector<GeneratedPoint>
  gen_raw_path(ControlVector& start, ControlVector& end, bool fast);
 
  std::vector<ProfilePoint>
  parameterize(const ControlVector start,
               const ControlVector end,
               const std::vector<GeneratedPoint>& raw_path,
               const double preferred_start_vel,
               const double preferred_end_vel,
               const double start_time);
 
  std::vector<ProfilePoint>
  integrate_constrained_states(std::vector<ConstrainedState> constrainedStates);
 
  ProfilePoint get_point_at_time(const ControlVector start,
                                 const ControlVector end,
                                 std::vector<ProfilePoint> points,
                                 double t);
 
  ProfilePoint lerp_point(QuinticPolynomial x_qp,
                          QuinticPolynomial y_qp,
                          ProfilePoint start,
                          ProfilePoint end,
                          double i);
 
  QuinticPolynomial get_x_spline(const ControlVector start,
                                 const ControlVector end,
                                 const double duration);
  QuinticPolynomial get_y_spline(const ControlVector start,
                                 const ControlVector end,
                                 const double duration);
 
  void enforce_accel_lims(ConstrainedState* state);
 
  void forward_pass(ConstrainedState* predecessor, ConstrainedState* successor);
 
  void backward_pass(ConstrainedState* predecessor,
                     ConstrainedState* successor);
 
  double vf(double vi, double a, double ds);
 
  double ai(double vf, double vi, double s);
 
  static constexpr double K_EPSILON = 1e-5;
};
} // namespace squiggles
 
#endif