[wpimath] Move math functionality into new wpimath library (#2629)

The wpimath library is a new library designed to separate the reusable math functionality
from the common utility library (wpiutil) and the hardware-dependent library (wpilibc/j).

Package names / include file names were NOT changed to minimize breakage.  In a future year
it would be good to revamp these for a more uniform user experience and to reduce the risk
of accidental naming conflicts.

While theoretically all of this functionality could be placed into wpiutil, several pieces
of this library (e.g. DARE) are very time-consuming to compile, so it's nice to avoid this
expense for users who only want cscore or ntcore.  It also allows for easy future separation
of build tasks vs number of workers on memory-constrained machines.

This moves the following functionality from wpiutil into wpimath:
- Eigen
- ejml
- Drake
- DARE
- wpiutil.math package (Matrix etc)
- units

And the following functionality from wpilibc/j into wpimath:
- Geometry
- Kinematics
- Spline
- Trajectory
- LinearFilter
- MedianFilter
- Feed-forward controllers

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

@@ -0,0 +1,163 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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"
+
+#include <wpi/json.h>
+
+#include "units/math.h"
+
+using namespace frc;
+
+bool Trajectory::State::operator==(const Trajectory::State& other) const {
+  return t == other.t && velocity == other.velocity &&
+         acceleration == other.acceleration && pose == other.pose &&
+         curvature == other.curvature;
+}
+
+bool Trajectory::State::operator!=(const Trajectory::State& other) const {
+  return !operator==(other);
+}
+
+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));
+}
+
+Trajectory Trajectory::TransformBy(const Transform2d& transform) {
+  auto& firstState = m_states[0];
+  auto& firstPose = firstState.pose;
+
+  // Calculate the transformed first pose.
+  auto newFirstPose = firstPose + transform;
+  auto newStates = m_states;
+  newStates[0].pose = newFirstPose;
+
+  for (unsigned int i = 1; i < newStates.size(); i++) {
+    auto& state = newStates[i];
+    // We are transforming relative to the coordinate frame of the new initial
+    // pose.
+    state.pose = newFirstPose + (state.pose - firstPose);
+  }
+
+  return Trajectory(newStates);
+}
+
+Trajectory Trajectory::RelativeTo(const Pose2d& pose) {
+  auto newStates = m_states;
+  for (auto& state : newStates) {
+    state.pose = state.pose.RelativeTo(pose);
+  }
+  return Trajectory(newStates);
+}
+
+void frc::to_json(wpi::json& json, const Trajectory::State& state) {
+  json = wpi::json{{"time", state.t.to<double>()},
+                   {"velocity", state.velocity.to<double>()},
+                   {"acceleration", state.acceleration.to<double>()},
+                   {"pose", state.pose},
+                   {"curvature", state.curvature.to<double>()}};
+}
+
+void frc::from_json(const wpi::json& json, Trajectory::State& state) {
+  state.pose = json.at("pose").get<Pose2d>();
+  state.t = units::second_t{json.at("time").get<double>()};
+  state.velocity =
+      units::meters_per_second_t{json.at("velocity").get<double>()};
+  state.acceleration =
+      units::meters_per_second_squared_t{json.at("acceleration").get<double>()};
+  state.curvature = units::curvature_t{json.at("curvature").get<double>()};
+}
+
+bool Trajectory::operator==(const Trajectory& other) const {
+  return m_states == other.States();
+}
+
+bool Trajectory::operator!=(const Trajectory& other) const {
+  return !operator==(other);
+}

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

