Add trajectory generation using hermite splines (#1843)

wpilibc/src/main/native/include/frc/trajectory/Trajectory.h

@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <vector>
+
+#include <units/units.h>
+
+#include "frc/geometry/Pose2d.h"
+
+namespace frc {
+
+/**
+ * Define a unit for curvature.
+ */
+using curvature_t = units::unit_t<
+    units::compound_unit<units::radian, units::inverse<units::meter>>>;
+
+/**
+ * Represents a time-parameterized trajectory. The trajectory contains of
+ * various States that represent the pose, curvature, time elapsed, velocity,
+ * and acceleration at that point.
+ */
+class Trajectory {
+ public:
+  /**
+   * Represents one point on the trajectory.
+   */
+  struct State {
+    // The time elapsed since the beginning of the trajectory.
+    units::second_t t = 0_s;
+
+    // The speed at that point of the trajectory.
+    units::meters_per_second_t velocity = 0_mps;
+
+    // The acceleration at that point of the trajectory.
+    units::meters_per_second_squared_t acceleration = 0_mps_sq;
+
+    // The pose at that point of the trajectory.
+    Pose2d pose;
+
+    // The curvature at that point of the trajectory.
+    curvature_t curvature{0.0};
+
+    /**
+     * Interpolates between two States.
+     *
+     * @param endValue The end value for the interpolation.
+     * @param i The interpolant (fraction).
+     *
+     * @return The interpolated state.
+     */
+    State Interpolate(State endValue, double i) const;
+  };
+
+  Trajectory() = default;
+
+  /**
+   * Constructs a trajectory from a vector of states.
+   */
+  explicit Trajectory(const std::vector<State>& states);
+
+  /**
+   * Returns the overall duration of the trajectory.
+   * @return The duration of the trajectory.
+   */
+  units::second_t TotalTime() const { return m_totalTime; }
+
+  /**
+   * Return the states of the trajectory.
+   * @return The states of the trajectory.
+   */
+  const std::vector<State>& States() const { return m_states; }
+
+  /**
+   * Sample the trajectory at a point in time.
+   *
+   * @param t The point in time since the beginning of the trajectory to sample.
+   * @return The state at that point in time.
+   */
+  State Sample(units::second_t t) const;
+
+ private:
+  std::vector<State> m_states;
+  units::second_t m_totalTime;
+
+  /**
+   * Linearly interpolates between two values.
+   *
+   * @param startValue The start value.
+   * @param endValue The end value.
+   * @param t The fraction for interpolation.
+   *
+   * @return The interpolated value.
+   */
+  template <typename T>
+  static T Lerp(const T& startValue, const T& endValue, const double t) {
+    return startValue + (endValue - startValue) * t;
+  }
+};
+}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h

@@ -0,0 +1,107 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "frc/spline/SplineParameterizer.h"
+#include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+/**
+ * Helper class used to generate trajectories with various constraints.
+ */
+class TrajectoryGenerator {
+ public:
+  using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
+
+  /**
+   * Generates a trajectory with the given waypoints and constraints.
+   *
+   * @param waypoints A vector of points that the trajectory must go through.
+   * @param constraints A vector of various velocity and acceleration
+   * constraints.
+   * @param startVelocity The start velocity for the trajectory.
+   * @param endVelocity The end velocity for the trajectory.
+   * @param maxVelocity The max velocity for the trajectory.
+   * @param maxAcceleration The max acceleration for the trajectory.
+   * @param reversed Whether the robot should move backwards. Note that the
+   * robot will still move from a -> b -> ... -> z as defined in the waypoints.
+   *
+   * @return The trajectory.
+   */
+  static Trajectory GenerateTrajectory(
+      const std::vector<Pose2d>& waypoints,
+      std::vector<std::unique_ptr<TrajectoryConstraint>>&& constraints,
+      units::meters_per_second_t startVelocity,
+      units::meters_per_second_t endVelocity,
+      units::meters_per_second_t maxVelocity,
+      units::meters_per_second_squared_t maxAcceleration, bool reversed);
+
+  /**
+   * Generates a trajectory with the given waypoints and constraints.
+   *
+   * @param start The starting pose for the trajectory.
+   * @param waypoints The interior waypoints for the trajectory. The headings
+   * will be determined automatically to ensure continuous curvature.
+   * @param end The ending pose for the trajectory.
+   * @param constraints A vector of various velocity and acceleration
+   * constraints.
+   * @param startVelocity The start velocity for the trajectory.
+   * @param endVelocity The end velocity for the trajectory.
+   * @param maxVelocity The max velocity for the trajectory.
+   * @param maxAcceleration The max acceleration for the trajectory.
+   * @param reversed Whether the robot should move backwards. Note that the
+   * robot will still move from a -> b -> ... -> z as defined in the waypoints.
+   *
+   * @return The trajectory.
+   */
+  static Trajectory GenerateTrajectory(
+      const Pose2d& start, const std::vector<Translation2d>& waypoints,
+      const Pose2d& end,
+      std::vector<std::unique_ptr<TrajectoryConstraint>>&& constraints,
+      units::meters_per_second_t startVelocity,
+      units::meters_per_second_t endVelocity,
+      units::meters_per_second_t maxVelocity,
+      units::meters_per_second_squared_t maxAcceleration, bool reversed);
+
+ private:
+  /**
+   * Generate spline points from a vector of splines by parameterizing the
+   * splines.
+   *
+   * @param splines The splines to parameterize.
+   *
+   * @return The spline points.
+   */
+  template <typename Spline>
+  static std::vector<PoseWithCurvature> SplinePointsFromSplines(
+      const std::vector<Spline>& splines) {
+    // Create the vector of spline points.
+    std::vector<PoseWithCurvature> splinePoints;
+
+    // Add the first point to the vector.
+    splinePoints.push_back(splines.front().GetPoint(0.0));
+
+    // Iterate through the vector and parameterize each spline, adding the
+    // parameterized points to the final vector.
+    for (auto&& spline : splines) {
+      auto points = SplineParameterizer::Parameterize(spline);
+      // Append the array of poses to the vector. We are removing the first
+      // point because it's a duplicate of the last point from the previous
+      // spline.
+      splinePoints.insert(std::end(splinePoints), std::begin(points) + 1,
+                          std::end(points));
+    }
+    return splinePoints;
+  }
+};
+}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h

