Initial commit - test serial

SerialTest/include/okapi/squiggles/constraints.hpp

@@ -0,0 +1,65 @@
+/**
+ * Copyright 2020 Jonathan Bayless
+ *
+ * Use of this source code is governed by an MIT-style license that can be found
+ * in the LICENSE file or at https://opensource.org/licenses/MIT.
+ */
+#ifndef _SQUIGGLES_CONSTRAINTS_HPP_
+#define _SQUIGGLES_CONSTRAINTS_HPP_
+
+#include <cmath>
+#include <string>
+
+namespace squiggles {
+struct Constraints {
+  /**
+   * Defines the motion constraints for a path.
+   *
+   * @param imax_vel The maximum allowable velocity for the robot in meters per
+   *                 second.
+   * @param imax_accel The maximum allowable acceleration for the robot in
+   *                   meters per second per second.
+   * @param imax_jerk The maximum allowable jerk for the robot in meters per
+   *                  second per second per second (m/s^3).
+   * @param imax_curvature The maximum allowable change in heading in radians
+   *                       per second. This is not set to the numeric limits by
+   *                       default as that will allow for wild paths.
+   * @param imin_accel The minimum allowable acceleration for the robot in
+   *                   meters per second per second.
+   */
+  Constraints(double imax_vel,
+              double imax_accel = std::numeric_limits<double>::max(),
+              double imax_jerk = std::numeric_limits<double>::max(),
+              double imax_curvature = 1000,
+              double imin_accel = std::nan(""))
+    : max_vel(imax_vel),
+      max_accel(imax_accel),
+      max_jerk(imax_jerk),
+      max_curvature(imax_curvature) {
+    if (imax_accel == std::numeric_limits<double>::max()) {
+      min_accel = std::numeric_limits<double>::lowest();
+    } else {
+      min_accel = std::isnan(imin_accel) ? -imax_accel : imin_accel;
+    }
+  }
+
+  /**
+   * Serializes the Constraints data for debugging.
+   *
+   * @return The Constraints data.
+   */
+  std::string to_string() const {
+    return "Constraints: {max_vel: " + std::to_string(max_vel) +
+           ", max_accel: " + std::to_string(max_accel) +
+           ", max_jerk: " + std::to_string(max_jerk) +
+           ", min_accel: " + std::to_string(min_accel) + "}";
+  }
+
+  double max_vel;
+  double max_accel;
+  double max_jerk;
+  double min_accel;
+  double max_curvature;
+};
+} // namespace squiggles
+#endif

SerialTest/include/okapi/squiggles/geometry/controlvector.hpp

@@ -0,0 +1,62 @@
+/**
+ * Copyright 2020 Jonathan Bayless
+ *
+ * Use of this source code is governed by an MIT-style license that can be found
+ * in the LICENSE file or at https://opensource.org/licenses/MIT.
+ */
+#ifndef _GEOMETRY_CONTROL_VECTOR_HPP_
+#define _GEOMETRY_CONTROL_VECTOR_HPP_
+
+#include <cmath>
+#include <string>
+
+#include "pose.hpp"
+
+namespace squiggles {
+class ControlVector {
+  public:
+  /**
+   * A vector used to specify a state along a hermite spline.
+   *
+   * @param ipose The 2D position and heading.
+   * @param ivel The velocity component of the vector.
+   * @param iaccel The acceleration component of the vector.
+   * @param ijerk The jerk component of the vector.
+   */
+  ControlVector(Pose ipose,
+                double ivel = std::nan(""),
+                double iaccel = 0.0,
+                double ijerk = 0.0)
+    : pose(ipose), vel(ivel), accel(iaccel), jerk(ijerk) {}
+
+  ControlVector() = default;
+
+  /**
+   * Serializes the Control Vector data for debugging.
+   *
+   * @return The Control Vector data.
+   */
+  std::string to_string() const {
+    return "ControlVector: {" + pose.to_string() +
+           ", v: " + std::to_string(vel) + ", a: " + std::to_string(accel) +
+           ", j: " + std::to_string(jerk) + "}";
+  }
+
+  std::string to_csv() const {
+    return pose.to_csv() + "," + std::to_string(vel) + "," +
+           std::to_string(accel) + "," + std::to_string(jerk);
+  }
+
+  bool operator==(const ControlVector& other) const {
+    return pose == other.pose && nearly_equal(vel, other.vel) &&
+           nearly_equal(accel, other.accel) && nearly_equal(jerk, other.jerk);
+  }
+
+  Pose pose;
+  double vel;
+  double accel;
+  double jerk;
+};
+} // namespace squiggles
+
+#endif