@@ -0,0 +1,118 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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 <wpi/raw_ostream.h>
+
+#include "frc/spline/SplineHelper.h"
+#include "frc/spline/SplineParameterizer.h"
+#include "frc/trajectory/TrajectoryParameterizer.h"
+
+using namespace frc;
+
+const Trajectory TrajectoryGenerator::kDoNothingTrajectory(
+    std::vector<Trajectory::State>{Trajectory::State()});
+std::function<void(const char*)> TrajectoryGenerator::s_errorFunc;
+
+void TrajectoryGenerator::ReportError(const char* error) {
+  if (s_errorFunc)
+    s_errorFunc(error);
+  else
+    wpi::errs() << "TrajectoryGenerator error: " << error << "\n";
+}
+
+Trajectory TrajectoryGenerator::GenerateTrajectory(
+    Spline<3>::ControlVector initial,
+    const std::vector<Translation2d>& interiorWaypoints,
+    Spline<3>::ControlVector end, const TrajectoryConfig& config) {
+  const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
+
+  // Make theta normal for trajectory generation if path is reversed.
+  // Flip the headings.
+  if (config.IsReversed()) {
+    initial.x[1] *= -1;
+    initial.y[1] *= -1;
+    end.x[1] *= -1;
+    end.y[1] *= -1;
+  }
+
+  std::vector<frc::SplineParameterizer::PoseWithCurvature> points;
+  try {
+    points =
+        SplinePointsFromSplines(SplineHelper::CubicSplinesFromControlVectors(
+            initial, interiorWaypoints, end));
+  } catch (SplineParameterizer::MalformedSplineException& e) {
+    ReportError(e.what());
+    return kDoNothingTrajectory;
+  }
+
+  // After trajectory generation, flip theta back so it's relative to the
+  // field. Also fix curvature.
+  if (config.IsReversed()) {
+    for (auto& point : points) {
+      point = {point.first + flip, -point.second};
+    }
+  }
+
+  return TrajectoryParameterizer::TimeParameterizeTrajectory(
+      points, config.Constraints(), config.StartVelocity(),
+      config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
+      config.IsReversed());
+}
+
+Trajectory TrajectoryGenerator::GenerateTrajectory(
+    const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
+    const Pose2d& end, const TrajectoryConfig& config) {
+  auto [startCV, endCV] = SplineHelper::CubicControlVectorsFromWaypoints(
+      start, interiorWaypoints, end);
+  return GenerateTrajectory(startCV, interiorWaypoints, endCV, config);
+}
+
+Trajectory TrajectoryGenerator::GenerateTrajectory(
+    std::vector<Spline<5>::ControlVector> controlVectors,
+    const TrajectoryConfig& config) {
+  const Transform2d flip{Translation2d(), Rotation2d(180_deg)};
+  // Make theta normal for trajectory generation if path is reversed.
+  if (config.IsReversed()) {
+    for (auto& vector : controlVectors) {
+      // Flip the headings.
+      vector.x[1] *= -1;
+      vector.y[1] *= -1;
+    }
+  }
+
+  std::vector<frc::SplineParameterizer::PoseWithCurvature> points;
+  try {
+    points = SplinePointsFromSplines(
+        SplineHelper::QuinticSplinesFromControlVectors(controlVectors));
+  } catch (SplineParameterizer::MalformedSplineException& e) {
+    ReportError(e.what());
+    return kDoNothingTrajectory;
+  }
+
+  // After trajectory generation, flip theta back so it's relative to the
+  // field. Also fix curvature.
+  if (config.IsReversed()) {
+    for (auto& point : points) {
+      point = {point.first + flip, -point.second};
+    }
+  }
+
+  return TrajectoryParameterizer::TimeParameterizeTrajectory(
+      points, config.Constraints(), config.StartVelocity(),
+      config.EndVelocity(), config.MaxVelocity(), config.MaxAcceleration(),
+      config.IsReversed());
+}
+
+Trajectory TrajectoryGenerator::GenerateTrajectory(
+    const std::vector<Pose2d>& waypoints, const TrajectoryConfig& config) {
+  return GenerateTrajectory(
+      SplineHelper::QuinticControlVectorsFromWaypoints(waypoints), config);
+}

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

