Add trajectory generation using hermite splines (#1843)

wpilibc/src/main/native/cpp/geometry/Pose2d.cpp

@@ -27,6 +27,11 @@ Pose2d& Pose2d::operator+=(const Transform2d& other) {
   return *this;
 }
 
+Transform2d Pose2d::operator-(const Pose2d& other) const {
+  const auto pose = this->RelativeTo(other);
+  return Transform2d(pose.Translation(), pose.Rotation());
+}
+
 bool Pose2d::operator==(const Pose2d& other) const {
   return m_translation == other.m_translation && m_rotation == other.m_rotation;
 }
@@ -67,3 +72,27 @@ Pose2d Pose2d::Exp(const Twist2d& twist) const {
 
   return *this + transform;
 }
+
+Twist2d Pose2d::Log(const Pose2d& end) const {
+  const auto transform = end.RelativeTo(*this);
+  const auto dtheta = transform.Rotation().Radians().to<double>();
+  const auto halfDtheta = dtheta / 2.0;
+
+  const auto cosMinusOne = transform.Rotation().Cos() - 1;
+
+  double halfThetaByTanOfHalfDtheta;
+
+  if (std::abs(cosMinusOne) < 1E-9) {
+    halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
+  } else {
+    halfThetaByTanOfHalfDtheta =
+        -(halfDtheta * transform.Rotation().Sin()) / cosMinusOne;
+  }
+
+  const Translation2d translationPart =
+      transform.Translation().RotateBy(
+          {halfThetaByTanOfHalfDtheta, -halfDtheta}) *
+      std::hypot(halfThetaByTanOfHalfDtheta, halfDtheta);
+
+  return {translationPart.X(), translationPart.Y(), units::radian_t(dtheta)};
+}

wpilibc/src/main/native/cpp/geometry/Rotation2d.cpp

@@ -52,6 +52,10 @@ Rotation2d& Rotation2d::operator-=(const Rotation2d& other) {
 
 Rotation2d Rotation2d::operator-() const { return Rotation2d(-m_value); }
 
+Rotation2d Rotation2d::operator*(double scalar) const {
+  return Rotation2d(m_value * scalar);
+}
+
 bool Rotation2d::operator==(const Rotation2d& other) const {
   return units::math::abs(m_value - other.m_value) < 1E-9_rad;
 }

wpilibc/src/main/native/cpp/spline/CubicHermiteSpline.cpp

@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/spline/CubicHermiteSpline.h"
+
+using namespace frc;
+
+CubicHermiteSpline::CubicHermiteSpline(
+    std::array<double, 2> xInitialControlVector,
+    std::array<double, 2> xFinalControlVector,
+    std::array<double, 2> yInitialControlVector,
+    std::array<double, 2> yFinalControlVector) {
+  const auto hermite = MakeHermiteBasis();
+  const auto x =
+      ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
+  const auto y =
+      ControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
+
+  // Populate first two rows with coefficients.
+  m_coefficients.template block<1, 4>(0, 0) = hermite * x;
+  m_coefficients.template block<1, 4>(1, 0) = hermite * y;
+
+  // Populate Row 2 and Row 3 with the derivatives of the equations above.
+  // Then populate row 4 and 5 with the second derivatives.
+  for (int i = 0; i < 4; i++) {
+    m_coefficients.template block<2, 1>(2, i) =
+        m_coefficients.template block<2, 1>(0, i) * (3 - i);
+
+    m_coefficients.template block<2, 1>(4, i) =
+        m_coefficients.template block<2, 1>(2, i) * (3 - i);
+  }
+}

wpilibc/src/main/native/cpp/spline/QuinticHermiteSpline.cpp

@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/spline/QuinticHermiteSpline.h"
+
+using namespace frc;
+
+QuinticHermiteSpline::QuinticHermiteSpline(
+    std::array<double, 3> xInitialControlVector,
+    std::array<double, 3> xFinalControlVector,
+    std::array<double, 3> yInitialControlVector,
+    std::array<double, 3> yFinalControlVector) {
+  const auto hermite = MakeHermiteBasis();
+  const auto x =
+      ControlVectorFromArrays(xInitialControlVector, xFinalControlVector);
+  const auto y =
+      ControlVectorFromArrays(yInitialControlVector, yFinalControlVector);
+
+  // Populate first two rows with coefficients.
+  m_coefficients.template block<1, 6>(0, 0) = (hermite * x).transpose();
+  m_coefficients.template block<1, 6>(1, 0) = (hermite * y).transpose();
+
+  // Populate Row 2 and Row 3 with the derivatives of the equations above.
+  // Then populate row 4 and 5 with the second derivatives.
+  for (int i = 0; i < 6; i++) {
+    m_coefficients.template block<2, 1>(2, i) =
+        m_coefficients.template block<2, 1>(0, i) * (5 - i);
+
+    m_coefficients.template block<2, 1>(4, i) =
+        m_coefficients.template block<2, 1>(2, i) * (5 - i);
+  }
+}

wpilibc/src/main/native/cpp/spline/SplineHelper.cpp

@@ -0,0 +1,190 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/spline/SplineHelper.h"
+
+#include <cstddef>
+
+using namespace frc;
+
+std::vector<CubicHermiteSpline> SplineHelper::CubicSplinesFromWaypoints(
+    const Pose2d& start, std::vector<Translation2d> waypoints,
+    const Pose2d& end) {
+  std::vector<CubicHermiteSpline> splines;
+
+  double scalar;
+  // This just makes the splines look better.
+  if (waypoints.empty()) {
+    scalar = 1.2 * start.Translation().Distance(end.Translation()).to<double>();
+  } else {
+    scalar = 1.2 * start.Translation().Distance(waypoints.front()).to<double>();
+  }
+
+  std::array<double, 2> startXControlVector{
+      start.Translation().X().to<double>(), start.Rotation().Cos() * scalar};
+
+  std::array<double, 2> startYControlVector{
+      start.Translation().Y().to<double>(), start.Rotation().Sin() * scalar};
+
+  // This just makes the splines look better.
+  if (!waypoints.empty()) {
+    scalar = 1.2 * end.Translation().Distance(waypoints.back()).to<double>();
+  }
+
+  std::array<double, 2> endXControlVector{end.Translation().X().to<double>(),
+                                          end.Rotation().Cos() * scalar};
+
+  std::array<double, 2> endYControlVector{end.Translation().Y().to<double>(),
+                                          end.Rotation().Sin() * scalar};
+
+  if (waypoints.size() > 1) {
+    waypoints.emplace(waypoints.begin(), start.Translation());
+    waypoints.emplace_back(end.Translation());
+
+    std::vector<double> a;
+    std::vector<double> b(waypoints.size() - 2, 4.0);
+    std::vector<double> c;
+    std::vector<double> dx, dy;
+    std::vector<double> fx(waypoints.size() - 2, 0.0),
+        fy(waypoints.size() - 2, 0.0);
+
+    a.emplace_back(0);
+    for (unsigned int i = 0; i < waypoints.size() - 3; i++) {
+      a.emplace_back(1);
+      c.emplace_back(1);
+    }
+    c.emplace_back(0);
+
+    dx.emplace_back(
+        3 * (waypoints[2].X().to<double>() - waypoints[0].X().to<double>()) -
+        startXControlVector[1]);
+    dy.emplace_back(
+        3 * (waypoints[2].Y().to<double>() - waypoints[0].Y().to<double>()) -
+        startYControlVector[1]);
+    if (waypoints.size() > 4) {
+      for (unsigned int i = 1; i <= waypoints.size() - 4; i++) {
+        dx.emplace_back(3 * (waypoints[i + 1].X().to<double>() -
+                             waypoints[i - 1].X().to<double>()));
+        dy.emplace_back(3 * (waypoints[i + 1].Y().to<double>() -
+                             waypoints[i - 1].Y().to<double>()));
+      }
+    }
+    dx.emplace_back(3 * (waypoints[waypoints.size() - 1].X().to<double>() -
+                         waypoints[waypoints.size() - 3].X().to<double>()) -
+                    endXControlVector[1]);
+    dy.emplace_back(3 * (waypoints[waypoints.size() - 1].Y().to<double>() -
+                         waypoints[waypoints.size() - 3].Y().to<double>()) -
+                    endYControlVector[1]);
+
+    ThomasAlgorithm(a, b, c, dx, &fx);
+    ThomasAlgorithm(a, b, c, dy, &fy);
+
+    fx.emplace(fx.begin(), startXControlVector[1]);
+    fx.emplace_back(endXControlVector[1]);
+    fy.emplace(fy.begin(), startYControlVector[1]);
+    fy.emplace_back(endYControlVector[1]);
+
+    for (unsigned int i = 0; i < fx.size() - 1; i++) {
+      // Create the spline.
+      const CubicHermiteSpline spline{
+          {waypoints[i].X().to<double>(), fx[i]},
+          {waypoints[i + 1].X().to<double>(), fx[i + 1]},
+          {waypoints[i].Y().to<double>(), fy[i]},
+          {waypoints[i + 1].Y().to<double>(), fy[i + 1]}};
+
+      splines.push_back(spline);
+    }
+  } else if (waypoints.size() == 1) {
+    const double xDeriv = (3 * (end.Translation().X().to<double>() -
+                                start.Translation().X().to<double>()) -
+                           endXControlVector[1] - startXControlVector[1]) /
+                          4.0;
+    const double yDeriv = (3 * (end.Translation().Y().to<double>() -
+                                start.Translation().Y().to<double>()) -
+                           endYControlVector[1] - startYControlVector[1]) /
+                          4.0;
+
+    std::array<double, 2> midXControlVector{waypoints[0].X().to<double>(),
+                                            xDeriv};
+    std::array<double, 2> midYControlVector{waypoints[0].Y().to<double>(),
+                                            yDeriv};
+
+    splines.emplace_back(startXControlVector, midXControlVector,
+                         startYControlVector, midYControlVector);
+    splines.emplace_back(midXControlVector, endXControlVector,
+                         midYControlVector, endYControlVector);
+
+  } else {
+    // Create the spline.
+    const CubicHermiteSpline spline{startXControlVector, endXControlVector,
+                                    startYControlVector, endYControlVector};
+    splines.push_back(spline);
+  }
+
+  return splines;
+}
+
+std::vector<QuinticHermiteSpline> SplineHelper::QuinticSplinesFromWaypoints(
+    const std::vector<Pose2d>& waypoints) {
+  std::vector<QuinticHermiteSpline> splines;
+  for (unsigned int i = 0; i < waypoints.size() - 1; i++) {
+    auto& p0 = waypoints[i];
+    auto& p1 = waypoints[i + 1];
+
+    // This just makes the splines look better.
+    const auto scalar =
+        1.2 * p0.Translation().Distance(p1.Translation()).to<double>();
+
+    const std::array<double, 3> xInitialControlVector{
+        p0.Translation().X().to<double>(), p0.Rotation().Cos() * scalar, 0.0};
+    const std::array<double, 3> xFinalControlVector{
+        p1.Translation().X().to<double>(), p1.Rotation().Cos() * scalar, 0.0};
+    const std::array<double, 3> yInitialControlVector{
+        p0.Translation().Y().to<double>(), p0.Rotation().Sin() * scalar, 0.0};
+    const std::array<double, 3> yFinalControlVector{
+        p1.Translation().Y().to<double>(), p1.Rotation().Sin() * scalar, 0.0};
+
+    splines.emplace_back(xInitialControlVector, xFinalControlVector,
+                         yInitialControlVector, yFinalControlVector);
+  }
+  return splines;
+}
+
+void SplineHelper::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) {
+  auto& f = *solutionVector;
+  size_t N = d.size();
+
+  // Create the temporary vectors
+  // Note that this is inefficient as it is possible to call
+  // this function many times. A better implementation would
+  // pass these temporary matrices by non-const reference to
+  // save excess allocation and deallocation
+  std::vector<double> c_star(N, 0.0);
+  std::vector<double> d_star(N, 0.0);
+
+  // This updates the coefficients in the first row
+  // Note that we should be checking for division by zero here
+  c_star[0] = c[0] / b[0];
+  d_star[0] = d[0] / b[0];
+
+  // Create the c_star and d_star coefficients in the forward sweep
+  for (unsigned int i = 1; i < N; i++) {
+    double m = 1.0 / (b[i] - a[i] * c_star[i - 1]);
+    c_star[i] = c[i] * m;
+    d_star[i] = (d[i] - a[i] * d_star[i - 1]) * m;
+  }
+
+  f[N - 1] = d_star[N - 1];
+  // This is the reverse sweep, used to update the solution vector f
+  for (int i = N - 2; i >= 0; i--) {
+    f[i] = d_star[i] - c_star[i] * f[i + 1];
+  }
+}

wpilibc/src/main/native/cpp/spline/SplineParameterizer.cpp

@@ -0,0 +1,12 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/spline/SplineParameterizer.h"
+
+constexpr units::meter_t frc::SplineParameterizer::kMaxDx;
+constexpr units::meter_t frc::SplineParameterizer::kMaxDy;
+constexpr units::radian_t frc::SplineParameterizer::kMaxDtheta;

wpilibc/src/main/native/cpp/trajectory/Trajectory.cpp

@@ -0,0 +1,96 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/Trajectory.h"
+
+using namespace frc;
+
+Trajectory::State Trajectory::State::Interpolate(State endValue,
+                                                 double i) const {
+  // Find the new [t] value.
+  const auto newT = Lerp(t, endValue.t, i);
+
+  // Find the delta time between the current state and the interpolated state.
+  const auto deltaT = newT - t;
+
+  // If delta time is negative, flip the order of interpolation.
+  if (deltaT < 0_s) return endValue.Interpolate(*this, 1.0 - i);
+
+  // Check whether the robot is reversing at this stage.
+  const auto reversing =
+      velocity < 0_mps ||
+      (units::math::abs(velocity) < 1E-9_mps && acceleration < 0_mps_sq);
+
+  // Calculate the new velocity.
+  // v = v_0 + at
+  const units::meters_per_second_t newV = velocity + (acceleration * deltaT);
+
+  // Calculate the change in position.
+  // delta_s = v_0 t + 0.5 at^2
+  const units::meter_t newS =
+      (velocity * deltaT + 0.5 * acceleration * deltaT * deltaT) *
+      (reversing ? -1.0 : 1.0);
+
+  // Return the new state. To find the new position for the new state, we need
+  // to interpolate between the two endpoint poses. The fraction for
+  // interpolation is the change in position (delta s) divided by the total
+  // distance between the two endpoints.
+  const double interpolationFrac =
+      newS / endValue.pose.Translation().Distance(pose.Translation());
+
+  return {newT, newV, acceleration,
+          Lerp(pose, endValue.pose, interpolationFrac),
+          Lerp(curvature, endValue.curvature, interpolationFrac)};
+}
+
+Trajectory::Trajectory(const std::vector<State>& states) : m_states(states) {
+  m_totalTime = states.back().t;
+}
+
+Trajectory::State Trajectory::Sample(units::second_t t) const {
+  if (t <= m_states.front().t) return m_states.front();
+  if (t >= m_totalTime) return m_states.back();
+
+  // To get the element that we want, we will use a binary search algorithm
+  // instead of iterating over a for-loop. A binary search is O(std::log(n))
+  // whereas searching using a loop is O(n).
+
+  // This starts at 1 because we use the previous state later on for
+  // interpolation.
+  int low = 1;
+  int high = m_states.size() - 1;
+
+  while (low != high) {
+    int mid = (low + high) / 2;
+    if (m_states[mid].t < t) {
+      // This index and everything under it are less than the requested
+      // timestamp. Therefore, we can discard them.
+      low = mid + 1;
+    } else {
+      // t is at least as large as the element at this index. This means that
+      // anything after it cannot be what we are looking for.
+      high = mid;
+    }
+  }
+
+  // High and Low should be the same.
+
+  // The sample's timestamp is now greater than or equal to the requested
+  // timestamp. If it is greater, we need to interpolate between the
+  // previous state and the current state to get the exact state that we
+  // want.
+  const auto sample = m_states[low];
+  const auto prevSample = m_states[low - 1];
+
+  // If the difference in states is negligible, then we are spot on!
+  if (units::math::abs(sample.t - prevSample.t) < 1E-9_s) {
+    return sample;
+  }
+  // Interpolate between the two states for the state that we want.
+  return prevSample.Interpolate(sample,
+                                (t - prevSample.t) / (sample.t - prevSample.t));
+}

wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp

@@ -0,0 +1,77 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/TrajectoryGenerator.h"
+
+#include <utility>
+
+#include "frc/spline/SplineHelper.h"
+#include "frc/trajectory/TrajectoryParameterizer.h"
+
+using namespace frc;
+
+Trajectory TrajectoryGenerator::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) {
+  const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
+
+  // Make theta normal for trajectory generation if path is reversed.
+  std::vector<Pose2d> newWaypoints;
+  newWaypoints.reserve(waypoints.size());
+  for (auto&& point : waypoints) {
+    newWaypoints.push_back(reversed ? point + flip : point);
+  }
+
+  auto points = SplinePointsFromSplines(
+      SplineHelper::QuinticSplinesFromWaypoints(newWaypoints));
+
+  // After trajectory generation, flip theta back so it's relative to the
+  // field. Also fix curvature.
+  if (reversed) {
+    for (auto& point : points) {
+      point = {point.first + flip, -point.second};
+    }
+  }
+
+  return TrajectoryParameterizer::TimeParameterizeTrajectory(
+      points, std::move(constraints), startVelocity, endVelocity, maxVelocity,
+      maxAcceleration, reversed);
+}
+
+Trajectory TrajectoryGenerator::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) {
+  const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
+  // Make theta normal for trajectory generation if path is reversed.
+
+  const Pose2d newStart = reversed ? start + flip : start;
+  const Pose2d newEnd = reversed ? end + flip : end;
+
+  auto points = SplinePointsFromSplines(
+      SplineHelper::CubicSplinesFromWaypoints(newStart, waypoints, newEnd));
+
+  // After trajectory generation, flip theta back so it's relative to the
+  // field. Also fix curvature.
+  if (reversed) {
+    for (auto& point : points) {
+      point = {point.first + flip, -point.second};
+    }
+  }
+
+  return TrajectoryParameterizer::TimeParameterizeTrajectory(
+      points, std::move(constraints), startVelocity, endVelocity, maxVelocity,
+      maxAcceleration, reversed);
+}