SerialTest/include/okapi/squiggles/geometry/pose.hpp

@@ -0,0 +1,67 @@
+/**
+ * Copyright 2020 Jonathan Bayless
+ *
+ * Use of this source code is governed by an MIT-style license that can be found
+ * in the LICENSE file or at https://opensource.org/licenses/MIT.
+ */
+#ifndef _GEOMETRY_POSE_HPP_
+#define _GEOMETRY_POSE_HPP_
+
+#include <cmath>
+#include <string>
+
+#include "math/utils.hpp"
+
+namespace squiggles {
+class Pose {
+  public:
+  /**
+   * Specifies a point and heading in 2D space.
+   *
+   * @param ix The x position of the point in meters.
+   * @param iy The y position of the point in meters.
+   * @param iyaw The heading at the point in radians.
+   */
+  Pose(double ix, double iy, double iyaw) : x(ix), y(iy), yaw(iyaw) {}
+
+  Pose() = default;
+
+  /**
+   * Calculates the Euclidean distance between this pose and another.
+   *
+   * @param other The point from which the distance will be calculated.
+   *
+   * @return The distance between this pose and Other.
+   */
+  double dist(const Pose& other) const {
+    return std::sqrt((x - other.x) * (x - other.x) +
+                     (y - other.y) * (y - other.y));
+  }
+
+  /**
+   * Serializes the Pose data for debugging.
+   *
+   * @return The Pose data.
+   */
+  std::string to_string() const {
+    return "Pose: {x: " + std::to_string(x) + ", y: " + std::to_string(y) +
+           ", yaw: " + std::to_string(yaw) + "}";
+  }
+
+  std::string to_csv() const {
+    return std::to_string(x) + "," + std::to_string(y) + "," +
+           std::to_string(yaw);
+  }
+
+  bool operator==(const Pose& other) const {
+    return nearly_equal(x, other.x) && nearly_equal(y, other.y) &&
+           nearly_equal(yaw, other.yaw);
+  }
+
+  double x;
+  double y;
+  double yaw;
+};
+} // namespace squiggles
+
+#endif

SerialTest/include/okapi/squiggles/geometry/profilepoint.hpp