@@ -0,0 +1,237 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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"
+
+#include <string>
+
+#include "units/math.h"
+
+using namespace frc;
+
+Trajectory TrajectoryParameterizer::TimeParameterizeTrajectory(
+    const std::vector<PoseWithCurvature>& points,
+    const 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 at iteration " +
+                                 std::to_string(i) +
+                                 " of time parameterization.");
+      }
+    }
+
+    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);
+
+    if (minMaxAccel.minAcceleration > minMaxAccel.maxAcceleration) {
+      throw std::runtime_error(
+          "The constraint's min acceleration was greater than its max "
+          "acceleration. To debug this, remove all constraints from the config "
+          "and add each one individually. If the offending constraint was "
+          "packaged with WPILib, please file a bug report.");
+    }
+
+    state->minAcceleration = units::math::max(
+        state->minAcceleration,
+        reverse ? -minMaxAccel.maxAcceleration : minMaxAccel.minAcceleration);
+
+    state->maxAcceleration = units::math::min(
+        state->maxAcceleration,
+        reverse ? -minMaxAccel.minAcceleration : minMaxAccel.maxAcceleration);
+  }
+}

wpimath/src/main/native/cpp/trajectory/TrajectoryUtil.cpp

@@ -0,0 +1,58 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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/TrajectoryUtil.h"
+
+#include <system_error>
+
+#include <wpi/SmallString.h>
+#include <wpi/json.h>
+#include <wpi/raw_istream.h>
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+void TrajectoryUtil::ToPathweaverJson(const Trajectory& trajectory,
+                                      const wpi::Twine& path) {
+  std::error_code error_code;
+
+  wpi::SmallString<128> buf;
+  wpi::raw_fd_ostream output{path.toStringRef(buf), error_code};
+  if (error_code) {
+    throw std::runtime_error(("Cannot open file: " + path).str());
+  }
+
+  wpi::json json = trajectory.States();
+  output << json;
+  output.flush();
+}
+
+Trajectory TrajectoryUtil::FromPathweaverJson(const wpi::Twine& path) {
+  std::error_code error_code;
+
+  wpi::SmallString<128> buf;
+  wpi::raw_fd_istream input{path.toStringRef(buf), error_code};
+  if (error_code) {
+    throw std::runtime_error(("Cannot open file: " + path).str());
+  }
+
+  wpi::json json;
+  input >> json;
+
+  return Trajectory{json.get<std::vector<Trajectory::State>>()};
+}
+
+std::string TrajectoryUtil::SerializeTrajectory(const Trajectory& trajectory) {
+  wpi::json json = trajectory.States();
+  return json.dump();
+}
+
+Trajectory TrajectoryUtil::DeserializeTrajectory(
+    const wpi::StringRef json_str) {
+  wpi::json json = wpi::json::parse(json_str);
+  return Trajectory{json.get<std::vector<Trajectory::State>>()};
+}

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

@@ -0,0 +1,42 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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"
+
+#include "units/math.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, units::curvature_t curvature,
+    units::meters_per_second_t velocity) const {
+  // 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, units::curvature_t curvature,
+    units::meters_per_second_t speed) const {
+  // The acceleration of the robot has no impact on the centripetal acceleration
+  // of the robot.
+  return {};
+}

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

@@ -0,0 +1,31 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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, units::curvature_t curvature,
+    units::meters_per_second_t velocity) const {
+  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, units::curvature_t curvature,
+    units::meters_per_second_t speed) const {
+  return {};
+}

wpimath/src/main/native/cpp/trajectory/constraint/DifferentialDriveVoltageConstraint.cpp