@@ -0,0 +1,107 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+/*
+ * MIT License
+ *
+ * Copyright (c) 2018 Team 254
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "frc/trajectory/Trajectory.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+/**
+ * Class used to parameterize a trajectory by time.
+ */
+class TrajectoryParameterizer {
+ public:
+  using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
+
+  /**
+   * Parameterize the trajectory by time. This is where the velocity profile is
+   * generated.
+   *
+   * The derivation of the algorithm used can be found here:
+   * <http://www2.informatik.uni-freiburg.de/~lau/students/Sprunk2008.pdf>
+   *
+   * @param points Reference to the spline points.
+   * @param constraints A vector of various velocity and acceleration
+   * constraints.
+   * @param startVelocity The start velocity for the trajectory.
+   * @param endVelocity The end velocity for the trajectory.
+   * @param maxVelocity The max velocity for the trajectory.
+   * @param maxAcceleration The max acceleration for the trajectory.
+   * @param reversed Whether the robot should move backwards. Note that the
+   * robot will still move from a -> b -> ... -> z as defined in the waypoints.
+   *
+   * @return The trajectory.
+   */
+  static Trajectory TimeParameterizeTrajectory(
+      const std::vector<PoseWithCurvature>& points,
+      std::vector<std::unique_ptr<TrajectoryConstraint>>&& constraints,
+      units::meters_per_second_t startVelocity,
+      units::meters_per_second_t endVelocity,
+      units::meters_per_second_t maxVelocity,
+      units::meters_per_second_squared_t maxAcceleration, bool reversed);
+
+ private:
+  constexpr static double kEpsilon = 1E-6;
+
+  /**
+   * Represents a constrained state that is used when time parameterizing a
+   * trajectory. Each state has the pose, curvature, distance from the start of
+   * the trajectory, max velocity, min acceleration and max acceleration.
+   */
+  struct ConstrainedState {
+    PoseWithCurvature pose = {Pose2d(), curvature_t(0.0)};
+    units::meter_t distance = 0_m;
+    units::meters_per_second_t maxVelocity = 0_mps;
+    units::meters_per_second_squared_t minAcceleration = 0_mps_sq;
+    units::meters_per_second_squared_t maxAcceleration = 0_mps_sq;
+  };
+
+  /**
+   * Enforces acceleration limits as defined by the constraints. This function
+   * is used when time parameterizing a trajectory.
+   *
+   * @param reverse Whether the robot is traveling backwards.
+   * @param constraints A vector of the user-defined velocity and acceleration
+   * constraints.
+   * @param state Pointer to the constrained state that we are operating on.
+   * This is mutated in place.
+   */
+  static void EnforceAccelerationLimits(
+      bool reverse,
+      const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
+      ConstrainedState* state);
+};
+}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/constraint/CentripetalAccelerationConstraint.h