@@ -0,0 +1,100 @@
+/**
+ * Copyright 2020 Jonathan Bayless
+ *
+ * Use of this source code is governed by an MIT-style license that can be found
+ * in the LICENSE file or at https://opensource.org/licenses/MIT.
+ */
+#ifndef _GEOMETRY_PROFILE_POINT_HPP_
+#define _GEOMETRY_PROFILE_POINT_HPP_
+
+#include <iostream>
+#include <string>
+#include <vector>
+
+#include "controlvector.hpp"
+#include "math/utils.hpp"
+
+namespace squiggles {
+struct ProfilePoint {
+  /**
+   * Defines a state along a motion profiled path.
+   *
+   * @param ivector The pose and associated dynamics at this state in the path.
+   * @param iwheel_velocities The component of the robot's velocity provided by
+   *                          each wheel in meters per second.
+   * @param icurvature The degree to which the curve deviates from a straight
+   *                   line at this point in 1 / meters.
+   * @param itime The timestamp for this state relative to the start of the
+   *              path in seconds.
+   */
+  ProfilePoint(ControlVector ivector,
+               std::vector<double> iwheel_velocities,
+               double icurvature,
+               double itime)
+    : vector(ivector),
+      wheel_velocities(iwheel_velocities),
+      curvature(icurvature),
+      time(itime) {}
+
+  ProfilePoint() = default;
+
+  /**
+   * Serializes the Profile Point data for debugging.
+   *
+   * @return The Profile Point data.
+   */
+  std::string to_string() const {
+    std::string wheels = "{";
+    for (auto& w : wheel_velocities) {
+      wheels += std::to_string(w);
+      wheels += ", ";
+    }
+    wheels += "}";
+    return "ProfilePoint: {" + vector.to_string() + ", wheels: " + wheels +
+           ", k: " + std::to_string(curvature) +
+           ", t: " + std::to_string(time) + "}";
+  }
+
+  std::string to_csv() const {
+    std::string wheels = "";
+    for (auto& w : wheel_velocities) {
+      wheels += ",";
+      wheels += std::to_string(w);
+    }
+    return vector.to_csv() + "," + std::to_string(curvature) + "," +
+           std::to_string(time) + wheels;
+  }
+
+  bool operator==(const ProfilePoint& other) const {
+    for (std::size_t i = 0; i < wheel_velocities.size(); ++i) {
+      if (!nearly_equal(wheel_velocities[i], other.wheel_velocities[i])) {
+        return false;
+      }
+    }
+    return vector == other.vector && nearly_equal(curvature, other.curvature) &&
+           nearly_equal(time, other.time);
+  }
+
+  friend std::ostream& operator<<(std::ostream& os, const ProfilePoint& p) {
+    return os << "ProfilePoint(ControlVector(Pose(" +
+                   std::to_string(p.vector.pose.x) + "," +
+                   std::to_string(p.vector.pose.y) + "," +
+                   std::to_string(p.vector.pose.yaw) + ")," +
+                   std::to_string(p.vector.vel) + "," +
+                   std::to_string(p.vector.accel) + "," +
+                   std::to_string(p.vector.jerk) + "),{" +
+                   std::to_string(p.wheel_velocities[0]) + "," +
+                   std::to_string(p.wheel_velocities[1]) + "}," +
+                   std::to_string(p.curvature) + "," + std::to_string(p.time) +
+                   "),";
+    // return os << p.to_string();
+  }
+
+  ControlVector vector;
+  std::vector<double> wheel_velocities;
+  double curvature;
+  double time;
+};
+} // namespace squiggles
+
+#endif

SerialTest/include/okapi/squiggles/io.hpp

@@ -0,0 +1,56 @@
+/**
+ * Copyright 2020 Jonathan Bayless
+ *
+ * Use of this source code is governed by an MIT-style license that can be found
+ * in the LICENSE file or at https://opensource.org/licenses/MIT.
+ */
+#ifndef _SQUIGGLES_IO_HPP_
+#define _SQUIGGLES_IO_HPP_
+
+#include <optional>
+#include <vector>
+
+#include "geometry/profilepoint.hpp"
+
+namespace squiggles {
+/**
+ * Writes the path data to a CSV file.
+ *
+ * @param out The output stream to write the CSV data to. This is usually a
+ * file.
+ * @param path The path to serialized
+ *
+ * @return 0 if the path was serialized succesfully or -1 if an error occurred.
+ */
+int serialize_path(std::ostream& out, std::vector<ProfilePoint> path);
+
+/**
+ * Converts CSV data into a path.
+ *
+ * @param in The input stream containing the CSV data. This is usually a file.
+ *
+ * @return The path specified by the CSV data or std::nullopt if de-serializing
+ *         the path was unsuccessful.
+ */
+std::optional<std::vector<ProfilePoint>> deserialize_path(std::istream& in);
+
+/**
+ * Converts CSV data from the Pathfinder library's format to a Squiggles path.
+ *
+ * NOTE: this code translates data from Jaci Brunning's Pathfinder library.
+ * The source for that library can be found at:
+ * https://github.com/JaciBrunning/Pathfinder/
+ *
+ * @param left The input stream containing the left wheels' CSV data. This is
+ *             usually a file.
+ * @param right The input stream containing the right wheels' CSV data. This is
+ *             usually a file.
+ *
+ * @return The path specified by the CSV data or std::nullopt if de-serializing
+ *         the path was unsuccessful.
+ */
+std::optional<std::vector<ProfilePoint>>
+deserialize_pathfinder_path(std::istream& left, std::istream& right);
+} // namespace squiggles
+
+#endif

SerialTest/include/okapi/squiggles/math/quinticpolynomial.hpp