@@ -0,0 +1,102 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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/DifferentialDriveVoltageConstraint.h"
+
+#include <algorithm>
+#include <limits>
+
+#include <wpi/MathExtras.h>
+
+#include "units/math.h"
+
+using namespace frc;
+
+DifferentialDriveVoltageConstraint::DifferentialDriveVoltageConstraint(
+    SimpleMotorFeedforward<units::meter> feedforward,
+    DifferentialDriveKinematics kinematics, units::volt_t maxVoltage)
+    : m_feedforward(feedforward),
+      m_kinematics(kinematics),
+      m_maxVoltage(maxVoltage) {}
+
+units::meters_per_second_t DifferentialDriveVoltageConstraint::MaxVelocity(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t velocity) const {
+  return units::meters_per_second_t(std::numeric_limits<double>::max());
+}
+
+TrajectoryConstraint::MinMax
+DifferentialDriveVoltageConstraint::MinMaxAcceleration(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t speed) const {
+  auto wheelSpeeds =
+      m_kinematics.ToWheelSpeeds({speed, 0_mps, speed * curvature});
+
+  auto maxWheelSpeed = std::max(wheelSpeeds.left, wheelSpeeds.right);
+  auto minWheelSpeed = std::min(wheelSpeeds.left, wheelSpeeds.right);
+
+  // Calculate maximum/minimum possible accelerations from motor dynamics
+  // and max/min wheel speeds
+  auto maxWheelAcceleration =
+      m_feedforward.MaxAchievableAcceleration(m_maxVoltage, maxWheelSpeed);
+  auto minWheelAcceleration =
+      m_feedforward.MinAchievableAcceleration(m_maxVoltage, minWheelSpeed);
+
+  // Robot chassis turning on radius = 1/|curvature|.  Outer wheel has radius
+  // increased by half of the trackwidth T.  Inner wheel has radius decreased
+  // by half of the trackwidth.  Achassis / radius = Aouter / (radius + T/2), so
+  // Achassis = Aouter * radius / (radius + T/2) = Aouter / (1 +
+  // |curvature|T/2). Inner wheel is similar.
+
+  // sgn(speed) term added to correctly account for which wheel is on
+  // outside of turn:
+  // If moving forward, max acceleration constraint corresponds to wheel on
+  // outside of turn If moving backward, max acceleration constraint corresponds
+  // to wheel on inside of turn
+
+  // When velocity is zero, then wheel velocities are uniformly zero (robot
+  // cannot be turning on its center) - we have to treat this as a special case,
+  // as it breaks the signum function.  Both max and min acceleration are
+  // *reduced in magnitude* in this case.
+
+  units::meters_per_second_squared_t maxChassisAcceleration;
+  units::meters_per_second_squared_t minChassisAcceleration;
+
+  if (speed == 0_mps) {
+    maxChassisAcceleration =
+        maxWheelAcceleration /
+        (1 + m_kinematics.trackWidth * units::math::abs(curvature) / (2_rad));
+    minChassisAcceleration =
+        minWheelAcceleration /
+        (1 + m_kinematics.trackWidth * units::math::abs(curvature) / (2_rad));
+  } else {
+    maxChassisAcceleration =
+        maxWheelAcceleration /
+        (1 + m_kinematics.trackWidth * units::math::abs(curvature) *
+                 wpi::sgn(speed) / (2_rad));
+    minChassisAcceleration =
+        minWheelAcceleration /
+        (1 - m_kinematics.trackWidth * units::math::abs(curvature) *
+                 wpi::sgn(speed) / (2_rad));
+  }
+
+  // When turning about a point inside of the wheelbase (i.e. radius less than
+  // half the trackwidth), the inner wheel's direction changes, but the
+  // magnitude remains the same.  The formula above changes sign for the inner
+  // wheel when this happens. We can accurately account for this by simply
+  // negating the inner wheel.
+
+  if ((m_kinematics.trackWidth / 2) > 1_rad / units::math::abs(curvature)) {
+    if (speed > 0_mps) {
+      minChassisAcceleration = -minChassisAcceleration;
+    } else if (speed < 0_mps) {
+      maxChassisAcceleration = -maxChassisAcceleration;
+    }
+  }
+
+  return {minChassisAcceleration, maxChassisAcceleration};
+}

wpimath/src/main/native/cpp/trajectory/constraint/EllipticalRegionConstraint.cpp