wpilibc/src/main/native/cpp/trajectory/TrajectoryParameterizer.cpp

@@ -0,0 +1,224 @@
+/*----------------------------------------------------------------------------*/
+/* 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.
+ */
+
+#include "frc/trajectory/TrajectoryParameterizer.h"
+
+using namespace frc;
+
+Trajectory TrajectoryParameterizer::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) {
+  std::vector<ConstrainedState> constrainedStates(points.size());
+
+  ConstrainedState predecessor{points.front(), 0_m, startVelocity,
+                               -maxAcceleration, maxAcceleration};
+
+  constrainedStates[0] = predecessor;
+
+  // Forward pass
+  for (unsigned int i = 0; i < points.size(); i++) {
+    auto& constrainedState = constrainedStates[i];
+    constrainedState.pose = points[i];
+
+    // Begin constraining based on predecessor
+    units::meter_t ds = constrainedState.pose.first.Translation().Distance(
+        predecessor.pose.first.Translation());
+    constrainedState.distance = ds + predecessor.distance;
+
+    // We may need to iterate to find the maximum end velocity and common
+    // acceleration, since acceleration limits may be a function of velocity.
+    while (true) {
+      // Enforce global max velocity and max reachable velocity by global
+      // acceleration limit. vf = std::sqrt(vi^2 + 2*a*d).
+
+      constrainedState.maxVelocity = units::math::min(
+          maxVelocity,
+          units::math::sqrt(predecessor.maxVelocity * predecessor.maxVelocity +
+                            predecessor.maxAcceleration * ds * 2.0));
+
+      constrainedState.minAcceleration = -maxAcceleration;
+      constrainedState.maxAcceleration = maxAcceleration;
+
+      // At this point, the constrained state is fully constructed apart from
+      // all the custom-defined user constraints.
+      for (const auto& constraint : constraints) {
+        constrainedState.maxVelocity = units::math::min(
+            constrainedState.maxVelocity,
+            constraint->MaxVelocity(constrainedState.pose.first,
+                                    constrainedState.pose.second,
+                                    constrainedState.maxVelocity));
+      }
+
+      // Now enforce all acceleration limits.
+      EnforceAccelerationLimits(reversed, constraints, &constrainedState);
+
+      if (ds.to<double>() < kEpsilon) break;
+
+      // If the actual acceleration for this state is higher than the max
+      // acceleration that we applied, then we need to reduce the max
+      // acceleration of the predecessor and try again.
+      units::meters_per_second_squared_t actualAcceleration =
+          (constrainedState.maxVelocity * constrainedState.maxVelocity -
+           predecessor.maxVelocity * predecessor.maxVelocity) /
+          (ds * 2.0);
+
+      // If we violate the max acceleration constraint, let's modify the
+      // predecessor.
+      if (constrainedState.maxAcceleration < actualAcceleration - 1E-6_mps_sq) {
+        predecessor.maxAcceleration = constrainedState.maxAcceleration;
+      } else {
+        // Constrain the predecessor's max acceleration to the current
+        // acceleration.
+        if (actualAcceleration > predecessor.minAcceleration + 1E-6_mps_sq) {
+          predecessor.maxAcceleration = actualAcceleration;
+        }
+        // If the actual acceleration is less than the predecessor's min
+        // acceleration, it will be repaired in the backward pass.
+        break;
+      }
+    }
+    predecessor = constrainedState;
+  }
+
+  ConstrainedState successor{points.back(), constrainedStates.back().distance,
+                             endVelocity, -maxAcceleration, maxAcceleration};
+
+  // Backward pass
+  for (int i = points.size() - 1; i >= 0; i--) {
+    auto& constrainedState = constrainedStates[i];
+    units::meter_t ds =
+        constrainedState.distance - successor.distance;  // negative
+
+    while (true) {
+      // Enforce max velocity limit (reverse)
+      // vf = std::sqrt(vi^2 + 2*a*d), where vi = successor.
+      units::meters_per_second_t newMaxVelocity =
+          units::math::sqrt(successor.maxVelocity * successor.maxVelocity +
+                            successor.minAcceleration * ds * 2.0);
+
+      // No more limits to impose! This state can be finalized.
+      if (newMaxVelocity >= constrainedState.maxVelocity) break;
+
+      constrainedState.maxVelocity = newMaxVelocity;
+
+      // Check all acceleration constraints with the new max velocity.
+      EnforceAccelerationLimits(reversed, constraints, &constrainedState);
+
+      if (ds.to<double>() > -kEpsilon) break;
+
+      // If the actual acceleration for this state is lower than the min
+      // acceleration, then we need to lower the min acceleration of the
+      // successor and try again.
+      units::meters_per_second_squared_t actualAcceleration =
+          (constrainedState.maxVelocity * constrainedState.maxVelocity -
+           successor.maxVelocity * successor.maxVelocity) /
+          (ds * 2.0);
+      if (constrainedState.minAcceleration > actualAcceleration + 1E-6_mps_sq) {
+        successor.minAcceleration = constrainedState.minAcceleration;
+      } else {
+        successor.minAcceleration = actualAcceleration;
+        break;
+      }
+    }
+    successor = constrainedState;
+  }
+
+  // Now we can integrate the constrained states forward in time to obtain our
+  // trajectory states.
+
+  std::vector<Trajectory::State> states(points.size());
+  units::second_t t = 0_s;
+  units::meter_t s = 0_m;
+  units::meters_per_second_t v = 0_mps;
+
+  for (unsigned int i = 0; i < constrainedStates.size(); i++) {
+    auto state = constrainedStates[i];
+
+    // Calculate the change in position between the current state and the
+    // previous state.
+    units::meter_t ds = state.distance - s;
+
+    // Calculate the acceleration between the current state and the previous
+    // state.
+    units::meters_per_second_squared_t accel =
+        (state.maxVelocity * state.maxVelocity - v * v) / (ds * 2);
+
+    // Calculate dt.
+    units::second_t dt = 0_s;
+    if (i > 0) {
+      states.at(i - 1).acceleration = reversed ? -accel : accel;
+      if (units::math::abs(accel) > 1E-6_mps_sq) {
+        // v_f = v_0 + a * t
+        dt = (state.maxVelocity - v) / accel;
+      } else if (units::math::abs(v) > 1E-6_mps) {
+        // delta_x = v * t
+        dt = ds / v;
+      } else {
+        throw std::runtime_error(
+            "Something went wrong during trajectory generation.");
+      }
+    }
+
+    v = state.maxVelocity;
+    s = state.distance;
+
+    t += dt;
+
+    states[i] = {t, reversed ? -v : v, reversed ? -accel : accel,
+                 state.pose.first, state.pose.second};
+  }
+
+  return Trajectory(states);
+}
+
+void TrajectoryParameterizer::EnforceAccelerationLimits(
+    bool reverse,
+    const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
+    ConstrainedState* state) {
+  for (auto&& constraint : constraints) {
+    double factor = reverse ? -1.0 : 1.0;
+
+    auto minMaxAccel = constraint->MinMaxAcceleration(
+        state->pose.first, state->pose.second, state->maxVelocity * factor);
+
+    state->minAcceleration = units::math::max(
+        state->minAcceleration,
+        reverse ? -minMaxAccel.maxAcceleration : minMaxAccel.minAcceleration);
+
+    state->maxAcceleration = units::math::min(
+        state->maxAcceleration,
+        reverse ? -minMaxAccel.minAcceleration : minMaxAccel.maxAcceleration);
+  }
+}