@@ -0,0 +1,65 @@
+/**
+ * Copyright 2020 Jonathan Bayless
+ *
+ * Use of this source code is governed by an MIT-style license that can be found
+ * in the LICENSE file or at https://opensource.org/licenses/MIT.
+ */
+#ifndef _MATH_QUINTIC_POLYNOMIAL_HPP_
+#define _MATH_QUINTIC_POLYNOMIAL_HPP_
+
+#include <string>
+
+namespace squiggles {
+class QuinticPolynomial {
+  public:
+  /**
+   * Defines the polynomial function for a spline in one dimension.
+   *
+   * @param s_p The starting position of the curve in meters.
+   * @param s_v The starting velocity of the curve in meters per second.
+   * @param s_a The starting acceleration of the curve in meters per second per
+   *            second.
+   * @param g_p The goal or ending position of the curve in meters.
+   * @param g_v The goal or ending velocity of the curve in meters per second.
+   * @param g_a The goal or ending acceleration of the curve in meters per
+   *            second per second.
+   * @param t The desired duration for the curve in seconds.
+   */
+  QuinticPolynomial(double s_p,
+                    double s_v,
+                    double s_a,
+                    double g_p,
+                    double g_v,
+                    double g_a,
+                    double t);
+
+  /**
+   * Calculates the values of the polynomial and its derivatives at the given
+   * time stamp.
+   */
+  double calc_point(double t);
+  double calc_first_derivative(double t);
+  double calc_second_derivative(double t);
+  double calc_third_derivative(double t);
+
+  /**
+   * Serializes the Quintic Polynomial data for debugging.
+   *
+   * @return The Quintic Polynomial data.
+   */
+  std::string to_string() const {
+    return "QuinticPolynomial: {0: " + std::to_string(a0) +
+           " 1: " + std::to_string(a1) + " 2: " + std::to_string(a2) +
+           " 3: " + std::to_string(a3) + " 4: " + std::to_string(a4) +
+           " 5: " + std::to_string(a5) + "}";
+  }
+
+  protected:
+  /**
+   * The coefficients for each term of the polynomial.
+   */
+  double a0, a1, a2, a3, a4, a5;
+};
+} // namespace squiggles
+
+#endif

SerialTest/include/okapi/squiggles/math/utils.hpp

@@ -0,0 +1,61 @@
+/**
+ * Copyright 2020 Jonathan Bayless
+ *
+ * Use of this source code is governed by an MIT-style license that can be found
+ * in the LICENSE file or at https://opensource.org/licenses/MIT.
+ */
+#ifndef _MATH_UTILS_HPP_
+#define _MATH_UTILS_HPP_
+
+#include <cmath>
+#include <iostream>
+
+namespace squiggles {
+/**
+ * Returns the sign value of the given value.
+ *
+ * @return 1 if the value is positive, -1 if the value is negative, and 0 if
+ *         the value is 0.
+ */
+template <class T> inline int sgn(T v) {
+  return (v > T(0)) - (v < T(0));
+}
+
+inline bool
+nearly_equal(const double& a, const double& b, double epsilon = 1e-5) {
+  return std::fabs(a - b) < epsilon;
+}
+} // namespace squiggles
+
+namespace std {
+// Copied from https://github.com/emsr/cxx_linear
+template <typename _Float>
+constexpr std::enable_if_t<
+  std::is_floating_point_v<_Float> &&
+    __cplusplus <= 201703L, // Only defines this function if C++ standard < 20
+  _Float>
+lerp(_Float __a, _Float __b, _Float __t) {
+  if (std::isnan(__a) || std::isnan(__b) || std::isnan(__t))
+    return std::numeric_limits<_Float>::quiet_NaN();
+  else if ((__a <= _Float{0} && __b >= _Float{0}) ||
+           (__a >= _Float{0} && __b <= _Float{0}))
+  // ab <= 0 but product could overflow.
+#ifndef FMA
+    return __t * __b + (_Float{1} - __t) * __a;
+#else
+    return std::fma(__t, __b, (_Float{1} - __t) * __a);
+#endif
+  else if (__t == _Float{1})
+    return __b;
+  else { // monotonic near t == 1.
+#ifndef FMA
+    const auto __x = __a + __t * (__b - __a);
+#else
+    const auto __x = std::fma(__t, __b - __a, __a);
+#endif
+    return (__t > _Float{1}) == (__b > __a) ? std::max(__b, __x)
+                                            : std::min(__b, __x);
+  }
+}
+} // namespace std
+#endif