@@ -0,0 +1,56 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/EllipticalRegionConstraint.h"
+
+#include <limits>
+
+#include "units/math.h"
+
+using namespace frc;
+
+EllipticalRegionConstraint::EllipticalRegionConstraint(
+    const Translation2d& center, units::meter_t xWidth, units::meter_t yWidth,
+    const Rotation2d& rotation, const TrajectoryConstraint& constraint)
+    : m_center(center),
+      m_radii(xWidth / 2.0, yWidth / 2.0),
+      m_constraint(constraint) {
+  m_radii = m_radii.RotateBy(rotation);
+}
+
+units::meters_per_second_t EllipticalRegionConstraint::MaxVelocity(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t velocity) const {
+  if (IsPoseInRegion(pose)) {
+    return m_constraint.MaxVelocity(pose, curvature, velocity);
+  } else {
+    return units::meters_per_second_t(std::numeric_limits<double>::infinity());
+  }
+}
+
+TrajectoryConstraint::MinMax EllipticalRegionConstraint::MinMaxAcceleration(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t speed) const {
+  if (IsPoseInRegion(pose)) {
+    return m_constraint.MinMaxAcceleration(pose, curvature, speed);
+  } else {
+    return {};
+  }
+}
+
+bool EllipticalRegionConstraint::IsPoseInRegion(const Pose2d& pose) const {
+  // The region (disk) bounded by the ellipse is given by the equation:
+  // ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1
+  // If the inequality is satisfied, then it is inside the ellipse; otherwise
+  // it is outside the ellipse.
+  // Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons.
+  return units::math::pow<2>(pose.X() - m_center.X()) *
+                 units::math::pow<2>(m_radii.Y()) +
+             units::math::pow<2>(pose.Y() - m_center.Y()) *
+                 units::math::pow<2>(m_radii.X()) <=
+         units::math::pow<2>(m_radii.X()) * units::math::pow<2>(m_radii.Y());
+}

wpimath/src/main/native/cpp/trajectory/constraint/MecanumDriveKinematicsConstraint.cpp

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019-2020 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/MecanumDriveKinematicsConstraint.h"
+
+#include "units/math.h"
+
+using namespace frc;
+
+MecanumDriveKinematicsConstraint::MecanumDriveKinematicsConstraint(
+    MecanumDriveKinematics kinematics, units::meters_per_second_t maxSpeed)
+    : m_kinematics(kinematics), m_maxSpeed(maxSpeed) {}
+
+units::meters_per_second_t MecanumDriveKinematicsConstraint::MaxVelocity(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t velocity) const {
+  auto xVelocity = velocity * pose.Rotation().Cos();
+  auto yVelocity = velocity * pose.Rotation().Sin();
+  auto wheelSpeeds =
+      m_kinematics.ToWheelSpeeds({xVelocity, yVelocity, velocity * curvature});
+  wheelSpeeds.Normalize(m_maxSpeed);
+
+  auto normSpeeds = m_kinematics.ToChassisSpeeds(wheelSpeeds);
+
+  return units::math::hypot(normSpeeds.vx, normSpeeds.vy);
+}
+
+TrajectoryConstraint::MinMax
+MecanumDriveKinematicsConstraint::MinMaxAcceleration(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t speed) const {
+  return {};
+}

wpimath/src/main/native/cpp/trajectory/constraint/RectangularRegionConstraint.cpp

@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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/RectangularRegionConstraint.h"
+
+#include <limits>
+
+using namespace frc;
+
+RectangularRegionConstraint::RectangularRegionConstraint(
+    const Translation2d& bottomLeftPoint, const Translation2d& topRightPoint,
+    const TrajectoryConstraint& constraint)
+    : m_bottomLeftPoint(bottomLeftPoint),
+      m_topRightPoint(topRightPoint),
+      m_constraint(constraint) {}
+
+units::meters_per_second_t RectangularRegionConstraint::MaxVelocity(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t velocity) const {
+  if (IsPoseInRegion(pose)) {
+    return m_constraint.MaxVelocity(pose, curvature, velocity);
+  } else {
+    return units::meters_per_second_t(std::numeric_limits<double>::infinity());
+  }
+}
+
+TrajectoryConstraint::MinMax RectangularRegionConstraint::MinMaxAcceleration(
+    const Pose2d& pose, units::curvature_t curvature,
+    units::meters_per_second_t speed) const {
+  if (IsPoseInRegion(pose)) {
+    return m_constraint.MinMaxAcceleration(pose, curvature, speed);
+  } else {
+    return {};
+  }
+}
+
+bool RectangularRegionConstraint::IsPoseInRegion(const Pose2d& pose) const {
+  return pose.X() >= m_bottomLeftPoint.X() && pose.X() <= m_topRightPoint.X() &&
+         pose.Y() >= m_bottomLeftPoint.Y() && pose.Y() <= m_topRightPoint.Y();
+}