Add trajectory generation using hermite splines (#1843)

wpilibc/src/main/native/include/frc/geometry/Pose2d.h

@@ -68,6 +68,14 @@ class Pose2d {
    */
   Pose2d& operator+=(const Transform2d& other);
 
+  /**
+   * Returns the Transform2d that maps the one pose to another.
+   *
+   * @param other The initial pose of the transformation.
+   * @return The transform that maps the other pose to the current pose.
+   */
+  Transform2d operator-(const Pose2d& other) const;
+
   /**
    * Checks equality between this Pose2d and another object.
    *
@@ -145,6 +153,16 @@ class Pose2d {
    */
   Pose2d Exp(const Twist2d& twist) const;
 
+  /**
+   * Returns a Twist2d that maps this pose to the end pose. If c is the output
+   * of a.Log(b), then a.Exp(c) would yield b.
+   *
+   * @param end The end pose for the transformation.
+   *
+   * @return The twist that maps this to end.
+   */
+  Twist2d Log(const Pose2d& end) const;
+
  private:
   Translation2d m_translation;
   Rotation2d m_rotation;

wpilibc/src/main/native/include/frc/geometry/Rotation2d.h

@@ -97,6 +97,14 @@ class Rotation2d {
    */
   Rotation2d operator-() const;
 
+  /**
+   * Multiplies the current rotation by a scalar.
+   * @param scalar The scalar.
+   *
+   * @return The new scaled Rotation2d.
+   */
+  Rotation2d operator*(double scalar) const;
+
   /**
    * Checks equality between this Rotation2d and another object.
    *

wpilibc/src/main/native/include/frc/geometry/Transform2d.h

@@ -53,6 +53,16 @@ class Transform2d {
    */
   const Rotation2d& Rotation() const { return m_rotation; }
 
+  /**
+   * Scales the transform by the scalar.
+   *
+   * @param scalar The scalar.
+   * @return The scaled Transform2d.
+   */
+  Transform2d operator*(double scalar) const {
+    return Transform2d(m_translation * scalar, m_rotation * scalar);
+  }
+
   /**
    * Checks equality between this Transform2d and another object.
    *

wpilibc/src/main/native/include/frc/spline/CubicHermiteSpline.h

@@ -0,0 +1,84 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <array>
+
+#include <Eigen/Core>
+
+#include "frc/spline/Spline.h"
+
+namespace frc {
+/**
+ * Represents a hermite spline of degree 3.
+ */
+class CubicHermiteSpline : public Spline<3> {
+ public:
+  /**
+   * Constructs a cubic hermite spline with the specified control vectors. Each
+   * control vector contains info about the location of the point and its first
+   * derivative.
+   *
+   * @param xInitialControlVector The control vector for the initial point in
+   * the x dimension.
+   * @param xFinalControlVector The control vector for the final point in
+   * the x dimension.
+   * @param yInitialControlVector The control vector for the initial point in
+   * the y dimension.
+   * @param yFinalControlVector The control vector for the final point in
+   * the y dimension.
+   */
+  CubicHermiteSpline(std::array<double, 2> xInitialControlVector,
+                     std::array<double, 2> xFinalControlVector,
+                     std::array<double, 2> yInitialControlVector,
+                     std::array<double, 2> yFinalControlVector);
+
+ protected:
+  /**
+   * Returns the coefficients matrix.
+   * @return The coefficients matrix.
+   */
+  Eigen::Matrix<double, 6, 3 + 1> Coefficients() const override {
+    return m_coefficients;
+  }
+
+ private:
+  Eigen::Matrix<double, 6, 4> m_coefficients;
+
+  /**
+   * Returns the hermite basis matrix for cubic hermite spline interpolation.
+   * @return The hermite basis matrix for cubic hermite spline interpolation.
+   */
+  static Eigen::Matrix<double, 4, 4> MakeHermiteBasis() {
+    // clang-format off
+    static auto basis = (Eigen::Matrix<double, 4, 4>() <<
+     +2.0, +1.0, -2.0, +1.0,
+     -3.0, -2.0, +3.0, -1.0,
+     +0.0, +1.0, +0.0, +0.0,
+     +1.0, +0.0, +0.0, +0.0).finished();
+    // clang-format on
+    return basis;
+  }
+
+  /**
+   * Returns the control vector for each dimension as a matrix from the
+   * user-provided arrays in the constructor.
+   *
+   * @param initialVector The control vector for the initial point.
+   * @param finalVector The control vector for the final point.
+   *
+   * @return The control vector matrix for a dimension.
+   */
+  static Eigen::Vector4d ControlVectorFromArrays(
+      std::array<double, 2> initialVector, std::array<double, 2> finalVector) {
+    return (Eigen::Vector4d() << initialVector[0], initialVector[1],
+            finalVector[0], finalVector[1])
+        .finished();
+  }
+};
+}  // namespace frc

wpilibc/src/main/native/include/frc/spline/QuinticHermiteSpline.h

@@ -0,0 +1,86 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <array>
+
+#include <Eigen/Core>
+
+#include "frc/spline/Spline.h"
+
+namespace frc {
+/**
+ * Represents a hermite spline of degree 5.
+ */
+class QuinticHermiteSpline : public Spline<5> {
+ public:
+  /**
+   * Constructs a quintic hermite spline with the specified control vectors.
+   * Each control vector contains into about the location of the point, its
+   * first derivative, and its second derivative.
+   *
+   * @param xInitialControlVector The control vector for the initial point in
+   * the x dimension.
+   * @param xFinalControlVector The control vector for the final point in
+   * the x dimension.
+   * @param yInitialControlVector The control vector for the initial point in
+   * the y dimension.
+   * @param yFinalControlVector The control vector for the final point in
+   * the y dimension.
+   */
+  QuinticHermiteSpline(std::array<double, 3> xInitialControlVector,
+                       std::array<double, 3> xFinalControlVector,
+                       std::array<double, 3> yInitialControlVector,
+                       std::array<double, 3> yFinalControlVector);
+
+ protected:
+  /**
+   * Returns the coefficients matrix.
+   * @return The coefficients matrix.
+   */
+  Eigen::Matrix<double, 6, 6> Coefficients() const override {
+    return m_coefficients;
+  }
+
+ private:
+  Eigen::Matrix<double, 6, 6> m_coefficients;
+
+  /**
+   * Returns the hermite basis matrix for quintic hermite spline interpolation.
+   * @return The hermite basis matrix for quintic hermite spline interpolation.
+   */
+  static Eigen::Matrix<double, 6, 6> MakeHermiteBasis() {
+    // clang-format off
+    static const auto basis = (Eigen::Matrix<double, 6, 6>() <<
+      -06.0, -03.0, -00.5, +06.0, -03.0, +00.5,
+      +15.0, +08.0, +01.5, -15.0, +07.0, +01.0,
+      -10.0, -06.0, -01.5, +10.0, -04.0, +00.5,
+      +00.0, +00.0, +00.5, +00.0, +00.0, +00.0,
+      +00.0, +01.0, +00.0, +00.0, +00.0, +00.0,
+      +01.0, +00.0, +00.0, +00.0, +00.0, +00.0).finished();
+    // clang-format on
+    return basis;
+  }
+
+  /**
+   * Returns the control vector for each dimension as a matrix from the
+   * user-provided arrays in the constructor.
+   *
+   * @param initialVector The control vector for the initial point.
+   * @param finalVector The control vector for the final point.
+   *
+   * @return The control vector matrix for a dimension.
+   */
+  static Eigen::Matrix<double, 6, 1> ControlVectorFromArrays(
+      std::array<double, 3> initialVector, std::array<double, 3> finalVector) {
+    return (Eigen::Matrix<double, 6, 1>() << initialVector[0], initialVector[1],
+            initialVector[2], finalVector[0], finalVector[1], finalVector[2])
+        .finished();
+  }
+};
+}  // namespace frc

wpilibc/src/main/native/include/frc/spline/Spline.h

@@ -0,0 +1,122 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <utility>
+#include <vector>
+
+#include <Eigen/Core>
+
+#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 two-dimensional parametric spline that interpolates between two
+ * points.
+ *
+ * @tparam Degree The degree of the spline.
+ */
+template <int Degree>
+class Spline {
+ public:
+  using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
+
+  Spline() = default;
+
+  Spline(const Spline&) = default;
+  Spline& operator=(const Spline&) = default;
+
+  Spline(Spline&&) = default;
+  Spline& operator=(Spline&&) = default;
+
+  virtual ~Spline() = default;
+
+  /**
+   * Gets the pose and curvature at some point t on the spline.
+   *
+   * @param t The point t
+   * @return The pose and curvature at that point.
+   */
+  PoseWithCurvature GetPoint(double t) const {
+    Eigen::Matrix<double, Degree + 1, 1> polynomialBases;
+
+    // Populate the polynomial bases
+    for (int i = 0; i <= Degree; i++) {
+      polynomialBases(i) = std::pow(t, Degree - i);
+    }
+
+    // This simply multiplies by the coefficients. We need to divide out t some
+    // n number of times where n is the derivative we want to take.
+    Eigen::Matrix<double, 6, 1> combined = Coefficients() * polynomialBases;
+
+    double dx, dy, ddx, ddy;
+
+    // If t = 0, all other terms in the equation cancel out to zero. We can use
+    // the last x^0 term in the equation.
+    if (t == 0.0) {
+      dx = Coefficients()(2, Degree - 1);
+      dy = Coefficients()(3, Degree - 1);
+      ddx = Coefficients()(4, Degree - 2);
+      ddy = Coefficients()(5, Degree - 2);
+    } else {
+      // Divide out t for first derivative.
+      dx = combined(2) / t;
+      dy = combined(3) / t;
+
+      // Divide out t for second derivative.
+      ddx = combined(4) / t / t;
+      ddy = combined(5) / t / t;
+    }
+
+    // Find the curvature.
+    const auto curvature =
+        (dx * ddy - ddx * dy) / ((dx * dx + dy * dy) * std::hypot(dx, dy));
+
+    return {
+        {FromVector(combined.template block<2, 1>(0, 0)), Rotation2d(dx, dy)},
+        curvature_t(curvature)};
+  }
+
+ protected:
+  /**
+   * Returns the coefficients of the spline.
+   *
+   * @return The coefficients of the spline.
+   */
+  virtual Eigen::Matrix<double, 6, Degree + 1> Coefficients() const = 0;
+
+  /**
+   * Converts a Translation2d into a vector that is compatible with Eigen.
+   *
+   * @param translation The Translation2d to convert.
+   * @return The vector.
+   */
+  static Eigen::Vector2d ToVector(const Translation2d& translation) {
+    return (Eigen::Vector2d() << translation.X().to<double>(),
+            translation.Y().to<double>())
+        .finished();
+  }
+
+  /**
+   * Converts an Eigen vector into a Translation2d.
+   *
+   * @param vector The vector to convert.
+   * @return The Translation2d.
+   */
+  static Translation2d FromVector(const Eigen::Vector2d& vector) {
+    return Translation2d(units::meter_t(vector(0)), units::meter_t(vector(1)));
+  }
+};
+}  // namespace frc

wpilibc/src/main/native/include/frc/spline/SplineHelper.h

@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* 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 "frc/spline/CubicHermiteSpline.h"
+#include "frc/spline/QuinticHermiteSpline.h"
+
+namespace frc {
+/**
+ * Helper class that is used to generate cubic and quintic splines from user
+ * provided waypoints.
+ */
+class SplineHelper {
+ public:
+  /**
+   * Returns a set of cubic splines corresponding to the provided waypoints. The
+   * user is free to set the direction of the start and end point. The
+   * directions for the middle waypoints are determined automatically to ensure
+   * continuous curvature throughout the path.
+   *
+   * The derivation for the algorithm used can be found here:
+   * <https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08/undervisningsmateriale/chap7alecture.pdf>
+   *
+   * @param start The starting waypoint.
+   * @param waypoints The middle waypoints. This can be left blank if you only
+   * wish to create a path with two waypoints.
+   * @param end The ending waypoint.
+   *
+   * @return A vector of cubic hermite splines that interpolate through the
+   * provided waypoints.
+   */
+  static std::vector<CubicHermiteSpline> CubicSplinesFromWaypoints(
+      const Pose2d& start, std::vector<Translation2d> waypoints,
+      const Pose2d& end);
+
+  /**
+   * Returns a set of quintic splines corresponding to the provided waypoints.
+   * The user is free to set the direction of all waypoints. Continuous
+   * curvature is guaranteed throughout the path.
+   *
+   * @param waypoints The waypoints.
+   * @return A vector of quintic hermite splines that interpolate through the
+   * provided waypoints.
+   */
+  static std::vector<QuinticHermiteSpline> QuinticSplinesFromWaypoints(
+      const std::vector<Pose2d>& waypoints);
+
+ private:
+  /**
+   * Thomas algorithm for solving tridiagonal systems Af = d.
+   *
+   * @param a the values of A above the diagonal
+   * @param b the values of A on the diagonal
+   * @param c the values of A below the diagonal
+   * @param d the vector on the rhs
+   * @param solutionVector the unknown (solution) vector, modified in-place
+   */
+  static void ThomasAlgorithm(const std::vector<double>& a,
+                              const std::vector<double>& b,
+                              const std::vector<double>& c,
+                              const std::vector<double>& d,
+                              std::vector<double>* solutionVector);
+};
+}  // namespace frc

wpilibc/src/main/native/include/frc/spline/SplineParameterizer.h

@@ -0,0 +1,114 @@
+/*----------------------------------------------------------------------------*/
+/* 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 <frc/spline/Spline.h>
+
+#include <utility>
+#include <vector>
+
+#include <units/units.h>
+
+namespace frc {
+
+/**
+ * Class used to parameterize a spline by its arc length.
+ */
+class SplineParameterizer {
+ public:
+  using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
+
+  /**
+   * Parameterizes the spline. This method breaks up the spline into various
+   * arcs until their dx, dy, and dtheta are within specific tolerances.
+   *
+   * @param spline The spline to parameterize.
+   * @param t0 Starting internal spline parameter. It is recommended to leave
+   * this as default.
+   * @param t1 Ending internal spline parameter. It is recommended to leave this
+   * as default.
+   *
+   * @return A vector of poses and curvatures that represents various points on
+   * the spline.
+   */
+  template <int Dim>
+  static std::vector<PoseWithCurvature> Parameterize(const Spline<Dim>& spline,
+                                                     double t0 = 0.0,
+                                                     double t1 = 1.0) {
+    std::vector<PoseWithCurvature> arr;
+
+    // The parameterization does not add the first initial point. Let's add
+    // that.
+    arr.push_back(spline.GetPoint(t0));
+
+    GetSegmentArc(spline, &arr, t0, t1);
+    return arr;
+  }
+
+ private:
+  // Constraints for spline parameterization.
+  static constexpr units::meter_t kMaxDx = 5_in;
+  static constexpr units::meter_t kMaxDy = 0.05_in;
+  static constexpr units::radian_t kMaxDtheta = 0.0872_rad;
+
+  /**
+   * Breaks up the spline into arcs until the dx, dy, and theta of each arc is
+   * within tolerance.
+   *
+   * @param spline The spline to parameterize.
+   * @param vector Pointer to vector of poses.
+   * @param t0 Starting point for arc.
+   * @param t1 Ending point for arc.
+   */
+  template <int Dim>
+  static void GetSegmentArc(const Spline<Dim>& spline,
+                            std::vector<PoseWithCurvature>* vector, double t0,
+                            double t1) {
+    const auto start = spline.GetPoint(t0);
+    const auto end = spline.GetPoint(t1);
+
+    const auto twist = start.first.Log(end.first);
+
+    if (units::math::abs(twist.dy) > kMaxDy ||
+        units::math::abs(twist.dx) > kMaxDx ||
+        units::math::abs(twist.dtheta) > kMaxDtheta) {
+      GetSegmentArc(spline, vector, t0, (t0 + t1) / 2);
+      GetSegmentArc(spline, vector, (t0 + t1) / 2, t1);
+    } else {
+      vector->push_back(spline.GetPoint(t1));
+    }
+  }
+
+  friend class CubicHermiteSplineTest;
+  friend class QuinticHermiteSplineTest;
+};
+}  // namespace frc

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