wpilibc/src/main/native/cpp/trajectory/constraint/CentripetalAccelerationConstraint.cpp

@@ -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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/CentripetalAccelerationConstraint.h"
+
+using namespace frc;
+
+CentripetalAccelerationConstraint::CentripetalAccelerationConstraint(
+    units::meters_per_second_squared_t maxCentripetalAcceleration)
+    : m_maxCentripetalAcceleration(maxCentripetalAcceleration) {}
+
+units::meters_per_second_t CentripetalAccelerationConstraint::MaxVelocity(
+    const Pose2d& pose, curvature_t curvature,
+    units::meters_per_second_t velocity) {
+  // ac = v^2 / r
+  // k (curvature) = 1 / r
+
+  // therefore, ac = v^2 * k
+  // ac / k = v^2
+  // v = std::sqrt(ac / k)
+
+  // We have to multiply by 1_rad here to get the units to cancel out nicely.
+  // The units library defines a unit for radians although it is technically
+  // unitless.
+  return units::math::sqrt(m_maxCentripetalAcceleration /
+                           units::math::abs(curvature) * 1_rad);
+}
+
+TrajectoryConstraint::MinMax
+CentripetalAccelerationConstraint::MinMaxAcceleration(
+    const Pose2d& pose, curvature_t curvature,
+    units::meters_per_second_t speed) {
+  // The acceleration of the robot has no impact on the centripetal acceleration
+  // of the robot.
+  return {};
+}

wpilibc/src/main/native/cpp/trajectory/constraint/DifferentialDriveKinematicsConstraint.cpp

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
+
+using namespace frc;
+
+DifferentialDriveKinematicsConstraint::DifferentialDriveKinematicsConstraint(
+    DifferentialDriveKinematics kinematics, units::meters_per_second_t maxSpeed)
+    : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
+
+units::meters_per_second_t DifferentialDriveKinematicsConstraint::MaxVelocity(
+    const Pose2d& pose, curvature_t curvature,
+    units::meters_per_second_t velocity) {
+  auto wheelSpeeds =
+      m_kinematics.ToWheelSpeeds({velocity, 0_mps, velocity * curvature});
+  wheelSpeeds.Normalize(m_maxSpeed);
+
+  return m_kinematics.ToChassisSpeeds(wheelSpeeds).vx;
+}
+
+TrajectoryConstraint::MinMax
+DifferentialDriveKinematicsConstraint::MinMaxAcceleration(
+    const Pose2d& pose, curvature_t curvature,
+    units::meters_per_second_t speed) {
+  return {};
+}