@@ -0,0 +1,40 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+namespace frc {
+
+/**
+ * A constraint on the maximum absolute centripetal acceleration allowed when
+ * traversing a trajectory. The centripetal acceleration of a robot is defined
+ * as the velocity squared divided by the radius of curvature.
+ *
+ * Effectively, limiting the maximum centripetal acceleration will cause the
+ * robot to slow down around tight turns, making it easier to track trajectories
+ * with sharp turns.
+ */
+class CentripetalAccelerationConstraint : public TrajectoryConstraint {
+ public:
+  explicit CentripetalAccelerationConstraint(
+      units::meters_per_second_squared_t maxCentripetalAcceleration);
+
+  units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, curvature_t curvature,
+      units::meters_per_second_t velocity) override;
+
+  MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
+                            units::meters_per_second_t speed) override;
+
+ private:
+  units::meters_per_second_squared_t m_maxCentripetalAcceleration;
+};
+}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h

@@ -0,0 +1,38 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <units/units.h>
+
+#include "frc/kinematics/DifferentialDriveKinematics.h"
+#include "frc/trajectory/constraint/TrajectoryConstraint.h"
+
+/**
+ * A class that enforces constraints on the differential drive kinematics.
+ * This can be used to ensure that the trajectory is constructed so that the
+ * commanded velocities for both sides of the drivetrain stay below a certain
+ * limit.
+ */
+namespace frc {
+class DifferentialDriveKinematicsConstraint : public TrajectoryConstraint {
+ public:
+  DifferentialDriveKinematicsConstraint(DifferentialDriveKinematics kinematics,
+                                        units::meters_per_second_t maxSpeed);
+
+  units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, curvature_t curvature,
+      units::meters_per_second_t velocity) override;
+
+  MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
+                            units::meters_per_second_t speed) override;
+
+ private:
+  DifferentialDriveKinematics m_kinematics;
+  units::meters_per_second_t m_maxSpeed;
+};
+}  // namespace frc

wpilibc/src/main/native/include/frc/trajectory/constraint/TrajectoryConstraint.h

@@ -0,0 +1,78 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <limits>
+
+#include <units/units.h>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/spline/Spline.h"
+
+namespace frc {
+/**
+ * An interface for defining user-defined velocity and acceleration constraints
+ * while generating trajectories.
+ */
+class TrajectoryConstraint {
+ public:
+  TrajectoryConstraint() = default;
+
+  TrajectoryConstraint(const TrajectoryConstraint&) = default;
+  TrajectoryConstraint& operator=(const TrajectoryConstraint&) = default;
+
+  TrajectoryConstraint(TrajectoryConstraint&&) = default;
+  TrajectoryConstraint& operator=(TrajectoryConstraint&&) = default;
+
+  virtual ~TrajectoryConstraint() = default;
+
+  /**
+   * Represents a minimum and maximum acceleration.
+   */
+  struct MinMax {
+    /**
+     * The minimum acceleration.
+     */
+    units::meters_per_second_squared_t minAcceleration{
+        -std::numeric_limits<double>::max()};
+
+    /**
+     * The maximum acceleration.
+     */
+    units::meters_per_second_squared_t maxAcceleration{
+        std::numeric_limits<double>::max()};
+  };
+
+  /**
+   * Returns the max velocity given the current pose and curvature.
+   *
+   * @param pose The pose at the current point in the trajectory.
+   * @param curvature The curvature at the current point in the trajectory.
+   * @param velocity The velocity at the current point in the trajectory before
+   *                                constraints are applied.
+   *
+   * @return The absolute maximum velocity.
+   */
+  virtual units::meters_per_second_t MaxVelocity(
+      const Pose2d& pose, curvature_t curvature,
+      units::meters_per_second_t velocity) = 0;
+
+  /**
+   * Returns the minimum and maximum allowable acceleration for the trajectory
+   * given pose, curvature, and speed.
+   *
+   * @param pose The pose at the current point in the trajectory.
+   * @param curvature The curvature at the current point in the trajectory.
+   * @param speed The speed at the current point in the trajectory.
+   *
+   * @return The min and max acceleration bounds.
+   */
+  virtual MinMax MinMaxAcceleration(const Pose2d& pose, curvature_t curvature,
+                                    units::meters_per_second_t speed) = 0;
+};
+}  // namespace frc