Program Listing for File spline.hpp

↰ Return to documentation for file (main/include/spline.hpp)

#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