SerialTest/include/okapi/squiggles/physicalmodel/passthroughmodel.hpp

@@ -0,0 +1,38 @@
+/**
+ * Copyright 2020 Jonathan Bayless
+ *
+ * Use of this source code is governed by an MIT-style license that can be found
+ * in the LICENSE file or at https://opensource.org/licenses/MIT.
+ */
+#ifndef _PHYSICAL_MODEL_PASSTHROUGH_MODEL_HPP_
+#define _PHYSICAL_MODEL_PASSTHROUGH_MODEL_HPP_
+
+#include "physicalmodel/physicalmodel.hpp"
+
+namespace squiggles {
+class PassthroughModel : public PhysicalModel {
+  public:
+  /**
+   * Defines a Physical Model that imposes no constraints of its own.
+   */
+  PassthroughModel() = default;
+
+  Constraints constraints([[maybe_unused]] const Pose pose,
+                          [[maybe_unused]] double curvature,
+                          double vel) override {
+    return Constraints(vel);
+  };
+
+  std::vector<double>
+  linear_to_wheel_vels([[maybe_unused]] double lin_vel,
+                       [[maybe_unused]] double curvature) override {
+    return std::vector<double>{};
+  }
+
+  std::string to_string() const override {
+    return "PassthroughModel {}";
+  }
+};
+} // namespace squiggles
+
+#endif

SerialTest/include/okapi/squiggles/physicalmodel/physicalmodel.hpp

@@ -0,0 +1,43 @@
+/**
+ * Copyright 2020 Jonathan Bayless
+ *
+ * Use of this source code is governed by an MIT-style license that can be found
+ * in the LICENSE file or at https://opensource.org/licenses/MIT.
+ */
+#ifndef _PHYSICAL_MODEL_PHYSICAL_MODEL_HPP_
+#define _PHYSICAL_MODEL_PHYSICAL_MODEL_HPP_
+
+#include "constraints.hpp"
+#include "geometry/pose.hpp"
+
+namespace squiggles {
+class PhysicalModel {
+  public:
+  /**
+   * Calculate a set of stricter constraints for the path at the given state
+   * than the general constraints based on the robot's kinematics.
+   *
+   * @param pose The 2D pose for this state in the path.
+   * @param curvature The change in heading at this state in the path in 1 /
+   *                  meters.
+   * @param vel The linear velocity at this state in the path in meters per
+   * second.
+   */
+  virtual Constraints
+  constraints(const Pose pose, double curvature, double vel) = 0;
+
+  /**
+   * Converts a linear velocity and desired curvature into the component for
+   * each wheel of the robot.
+   *
+   * @param linear The linear velocity for the robot in meters per second.
+   * @param curvature The change in heading for the robot in 1 / meters.
+   */
+  virtual std::vector<double> linear_to_wheel_vels(double linear,
+                                                   double curvature) = 0;
+
+  virtual std::string to_string() const = 0;
+};
+} // namespace squiggles
+
+#endif

SerialTest/include/okapi/squiggles/physicalmodel/tankmodel.hpp

@@ -0,0 +1,45 @@
+/**
+ * Copyright 2020 Jonathan Bayless
+ *
+ * Use of this source code is governed by an MIT-style license that can be found
+ * in the LICENSE file or at https://opensource.org/licenses/MIT.
+ */
+#ifndef _PHYSICAL_MODEL_TANK_MODEL_HPP_
+#define _PHYSICAL_MODEL_TANK_MODEL_HPP_
+
+#include <tuple>
+#include <vector>
+
+#include "physicalmodel/physicalmodel.hpp"
+
+namespace squiggles {
+class TankModel : public PhysicalModel {
+  public:
+  /**
+   * Defines a model of a tank drive or differential drive robot.
+   *
+   * @param itrack_width The distance between the the wheels on each side of the
+   *                     robot in meters.
+   * @param ilinear_constraints The maximum values for the robot's movement.
+   */
+  TankModel(double itrack_width, Constraints ilinear_constraints);
+
+  Constraints
+  constraints(const Pose pose, double curvature, double vel) override;
+
+  std::vector<double> linear_to_wheel_vels(double lin_vel,
+                                           double curvature) override;
+
+  std::string to_string() const override;
+
+  private:
+  double vel_constraint(const Pose pose, double curvature, double vel);
+  std::tuple<double, double>
+  accel_constraint(const Pose pose, double curvature, double vel) const;
+
+  double track_width;
+  Constraints linear_constraints;
+};
+} // namespace squiggles
+
+#endif

