diff --git a/y2023/control_loops/superstructure/arm/BUILD b/y2023/control_loops/superstructure/arm/BUILD
index c125fac..0ef2cf9 100644
--- a/y2023/control_loops/superstructure/arm/BUILD
+++ b/y2023/control_loops/superstructure/arm/BUILD
@@ -78,3 +78,50 @@
         "//frc971/control_loops/double_jointed_arm:dynamics",
     ],
 )
+
+cc_library(
+    name = "trajectory",
+    srcs = ["trajectory.cc"],
+    hdrs = ["trajectory.h"],
+    target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        "//frc971/control_loops:binomial",
+        "//frc971/control_loops:dlqr",
+        "//frc971/control_loops:fixed_quadrature",
+        "//frc971/control_loops:hybrid_state_feedback_loop",
+        "//frc971/control_loops/double_jointed_arm:dynamics",
+        "//frc971/control_loops/double_jointed_arm:ekf",
+        "@org_tuxfamily_eigen//:eigen",
+    ],
+)
+
+cc_test(
+    name = "trajectory_test",
+    srcs = ["trajectory_test.cc"],
+    target_compatible_with = ["@platforms//os:linux"],
+    deps = [
+        ":arm_constants",
+        ":trajectory",
+        "//aos/testing:googletest",
+        "//y2023/control_loops/superstructure/roll:roll_plants",
+    ],
+)
+
+cc_binary(
+    name = "trajectory_plot",
+    srcs = [
+        "trajectory_plot.cc",
+    ],
+    target_compatible_with = ["@platforms//cpu:x86_64"],
+    deps = [
+        ":arm_constants",
+        ":trajectory",
+        "//frc971/analysis:in_process_plotter",
+        "//frc971/control_loops:binomial",
+        "//frc971/control_loops:fixed_quadrature",
+        "//frc971/control_loops/double_jointed_arm:ekf",
+        "//y2023/control_loops/superstructure/roll:roll_plants",
+        "@com_github_gflags_gflags//:gflags",
+        "@org_tuxfamily_eigen//:eigen",
+    ],
+)
diff --git a/y2023/control_loops/superstructure/arm/trajectory.cc b/y2023/control_loops/superstructure/arm/trajectory.cc
new file mode 100644
index 0000000..ad6720f
--- /dev/null
+++ b/y2023/control_loops/superstructure/arm/trajectory.cc
@@ -0,0 +1,955 @@
+#include "y2023/control_loops/superstructure/arm/trajectory.h"
+
+#include "Eigen/Dense"
+#include "aos/logging/logging.h"
+#include "frc971/control_loops/dlqr.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/jacobian.h"
+#include "frc971/control_loops/runge_kutta.h"
+#include "gflags/gflags.h"
+
+DEFINE_double(lqr_proximal_pos, 0.15, "Position LQR gain");
+DEFINE_double(lqr_proximal_vel, 4.0, "Velocity LQR gain");
+DEFINE_double(lqr_distal_pos, 0.20, "Position LQR gain");
+DEFINE_double(lqr_distal_vel, 4.0, "Velocity LQR gain");
+
+namespace y2023 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+::Eigen::Matrix<double, 3, 1> CosSpline::Theta(double alpha) const {
+  ::Eigen::Matrix<double, 3, 1> result;
+  result.block<2, 1>(0, 0) = spline_.Theta(alpha);
+
+  const std::pair<AlphaTheta, AlphaTheta> roll_points = RollPoints(alpha);
+  const double alpha_percent =
+      (alpha - roll_points.first.alpha) /
+      (roll_points.second.alpha - roll_points.first.alpha);
+
+  result(2) = (0.5 - 0.5 * std::cos(alpha_percent * M_PI)) *
+                  (roll_points.second.theta - roll_points.first.theta) +
+              roll_points.first.theta;
+  return result;
+}
+
+::Eigen::Matrix<double, 3, 1> CosSpline::Omega(double alpha) const {
+  ::Eigen::Matrix<double, 3, 1> result;
+  result.block<2, 1>(0, 0) = spline_.Omega(alpha);
+
+  const std::pair<AlphaTheta, AlphaTheta> roll_points = RollPoints(alpha);
+  const double dalpha = (roll_points.second.alpha - roll_points.first.alpha);
+  const double alpha_percent = (alpha - roll_points.first.alpha) / dalpha;
+
+  result(2) = 0.5 * std::sin(alpha_percent * M_PI) *
+              (roll_points.second.theta - roll_points.first.theta) * M_PI /
+              dalpha;
+  return result;
+}
+
+::Eigen::Matrix<double, 3, 1> CosSpline::Alpha(double alpha) const {
+  ::Eigen::Matrix<double, 3, 1> result;
+  result.block<2, 1>(0, 0) = spline_.Alpha(alpha);
+
+  const std::pair<AlphaTheta, AlphaTheta> roll_points = RollPoints(alpha);
+  const double dalpha = (roll_points.second.alpha - roll_points.first.alpha);
+  const double alpha_percent = (alpha - roll_points.first.alpha) / dalpha;
+
+  result(2) = 0.5 * std::cos(alpha_percent * M_PI) *
+              (roll_points.second.theta - roll_points.first.theta) * M_PI *
+              M_PI / dalpha / dalpha;
+  return result;
+}
+
+std::pair<CosSpline::AlphaTheta, CosSpline::AlphaTheta> CosSpline::RollPoints(
+    double alpha) const {
+  if (alpha <= 0.0) {
+    return std::make_pair(roll_[0], roll_[1]);
+  }
+  if (alpha >= 1.0) {
+    return std::make_pair(roll_[roll_.size() - 2], roll_[roll_.size() - 1]);
+  }
+
+  // Find the distance right below our number using a binary search.
+  size_t after = ::std::distance(
+      roll_.begin(), ::std::lower_bound(roll_.begin(), roll_.end(), alpha,
+                                        [](AlphaTheta at, double alpha) {
+                                          return at.alpha < alpha;
+                                        }));
+  DCHECK_GT(after, 0u);
+  size_t before = after - 1;
+  DCHECK_LT(before, roll_.size());
+  return std::make_pair(roll_[before], roll_[after]);
+}
+
+CosSpline CosSpline::Reversed() const {
+  std::vector<AlphaTheta> new_roll(roll_.size());
+  for (size_t i = 0; i < roll_.size(); ++i) {
+    new_roll[roll_.size() - 1 - i] = {1.0 - roll_[i].alpha, roll_[i].theta};
+  }
+
+  Eigen::Matrix<double, 2, 4> new_control_points;
+  new_control_points = spline_.control_points().rowwise().reverse();
+  return CosSpline(NSpline<4, 2>(new_control_points), std::move(new_roll));
+}
+
+::Eigen::Matrix<double, 3, 1> Path::Theta(double distance) const {
+  const double alpha = DistanceToAlpha(distance);
+  return spline_.Theta(alpha);
+}
+
+::Eigen::Matrix<double, 3, 1> Path::Omega(double distance) const {
+  const double alpha = DistanceToAlpha(distance);
+  return spline_.Omega(alpha).normalized();
+}
+
+::Eigen::Matrix<double, 3, 1> Path::Alpha(double distance) const {
+  const double alpha = DistanceToAlpha(distance);
+  const ::Eigen::Matrix<double, 3, 1> dspline_point = spline_.Omega(alpha);
+  const ::Eigen::Matrix<double, 3, 1> ddspline_point = spline_.Alpha(alpha);
+
+  const double squared_norm = dspline_point.squaredNorm();
+
+  return ddspline_point / squared_norm -
+         dspline_point * (dspline_point.transpose() * ddspline_point) /
+             ::std::pow(squared_norm, 2);
+}
+
+std::vector<float> Path::BuildDistances(size_t num_alpha) {
+  num_alpha = num_alpha ? num_alpha : 100;
+  std::vector<float> distances;
+  distances.push_back(0.0);
+
+  const double dalpha = 1.0 / static_cast<double>(num_alpha - 1);
+  double last_alpha = 0.0;
+  for (size_t i = 1; i < num_alpha; ++i) {
+    const double alpha = dalpha * i;
+    distances.push_back(
+        distances.back() +
+        GaussianQuadrature5(
+            [this](double alpha) { return this->spline_.Omega(alpha).norm(); },
+            last_alpha, alpha));
+    last_alpha = alpha;
+  }
+  return distances;
+}
+
+double Path::DistanceToAlpha(double distance) const {
+  if (distance <= 0.0) {
+    return 0.0;
+  }
+  if (distance >= length()) {
+    return 1.0;
+  }
+
+  // Find the distance right below our number using a binary search.
+  size_t after = ::std::distance(
+      distances().begin(),
+      ::std::lower_bound(distances().begin(), distances().end(), distance));
+  size_t before = after - 1;
+  const double distance_step_size =
+      (1.0 / static_cast<double>(distances().size() - 1));
+
+  const double alpha = (distance - distances()[before]) /
+                           (distances()[after] - distances()[before]) *
+                           distance_step_size +
+                       static_cast<double>(before) * distance_step_size;
+  CHECK_GT(alpha, 0.0);
+  CHECK_LT(alpha, 1.0);
+  return alpha;
+}
+
+Path Path::Reversed() const { return Path(spline_.Reversed()); }
+
+::std::unique_ptr<Path> Path::Reversed(::std::unique_ptr<Path> p) {
+  return ::std::make_unique<Path>(p->Reversed());
+}
+
+double Trajectory::MaxCurvatureSpeed(
+    double goal_distance, const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer,
+    double plan_vmax) {
+  // TODO(austin): We are looking up the index in the path 3 times here.
+  const ::Eigen::Matrix<double, 3, 1> theta = path().Theta(goal_distance);
+  const ::Eigen::Matrix<double, 3, 1> omega = path().Omega(goal_distance);
+  const ::Eigen::Matrix<double, 3, 1> alpha = path().Alpha(goal_distance);
+
+  const ::Eigen::Matrix<double, 4, 1> X =
+      (::Eigen::Matrix<double, 4, 1>() << theta(0, 0), 0.0, theta(1, 0), 0.0)
+          .finished();
+
+  ::Eigen::Matrix<double, 2, 2> K1;
+  ::Eigen::Matrix<double, 2, 2> K2;
+
+  const ::Eigen::Matrix<double, 2, 1> gravity_volts =
+      dynamics_->K3_inverse() * dynamics_->GravityTorque(X);
+
+  dynamics_->NormilizedMatriciesForState(X, &K1, &K2);
+  const ::Eigen::Matrix<double, 2, 2> omega_square =
+      (::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
+          .finished();
+
+  // Here, we can say that
+  //   d^2/dt^2 theta = d^2/dd^2 theta(d) * (d d/dt)^2
+  // Normalize so that the max accel is 1, and take magnitudes. This
+  // gives us the max velocity we can be at each point due to
+  // curvature.
+  double min_good_ddot =
+      ::std::sqrt(1.0 / ::std::max(0.001, (alpha_unitizer * alpha).norm()));
+
+  // Now, solve for the max speed we can follow the path without hitting the
+  // vmax limit.  This makes it such that we can always hold vmax and follow
+  // the path.  This helps when there are cases where holding constant speed
+  // into a reducing radius turn will saturate our voltage limit. Technically,
+  // there are cases where faster paths exist, but they are a lot harder to
+  // solve due to the nonlinearities.
+  const ::Eigen::Matrix<double, 2, 1> vk1 =
+      dynamics_->K3_inverse() * (K1 * alpha.block<2, 1>(0, 0) +
+                                 K2 * omega_square * omega.block<2, 1>(0, 0));
+  const ::Eigen::Matrix<double, 2, 1> vk2 =
+      dynamics_->K3_inverse() * dynamics_->K4() * omega.block<2, 1>(0, 0);
+
+  // Loop through all the various vmin, plan_vmax combinations.
+  for (const double c : {-plan_vmax, plan_vmax}) {
+    // Also loop through saturating theta0 and theta1
+    for (const ::std::tuple<double, double, double> &abgravity :
+         {::std::tuple<double, double, double>{vk1(0), vk2(0),
+                                               gravity_volts(0)},
+          ::std::tuple<double, double, double>{vk1(1), vk2(1),
+                                               gravity_volts(1)}}) {
+      const double a = ::std::get<0>(abgravity);
+      const double b = ::std::get<1>(abgravity);
+      const double gravity = ::std::get<2>(abgravity);
+      const double sqrt_number = b * b - 4.0 * a * (c - gravity);
+
+      // Throw out imaginary solutions to the quadratic.
+      if (sqrt_number > 0) {
+        const double sqrt_result = ::std::sqrt(sqrt_number);
+        const double ddot1 = (-b + sqrt_result) / (2.0 * a);
+        const double ddot2 = (-b - sqrt_result) / (2.0 * a);
+        // Loop through both solutions.
+        for (const double ddot : {ddot1, ddot2}) {
+          const ::Eigen::Matrix<double, 2, 1> U =
+              vk1 * ddot * ddot + vk2 * ddot - gravity_volts;
+
+          // Finally, make sure the velocity is positive and valid.
+          if ((U.array().abs() <= plan_vmax + 1e-6).all() && ddot > 0.0) {
+            min_good_ddot = ::std::min(min_good_ddot, ddot);
+          }
+        }
+      }
+    }
+  }
+
+  return min_good_ddot;
+}
+
+::std::vector<double> Trajectory::CurvatureOptimizationPass(
+    const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax) {
+  ::std::vector<double> max_dvelocity_unfiltered;
+  max_dvelocity_unfiltered.reserve(num_plan_points_);
+  for (size_t i = 0; i < num_plan_points_; ++i) {
+    const double distance = DistanceForIndex(i);
+
+    max_dvelocity_unfiltered.push_back(
+        MaxCurvatureSpeed(distance, alpha_unitizer, plan_vmax));
+  }
+  return max_dvelocity_unfiltered;
+}
+
+double Trajectory::FeasableForwardsAcceleration(
+    double goal_distance, double goal_velocity, double /*plan_vmax*/,
+    const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer) const {
+  const ::Eigen::Matrix<double, 3, 1> omega = path().Omega(goal_distance);
+  const ::Eigen::Matrix<double, 3, 1> alpha = path().Alpha(goal_distance);
+
+  return ::std::sqrt(
+             ::std::max(0.0, 1.0 - ::std::pow((alpha_unitizer * alpha).norm() *
+                                                  goal_velocity * goal_velocity,
+                                              2))) /
+         (alpha_unitizer * omega).norm();
+}
+
+double Trajectory::FeasableForwardsVoltage(
+    double goal_distance, double goal_velocity, double plan_vmax,
+    const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer) const {
+  // TODO(austin): We are looking up the index in the path 3 times here.
+  const ::Eigen::Matrix<double, 3, 1> theta = path().Theta(goal_distance);
+  const ::Eigen::Matrix<double, 3, 1> omega = path().Omega(goal_distance);
+  const ::Eigen::Matrix<double, 3, 1> alpha = path().Alpha(goal_distance);
+
+  const ::Eigen::Matrix<double, 4, 1> arm_X =
+      (::Eigen::Matrix<double, 4, 1>() << theta(0, 0), 0.0, theta(1, 0), 0.0)
+          .finished();
+
+  ::Eigen::Matrix<double, 2, 2> K1;
+  ::Eigen::Matrix<double, 2, 2> K2_normalized;
+
+  dynamics_->NormilizedMatriciesForState(arm_X, &K1, &K2_normalized);
+
+  const ::Eigen::Matrix<double, 2, 2> omega_square =
+      (::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
+          .finished();
+
+  // The derivitives of f(d) wrt t are:
+  // theta = f(d)
+  // dtheta/dt = d/dd f(d) * dd/dt
+  // d^2 theta/dt^2 = d^2/dd^2 f(d) * (dd/dt)^2 + d^2d/dt^2 * d/dd f(d)
+  //
+  // And, our arm dynamics are:
+  // K1 d^2 theta/dt^2 + K2_normalized * diag(dtheta/dt) * dtheta/dt = k3 * V -
+  // K4 * dtheta/dt
+  //
+  // And our roll dynamics are:
+  //
+  // d^2 theta2/dt^2 = A[1, 1] d theta2/dt + B[1, 0] * V
+  //
+  // Plug the derivitives in to our dynamics, solve for d^2d/dt^2, and ship it!
+  //
+  // We want things in the form V = k_constant + k_scalar * d^2d/dt^2
+
+  const ::Eigen::Matrix<double, 3, 1> k_constant =
+      (::Eigen::Matrix<double, 3, 1>()
+           << (dynamics_->K3_inverse() *
+               ((K1 * alpha.block<2, 1>(0, 0) +
+                 K2_normalized * omega_square * omega.block<2, 1>(0, 0)) *
+                    goal_velocity * goal_velocity +
+                dynamics_->K4() * omega.block<2, 1>(0, 0) * goal_velocity -
+                dynamics_->GravityTorque(arm_X))),
+       ((alpha(2, 0) * goal_velocity * goal_velocity -
+         roll_->coefficients().A_continuous(1, 1) * omega(2, 0) *
+             goal_velocity) /
+        roll_->coefficients().B_continuous(1, 0)))
+          .finished();
+
+  const ::Eigen::Matrix<double, 3, 1> k_scalar =
+      (::Eigen::Matrix<double, 3, 1>()
+           << dynamics_->K3_inverse() * K1 * omega.block<2, 1>(0, 0),
+       (omega(2, 0) / roll_->coefficients().B_continuous(1, 0)))
+          .finished();
+
+  const double constraint_goal_acceleration =
+      ::std::sqrt(
+          ::std::max(0.0, 1.0 - ::std::pow((alpha_unitizer * alpha).norm() *
+                                               goal_velocity * goal_velocity,
+                                           2))) /
+      (alpha_unitizer * omega).norm();
+
+  double min_goal_acceleration = ::std::numeric_limits<double>::infinity();
+  double max_goal_acceleration = -::std::numeric_limits<double>::infinity();
+  for (double c : {-plan_vmax, plan_vmax}) {
+    for (const ::std::pair<double, double> &ab :
+         {::std::pair<double, double>{k_constant(0, 0), k_scalar(0, 0)},
+          ::std::pair<double, double>{k_constant(1, 0), k_scalar(1, 0)},
+          ::std::pair<double, double>{k_constant(2, 0), k_scalar(2, 0)}}) {
+      const double a = ab.first;
+      const double b = ab.second;
+      const double voltage_accel = (c - a) / b;
+      const ::Eigen::Matrix<double, 3, 1> U =
+          k_constant + k_scalar * voltage_accel;
+
+      VLOG(1) << "Trying, U is " << U.transpose() << ", accel "
+              << voltage_accel;
+      if ((U.array().abs() <= plan_vmax + 1e-6).all()) {
+        min_goal_acceleration = std::min(voltage_accel, min_goal_acceleration);
+        max_goal_acceleration = std::max(voltage_accel, max_goal_acceleration);
+      }
+    }
+  }
+  if (min_goal_acceleration == ::std::numeric_limits<double>::infinity() ||
+      max_goal_acceleration == -::std::numeric_limits<double>::infinity()) {
+    // TODO(austin): The math above doesn't always give a valid solution.
+    // This happens when things get pretty ill-conditioned.  Figure out what to
+    // do about it.
+    VLOG(1)
+        << "Failed to find a valid accel at distance " << goal_distance
+        << ", voltage "
+        << (k_constant + k_scalar * constraint_goal_acceleration).transpose()
+        << ", accel " << constraint_goal_acceleration << " vs vmax "
+        << plan_vmax;
+    return constraint_goal_acceleration;
+  }
+
+  const double goal_acceleration =
+      std::abs(max_goal_acceleration - constraint_goal_acceleration) <
+              std::abs(min_goal_acceleration - constraint_goal_acceleration)
+          ? max_goal_acceleration
+          : min_goal_acceleration;
+
+  if (min_goal_acceleration < constraint_goal_acceleration &&
+      constraint_goal_acceleration < max_goal_acceleration) {
+    VLOG(1)
+        << "Solved valid accel at distance " << goal_distance << ", voltage "
+        << (k_constant + k_scalar * constraint_goal_acceleration).transpose()
+        << ", accel " << constraint_goal_acceleration
+        << ", overall accel limited, U limit is " << goal_acceleration
+        << " with U of "
+        << (k_constant + k_scalar * goal_acceleration).transpose();
+
+    if (!((k_constant + k_scalar * constraint_goal_acceleration)
+              .array()
+              .abs() <= plan_vmax + 1e-6)
+             .all()) {
+      LOG(FATAL) << "Accel in range, but constraint voltage out of range.";
+    }
+    return constraint_goal_acceleration;
+  }
+
+  VLOG(1) << "Solved valid accel at distance " << goal_distance << ", voltage "
+          << (k_constant + k_scalar * goal_acceleration).transpose()
+          << ", decel limits [" << min_goal_acceleration << ", "
+          << max_goal_acceleration << "], picked " << goal_acceleration
+          << " voltage limited, constraint accel "
+          << constraint_goal_acceleration << " constraint voltage "
+          << (k_constant + k_scalar * constraint_goal_acceleration).transpose();
+
+  return goal_acceleration;
+}
+
+double Trajectory::FeasableBackwardsAcceleration(
+    double goal_distance, double goal_velocity, double /*plan_vmax*/,
+    const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer) const {
+  const ::Eigen::Matrix<double, 3, 1> omega = path().Omega(goal_distance);
+  const ::Eigen::Matrix<double, 3, 1> alpha = path().Alpha(goal_distance);
+
+  return ::std::sqrt(
+             ::std::max(0.0, 1.0 - ::std::pow(((alpha_unitizer * alpha).norm() *
+                                               goal_velocity * goal_velocity),
+                                              2))) /
+         (alpha_unitizer * omega).norm();
+}
+
+double Trajectory::FeasableBackwardsVoltage(
+    double goal_distance, double goal_velocity, double plan_vmax,
+    const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer) const {
+  const ::Eigen::Matrix<double, 3, 1> theta = path().Theta(goal_distance);
+  const ::Eigen::Matrix<double, 3, 1> omega = path().Omega(goal_distance);
+  const ::Eigen::Matrix<double, 3, 1> alpha = path().Alpha(goal_distance);
+  const ::Eigen::Matrix<double, 4, 1> arm_X =
+      (::Eigen::Matrix<double, 4, 1>() << theta(0, 0), 0.0, theta(1, 0), 0.0)
+          .finished();
+  ::Eigen::Matrix<double, 2, 2> K1;
+  ::Eigen::Matrix<double, 2, 2> K2_normalized;
+
+  dynamics_->NormilizedMatriciesForState(arm_X, &K1, &K2_normalized);
+
+  const ::Eigen::Matrix<double, 2, 2> omega_square =
+      (::Eigen::Matrix<double, 2, 2>() << omega(0, 0), 0.0, 0.0, omega(1, 0))
+          .finished();
+
+  const ::Eigen::Matrix<double, 3, 1> k_constant =
+      (::Eigen::Matrix<double, 3, 1>()
+           << dynamics_->K3_inverse() *
+                  ((K1 * alpha.block<2, 1>(0, 0) +
+                    K2_normalized * omega_square * omega.block<2, 1>(0, 0)) *
+                       goal_velocity * goal_velocity +
+                   dynamics_->K4() * omega.block<2, 1>(0, 0) * goal_velocity -
+                   dynamics_->GravityTorque(arm_X)),
+       ((alpha(2, 0) * goal_velocity * goal_velocity -
+         roll_->coefficients().A_continuous(1, 1) * omega(2, 0) *
+             goal_velocity) /
+        roll_->coefficients().B_continuous(1, 0)))
+          .finished();
+  const ::Eigen::Matrix<double, 3, 1> k_scalar =
+      (::Eigen::Matrix<double, 3, 1>()
+           << dynamics_->K3_inverse() * K1 * omega.block<2, 1>(0, 0),
+       (omega(2, 0) / roll_->coefficients().B_continuous(1, 0)))
+          .finished();
+
+  const double constraint_goal_acceleration =
+      ::std::sqrt(
+          ::std::max(0.0, 1.0 - ::std::pow(((alpha_unitizer * alpha).norm() *
+                                            goal_velocity * goal_velocity),
+                                           2))) /
+      (alpha_unitizer * omega).norm();
+
+  double min_goal_acceleration = ::std::numeric_limits<double>::infinity();
+  double max_goal_acceleration = -::std::numeric_limits<double>::infinity();
+  for (double c : {-plan_vmax, plan_vmax}) {
+    for (const ::std::pair<double, double> &ab :
+         {::std::pair<double, double>{k_constant(0, 0), k_scalar(0, 0)},
+          ::std::pair<double, double>{k_constant(1, 0), k_scalar(1, 0)},
+          ::std::pair<double, double>{k_constant(2, 0), k_scalar(2, 0)}}) {
+      const double a = ab.first;
+      const double b = ab.second;
+      // This time, we are doing the other pass.  So, find all the
+      // decelerations (and flip them) to find the prior velocity.
+      const double voltage_accel = (c - a) / b;
+
+      const ::Eigen::Matrix<double, 3, 1> U =
+          k_constant + k_scalar * voltage_accel;
+
+      // TODO(austin): This doesn't always give a valid solution.  It really
+      // should.  Figure out why.
+      VLOG(1) << "Trying, U is " << U.transpose() << ", accel "
+              << voltage_accel;
+      if ((U.array().abs() <= plan_vmax + 1e-6).all()) {
+        min_goal_acceleration = std::min(-voltage_accel, min_goal_acceleration);
+        max_goal_acceleration = std::max(-voltage_accel, max_goal_acceleration);
+      }
+    }
+  }
+
+  if (min_goal_acceleration == ::std::numeric_limits<double>::infinity() ||
+      max_goal_acceleration == -::std::numeric_limits<double>::infinity()) {
+    VLOG(1)
+        << "Failed to find a valid decel at distance " << goal_distance
+        << ", voltage "
+        << (k_constant + k_scalar * constraint_goal_acceleration).transpose()
+        << ", accel " << constraint_goal_acceleration << "  vs vmax "
+        << plan_vmax;
+    return constraint_goal_acceleration;
+  }
+
+  if (min_goal_acceleration < constraint_goal_acceleration &&
+      constraint_goal_acceleration < max_goal_acceleration) {
+    VLOG(1)
+        << "Solved valid decel at distance " << goal_distance << ", voltage "
+        << (k_constant - k_scalar * constraint_goal_acceleration).transpose()
+        << ", decel " << min_goal_acceleration
+        << " <= " << constraint_goal_acceleration
+        << " <= " << max_goal_acceleration << ", accel limited";
+
+    if (!((k_constant - k_scalar * constraint_goal_acceleration)
+              .array()
+              .abs() <= plan_vmax + 1e-6)
+             .all()) {
+      LOG(FATAL) << "Accel in range, but constraint voltage out of range.";
+    }
+
+    return constraint_goal_acceleration;
+  }
+
+  const double goal_acceleration =
+      std::abs(max_goal_acceleration - constraint_goal_acceleration) <
+              std::abs(min_goal_acceleration - constraint_goal_acceleration)
+          ? max_goal_acceleration
+          : min_goal_acceleration;
+
+  VLOG(1) << "Solved valid decel at distance " << goal_distance << ", voltage "
+          << (k_constant - k_scalar * goal_acceleration).transpose()
+          << ", decel limits [" << min_goal_acceleration << ", "
+          << max_goal_acceleration << "], picked " << goal_acceleration
+          << " voltage limited, constraint accel "
+          << constraint_goal_acceleration << " constraint voltage "
+          << (k_constant - k_scalar * constraint_goal_acceleration).transpose();
+
+  return goal_acceleration;
+}
+
+template <typename F>
+double IntegrateAccelForDistance(const F &fn, double v, double x, double dx) {
+  // Use a trick from
+  // https://www.johndcook.com/blog/2012/02/21/care-and-treatment-of-singularities/
+  const double a0 = fn(x, v);
+
+  return (frc971::control_loops::RungeKuttaSteps(
+              [&fn, &a0](double t, double y) {
+                // Since we know that a0 == a(0) and that they are asymtotically
+                // the same at 0, we know that the limit is 0 at 0.  This is
+                // true because when starting from a stop, under sane
+                // accelerations, we can assume that we will start with a
+                // constant acceleration.  So, hard-code it.
+                if (std::abs(y) < 1e-6) {
+                  return 0.0;
+                }
+                return (fn(t, y) - a0) / y;
+              },
+              v, x, dx, 10) -
+          v) +
+         std::sqrt(2.0 * a0 * dx + v * v);
+}
+
+::std::vector<double> Trajectory::BackwardsOptimizationAccelerationPass(
+    const ::std::vector<double> &max_dvelocity,
+    const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax) {
+  ::std::vector<double> max_dvelocity_back_pass = max_dvelocity;
+
+  // Now, iterate over the list of velocities and constrain the acceleration.
+  for (int i = num_plan_points_ - 2; i >= 0; --i) {
+    const double previous_velocity = max_dvelocity_back_pass[i + 1];
+    const double previous_distance = DistanceForIndex(i + 1);
+
+    // Now, integrate with respect to distance (not time like normal).
+    const double integrated_velocity = IntegrateAccelForDistance(
+        [&](double x, double v) {
+          return FeasableBackwardsAcceleration(x, v, plan_vmax, alpha_unitizer);
+        },
+        previous_velocity, previous_distance, step_size_);
+    max_dvelocity_back_pass[i] =
+        ::std::min(integrated_velocity, max_dvelocity_back_pass[i]);
+  }
+
+  return max_dvelocity_back_pass;
+}
+
+::std::vector<double> Trajectory::BackwardsOptimizationVoltagePass(
+    const ::std::vector<double> &max_dvelocity,
+    const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax) {
+  ::std::vector<double> max_dvelocity_back_pass = max_dvelocity;
+
+  // Now, iterate over the list of velocities and constrain the acceleration.
+  for (int i = num_plan_points_ - 2; i >= 0; --i) {
+    const double previous_velocity = max_dvelocity_back_pass[i + 1];
+    const double previous_distance = DistanceForIndex(i + 1);
+
+    // Now, integrate with respect to distance (not time like normal).
+    const double integrated_velocity = IntegrateAccelForDistance(
+        [&](double x, double v) {
+          return FeasableBackwardsVoltage(x, v, plan_vmax, alpha_unitizer);
+        },
+        previous_velocity, previous_distance, step_size_);
+    max_dvelocity_back_pass[i] =
+        ::std::min(integrated_velocity, max_dvelocity_back_pass[i]);
+  }
+
+  return max_dvelocity_back_pass;
+}
+
+::std::vector<double> Trajectory::ForwardsOptimizationAccelerationPass(
+    const ::std::vector<double> &max_dvelocity,
+    const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax) {
+  ::std::vector<double> max_dvelocity_forwards_pass = max_dvelocity;
+  // Now, iterate over the list of velocities and constrain the acceleration.
+  for (size_t i = 1; i < num_plan_points_; ++i) {
+    const double previous_velocity = max_dvelocity_forwards_pass[i - 1];
+    const double previous_distance = DistanceForIndex(i - 1);
+
+    // Now, integrate with respect to distance (not time like normal).
+    const double integrated_velocity = IntegrateAccelForDistance(
+        [&](double x, double v) {
+          return FeasableForwardsAcceleration(x, v, plan_vmax, alpha_unitizer);
+        },
+        previous_velocity, previous_distance, step_size_);
+
+    max_dvelocity_forwards_pass[i] =
+        ::std::min(integrated_velocity, max_dvelocity_forwards_pass[i]);
+  }
+
+  return max_dvelocity_forwards_pass;
+}
+
+::std::vector<double> Trajectory::ForwardsOptimizationVoltagePass(
+    const ::std::vector<double> &max_dvelocity,
+    const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax) {
+  ::std::vector<double> max_dvelocity_forwards_pass = max_dvelocity;
+  // Now, iterate over the list of velocities and constrain the acceleration.
+  for (size_t i = 1; i < num_plan_points_; ++i) {
+    const double previous_velocity = max_dvelocity_forwards_pass[i - 1];
+    const double previous_distance = DistanceForIndex(i - 1);
+
+    // Now, integrate with respect to distance (not time like normal).
+    const double integrated_velocity = IntegrateAccelForDistance(
+        [&](double x, double v) {
+          return FeasableForwardsVoltage(x, v, plan_vmax, alpha_unitizer);
+        },
+        previous_velocity, previous_distance, step_size_);
+
+    max_dvelocity_forwards_pass[i] =
+        ::std::min(integrated_velocity, max_dvelocity_forwards_pass[i]);
+  }
+
+  return max_dvelocity_forwards_pass;
+}
+
+void TrajectoryFollower::Reset() {
+  next_goal_ = last_goal_ = goal_ = ::Eigen::Matrix<double, 2, 1>::Zero();
+  U_unsaturated_ = U_ff_ = U_ = ::Eigen::Matrix<double, 3, 1>::Zero();
+  goal_acceleration_ = 0.0;
+  saturation_fraction_along_path_ = 0.0;
+  omega_.setZero();
+  if (trajectory_ != nullptr) {
+    set_theta(trajectory_->ThetaT(goal_(0)));
+  }
+}
+
+::Eigen::Matrix<double, 2, 6> TrajectoryFollower::ArmK_at_state(
+    const Eigen::Ref<const ::Eigen::Matrix<double, 6, 1>> arm_X,
+    const Eigen::Ref<const ::Eigen::Matrix<double, 2, 1>> arm_U) {
+  const double kProximalPos = FLAGS_lqr_proximal_pos;
+  const double kProximalVel = FLAGS_lqr_proximal_vel;
+  const double kDistalPos = FLAGS_lqr_distal_pos;
+  const double kDistalVel = FLAGS_lqr_distal_vel;
+  const ::Eigen::DiagonalMatrix<double, 4> Q =
+      (::Eigen::DiagonalMatrix<double, 4>().diagonal()
+           << 1.0 / ::std::pow(kProximalPos, 2),
+       1.0 / ::std::pow(kProximalVel, 2), 1.0 / ::std::pow(kDistalPos, 2),
+       1.0 / ::std::pow(kDistalVel, 2))
+          .finished()
+          .asDiagonal();
+
+  const ::Eigen::DiagonalMatrix<double, 2> R =
+      (::Eigen::DiagonalMatrix<double, 2>().diagonal()
+           << 1.0 / ::std::pow(12.0, 2),
+       1.0 / ::std::pow(12.0, 2))
+          .finished()
+          .asDiagonal();
+
+  const auto x_blocked = arm_X.block<4, 1>(0, 0);
+
+  const ::Eigen::Matrix<double, 4, 4> final_A =
+      ::frc971::control_loops::NumericalJacobianX<4, 2>(
+          [this](const auto &x_blocked, const auto &U, double dt) {
+            return this->dynamics_->UnboundedDiscreteDynamics(x_blocked, U, dt);
+          },
+          x_blocked, arm_U, 0.00505);
+
+  const ::Eigen::Matrix<double, 4, 2> final_B =
+      ::frc971::control_loops::NumericalJacobianU<4, 2>(
+          [this](const auto &x_blocked, const auto &U, double dt) {
+            return this->dynamics_->UnboundedDiscreteDynamics(x_blocked, U, dt);
+          },
+          x_blocked, arm_U, 0.00505);
+
+  ::Eigen::Matrix<double, 4, 4> S;
+  ::Eigen::Matrix<double, 2, 4> sub_K;
+  if (::frc971::controls::dlqr<4, 2>(final_A, final_B, Q, R, &sub_K, &S) == 0) {
+    ::Eigen::EigenSolver<::Eigen::Matrix<double, 4, 4>> eigensolver(
+        final_A - final_B * sub_K);
+
+    ::Eigen::Matrix<double, 2, 6> K;
+    K.setZero();
+    K.block<2, 4>(0, 0) = sub_K;
+    K(0, 4) = 1.0;
+    K(1, 5) = 1.0;
+    failed_solutions_ = 0;
+    last_K_ = K;
+  } else {
+    ++failed_solutions_;
+  }
+  return last_K_;
+}
+
+void TrajectoryFollower::USaturationSearch(
+    double goal_distance, double last_goal_distance, double goal_velocity,
+    double last_goal_velocity, double saturation_fraction_along_path,
+    const ::Eigen::Matrix<double, 2, 6> &arm_K,
+    const ::Eigen::Matrix<double, 9, 1> &X, const Trajectory &trajectory,
+    ::Eigen::Matrix<double, 3, 1> *U, double *saturation_goal_velocity,
+    double *saturation_goal_acceleration) {
+  double saturation_goal_distance =
+      ((goal_distance - last_goal_distance) * saturation_fraction_along_path +
+       last_goal_distance);
+
+  const ::Eigen::Matrix<double, 3, 1> theta_t =
+      trajectory.ThetaT(saturation_goal_distance);
+  *saturation_goal_velocity = trajectory.InterpolateVelocity(
+      saturation_goal_distance, last_goal_distance, goal_distance,
+      last_goal_velocity, goal_velocity);
+  *saturation_goal_acceleration = trajectory.InterpolateAcceleration(
+      last_goal_distance, goal_distance, last_goal_velocity, goal_velocity);
+  const ::Eigen::Matrix<double, 3, 1> omega_t =
+      trajectory.OmegaT(saturation_goal_distance, *saturation_goal_velocity);
+  const ::Eigen::Matrix<double, 3, 1> alpha_t =
+      trajectory.AlphaT(saturation_goal_distance, *saturation_goal_velocity,
+                        *saturation_goal_acceleration);
+  const ::Eigen::Matrix<double, 9, 1> R = trajectory.R(theta_t, omega_t);
+
+  const ::Eigen::Matrix<double, 3, 1> U_ff = ComputeFF_U(R, omega_t, alpha_t);
+
+  *U = U_ff;
+  U->block<2, 1>(0, 0) += arm_K * (R.block<6, 1>(0, 0) - X.block<6, 1>(0, 0));
+  U->block<1, 1>(2, 0) +=
+      roll_->controller().K() * (R.block<3, 1>(6, 0) - X.block<3, 1>(6, 0));
+}
+
+::Eigen::Matrix<double, 2, 1> TrajectoryFollower::PlanNextGoal(
+    const ::Eigen::Matrix<double, 2, 1> &goal, double plan_vmax, double dt) {
+  // Figure out where we would be if we were to accelerate as fast as
+  // our constraints allow.
+  ::Eigen::Matrix<double, 2, 1> next_goal = ::frc971::control_loops::RungeKutta(
+      [this, &plan_vmax](const ::Eigen::Matrix<double, 2, 1> &X) {
+        return (::Eigen::Matrix<double, 2, 1>() << X(1),
+                trajectory_->FeasableForwardsVoltage(
+                    X(0), X(1), plan_vmax, trajectory_->alpha_unitizer()))
+            .finished();
+      },
+      goal, dt);
+
+  // Min that with the backwards pass velocity in case the backwards pass
+  // wants us to slow down.
+  const double next_trajectory_velocity =
+      trajectory_->GetDVelocity(next_goal(0));
+  if (next_trajectory_velocity < next_goal(1)) {
+    next_goal(1) = next_trajectory_velocity;
+    // And then recompute how far to go with our new trajectory in mind.
+    double goal_acceleration = trajectory_->InterpolateAcceleration(
+        goal(0), next_goal(0), goal(1), next_goal(1));
+    next_goal(0) = (goal(0) + goal(1) * dt + 0.5 * dt * dt * goal_acceleration);
+    next_goal(1) = trajectory_->GetDVelocity(next_goal(0));
+  }
+  return next_goal;
+}
+
+Eigen::Matrix<double, 3, 1> TrajectoryFollower::ComputeFF_U(
+    const Eigen::Matrix<double, 9, 1> &X,
+    const Eigen::Matrix<double, 3, 1> &omega_t,
+    const Eigen::Matrix<double, 3, 1> &alpha_t) const {
+  Eigen::Matrix<double, 3, 1> result;
+  result.block<2, 1>(0, 0) =
+      dynamics_->FF_U(X.block<4, 1>(0, 0), omega_t.block<2, 1>(0, 0),
+                      alpha_t.block<2, 1>(0, 0));
+  result(2, 0) =
+      (alpha_t(2, 0) -
+       roll_->plant().coefficients().A_continuous(1, 1) * omega_t(2, 0)) /
+      roll_->plant().coefficients().B_continuous(1, 0);
+
+  return result;
+}
+
+void TrajectoryFollower::Update(const ::Eigen::Matrix<double, 9, 1> &X,
+                                bool disabled, double dt, double plan_vmax,
+                                double voltage_limit) {
+  // TODO(austin): Separate voltage limit for shoulder for no path.
+  last_goal_ = goal_;
+
+  if (!has_path()) {
+    // No path, so go to the last theta (no velocity).
+    last_goal_.setZero();
+    next_goal_.setZero();
+    goal_.setZero();
+    goal_acceleration_ = 0.0;
+    saturation_fraction_along_path_ = 0.0;
+
+    if (disabled) {
+      U_ff_.setZero();
+      U_.setZero();
+      U_unsaturated_.setZero();
+    } else {
+      const ::Eigen::Matrix<double, 9, 1> R =
+          Trajectory::R(theta_, ::Eigen::Matrix<double, 3, 1>::Zero());
+
+      U_ff_ = ComputeFF_U(X, ::Eigen::Matrix<double, 3, 1>::Zero(),
+                          ::Eigen::Matrix<double, 3, 1>::Zero());
+
+      const ::Eigen::Matrix<double, 2, 6> arm_K =
+          ArmK_at_state(X.block<6, 1>(0, 0), U_ff_.block<2, 1>(0, 0));
+
+      U_unsaturated_.block<2, 1>(0, 0) =
+          U_ff_.block<2, 1>(0, 0) +
+          arm_K * (R.block<6, 1>(0, 0) - X.block<6, 1>(0, 0));
+      U_unsaturated_(2, 0) =
+          U_ff_(2, 0) +
+          roll_->controller().K() * (R.block<3, 1>(6, 0) - X.block<3, 1>(6, 0));
+
+      U_ = U_unsaturated_;
+
+      U_ = U_.array().max(-voltage_limit).min(voltage_limit);
+    }
+    return;
+  }
+
+  if (disabled) {
+    // If we are disabled, it's likely for a bit of time.  So, lets freeze
+    // ourselves on the path (accept the previous motion, but then zero out the
+    // velocity).  Set all outputs to 0 as well.
+    next_goal_(1) = 0.0;
+    goal_ = next_goal_;
+    goal_acceleration_ = 0.0;
+    U_unsaturated_.setZero();
+    U_.setZero();
+    U_ff_.setZero();
+    saturation_fraction_along_path_ = 1.0;
+    theta_ = trajectory_->ThetaT(goal_(0));
+    omega_.setZero();
+    return;
+  }
+
+  // To avoid exposing the new goals before the outer code has a chance to
+  // querry the internal state, move to the new goals here.
+  goal_ = next_goal_;
+
+  if (::std::abs(goal_(0) - path()->length()) < 1e-2) {
+    // If we go backwards along the path near the goal, snap us to the end
+    // point or we'll never actually finish.
+    if (goal_acceleration_ * dt + goal_(1) < 0.0 ||
+        goal_(0) > path()->length()) {
+      goal_(0) = path()->length();
+      goal_(1) = 0.0;
+    }
+  }
+
+  if (goal_(0) == path()->length()) {
+    next_goal_(0) = goal_(0);
+    next_goal_(1) = 0.0;
+    goal_acceleration_ = 0.0;
+  } else {
+    // Figure out where we would be if we were to accelerate as fast as
+    // our constraints allow.
+    next_goal_ = PlanNextGoal(goal_, plan_vmax, dt);
+
+    goal_acceleration_ = trajectory_->InterpolateAcceleration(
+        goal_(0), next_goal_(0), goal_(1), next_goal_(1));
+  }
+
+  const ::Eigen::Matrix<double, 3, 1> theta_t = trajectory_->ThetaT(goal_(0));
+  const ::Eigen::Matrix<double, 3, 1> omega_t =
+      trajectory_->OmegaT(goal_(0), goal_(1));
+  const ::Eigen::Matrix<double, 3, 1> alpha_t =
+      trajectory_->AlphaT(goal_(0), goal_(1), goal_acceleration_);
+
+  const ::Eigen::Matrix<double, 9, 1> R = Trajectory::R(theta_t, omega_t);
+
+  U_ff_ = ComputeFF_U(R, omega_t, alpha_t);
+
+  const ::Eigen::Matrix<double, 2, 6> arm_K =
+      ArmK_at_state(X.block<6, 1>(0, 0), U_ff_.block<2, 1>(0, 0));
+  U_unsaturated_ = U_ff_;
+  U_unsaturated_.block<2, 1>(0, 0) +=
+      arm_K * (R.block<6, 1>(0, 0) - X.block<6, 1>(0, 0));
+  U_unsaturated_.block<1, 1>(2, 0) +=
+      roll_->controller().K() * (R.block<3, 1>(6, 0) - X.block<3, 1>(6, 0));
+  U_ = U_unsaturated_;
+
+  // Ok, now we know if we are staturated or not.  If we are, time to search
+  // between here and our previous goal either until we find a state where we
+  // aren't saturated, or we are really close to our starting point.
+  saturation_fraction_along_path_ = 1.0;
+  if ((U_.array().abs() > voltage_limit).any()) {
+    // Saturated.  Let's do a binary search.
+    double step_size;
+    if ((goal_(0) - last_goal_(0)) < 1e-8) {
+      // print "Not bothering to move"
+      // Avoid the divide by 0 when interpolating.  Just don't move since we
+      // are saturated.
+      saturation_fraction_along_path_ = 0.0;
+      step_size = 0.0;
+    } else {
+      saturation_fraction_along_path_ = 0.5;
+      step_size = 0.5;
+    }
+
+    // Pull us back to the previous point until we aren't saturated anymore.
+    double saturation_goal_velocity;
+    double saturation_goal_acceleration;
+    while (step_size > 0.01) {
+      USaturationSearch(goal_(0), last_goal_(0), goal_(1), last_goal_(1),
+                        saturation_fraction_along_path_, arm_K, X, *trajectory_,
+                        &U_, &saturation_goal_velocity,
+                        &saturation_goal_acceleration);
+      step_size = step_size * 0.5;
+      if ((U_.array().abs() > voltage_limit).any()) {
+        saturation_fraction_along_path_ -= step_size;
+      } else {
+        saturation_fraction_along_path_ += step_size;
+      }
+    }
+
+    goal_(0) = ((goal_(0) - last_goal_(0)) * saturation_fraction_along_path_ +
+                last_goal_(0));
+    goal_(1) = saturation_goal_velocity;
+
+    next_goal_ = PlanNextGoal(goal_, plan_vmax, dt);
+
+    goal_acceleration_ = trajectory_->InterpolateAcceleration(
+        goal_(0), next_goal_(0), goal_(1), next_goal_(1));
+
+    U_ = U_.array().max(-voltage_limit).min(voltage_limit);
+  }
+  theta_ = trajectory_->ThetaT(goal_(0));
+  omega_ = trajectory_->OmegaT(goal_(0), goal_(1));
+}
+
+}  // namespace arm
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2023
diff --git a/y2023/control_loops/superstructure/arm/trajectory.h b/y2023/control_loops/superstructure/arm/trajectory.h
new file mode 100644
index 0000000..17ec3aa
--- /dev/null
+++ b/y2023/control_loops/superstructure/arm/trajectory.h
@@ -0,0 +1,614 @@
+#ifndef Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
+#define Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
+
+#include <array>
+#include <initializer_list>
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include "Eigen/Dense"
+#include "frc971/control_loops/binomial.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/fixed_quadrature.h"
+#include "frc971/control_loops/hybrid_state_feedback_loop.h"
+#include "frc971/control_loops/state_feedback_loop.h"
+
+namespace y2023 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+
+using frc971::control_loops::Binomial;
+using frc971::control_loops::GaussianQuadrature5;
+
+template <int N, int M>
+class NSpline {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  // Computes the characteristic matrix of a given spline order. This is an
+  // upper triangular matrix rather than lower because our splines are
+  // represented as rows rather than columns.
+  // Each row represents the impact of each point with increasing powers of
+  // alpha. Row i, column j contains the effect point i has with the j'th power
+  // of alpha.
+  static ::Eigen::Matrix<double, N, N> SplineMatrix() {
+    ::Eigen::Matrix<double, N, N> matrix =
+        ::Eigen::Matrix<double, N, N>::Zero();
+
+    for (int i = 0; i < N; ++i) {
+      // Binomial(N - 1, i) * (1 - t) ^ (N - i - 1) * (t ^ i) * P[i]
+      const double binomial = Binomial(N - 1, i);
+      const int one_minus_t_power = N - i - 1;
+
+      // Then iterate over the powers of t and add the pieces to the matrix.
+      for (int j = i; j < N; ++j) {
+        // j is the power of t we are placing in the matrix.
+        // k is the power of t in the (1 - t) expression that we need to
+        // evaluate.
+        const int k = j - i;
+        const double tscalar =
+            binomial * Binomial(one_minus_t_power, k) * ::std::pow(-1.0, k);
+        matrix(i, j) = tscalar;
+      }
+    }
+    return matrix;
+  }
+
+  // Computes the matrix to multiply by [1, a, a^2, ...] to evaluate the spline.
+  template <int D>
+  static ::Eigen::Matrix<double, M, N - D> SplinePolynomial(
+      const ::Eigen::Matrix<double, M, N> &control_points) {
+    // We use rows for the spline, so the multiplication looks "backwards"
+    ::Eigen::Matrix<double, M, N> polynomial = control_points * SplineMatrix();
+
+    // Now, compute the derivative requested.
+    for (int i = D; i < N; ++i) {
+      // Start with t^i, and multiply i, i-1, i-2 until you have done this
+      // Derivative times.
+      double scalar = 1.0;
+      for (int j = i; j > i - D; --j) {
+        scalar *= j;
+      }
+      polynomial.template block<M, 1>(0, i) =
+          polynomial.template block<M, 1>(0, i) * scalar;
+    }
+    return polynomial.template block<M, N - D>(0, D);
+  }
+
+  // Computes an order K-1 polynomial matrix in alpha.  [1, alpha, alpha^2, ...]
+  template <int K>
+  static ::Eigen::Matrix<double, K, 1> AlphaPolynomial(double alpha) {
+    alpha = std::min(1.0, std::max(0.0, alpha));
+    ::Eigen::Matrix<double, K, 1> polynomial =
+        ::Eigen::Matrix<double, K, 1>::Zero();
+    polynomial(0) = 1.0;
+    for (int i = 1; i < K; ++i) {
+      polynomial(i) = polynomial(i - 1) * alpha;
+    }
+    return polynomial;
+  }
+
+  // Constructs a spline.  control_points is a matrix of start, control1,
+  // control2, ..., end.
+  NSpline(::Eigen::Matrix<double, M, N> control_points)
+      : control_points_(control_points),
+        spline_polynomial_(SplinePolynomial<0>(control_points_)),
+        dspline_polynomial_(SplinePolynomial<1>(control_points_)),
+        ddspline_polynomial_(SplinePolynomial<2>(control_points_)) {}
+
+  // Returns the xy coordiate of the spline for a given alpha.
+  ::Eigen::Matrix<double, M, 1> Theta(double alpha) const {
+    return spline_polynomial_ * AlphaPolynomial<N>(alpha);
+  }
+
+  // Returns the dspline/dalpha for a given alpha.
+  ::Eigen::Matrix<double, M, 1> Omega(double alpha) const {
+    return dspline_polynomial_ * AlphaPolynomial<N - 1>(alpha);
+  }
+
+  // Returns the d^2spline/dalpha^2 for a given alpha.
+  ::Eigen::Matrix<double, M, 1> Alpha(double alpha) const {
+    return ddspline_polynomial_ * AlphaPolynomial<N - 2>(alpha);
+  }
+
+  const ::Eigen::Matrix<double, M, N> &control_points() const {
+    return control_points_;
+  }
+
+ private:
+  const ::Eigen::Matrix<double, M, N> control_points_;
+
+  // Each of these polynomials gets multiplied by [x^(n-1), x^(n-2), ..., x, 1]
+  // depending on the size of the polynomial.
+  const ::Eigen::Matrix<double, M, N> spline_polynomial_;
+  const ::Eigen::Matrix<double, M, N - 1> dspline_polynomial_;
+  const ::Eigen::Matrix<double, M, N - 2> ddspline_polynomial_;
+};
+
+// Add a cos wave to the bottom of the 2d spline to handle the roll.
+class CosSpline {
+ public:
+  // Struct defining pairs of alphas and thetas.
+  struct AlphaTheta {
+    double alpha;
+    double theta;
+  };
+
+  CosSpline(NSpline<4, 2> spline, std::vector<AlphaTheta> roll)
+      : spline_(spline), roll_(std::move(roll)) {
+    CHECK_GE(roll_.size(), 2u);
+    CHECK_EQ(roll_[0].alpha, 0.0);
+    CHECK_EQ(roll_[roll_.size() - 1].alpha, 1.0);
+  }
+
+  // Returns the xy coordiate of the spline for a given alpha.
+  ::Eigen::Matrix<double, 3, 1> Theta(double alpha) const;
+
+  // Returns the dspline/dalpha for a given alpha.
+  ::Eigen::Matrix<double, 3, 1> Omega(double alpha) const;
+
+  // Returns the d^2spline/dalpha^2 for a given alpha.
+  ::Eigen::Matrix<double, 3, 1> Alpha(double alpha) const;
+
+  CosSpline Reversed() const;
+
+ private:
+  NSpline<4, 2> spline_;
+  const std::vector<AlphaTheta> roll_;
+
+  // Returns the two control points for the roll for an alpha.
+  std::pair<AlphaTheta, AlphaTheta> RollPoints(double alpha) const;
+};
+
+// Class to hold a spline as a function of distance.
+class Path {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  Path(const CosSpline &spline, int num_alpha = 100)
+      : spline_(spline), distances_(BuildDistances(num_alpha)) {}
+
+  virtual ~Path() {}
+
+  // Returns a point on the spline as a function of distance.
+  ::Eigen::Matrix<double, 3, 1> Theta(double distance) const;
+
+  // Returns the velocity as a function of distance.
+  ::Eigen::Matrix<double, 3, 1> Omega(double distance) const;
+
+  // Returns the acceleration as a function of distance.
+  ::Eigen::Matrix<double, 3, 1> Alpha(double distance) const;
+
+  // Returns the length of the path in meters.
+  double length() const { return distances().back(); }
+
+  const absl::Span<const float> distances() const { return distances_; }
+
+  Path Reversed() const;
+  static ::std::unique_ptr<Path> Reversed(::std::unique_ptr<Path> p);
+
+ private:
+  std::vector<float> BuildDistances(size_t num_alpha);
+
+  // Computes alpha for a distance
+  double DistanceToAlpha(double distance) const;
+
+  const CosSpline spline_;
+  // An interpolation table of distances evenly distributed in alpha.
+  const ::std::vector<float> distances_;
+};
+
+namespace testing {
+class TrajectoryTest_IndicesForDistanceTest_Test;
+}  // namespace testing
+
+// A trajectory is a path and a set of velocities as a function of distance.
+class Trajectory {
+ public:
+  // Constructs a trajectory (but doesn't calculate it) given a path and a step
+  // size.
+  Trajectory(const frc971::control_loops::arm::Dynamics *dynamics,
+             const StateFeedbackHybridPlant<3, 1, 1> *roll,
+             ::std::unique_ptr<const Path> path, double gridsize)
+      : dynamics_(dynamics),
+        roll_(roll),
+        path_(::std::move(path)),
+        num_plan_points_(
+            static_cast<size_t>(::std::ceil(path_->length() / gridsize) + 1)),
+        step_size_(path_->length() /
+                   static_cast<double>(num_plan_points_ - 1)) {
+    alpha_unitizer_.setZero();
+  }
+
+  // Optimizes the trajectory.  The path will adhere to the constraints that
+  // || angular acceleration * alpha_unitizer || < 1, and the applied voltage <
+  // plan_vmax.
+  void OptimizeTrajectory(const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer,
+                          double plan_vmax) {
+    if (path_ == nullptr) {
+      abort();
+    }
+    alpha_unitizer_ = alpha_unitizer;
+
+    max_dvelocity_unfiltered_ =
+        CurvatureOptimizationPass(alpha_unitizer, plan_vmax);
+
+    // We need to start and end the trajectory with 0 velocity.
+    max_dvelocity_unfiltered_[0] = 0.0;
+    max_dvelocity_unfiltered_[max_dvelocity_unfiltered_.size() - 1] = 0.0;
+
+    max_dvelocity_backwards_accel_ = BackwardsOptimizationAccelerationPass(
+        max_dvelocity_unfiltered_, alpha_unitizer, plan_vmax);
+    max_dvelocity_forwards_accel_ = ForwardsOptimizationAccelerationPass(
+        max_dvelocity_backwards_accel_, alpha_unitizer, plan_vmax);
+
+    max_dvelocity_backwards_voltage_ = BackwardsOptimizationVoltagePass(
+        max_dvelocity_forwards_accel_, alpha_unitizer, plan_vmax);
+
+    max_dvelocity_forwards_voltage_ = ForwardsOptimizationVoltagePass(
+        max_dvelocity_backwards_voltage_, alpha_unitizer, plan_vmax);
+  }
+
+  // Returns an array of the distances used in the plan.  The starting point
+  // (0.0), and end point (path->length()) are included in the array.
+  ::std::vector<double> DistanceArray() const {
+    ::std::vector<double> result;
+    result.reserve(num_plan_points_);
+    for (size_t i = 0; i < num_plan_points_; ++i) {
+      result.push_back(DistanceForIndex(i));
+    }
+    return result;
+  }
+
+  // Computes the maximum velocity that we can follow the path while adhering to
+  // the constraints that || angular acceleration * alpha_unitizer || < 1, and
+  // the applied voltage < plan_vmax.  Returns the velocities.
+  ::std::vector<double> CurvatureOptimizationPass(
+      const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax);
+
+  double MaxCurvatureSpeed(double goal_distance,
+                           const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer,
+                           double plan_vmax);
+
+  // Computes the maximum forwards feasable acceleration at a given position
+  // while adhering to the plan_vmax and alpha_unitizer constraints.
+  // This gives us the maximum path distance acceleration (d^2d/dt^2) for any
+  // initial position and velocity for the forwards path.
+  double FeasableForwardsAcceleration(
+      double goal_distance, double goal_velocity, double plan_vmax,
+      const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer) const;
+  double FeasableForwardsVoltage(
+      double goal_distance, double goal_velocity, double plan_vmax,
+      const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer) const;
+
+  // Computes the maximum backwards feasable acceleration at a given position
+  // while adhering to the plan_vmax and alpha_unitizer constraints.
+  // This gives us the maximum path distance acceleration (d^2d/dt^2) for any
+  // initial position and velocity for the backwards path.
+  // Note: positive acceleration means speed up while going in the negative
+  // direction on the path.
+  double FeasableBackwardsAcceleration(
+      double goal_distance, double goal_velocity, double plan_vmax,
+      const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer) const;
+  double FeasableBackwardsVoltage(
+      double goal_distance, double goal_velocity, double plan_vmax,
+      const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer) const;
+
+  // Executes the backwards path optimization pass.
+  ::std::vector<double> BackwardsOptimizationAccelerationPass(
+      const ::std::vector<double> &max_dvelocity,
+      const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax);
+  ::std::vector<double> BackwardsOptimizationVoltagePass(
+      const ::std::vector<double> &max_dvelocity,
+      const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax);
+
+  // Executes the forwards path optimization pass.
+  ::std::vector<double> ForwardsOptimizationAccelerationPass(
+      const ::std::vector<double> &max_dvelocity,
+      const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax);
+  ::std::vector<double> ForwardsOptimizationVoltagePass(
+      const ::std::vector<double> &max_dvelocity,
+      const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer, double plan_vmax);
+
+  // Returns the number of points used for the plan.
+  size_t num_plan_points() const { return num_plan_points_; }
+
+  // Returns the curvature only velocity plan.
+  const ::std::vector<double> &max_dvelocity_unfiltered() const {
+    return max_dvelocity_unfiltered_;
+  }
+  // Returns the backwards pass + curvature plan.
+  const ::std::vector<double> &max_dvelocity_backward_accel() const {
+    return max_dvelocity_backwards_accel_;
+  }
+  const ::std::vector<double> &max_dvelocity_backward_voltage() const {
+    return max_dvelocity_backwards_voltage_;
+  }
+  // Returns the full plan.  This isn't useful at runtime since we don't want to
+  // be tied to a specific acceleration plan when we hit saturation.
+  const ::std::vector<double> &max_dvelocity_forwards_accel() const {
+    return max_dvelocity_forwards_accel_;
+  }
+  const ::std::vector<double> &max_dvelocity_forwards_voltage() const {
+    return max_dvelocity_forwards_voltage_;
+  }
+
+  // Returns the interpolated velocity at the distance d along the path.  The
+  // math assumes constant acceleration from point (d0, v0), to point (d1, v1),
+  // and that we are at point d between the two.
+  static double InterpolateVelocity(double d, double d0, double d1, double v0,
+                                    double v1) {
+    // We don't support negative velocities.  Report 0 velocity in that case.
+    // TODO(austin): Verify this doesn't show up in the real world.
+    if (v0 < 0 || v1 < 0) {
+      return 0.0;
+    }
+    if (d <= d0) {
+      return v0;
+    }
+    if (d >= d1) {
+      return v1;
+    }
+    return ::std::sqrt(v0 * v0 + (v1 * v1 - v0 * v0) * (d - d0) / (d1 - d0));
+  }
+
+  // Computes the path distance velocity of the plan as a function of the
+  // distance.
+  double GetDVelocity(double distance) const {
+    return GetDVelocity(distance, max_dvelocity_forwards_voltage_);
+  }
+  double GetDVelocity(double d, const ::std::vector<double> &plan) const {
+    ::std::pair<size_t, size_t> indices = IndicesForDistance(d);
+    const double v0 = plan[indices.first];
+    const double v1 = plan[indices.second];
+    const double d0 = DistanceForIndex(indices.first);
+    const double d1 = DistanceForIndex(indices.second);
+    return InterpolateVelocity(d, d0, d1, v0, v1);
+  }
+
+  // Computes the path distance acceleration of the plan as a function of the
+  // distance.
+  double GetDAcceleration(double distance) const {
+    return GetDAcceleration(distance, max_dvelocity_forwards_voltage_);
+  }
+  double GetDAcceleration(double distance,
+                          const ::std::vector<double> &plan) const {
+    ::std::pair<size_t, size_t> indices = IndicesForDistance(distance);
+    const double v0 = plan[indices.first];
+    const double v1 = plan[indices.second];
+    const double d0 = DistanceForIndex(indices.first);
+    const double d1 = DistanceForIndex(indices.second);
+    return InterpolateAcceleration(d0, d1, v0, v1);
+  }
+
+  // Returns the acceleration along the path segment assuming constant
+  // acceleration.
+  static double InterpolateAcceleration(double d0, double d1, double v0,
+                                        double v1) {
+    return 0.5 * (::std::pow(v1, 2) - ::std::pow(v0, 2)) / (d1 - d0);
+  }
+
+  ::Eigen::Matrix<double, 3, 1> ThetaT(double d) const {
+    return path_->Theta(d);
+  }
+
+  // Returns d theta/dt at a specified path distance and velocity.
+  ::Eigen::Matrix<double, 3, 1> OmegaT(double distance, double velocity) const {
+    if (distance > path_->length() || distance < 0.0) {
+      return ::Eigen::Matrix<double, 3, 1>::Zero();
+    } else {
+      return path_->Omega(distance) * velocity;
+    }
+  }
+
+  // Returns d^2 theta/dt^2 at a specified path distance, velocity and
+  // acceleration.
+  ::Eigen::Matrix<double, 3, 1> AlphaT(double distance, double velocity,
+                                       double acceleration) const {
+    if (distance > path_->length() || distance < 0.0) {
+      return ::Eigen::Matrix<double, 3, 1>::Zero();
+    } else {
+      return path_->Alpha(distance) * ::std::pow(velocity, 2) +
+             path_->Omega(distance) * acceleration;
+    }
+  }
+
+  // Converts a theta and omega vector to a full state vector.
+  static ::Eigen::Matrix<double, 9, 1> R(
+      ::Eigen::Matrix<double, 3, 1> theta_t,
+      ::Eigen::Matrix<double, 3, 1> omega_t) {
+    return (::Eigen::Matrix<double, 9, 1>() << theta_t(0, 0), omega_t(0, 0),
+            theta_t(1, 0), omega_t(1, 0), 0.0, 0.0, theta_t(2, 0),
+            omega_t(2, 0), 0.0)
+        .finished();
+  }
+
+  const ::Eigen::Matrix<double, 3, 3> &alpha_unitizer() const {
+    return alpha_unitizer_;
+  }
+
+  const Path &path() const { return *path_; }
+
+ private:
+  friend class testing::TrajectoryTest_IndicesForDistanceTest_Test;
+
+  // Returns the distance along the path for a specific index in the plan.
+  double DistanceForIndex(size_t index) const {
+    return static_cast<double>(index) * step_size_;
+  }
+
+  // Returns the before and after indices for a specific distance in the plan.
+  ::std::pair<size_t, size_t> IndicesForDistance(double distance) const {
+    const double path_length = path_->length();
+    if (distance <= 0.0) {
+      return ::std::pair<size_t, size_t>(0, 1);
+    }
+    const size_t lower_index =
+        ::std::min(static_cast<size_t>(num_plan_points_ - 2),
+                   static_cast<size_t>(::std::floor((num_plan_points_ - 1) *
+                                                    distance / path_length)));
+
+    return ::std::pair<size_t, size_t>(lower_index, lower_index + 1);
+  }
+
+  const frc971::control_loops::arm::Dynamics *dynamics_;
+  const StateFeedbackHybridPlant<3, 1, 1> *roll_;
+
+  // The path to follow.
+  ::std::unique_ptr<const Path> path_;
+  // The number of points in the plan.
+  const size_t num_plan_points_;
+  // A cached version of the step size since we need this a *lot*.
+  const double step_size_;
+
+  ::std::vector<double> max_dvelocity_unfiltered_;
+  ::std::vector<double> max_dvelocity_backwards_voltage_;
+  ::std::vector<double> max_dvelocity_backwards_accel_;
+  ::std::vector<double> max_dvelocity_forwards_accel_;
+  ::std::vector<double> max_dvelocity_forwards_voltage_;
+
+  ::Eigen::Matrix<double, 3, 3> alpha_unitizer_;
+};
+
+// This class tracks the current goal along trajectories and paths.
+class TrajectoryFollower {
+ public:
+  TrajectoryFollower(const frc971::control_loops::arm::Dynamics *dynamics,
+                     const StateFeedbackLoop<3, 1, 1, double,
+                                             StateFeedbackHybridPlant<3, 1, 1>,
+                                             HybridKalman<3, 1, 1>> *roll,
+                     const ::Eigen::Matrix<double, 3, 1> &theta)
+      : dynamics_(dynamics), roll_(roll), trajectory_(nullptr), theta_(theta) {
+    omega_.setZero();
+    last_K_.setZero();
+    Reset();
+  }
+
+  TrajectoryFollower(const frc971::control_loops::arm::Dynamics *dynamics,
+                     const StateFeedbackLoop<3, 1, 1, double,
+                                             StateFeedbackHybridPlant<3, 1, 1>,
+                                             HybridKalman<3, 1, 1>> *roll,
+                     Trajectory *const trajectory)
+      : dynamics_(dynamics), roll_(roll), trajectory_(trajectory) {
+    last_K_.setZero();
+    Reset();
+  }
+
+  bool has_path() const { return trajectory_ != nullptr; }
+
+  void set_theta(const ::Eigen::Matrix<double, 3, 1> &theta) { theta_ = theta; }
+
+  // Returns the goal distance along the path.
+  const ::Eigen::Matrix<double, 2, 1> &goal() const { return goal_; }
+  double goal(int i) const { return goal_(i); }
+
+  // Starts over at the beginning of the path.
+  void Reset();
+
+  // Switches paths and starts at the beginning of the path.
+  void SwitchTrajectory(const Trajectory *trajectory) {
+    trajectory_ = trajectory;
+    Reset();
+  }
+
+  // Returns the controller gain at the provided state.
+  ::Eigen::Matrix<double, 2, 6> ArmK_at_state(
+      const Eigen::Ref<const ::Eigen::Matrix<double, 6, 1>> arm_X,
+      const Eigen::Ref<const ::Eigen::Matrix<double, 2, 1>> arm_U);
+
+  // Returns the voltage, velocity and acceleration if we were to be partially
+  // along the path.
+  void USaturationSearch(double goal_distance, double last_goal_distance,
+                         double goal_velocity, double last_goal_velocity,
+                         double saturation_fraction_along_path,
+                         const ::Eigen::Matrix<double, 2, 6> &K,
+                         const ::Eigen::Matrix<double, 9, 1> &X,
+                         const Trajectory &trajectory,
+                         ::Eigen::Matrix<double, 3, 1> *U,
+                         double *saturation_goal_velocity,
+                         double *saturation_goal_acceleration);
+
+  // Returns the next goal given a planning plan_vmax and timestep.  This
+  // ignores the backwards pass.
+  ::Eigen::Matrix<double, 2, 1> PlanNextGoal(
+      const ::Eigen::Matrix<double, 2, 1> &goal, double plan_vmax, double dt);
+
+  // Plans the next cycle and updates the internal state for consumption.
+  void Update(const ::Eigen::Matrix<double, 9, 1> &X, bool disabled, double dt,
+              double plan_vmax, double voltage_limit);
+
+  // Returns the goal acceleration for this cycle.
+  double goal_acceleration() const { return goal_acceleration_; }
+
+  // Returns U(s) for this cycle.
+  const ::Eigen::Matrix<double, 3, 1> &U() const { return U_; }
+  double U(int i) const { return U_(i); }
+  const ::Eigen::Matrix<double, 3, 1> &U_unsaturated() const {
+    return U_unsaturated_;
+  }
+  const ::Eigen::Matrix<double, 3, 1> &U_ff() const { return U_ff_; }
+
+  double saturation_fraction_along_path() const {
+    return saturation_fraction_along_path_;
+  }
+
+  const ::Eigen::Matrix<double, 3, 1> &theta() const { return theta_; }
+  double theta(int i) const { return theta_(i); }
+  const ::Eigen::Matrix<double, 3, 1> &omega() const { return omega_; }
+  double omega(int i) const { return omega_(i); }
+
+  // Distance left on the path before we get to the end of the path.
+  double path_distance_to_go() const {
+    if (has_path()) {
+      return ::std::max(0.0, path()->length() - goal_(0));
+    } else {
+      return 0.0;
+    }
+  }
+
+  const Path *path() const { return &trajectory_->path(); }
+
+  int failed_solutions() const { return failed_solutions_; }
+
+  Eigen::Matrix<double, 3, 1> ComputeFF_U(
+      const Eigen::Matrix<double, 9, 1> &X,
+      const Eigen::Matrix<double, 3, 1> &omega_t,
+      const Eigen::Matrix<double, 3, 1> &alpha_t) const;
+
+ private:
+  const frc971::control_loops::arm::Dynamics *dynamics_;
+  const StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+                          HybridKalman<3, 1, 1>> *roll_;
+  // The trajectory plan.
+  const Trajectory *trajectory_ = nullptr;
+
+  // The current goal.
+  ::Eigen::Matrix<double, 2, 1> goal_;
+  // The previously executed goal.
+  ::Eigen::Matrix<double, 2, 1> last_goal_;
+  // The goal to use next cycle.  We always plan 1 cycle ahead.
+  ::Eigen::Matrix<double, 2, 1> next_goal_;
+
+  ::Eigen::Matrix<double, 3, 1> U_;
+  ::Eigen::Matrix<double, 3, 1> U_unsaturated_;
+  ::Eigen::Matrix<double, 3, 1> U_ff_;
+  double goal_acceleration_ = 0.0;
+
+  double saturation_fraction_along_path_ = 1.0;
+
+  // Holds the last valid goal position for when we loose our path.
+  ::Eigen::Matrix<double, 3, 1> theta_;
+  ::Eigen::Matrix<double, 3, 1> omega_;
+
+  ::Eigen::Matrix<double, 2, 6> last_K_;
+  int failed_solutions_ = 0;
+};
+
+}  // namespace arm
+}  // namespace control_loops
+}  // namespace frc971
+}  // namespace y2023
+
+#endif  // Y2023_CONTROL_LOOPS_SUPERSTRUCTURE_ARM_TRAJECTORY_H_
diff --git a/y2023/control_loops/superstructure/arm/trajectory_plot.cc b/y2023/control_loops/superstructure/arm/trajectory_plot.cc
new file mode 100644
index 0000000..2b05a30
--- /dev/null
+++ b/y2023/control_loops/superstructure/arm/trajectory_plot.cc
@@ -0,0 +1,535 @@
+#include "aos/init.h"
+#include "frc971/analysis/in_process_plotter.h"
+#include "frc971/control_loops/binomial.h"
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/ekf.h"
+#include "frc971/control_loops/fixed_quadrature.h"
+#include "gflags/gflags.h"
+#include "y2023/control_loops/superstructure/arm/arm_constants.h"
+#include "y2023/control_loops/superstructure/arm/trajectory.h"
+#include "y2023/control_loops/superstructure/roll/integral_hybrid_roll_plant.h"
+#include "y2023/control_loops/superstructure/roll/integral_roll_plant.h"
+
+DEFINE_bool(forwards, true, "If true, run the forwards simulation.");
+DEFINE_bool(plot, true, "If true, plot");
+DEFINE_bool(plot_thetas, true, "If true, plot the angles");
+
+namespace y2023 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+using frc971::control_loops::MatrixGaussianQuadrature5;
+
+void Main() {
+  frc971::control_loops::arm::Dynamics dynamics(kArmConstants);
+  StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+                    HybridKalman<3, 1, 1>>
+      hybrid_roll = superstructure::roll::MakeIntegralHybridRollLoop();
+
+  Eigen::Matrix<double, 3, 4> spline_params;
+
+  spline_params << 0.30426338, 0.42813912, 0.64902386, 0.55127045, -1.73611082,
+      -1.64478944, -1.44763868, -1.22624244, 0.1, 0.2, 0.3, 0.5;
+  LOG(INFO) << "Spline " << spline_params;
+  NSpline<4, 2> spline(spline_params.block<2, 4>(0, 0));
+  CosSpline cos_spline(spline,
+                       {{0.0, 0.1}, {0.3, 0.1}, {0.7, 0.2}, {1.0, 0.2}});
+  Path distance_spline(cos_spline, 100);
+
+  Trajectory trajectory(&dynamics, &hybrid_roll.plant(),
+                        std::make_unique<Path>(cos_spline), 0.001);
+
+  constexpr double kAlpha0Max = 20.0;
+  constexpr double kAlpha1Max = 30.0;
+  constexpr double kAlpha2Max = 40.0;
+  constexpr double vmax = 9.0;
+  constexpr double sim_dt = 0.00505;
+
+  const ::Eigen::DiagonalMatrix<double, 3> alpha_unitizer(
+      (::Eigen::DiagonalMatrix<double, 3>().diagonal() << (1.0 / kAlpha0Max),
+       (1.0 / kAlpha1Max), (1.0 / kAlpha2Max))
+          .finished());
+  trajectory.OptimizeTrajectory(alpha_unitizer, vmax);
+
+  const ::std::vector<double> distance_array = trajectory.DistanceArray();
+
+  ::std::vector<double> theta0_array;
+  ::std::vector<double> theta1_array;
+  ::std::vector<double> theta2_array;
+  ::std::vector<double> omega0_array;
+  ::std::vector<double> omega1_array;
+  ::std::vector<double> omega2_array;
+  ::std::vector<double> alpha0_array;
+  ::std::vector<double> alpha1_array;
+  ::std::vector<double> alpha2_array;
+
+  ::std::vector<double> integrated_distance;
+  ::std::vector<double> integrated_theta0_array;
+  ::std::vector<double> integrated_theta1_array;
+  ::std::vector<double> integrated_theta2_array;
+  ::std::vector<double> integrated_omega0_array;
+  ::std::vector<double> integrated_omega1_array;
+  ::std::vector<double> integrated_omega2_array;
+
+  ::Eigen::Matrix<double, 3, 1> integrated_theta = distance_spline.Theta(0.0);
+  ::Eigen::Matrix<double, 3, 1> integrated_omega = distance_spline.Omega(0.0);
+
+  // Plot the splines and their integrals to check consistency.
+  for (const double d : distance_array) {
+    const ::Eigen::Matrix<double, 3, 1> theta = distance_spline.Theta(d);
+    const ::Eigen::Matrix<double, 3, 1> omega = distance_spline.Omega(d);
+    const ::Eigen::Matrix<double, 3, 1> alpha = distance_spline.Alpha(d);
+    theta0_array.push_back(theta(0, 0));
+    theta1_array.push_back(theta(1, 0));
+    theta2_array.push_back(theta(2, 0));
+    omega0_array.push_back(omega(0, 0));
+    omega1_array.push_back(omega(1, 0));
+    omega2_array.push_back(omega(2, 0));
+    alpha0_array.push_back(alpha(0, 0));
+    alpha1_array.push_back(alpha(1, 0));
+    alpha2_array.push_back(alpha(2, 0));
+  }
+
+  const double dd = distance_spline.length() / 1000.0;
+  for (double d = 0; d <= distance_spline.length(); d += dd) {
+    integrated_distance.push_back(d);
+    integrated_omega0_array.push_back(integrated_omega(0));
+    integrated_omega1_array.push_back(integrated_omega(1));
+    integrated_omega2_array.push_back(integrated_omega(2));
+    integrated_theta0_array.push_back(integrated_theta(0));
+    integrated_theta1_array.push_back(integrated_theta(1));
+    integrated_theta2_array.push_back(integrated_theta(2));
+
+    integrated_theta += MatrixGaussianQuadrature5<3>(
+        [&](double distance) { return distance_spline.Omega(distance); }, d,
+        d + dd);
+    integrated_omega += MatrixGaussianQuadrature5<3>(
+        [&](double distance) { return distance_spline.Alpha(distance); }, d,
+        d + dd);
+  }
+
+  // Next step: see what U is as a function of distance for all the passes.
+  ::std::vector<double> Uff0_distance_array_curvature;
+  ::std::vector<double> Uff1_distance_array_curvature;
+  ::std::vector<double> Uff2_distance_array_curvature;
+  ::std::vector<double> Uff0_distance_array_backwards_accel_only;
+  ::std::vector<double> Uff1_distance_array_backwards_accel_only;
+  ::std::vector<double> Uff2_distance_array_backwards_accel_only;
+  ::std::vector<double> Uff0_distance_array_forwards_accel_only;
+  ::std::vector<double> Uff1_distance_array_forwards_accel_only;
+  ::std::vector<double> Uff2_distance_array_forwards_accel_only;
+  ::std::vector<double> Uff0_distance_array_backwards_voltage_only;
+  ::std::vector<double> Uff1_distance_array_backwards_voltage_only;
+  ::std::vector<double> Uff2_distance_array_backwards_voltage_only;
+  ::std::vector<double> Uff0_distance_array_forwards_voltage_only;
+  ::std::vector<double> Uff1_distance_array_forwards_voltage_only;
+  ::std::vector<double> Uff2_distance_array_forwards_voltage_only;
+
+  TrajectoryFollower follower(&dynamics, &hybrid_roll, &trajectory);
+
+  for (const double distance : distance_array) {
+    const ::Eigen::Matrix<double, 3, 1> theta_t = trajectory.ThetaT(distance);
+
+    {
+      const double goal_velocity = trajectory.GetDVelocity(
+          distance, trajectory.max_dvelocity_unfiltered());
+      const double goal_acceleration = trajectory.GetDAcceleration(
+          distance, trajectory.max_dvelocity_unfiltered());
+      const ::Eigen::Matrix<double, 3, 1> omega_t =
+          trajectory.OmegaT(distance, goal_velocity);
+      const ::Eigen::Matrix<double, 3, 1> alpha_t =
+          trajectory.AlphaT(distance, goal_velocity, goal_acceleration);
+
+      const ::Eigen::Matrix<double, 9, 1> R = trajectory.R(theta_t, omega_t);
+      const ::Eigen::Matrix<double, 3, 1> U =
+          follower.ComputeFF_U(R, omega_t, alpha_t).array().max(-20).min(20);
+
+      Uff0_distance_array_curvature.push_back(U(0));
+      Uff1_distance_array_curvature.push_back(U(1));
+      Uff2_distance_array_curvature.push_back(U(2));
+    }
+    {
+      const double goal_velocity = trajectory.GetDVelocity(
+          distance, trajectory.max_dvelocity_backward_accel());
+      const double goal_acceleration = trajectory.GetDAcceleration(
+          distance, trajectory.max_dvelocity_backward_accel());
+      const ::Eigen::Matrix<double, 3, 1> omega_t =
+          trajectory.OmegaT(distance, goal_velocity);
+      const ::Eigen::Matrix<double, 3, 1> alpha_t =
+          trajectory.AlphaT(distance, goal_velocity, goal_acceleration);
+
+      const ::Eigen::Matrix<double, 9, 1> R = trajectory.R(theta_t, omega_t);
+      const ::Eigen::Matrix<double, 3, 1> U =
+          follower.ComputeFF_U(R, omega_t, alpha_t).array().max(-20).min(20);
+
+      Uff0_distance_array_backwards_accel_only.push_back(U(0));
+      Uff1_distance_array_backwards_accel_only.push_back(U(1));
+      Uff2_distance_array_backwards_accel_only.push_back(U(2));
+    }
+    {
+      const double goal_velocity = trajectory.GetDVelocity(
+          distance, trajectory.max_dvelocity_forwards_accel());
+      const double goal_acceleration = trajectory.GetDAcceleration(
+          distance, trajectory.max_dvelocity_forwards_accel());
+      const ::Eigen::Matrix<double, 3, 1> omega_t =
+          trajectory.OmegaT(distance, goal_velocity);
+      const ::Eigen::Matrix<double, 3, 1> alpha_t =
+          trajectory.AlphaT(distance, goal_velocity, goal_acceleration);
+
+      const ::Eigen::Matrix<double, 9, 1> R = trajectory.R(theta_t, omega_t);
+      const ::Eigen::Matrix<double, 3, 1> U =
+          follower.ComputeFF_U(R, omega_t, alpha_t).array().max(-20).min(20);
+
+      Uff0_distance_array_forwards_accel_only.push_back(U(0));
+      Uff1_distance_array_forwards_accel_only.push_back(U(1));
+      Uff2_distance_array_forwards_accel_only.push_back(U(2));
+    }
+    {
+      const double goal_velocity = trajectory.GetDVelocity(
+          distance, trajectory.max_dvelocity_backward_voltage());
+      const double goal_acceleration = trajectory.GetDAcceleration(
+          distance, trajectory.max_dvelocity_backward_voltage());
+      const ::Eigen::Matrix<double, 3, 1> omega_t =
+          trajectory.OmegaT(distance, goal_velocity);
+      const ::Eigen::Matrix<double, 3, 1> alpha_t =
+          trajectory.AlphaT(distance, goal_velocity, goal_acceleration);
+
+      const ::Eigen::Matrix<double, 9, 1> R = trajectory.R(theta_t, omega_t);
+      const ::Eigen::Matrix<double, 3, 1> U =
+          follower.ComputeFF_U(R, omega_t, alpha_t).array().max(-20).min(20);
+
+      Uff0_distance_array_backwards_voltage_only.push_back(U(0));
+      Uff1_distance_array_backwards_voltage_only.push_back(U(1));
+      Uff2_distance_array_backwards_voltage_only.push_back(U(2));
+    }
+    {
+      const double goal_velocity = trajectory.GetDVelocity(
+          distance, trajectory.max_dvelocity_forwards_voltage());
+      const double goal_acceleration = trajectory.GetDAcceleration(
+          distance, trajectory.max_dvelocity_forwards_voltage());
+      const ::Eigen::Matrix<double, 3, 1> omega_t =
+          trajectory.OmegaT(distance, goal_velocity);
+      const ::Eigen::Matrix<double, 3, 1> alpha_t =
+          trajectory.AlphaT(distance, goal_velocity, goal_acceleration);
+
+      const ::Eigen::Matrix<double, 9, 1> R = trajectory.R(theta_t, omega_t);
+      const ::Eigen::Matrix<double, 3, 1> U =
+          follower.ComputeFF_U(R, omega_t, alpha_t).array().max(-20).min(20);
+
+      Uff0_distance_array_forwards_voltage_only.push_back(U(0));
+      Uff1_distance_array_forwards_voltage_only.push_back(U(1));
+      Uff2_distance_array_forwards_voltage_only.push_back(U(2));
+    }
+  }
+
+  double t = 0;
+  ::Eigen::Matrix<double, 4, 1> arm_X;
+  ::Eigen::Matrix<double, 2, 1> roll_X;
+  {
+    ::Eigen::Matrix<double, 3, 1> theta_t = trajectory.ThetaT(0.0);
+    arm_X << theta_t(0), 0.0, theta_t(1), 0.0;
+    roll_X << theta_t(2), 0.0;
+  }
+
+  ::std::vector<double> t_array;
+  ::std::vector<double> theta0_goal_t_array;
+  ::std::vector<double> theta1_goal_t_array;
+  ::std::vector<double> theta2_goal_t_array;
+  ::std::vector<double> omega0_goal_t_array;
+  ::std::vector<double> omega1_goal_t_array;
+  ::std::vector<double> omega2_goal_t_array;
+  ::std::vector<double> alpha0_goal_t_array;
+  ::std::vector<double> alpha1_goal_t_array;
+  ::std::vector<double> alpha2_goal_t_array;
+  ::std::vector<double> theta0_t_array;
+  ::std::vector<double> omega0_t_array;
+  ::std::vector<double> theta1_t_array;
+  ::std::vector<double> omega1_t_array;
+  ::std::vector<double> theta2_t_array;
+  ::std::vector<double> omega2_t_array;
+  ::std::vector<double> distance_t_array;
+  ::std::vector<double> velocity_t_array;
+  ::std::vector<double> acceleration_t_array;
+  ::std::vector<double> u0_unsaturated_array;
+  ::std::vector<double> u1_unsaturated_array;
+  ::std::vector<double> u2_unsaturated_array;
+  ::std::vector<double> alpha0_t_array;
+  ::std::vector<double> alpha1_t_array;
+  ::std::vector<double> alpha2_t_array;
+  ::std::vector<double> uff0_array;
+  ::std::vector<double> uff1_array;
+  ::std::vector<double> uff2_array;
+  ::std::vector<double> u0_array;
+  ::std::vector<double> u1_array;
+  ::std::vector<double> u2_array;
+  ::std::vector<double> theta0_hat_t_array;
+  ::std::vector<double> omega0_hat_t_array;
+  ::std::vector<double> theta1_hat_t_array;
+  ::std::vector<double> omega1_hat_t_array;
+  ::std::vector<double> theta2_hat_t_array;
+  ::std::vector<double> omega2_hat_t_array;
+  ::std::vector<double> torque0_hat_t_array;
+  ::std::vector<double> torque1_hat_t_array;
+  ::std::vector<double> torque2_hat_t_array;
+
+  // Now follow the trajectory.
+  frc971::control_loops::arm::EKF arm_ekf(&dynamics);
+  arm_ekf.Reset(arm_X);
+  StateFeedbackLoop<3, 1, 1, double, StateFeedbackPlant<3, 1, 1>,
+                    StateFeedbackObserver<3, 1, 1>>
+      roll = superstructure::roll::MakeIntegralRollLoop();
+  roll.mutable_X_hat().setZero();
+  roll.mutable_X_hat().block<2, 1>(0, 0) = roll_X;
+
+  ::std::cout << "Reset P: " << arm_ekf.P_reset() << ::std::endl;
+  ::std::cout << "Stabilized P: " << arm_ekf.P_half_converged() << ::std::endl;
+  ::std::cout << "Really stabilized P: " << arm_ekf.P_converged()
+              << ::std::endl;
+
+  while (t < 1.2) {
+    t_array.push_back(t);
+    arm_ekf.Correct(
+        (::Eigen::Matrix<double, 2, 1>() << arm_X(0), arm_X(2)).finished(),
+        sim_dt);
+    roll.Correct((::Eigen::Matrix<double, 1, 1>() << roll_X(0)).finished());
+    follower.Update(
+        (Eigen::Matrix<double, 9, 1>() << arm_ekf.X_hat(), roll.X_hat())
+            .finished(),
+        false, sim_dt, vmax, 12.0);
+
+    const ::Eigen::Matrix<double, 3, 1> theta_t =
+        trajectory.ThetaT(follower.goal()(0));
+    const ::Eigen::Matrix<double, 3, 1> omega_t =
+        trajectory.OmegaT(follower.goal()(0), follower.goal()(1));
+    const ::Eigen::Matrix<double, 3, 1> alpha_t = trajectory.AlphaT(
+        follower.goal()(0), follower.goal()(1), follower.goal_acceleration());
+
+    theta0_goal_t_array.push_back(theta_t(0));
+    theta1_goal_t_array.push_back(theta_t(1));
+    theta2_goal_t_array.push_back(theta_t(2));
+    omega0_goal_t_array.push_back(omega_t(0));
+    omega1_goal_t_array.push_back(omega_t(1));
+    omega2_goal_t_array.push_back(omega_t(2));
+    alpha0_goal_t_array.push_back(alpha_t(0));
+    alpha1_goal_t_array.push_back(alpha_t(1));
+    alpha2_goal_t_array.push_back(alpha_t(2));
+    theta0_t_array.push_back(arm_X(0));
+    omega0_t_array.push_back(arm_X(1));
+    theta1_t_array.push_back(arm_X(2));
+    omega1_t_array.push_back(arm_X(3));
+    theta2_t_array.push_back(roll_X(0));
+    omega2_t_array.push_back(roll_X(1));
+    theta0_hat_t_array.push_back(arm_ekf.X_hat(0));
+    omega0_hat_t_array.push_back(arm_ekf.X_hat(1));
+    torque0_hat_t_array.push_back(arm_ekf.X_hat(4));
+    theta1_hat_t_array.push_back(arm_ekf.X_hat(2));
+    omega1_hat_t_array.push_back(arm_ekf.X_hat(3));
+    torque1_hat_t_array.push_back(arm_ekf.X_hat(5));
+
+    theta2_hat_t_array.push_back(roll.X_hat(0));
+    omega2_hat_t_array.push_back(roll.X_hat(1));
+    torque2_hat_t_array.push_back(roll.X_hat(2));
+
+    distance_t_array.push_back(follower.goal()(0));
+    velocity_t_array.push_back(follower.goal()(1));
+    acceleration_t_array.push_back(follower.goal_acceleration());
+
+    u0_unsaturated_array.push_back(follower.U_unsaturated()(0));
+    u1_unsaturated_array.push_back(follower.U_unsaturated()(1));
+    u2_unsaturated_array.push_back(follower.U_unsaturated()(2));
+
+    ::Eigen::Matrix<double, 3, 1> actual_U = follower.U();
+    // Add in a disturbance force to see how well the arm learns it.
+    // actual_U(0) += 1.0;
+
+    const ::Eigen::Matrix<double, 4, 1> arm_xdot =
+        dynamics.Acceleration(arm_X, actual_U.block<2, 1>(0, 0));
+    const ::Eigen::Matrix<double, 2, 1> roll_xdot =
+        hybrid_roll.plant().coefficients().A_continuous.block<2, 2>(0, 0) *
+            roll_X +
+        hybrid_roll.plant().coefficients().B_continuous.block<2, 1>(0, 0) *
+            actual_U.block<1, 1>(2, 0);
+
+    arm_X = dynamics.UnboundedDiscreteDynamics(
+        arm_X, actual_U.block<2, 1>(0, 0), sim_dt);
+    arm_ekf.Predict(follower.U().block<2, 1>(0, 0), sim_dt);
+    roll_X =
+        roll.plant()
+            .Update((Eigen::Matrix<double, 3, 1>() << roll_X, 0.0).finished(),
+                    follower.U().block<1, 1>(2, 0))
+            .block<2, 1>(0, 0);
+    roll.UpdateObserver(follower.U().block<1, 1>(2, 0),
+                        std::chrono::duration_cast<std::chrono::nanoseconds>(
+                            std::chrono::duration<double>(sim_dt)));
+
+    alpha0_t_array.push_back(arm_xdot(1));
+    alpha1_t_array.push_back(arm_xdot(3));
+    alpha2_t_array.push_back(roll_xdot(1));
+
+    uff0_array.push_back(follower.U_ff()(0));
+    uff1_array.push_back(follower.U_ff()(1));
+    uff2_array.push_back(follower.U_ff()(2));
+    u0_array.push_back(follower.U()(0));
+    u1_array.push_back(follower.U()(1));
+    u2_array.push_back(follower.U()(2));
+
+    t += sim_dt;
+  }
+
+  if (FLAGS_plot) {
+    frc971::analysis::Plotter plotter;
+
+    plotter.AddFigure();
+    plotter.Title("Input spline");
+    plotter.AddLine(distance_array, theta0_array, "theta0");
+    plotter.AddLine(distance_array, theta1_array, "theta1");
+    plotter.AddLine(distance_array, theta2_array, "theta2");
+    plotter.AddLine(distance_array, omega0_array, "omega0");
+    plotter.AddLine(distance_array, omega1_array, "omega1");
+    plotter.AddLine(distance_array, omega2_array, "omega2");
+    plotter.AddLine(distance_array, alpha0_array, "alpha0");
+    plotter.AddLine(distance_array, alpha1_array, "alpha1");
+    plotter.AddLine(distance_array, alpha2_array, "alpha2");
+
+    plotter.AddLine(integrated_distance, integrated_theta0_array,
+                    "integrated theta0");
+    plotter.AddLine(integrated_distance, integrated_theta1_array,
+                    "integrated theta1");
+    plotter.AddLine(integrated_distance, integrated_theta2_array,
+                    "integrated theta2");
+    plotter.AddLine(integrated_distance, integrated_omega0_array,
+                    "integrated omega0");
+    plotter.AddLine(integrated_distance, integrated_omega1_array,
+                    "integrated omega1");
+    plotter.AddLine(integrated_distance, integrated_omega2_array,
+                    "integrated omega2");
+    plotter.Publish();
+
+    plotter.AddFigure();
+    plotter.Title("Trajectory");
+    plotter.AddLine(theta0_array, theta1_array, "desired path");
+    plotter.AddLine(theta0_t_array, theta1_t_array, "actual path");
+    plotter.Publish();
+
+    plotter.AddFigure();
+    plotter.Title("Solver passes");
+    plotter.AddLine(distance_array, trajectory.max_dvelocity_unfiltered(),
+                    "pass0");
+    plotter.AddLine(distance_array, trajectory.max_dvelocity_backward_accel(),
+                    "passb accel");
+    plotter.AddLine(distance_array, trajectory.max_dvelocity_forwards_accel(),
+                    "passf accel");
+    plotter.AddLine(distance_array, trajectory.max_dvelocity_backward_voltage(),
+                    "passb voltage");
+    plotter.AddLine(distance_array, trajectory.max_dvelocity_forwards_voltage(),
+                    "passf voltage");
+    plotter.Publish();
+
+    plotter.AddFigure();
+    plotter.Title("Time Goals");
+    plotter.AddLine(t_array, alpha0_goal_t_array, "alpha0_t_goal");
+    plotter.AddLine(t_array, alpha0_t_array, "alpha0_t");
+    plotter.AddLine(t_array, alpha1_goal_t_array, "alpha1_t_goal");
+    plotter.AddLine(t_array, alpha1_t_array, "alpha1_t");
+    plotter.AddLine(t_array, alpha2_goal_t_array, "alpha2_t_goal");
+    plotter.AddLine(t_array, alpha2_t_array, "alpha2_t");
+    plotter.AddLine(t_array, distance_t_array, "distance_t");
+    plotter.AddLine(t_array, velocity_t_array, "velocity_t");
+    plotter.AddLine(t_array, acceleration_t_array, "acceleration_t");
+    plotter.Publish();
+
+    plotter.AddFigure();
+    plotter.Title("Angular Velocities");
+    plotter.AddLine(t_array, omega0_goal_t_array, "omega0_t_goal");
+    plotter.AddLine(t_array, omega0_t_array, "omega0_t");
+    plotter.AddLine(t_array, omega0_hat_t_array, "omega0_hat_t");
+
+    plotter.AddLine(t_array, omega1_goal_t_array, "omega1_t_goal");
+    plotter.AddLine(t_array, omega1_t_array, "omega1_t");
+    plotter.AddLine(t_array, omega1_hat_t_array, "omega1_hat_t");
+
+    plotter.AddLine(t_array, omega2_goal_t_array, "omega2_t_goal");
+    plotter.AddLine(t_array, omega2_t_array, "omega2_t");
+    plotter.AddLine(t_array, omega2_hat_t_array, "omega2_hat_t");
+    plotter.Publish();
+
+    plotter.AddFigure();
+    plotter.Title("Voltages");
+    plotter.AddLine(t_array, u0_unsaturated_array, "u0_full");
+    plotter.AddLine(t_array, u0_array, "u0");
+    plotter.AddLine(t_array, uff0_array, "uff0");
+    plotter.AddLine(t_array, u1_unsaturated_array, "u1_full");
+    plotter.AddLine(t_array, u1_array, "u1");
+    plotter.AddLine(t_array, uff1_array, "uff1");
+    plotter.AddLine(t_array, u2_unsaturated_array, "u2_full");
+    plotter.AddLine(t_array, u2_array, "u2");
+    plotter.AddLine(t_array, uff2_array, "uff2");
+    plotter.AddLine(t_array, torque0_hat_t_array, "torque0_hat");
+    plotter.AddLine(t_array, torque1_hat_t_array, "torque1_hat");
+    plotter.AddLine(t_array, torque2_hat_t_array, "torque2_hat");
+    plotter.Publish();
+
+    if (FLAGS_plot_thetas) {
+      plotter.AddFigure();
+      plotter.Title("Angles");
+      plotter.AddLine(t_array, theta0_goal_t_array, "theta0_t_goal");
+      plotter.AddLine(t_array, theta0_t_array, "theta0_t");
+      plotter.AddLine(t_array, theta0_hat_t_array, "theta0_hat_t");
+      plotter.AddLine(t_array, theta1_goal_t_array, "theta1_t_goal");
+      plotter.AddLine(t_array, theta1_t_array, "theta1_t");
+      plotter.AddLine(t_array, theta1_hat_t_array, "theta1_hat_t");
+      plotter.AddLine(t_array, theta2_goal_t_array, "theta2_t_goal");
+      plotter.AddLine(t_array, theta2_t_array, "theta2_t");
+      plotter.AddLine(t_array, theta2_hat_t_array, "theta2_hat_t");
+      plotter.Publish();
+    }
+
+    plotter.AddFigure();
+    plotter.Title("ff for distance");
+    plotter.AddLine(distance_array, Uff0_distance_array_forwards_voltage_only,
+                    "ff0");
+    plotter.AddLine(distance_array, Uff1_distance_array_forwards_voltage_only,
+                    "ff1");
+    plotter.AddLine(distance_array, Uff2_distance_array_forwards_voltage_only,
+                    "ff2");
+
+    plotter.AddLine(distance_array, Uff0_distance_array_backwards_voltage_only,
+                    "ff0_back voltage");
+    plotter.AddLine(distance_array, Uff1_distance_array_backwards_voltage_only,
+                    "ff1_back voltage");
+    plotter.AddLine(distance_array, Uff2_distance_array_backwards_voltage_only,
+                    "ff2_back voltage");
+
+    plotter.AddLine(distance_array, Uff0_distance_array_forwards_accel_only,
+                    "ff0_forward accel");
+    plotter.AddLine(distance_array, Uff1_distance_array_forwards_accel_only,
+                    "ff1_forward accel");
+    plotter.AddLine(distance_array, Uff2_distance_array_forwards_accel_only,
+                    "ff2_forward accel");
+
+    plotter.AddLine(distance_array, Uff0_distance_array_backwards_accel_only,
+                    "ff0_back accel");
+    plotter.AddLine(distance_array, Uff1_distance_array_backwards_accel_only,
+                    "ff1_back accel");
+    plotter.AddLine(distance_array, Uff2_distance_array_backwards_accel_only,
+                    "ff2_back accel");
+
+    plotter.AddLine(distance_array, Uff0_distance_array_curvature, "ff0_curve");
+    plotter.AddLine(distance_array, Uff1_distance_array_curvature, "ff1_curve");
+    plotter.AddLine(distance_array, Uff2_distance_array_curvature, "ff2_curve");
+
+    plotter.Publish();
+    plotter.Spin();
+  }
+}
+
+}  // namespace arm
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2023
+
+int main(int argc, char **argv) {
+  ::aos::InitGoogle(&argc, &argv);
+  ::y2023::control_loops::superstructure::arm::Main();
+  return 0;
+}
diff --git a/y2023/control_loops/superstructure/arm/trajectory_test.cc b/y2023/control_loops/superstructure/arm/trajectory_test.cc
new file mode 100644
index 0000000..bcf338c
--- /dev/null
+++ b/y2023/control_loops/superstructure/arm/trajectory_test.cc
@@ -0,0 +1,232 @@
+#include "y2023/control_loops/superstructure/arm/trajectory.h"
+
+#include "frc971/control_loops/double_jointed_arm/dynamics.h"
+#include "frc971/control_loops/double_jointed_arm/ekf.h"
+#include "gtest/gtest.h"
+#include "y2023/control_loops/superstructure/arm/arm_constants.h"
+#include "y2023/control_loops/superstructure/roll/integral_hybrid_roll_plant.h"
+#include "y2023/control_loops/superstructure/roll/integral_roll_plant.h"
+
+namespace y2023 {
+namespace control_loops {
+namespace superstructure {
+namespace arm {
+namespace testing {
+
+using frc971::control_loops::MatrixGaussianQuadrature5;
+using frc971::control_loops::arm::Dynamics;
+using frc971::control_loops::arm::EKF;
+
+// Tests that we can pull out values along the path.
+TEST(TrajectoryTest, Theta) {
+  Eigen::Matrix<double, 2, 4> spline_params;
+  spline_params << 0.3, 0.4, 0.6, 0.5, -1.7, -1.6, -1.4, -1.2;
+
+  NSpline<4, 2> spline(spline_params);
+  CosSpline cos_spline(spline,
+                       {{0.0, 0.1}, {0.3, 0.1}, {0.7, 0.2}, {1.0, 0.2}});
+
+  EXPECT_TRUE(cos_spline.Theta(-1.0).isApprox(
+      (Eigen::Matrix<double, 3, 1>() << 0.3, -1.7, 0.1).finished()))
+      << cos_spline.Theta(-1.0).transpose();
+
+  EXPECT_TRUE(cos_spline.Theta(0.0).isApprox(
+      (Eigen::Matrix<double, 3, 1>() << 0.3, -1.7, 0.1).finished()))
+      << cos_spline.Theta(0.0).transpose();
+
+  EXPECT_TRUE(cos_spline.Theta(0.3).isApprox(
+      (Eigen::Matrix<double, 3, 1>() << 0.4062, -1.5857, 0.1).finished()))
+      << cos_spline.Theta(0.3).transpose();
+
+  EXPECT_TRUE(cos_spline.Theta(0.5).isApprox(
+      (Eigen::Matrix<double, 3, 1>() << 0.475, -1.4875, 0.15).finished()))
+      << cos_spline.Theta(0.5).transpose();
+
+  EXPECT_TRUE(cos_spline.Theta(0.7).isApprox(
+      (Eigen::Matrix<double, 3, 1>() << 0.5198, -1.3773, 0.2).finished()))
+      << cos_spline.Theta(0.7).transpose();
+
+  EXPECT_TRUE(cos_spline.Theta(1.0).isApprox(
+      (Eigen::Matrix<double, 3, 1>() << 0.5, -1.2, 0.2).finished()))
+      << cos_spline.Theta(1.0).transpose();
+
+  EXPECT_TRUE(cos_spline.Theta(2.0).isApprox(
+      (Eigen::Matrix<double, 3, 1>() << 0.5, -1.2, 0.2).finished()))
+      << cos_spline.Theta(2.0).transpose();
+}
+
+// Tests that the integral of alpha and omega matches the functions they were
+// differentiated from on Path.
+TEST(TrajectoryTest, IntegrateAccel) {
+  Eigen::Matrix<double, 2, 4> spline_params;
+  spline_params << 0.3, 0.4, 0.6, 0.5, -1.7, -1.6, -1.4, -1.2;
+  NSpline<4, 2> spline(spline_params);
+  CosSpline cos_spline(spline,
+                       {{0.0, 0.1}, {0.3, 0.1}, {0.7, 0.2}, {1.0, 0.2}});
+  Path distance_spline(cos_spline, 100);
+
+  Eigen::Matrix<double, 3, 1> integrated_theta = distance_spline.Theta(0.0);
+  Eigen::Matrix<double, 3, 1> integrated_omega = distance_spline.Omega(0.0);
+
+  constexpr size_t kSlices = 1000;
+  for (size_t i = 0; i < kSlices; ++i) {
+    const double d = i * distance_spline.length() / kSlices;
+    const double next_d = (i + 1) * distance_spline.length() / kSlices;
+
+    integrated_theta += MatrixGaussianQuadrature5<3>(
+        [&](double distance) { return distance_spline.Omega(distance); }, d,
+        next_d);
+    integrated_omega += MatrixGaussianQuadrature5<3>(
+        [&](double distance) { return distance_spline.Alpha(distance); }, d,
+        next_d);
+
+    EXPECT_TRUE(integrated_theta.isApprox(distance_spline.Theta(next_d), 1e-3))
+        << ": Got " << integrated_theta.transpose() << ", expected "
+        << distance_spline.Theta(next_d).transpose();
+    EXPECT_TRUE(integrated_omega.isApprox(distance_spline.Omega(next_d), 1e-3))
+        << ": Got " << integrated_omega.transpose() << ", expected "
+        << distance_spline.Omega(next_d).transpose();
+  }
+}
+
+// Tests that we can correctly interpolate velocities between two points
+TEST(TrajectoryTest, InterpolateVelocity) {
+  // x = 0.5 * a * t^2
+  // v = a * t
+  // a = 2.0
+  EXPECT_EQ(0.0, Trajectory::InterpolateVelocity(0.0, 0.0, 1.0, 0.0, 2.0));
+  EXPECT_EQ(2.0, Trajectory::InterpolateVelocity(1.0, 0.0, 1.0, 0.0, 2.0));
+  EXPECT_EQ(0.0, Trajectory::InterpolateVelocity(-1.0, 0.0, 1.0, 0.0, 2.0));
+  EXPECT_EQ(2.0, Trajectory::InterpolateVelocity(20.0, 0.0, 1.0, 0.0, 2.0));
+}
+
+// Tests that we can correctly interpolate velocities between two points
+TEST(TrajectoryTest, InterpolateAcceleration) {
+  // x = 0.5 * a * t^2
+  // v = a * t
+  // a = 2.0
+  EXPECT_EQ(2.0, Trajectory::InterpolateAcceleration(0.0, 1.0, 0.0, 2.0));
+}
+
+std::unique_ptr<Path> MakeDemoPath() {
+  Eigen::Matrix<double, 2, 4> spline_params;
+  spline_params << 0.3, 0.4, 0.6, 0.5, -1.7, -1.6, -1.4, -1.2;
+  NSpline<4, 2> spline(spline_params);
+  CosSpline cos_spline(spline,
+                       {{0.0, 0.1}, {0.3, 0.1}, {0.7, 0.2}, {1.0, 0.2}});
+  return std::make_unique<Path>(cos_spline, 100);
+}
+
+// Tests that we can correctly interpolate velocities between two points
+TEST(TrajectoryTest, ReversedPath) {
+  // Tests that a reversed path is actually reversed.
+  ::std::unique_ptr<Path> path = MakeDemoPath();
+  ::std::unique_ptr<Path> reversed_path = Path::Reversed(MakeDemoPath());
+
+  EXPECT_NEAR(path->length(), reversed_path->length(), 1e-6);
+
+  for (double d = 0; d < path->length(); d += 0.01) {
+    EXPECT_LT(
+        (path->Theta(d) - reversed_path->Theta(path->length() - d)).norm(),
+        1e-5);
+    EXPECT_LT(
+        (path->Omega(d) + reversed_path->Omega(path->length() - d)).norm(),
+        1e-5);
+    EXPECT_LT(
+        (path->Alpha(d) - reversed_path->Alpha(path->length() - d)).norm(),
+        1e-5);
+  }
+}
+
+// Tests that we can follow a path.  Look at :trajectory_plot if you want to see
+// the path.
+TEST(TrajectoryTest, RunTrajectory) {
+  Dynamics dynamics(kArmConstants);
+  StateFeedbackLoop<3, 1, 1, double, StateFeedbackHybridPlant<3, 1, 1>,
+                    HybridKalman<3, 1, 1>>
+      hybrid_roll = y2023::control_loops::superstructure::roll::
+          MakeIntegralHybridRollLoop();
+  ::std::unique_ptr<Path> path = MakeDemoPath();
+  Trajectory trajectory(&dynamics, &hybrid_roll.plant(), ::std::move(path),
+                        0.001);
+
+  constexpr double kAlpha0Max = 40.0;
+  constexpr double kAlpha1Max = 60.0;
+  constexpr double kAlpha2Max = 60.0;
+  constexpr double vmax = 11.95;
+
+  const ::Eigen::DiagonalMatrix<double, 3> alpha_unitizer(
+      (::Eigen::DiagonalMatrix<double, 3>().diagonal() << (1.0 / kAlpha0Max),
+       (1.0 / kAlpha1Max), (1.0 / kAlpha2Max))
+          .finished());
+  trajectory.OptimizeTrajectory(alpha_unitizer, vmax);
+
+  double t = 0;
+  ::Eigen::Matrix<double, 4, 1> arm_X;
+  ::Eigen::Matrix<double, 2, 1> roll_X;
+  {
+    ::Eigen::Matrix<double, 3, 1> theta_t = trajectory.ThetaT(0.0);
+    arm_X << theta_t(0), 0.0, theta_t(1), 0.0;
+    roll_X << theta_t(2), 0.0;
+  }
+
+  EKF arm_ekf(&dynamics);
+  arm_ekf.Reset(arm_X);
+  StateFeedbackLoop<3, 1, 1, double, StateFeedbackPlant<3, 1, 1>,
+                    StateFeedbackObserver<3, 1, 1>>
+      roll = y2023::control_loops::superstructure::roll::MakeIntegralRollLoop();
+  roll.mutable_X_hat().setZero();
+  roll.mutable_X_hat().block<2, 1>(0, 0) = roll_X;
+
+  TrajectoryFollower follower(&dynamics, &hybrid_roll, &trajectory);
+  constexpr double sim_dt = 0.00505;
+  while (t < 1.0) {
+    arm_ekf.Correct(
+        (::Eigen::Matrix<double, 2, 1>() << arm_X(0), arm_X(2)).finished(),
+        sim_dt);
+    roll.Correct((::Eigen::Matrix<double, 1, 1>() << roll_X(0)).finished());
+    follower.Update(
+        (Eigen::Matrix<double, 9, 1>() << arm_ekf.X_hat(), roll.X_hat())
+            .finished(),
+        false, sim_dt, vmax, 12.0);
+
+    arm_X = dynamics.UnboundedDiscreteDynamics(
+        arm_X, follower.U().block<2, 1>(0, 0), sim_dt);
+    arm_ekf.Predict(follower.U().block<2, 1>(0, 0), sim_dt);
+
+    roll_X =
+        roll.plant()
+            .Update((Eigen::Matrix<double, 3, 1>() << roll_X, 0.0).finished(),
+                    follower.U().block<1, 1>(2, 0))
+            .block<2, 1>(0, 0);
+    roll.UpdateObserver(follower.U().block<1, 1>(2, 0),
+                        std::chrono::duration_cast<std::chrono::nanoseconds>(
+                            std::chrono::duration<double>(sim_dt)));
+    t += sim_dt;
+  }
+
+  ::Eigen::Matrix<double, 4, 1> final_arm_X;
+  ::Eigen::Matrix<double, 2, 1> final_roll_X;
+  ::Eigen::Matrix<double, 3, 1> final_theta_t =
+      trajectory.ThetaT(trajectory.path().length());
+  final_arm_X << final_theta_t(0), 0.0, final_theta_t(1), 0.0;
+  final_roll_X << final_theta_t(2), 0.0;
+
+  // Verify that we got to the end.
+  EXPECT_TRUE(arm_X.isApprox(final_arm_X, 0.01))
+      << ": X is " << arm_X.transpose() << " final_X is "
+      << final_arm_X.transpose();
+  EXPECT_TRUE(roll_X.isApprox(final_roll_X, 0.01))
+      << ": X is " << roll_X.transpose() << " final_X is "
+      << final_roll_X.transpose();
+
+  // Verify that our goal is at the end.
+  EXPECT_TRUE(
+      final_theta_t.isApprox(trajectory.path().Theta(follower.goal(0))));
+}
+
+}  // namespace testing
+}  // namespace arm
+}  // namespace superstructure
+}  // namespace control_loops
+}  // namespace y2023