SerialTest/include/okapi/squiggles/spline.hpp

@@ -0,0 +1,310 @@
+/**
+ * Copyright 2020 Jonathan Bayless
+ *
+ * Use of this source code is governed by an MIT-style license that can be found
+ * in the LICENSE file or at https://opensource.org/licenses/MIT.
+ */
+#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:
+  /**
+   * Generates curves that match the given motion constraints.
+   *
+   * @param iconstraints The maximum allowable values for the robot's motion.
+   * @param imodel The robot's physical characteristics and constraints
+   * @param idt The difference in time in seconds between each state for the
+   *            generated paths.
+   */
+  SplineGenerator(Constraints iconstraints,
+                  std::shared_ptr<PhysicalModel> imodel =
+                    std::make_shared<PassthroughModel>(),
+                  double idt = 0.1);
+
+  /**
+   * Creates a motion profiled path between the given waypoints.
+   *
+   * @param iwaypoints The list of poses that the robot should reach along the
+   *                   path.
+   * @param fast If true, the path optimization process will stop as soon as the
+   *             constraints are met. If false, the optimizer will find the
+   *             smoothest possible path between the points.
+   *
+   * @return A series of robot states defining a path between the poses.
+   */
+  std::vector<ProfilePoint> generate(std::vector<Pose> iwaypoints,
+                                     bool fast = false);
+  std::vector<ProfilePoint> generate(std::initializer_list<Pose> iwaypoints,
+                                     bool fast = false);
+
+  /**
+   * Creates a motion profiled path between the given waypoints.
+   *
+   * @param iwaypoints The list of vectors that the robot should reach along the
+   *                   path.
+   *
+   * @return A series of robot states defining a path between the vectors.
+   */
+  std::vector<ProfilePoint> generate(std::vector<ControlVector> iwaypoints);
+  std::vector<ProfilePoint>
+  generate(std::initializer_list<ControlVector> iwaypoints);
+
+  protected:
+  /**
+   * The maximum allowable values for the robot's motion.
+   */
+  Constraints constraints;
+
+  /**
+   * Defines the physical structure of the robot and translates the linear
+   * kinematics to wheel velocities.
+   */
+  std::shared_ptr<PhysicalModel> model;
+
+  /**
+   * The time difference between each value in the generated path.
+   */
+  double dt;
+
+  /**
+   * The minimum and maximum durations for a path to take. A larger range allows
+   * for longer possible paths at the expense of a longer path generation time.
+   */
+  const int T_MIN = 2;
+  const int T_MAX = 15;
+  const int MAX_GRAD_DESCENT_ITERATIONS = 10;
+
+  /**
+   * This is factor is used to create a "dummy velocity" in the initial path
+   * generation step one or both of the preferred start or end velocities is
+   * zero. The velocity will be replaced with the preferred start/end velocity
+   * in parameterization but a nonzero velocity is needed for the spline
+   * calculation.
+   *
+   * This was 1.2 in the WPILib example but that large of a value seems to
+   * create wild paths, 0.12 worked better in testing with VEX-sized paths.
+   */
+  public:
+  const double K_DEFAULT_VEL = 1.0;
+
+  /**
+   * The output of the initial, "naive" generation step. We discard the
+   * derivative values to replace them with values from a motion profile.
+   */
+
+  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;
+  };
+
+  /**
+   * An intermediate value used in the "naive" generation step. Contains the
+   * final GeneratedPoint value that will be returned as well as the spline's
+   * derivative values to perform the initial check against the constraints.
+   */
+  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);
+  /**
+   * Runs a Gradient Descent algorithm to minimize the linear acceleration,
+   * linear jerk, and curvature for the generated path.
+   *
+   * This is used when there is not a start/end velocity specified for a given
+   * path.
+   */
+  std::vector<GeneratedPoint>
+  gradient_descent(ControlVector& start, ControlVector& end, bool fast);
+
+  /**
+   * An intermediate value used in the parameterization step. Adds the
+   * constrained values from the motion profile to the output from the "naive"
+   * generation step.
+   */
+  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) + "}";
+    }
+  };
+
+  /**
+   * The actual function called by the "generate" functions.
+   *
+   * @param start An iterator pointing to the first ControlVector in the path
+   * @param end An iterator pointting to the last ControlVector in the path
+   *
+   * @return The points from each path concatenated together
+   */
+  template <class Iter>
+  std::vector<ProfilePoint> _generate(Iter start, Iter end, bool fast);
+
+  public:
+  /**
+   * Performs the "naive" generation step.
+   *
+   * This step calculates the spline polynomials that fit within the
+   * SplineGenerator's acceleration and jerk constraints and returns the points
+   * that form the curve.
+   */
+  std::vector<GeneratedPoint>
+  gen_raw_path(ControlVector& start, ControlVector& end, bool fast);
+
+  /**
+   * Imposes a linear motion profile on the raw path.
+   *
+   * This step creates the velocity and acceleration values to command the robot
+   * at each point along the curve.
+   */
+  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);
+
+  /**
+   * Finds the new timestamps for each point along the curve based on the motion
+   * profile.
+   */
+  std::vector<ProfilePoint>
+  integrate_constrained_states(std::vector<ConstrainedState> constrainedStates);
+
+  /**
+   * Finds the ProfilePoint on the profiled curve for the given timestamp.
+   *
+   * This with interpolate between points on the curve if a point with an exact
+   * matching timestamp is not found.
+   */
+  ProfilePoint get_point_at_time(const ControlVector start,
+                                 const ControlVector end,
+                                 std::vector<ProfilePoint> points,
+                                 double t);
+
+  /**
+   * Linearly interpolates between points along the profiled curve.
+   */
+  ProfilePoint lerp_point(QuinticPolynomial x_qp,
+                          QuinticPolynomial y_qp,
+                          ProfilePoint start,
+                          ProfilePoint end,
+                          double i);
+
+  /**
+   * Returns the spline curve for the given control vectors and path duration.
+   */
+  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);
+
+  /**
+   * Applies the general constraints and model constraints to the given state.
+   */
+  void enforce_accel_lims(ConstrainedState* state);
+
+  /**
+   * Imposes the motion profile constraints on a segment of the path from the
+   * perspective of iterating forwards through the path.
+   */
+  void forward_pass(ConstrainedState* predecessor, ConstrainedState* successor);
+
+  /**
+   * Imposes the motion profile constraints on a segment of the path from the
+   * perspective of iterating backwards through the path.
+   */
+  void backward_pass(ConstrainedState* predecessor,
+                     ConstrainedState* successor);
+
+  /**
+   * Calculates the final velocity for a path segment.
+   */
+  double vf(double vi, double a, double ds);
+
+  /**
+   * Calculates the initial acceleration needed to match the segments'
+   * velocities.
+   */
+  double ai(double vf, double vi, double s);
+
+  /**
+   * Values that are closer to each other than this value are considered equal.
+   */
+  static constexpr double K_EPSILON = 1e-5;
+};
+} // namespace squiggles
+
+#endif

SerialTest/include/okapi/squiggles/squiggles.hpp

@@ -0,0 +1,22 @@
+/**
+ * Copyright 2020 Jonathan Bayless
+ *
+ * Use of this source code is governed by an MIT-style license that can be found
+ * in the LICENSE file or at https://opensource.org/licenses/MIT.
+ */
+#ifndef _ROBOT_SQUIGGLES_H_
+#define _ROBOT_SQUIGGLES_H_
+
+#include "geometry/controlvector.hpp"
+#include "geometry/pose.hpp"
+#include "geometry/profilepoint.hpp"
+
+#include "physicalmodel/passthroughmodel.hpp"
+#include "physicalmodel/physicalmodel.hpp"
+#include "physicalmodel/tankmodel.hpp"
+
+#include "constraints.hpp"
+#include "io.hpp"
+#include "spline.hpp"
+
+#endif