diff --git a/y2018/control_loops/superstructure/arm/BUILD b/y2018/control_loops/superstructure/arm/BUILD
index cbb443d..dfdaef4 100644
--- a/y2018/control_loops/superstructure/arm/BUILD
+++ b/y2018/control_loops/superstructure/arm/BUILD
@@ -6,7 +6,10 @@
     hdrs = [
         "trajectory.h",
     ],
-    deps = ["//third_party/eigen"],
+    deps = [
+        ":dynamics",
+        "//third_party/eigen",
+    ],
 )
 
 cc_test(
diff --git a/y2018/control_loops/superstructure/arm/dynamics.cc b/y2018/control_loops/superstructure/arm/dynamics.cc
index 5086e78..87d2178 100644
--- a/y2018/control_loops/superstructure/arm/dynamics.cc
+++ b/y2018/control_loops/superstructure/arm/dynamics.cc
@@ -12,6 +12,8 @@
                    Dynamics::kResistance)
         .finished();
 
+const ::Eigen::Matrix<double, 2, 2> Dynamics::K3_inverse = K3.inverse();
+
 const ::Eigen::Matrix<double, 2, 2> Dynamics::K4 =
     (::Eigen::Matrix<double, 2, 2>()
          << Dynamics::kG1 * Dynamics::kG1 * Dynamics::Kt /
diff --git a/y2018/control_loops/superstructure/arm/dynamics.h b/y2018/control_loops/superstructure/arm/dynamics.h
index 7ad6696..832796a 100644
--- a/y2018/control_loops/superstructure/arm/dynamics.h
+++ b/y2018/control_loops/superstructure/arm/dynamics.h
@@ -52,6 +52,7 @@
 
   // K3, K4 matricies described below.
   static const ::Eigen::Matrix<double, 2, 2> K3;
+  static const ::Eigen::Matrix<double, 2, 2> K3_inverse;
   static const ::Eigen::Matrix<double, 2, 2> K4;
 
   // Generates K1-2 for the arm ODE.
@@ -114,7 +115,7 @@
 
     MatriciesForState(X, &K1, &K2);
 
-    return K3.inverse() * (K1 * alpha_t + K2 * omega_t + K4 * omega_t);
+    return K3_inverse * (K1 * alpha_t + K2 * omega_t + K4 * omega_t);
   }
 
   static const ::Eigen::Matrix<double, 4, 1> UnboundedDiscreteDynamics(
diff --git a/y2018/control_loops/superstructure/arm/trajectory.cc b/y2018/control_loops/superstructure/arm/trajectory.cc
index e5fdfe5..fb1e0a5 100644
--- a/y2018/control_loops/superstructure/arm/trajectory.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory.cc
@@ -4,6 +4,33 @@
 namespace control_loops {
 namespace superstructure {
 namespace arm {
+
+Path::Path(::std::vector<::std::array<double, 6>> list) {
+  distances_.reserve(list.size());
+  ::Eigen::Matrix<double, 2, 1> last_theta;
+  bool first = true;
+
+  // Pull apart the initializer list into points, and compute the distance
+  // lookup for efficient path lookups.
+  for (const auto &e : list) {
+    thetas_.push_back(
+        (::Eigen::Matrix<double, 2, 1>() << e[0], e[1]).finished());
+    omegas_.push_back(
+        (::Eigen::Matrix<double, 2, 1>() << e[2], e[3]).finished());
+    alphas_.push_back(
+        (::Eigen::Matrix<double, 2, 1>() << e[4], e[5]).finished());
+
+    if (first) {
+      distances_.push_back(0.0);
+    } else {
+      distances_.push_back((thetas_.back() - last_theta).norm() +
+                           distances_.back());
+    }
+    first = false;
+    last_theta = thetas_.back();
+  }
+}
+
 Path::Path(::std::initializer_list<::std::array<double, 6>> list) {
   distances_.reserve(list.size());
   ::Eigen::Matrix<double, 2, 1> last_theta;
@@ -29,6 +56,253 @@
     last_theta = thetas_.back();
   }
 }
+
+::std::vector<double> Trajectory::CurvatureOptimizationPass(
+    const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double 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);
+
+    // TODO(austin): We are looking up the index in the path 3 times here.
+    const ::Eigen::Matrix<double, 2, 1> theta = path_->Theta(distance);
+    const ::Eigen::Matrix<double, 2, 1> omega = path_->Omega(distance);
+    const ::Eigen::Matrix<double, 2, 1> alpha = path_->Alpha(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;
+
+    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.
+    const ::Eigen::Matrix<double, 2, 1> vk1 =
+        Dynamics::K3_inverse * (K1 * alpha + K2 * omega_square * omega);
+    const ::Eigen::Matrix<double, 2, 1> vk2 =
+        Dynamics::K3_inverse * Dynamics::K4 * omega;
+
+    double min_good_ddot =
+        ::std::sqrt(1.0 / ::std::max(0.001, (alpha_unitizer * alpha).norm()));
+
+    // Loop through all the various vmin, vmax combinations.
+    for (const double c : {-vmax, vmax}) {
+      // Also loop through saturating theta0 and theta1
+      for (const ::std::pair<double, double> ab :
+           {::std::pair<double, double>{vk1(0, 0), vk2(0, 0)},
+            ::std::pair<double, double>{vk1(1, 0), vk2(1, 0)}}) {
+        const double a = ab.first;
+        const double b = ab.second;
+        const double sqrt_number = b * b - 4.0 * a * c;
+        // 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;
+            // Finally, make sure the velocity is positive and valid.
+            if ((U.array().abs() <= vmax + 1e-6).all() && ddot > 0.0) {
+              min_good_ddot = ::std::min(min_good_ddot, ddot);
+            }
+          }
+        }
+      }
+    }
+
+    max_dvelocity_unfiltered.push_back(min_good_ddot);
+  }
+  return max_dvelocity_unfiltered;
+}
+
+double Trajectory::FeasableForwardsAcceleration(
+    double goal_distance, double goal_velocity, double vmax,
+    const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer) {
+  // TODO(austin): We are looking up the index in the path 3 times here.
+  const ::Eigen::Matrix<double, 2, 1> theta = path_->Theta(goal_distance);
+  const ::Eigen::Matrix<double, 2, 1> omega = path_->Omega(goal_distance);
+  const ::Eigen::Matrix<double, 2, 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;
+
+  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();
+
+  const ::Eigen::Matrix<double, 2, 1> k_constant =
+      Dynamics::K3_inverse * ((K1 * alpha + K2 * omega_square * omega) *
+                                  goal_velocity * goal_velocity +
+                              Dynamics::K4 * omega * goal_velocity);
+  const ::Eigen::Matrix<double, 2, 1> k_scalar =
+      Dynamics::K3_inverse * K1 * omega;
+
+  double 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();
+
+  for (double c : {-vmax, 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)}}) {
+      const double a = ab.first;
+      const double b = ab.second;
+      const double voltage_accel = (c - a) / b;
+      const ::Eigen::Matrix<double, 2, 1> U =
+          k_constant + k_scalar * voltage_accel;
+
+      if (voltage_accel > 0.0 && (U.array().abs() <= vmax + 1e-6).all())
+        goal_acceleration = ::std::min(voltage_accel, goal_acceleration);
+    }
+  }
+
+  return goal_acceleration;
+}
+
+double Trajectory::FeasableBackwardsAcceleration(
+    double goal_distance, double goal_velocity, double vmax,
+    const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer) {
+    const ::Eigen::Matrix<double, 2, 1> theta = path_->Theta(goal_distance);
+    const ::Eigen::Matrix<double, 2, 1> omega = path_->Omega(goal_distance);
+    const ::Eigen::Matrix<double, 2, 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;
+
+    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();
+
+    const ::Eigen::Matrix<double, 2, 1> k_constant =
+        Dynamics::K3_inverse *
+        ((K1 * alpha + K2 * omega_square * omega) * goal_velocity * goal_velocity +
+         Dynamics::K4 * omega * goal_velocity);
+    const ::Eigen::Matrix<double, 2, 1> k_scalar =
+        Dynamics::K3_inverse * K1 * omega;
+
+    double 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();
+    for (double c : {-vmax, 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)}}) {
+        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, 2, 1> U =
+            k_constant + k_scalar * voltage_accel;
+
+        if (voltage_accel < 0.0 && (U.array().abs() <= vmax + 1e-6).all()) {
+          goal_acceleration = ::std::min(-voltage_accel, goal_acceleration);
+        }
+      }
+    }
+
+    return goal_acceleration;
+}
+
+::std::vector<double> Trajectory::BackwardsOptimizationPass(
+    const ::std::vector<double> &max_dvelocity,
+    const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double 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 prev_distance = DistanceForIndex(i + 1);
+
+    // Now, integrate with respect to distance (not time like normal).
+    // Subdivide the plan step size into kNumSteps steps.
+    double integrated_distance = 0.0;
+    double integrated_velocity = previous_velocity;
+    constexpr int kNumSteps = 10;
+    for (int j = 0; j < kNumSteps; ++j) {
+      // Compute the acceleration with respect to time.
+      const double acceleration = FeasableBackwardsAcceleration(
+          prev_distance - integrated_distance, integrated_velocity, vmax,
+          alpha_unitizer);
+
+      // And then integrate it with respect to distance.
+      const double integration_step_size_distance =
+          step_size_ / static_cast<double>(kNumSteps);
+      integrated_distance += integration_step_size_distance;
+      integrated_velocity =
+          ::std::sqrt(2.0 * acceleration * integration_step_size_distance +
+                      ::std::pow(integrated_velocity, 2));
+    }
+    max_dvelocity_back_pass[i] =
+        ::std::min(integrated_velocity, max_dvelocity_back_pass[i]);
+  }
+
+  return max_dvelocity_back_pass;
+}
+
+::std::vector<double> Trajectory::ForwardsOptimizationPass(
+    const ::std::vector<double> &max_dvelocity,
+    const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double vmax) {
+  ::std::vector<double> max_dvelocity_forward_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_forward_pass[i - 1];
+    const double previous_distance = DistanceForIndex(i - 1);
+
+    // Now, integrate with respect to distance (not time like normal).
+    // Subdivide the plan step size into kNumSteps steps.
+    double integrated_distance = 0.0;
+    double integrated_velocity = previous_velocity;
+    constexpr int kNumSteps = 10;
+    for (int j = 0; j < kNumSteps; ++j) {
+      // Compute the acceleration with respect to time.
+      const double acceleration = FeasableForwardsAcceleration(
+          previous_distance + integrated_distance, integrated_velocity, vmax,
+          alpha_unitizer);
+
+      // And then integrate it with respect to distance.
+      const double integration_step_size_distance =
+          step_size_ / static_cast<double>(kNumSteps);
+      integrated_distance += integration_step_size_distance;
+      integrated_velocity =
+          ::std::sqrt(2.0 * acceleration * integration_step_size_distance +
+                      ::std::pow(integrated_velocity, 2));
+    }
+
+    max_dvelocity_forward_pass[i] =
+        ::std::min(integrated_velocity, max_dvelocity_forward_pass[i]);
+  }
+
+  return max_dvelocity_forward_pass;
+}
+
 }  // namespace arm
 }  // namespace superstructure
 }  // namespace control_loops
diff --git a/y2018/control_loops/superstructure/arm/trajectory.h b/y2018/control_loops/superstructure/arm/trajectory.h
index a9136b4..2c9d68c 100644
--- a/y2018/control_loops/superstructure/arm/trajectory.h
+++ b/y2018/control_loops/superstructure/arm/trajectory.h
@@ -3,6 +3,8 @@
 
 #include "Eigen/Dense"
 
+#include "y2018/control_loops/superstructure/arm/dynamics.h"
+
 namespace y2018 {
 namespace control_loops {
 namespace superstructure {
@@ -16,6 +18,9 @@
   // Constructs a path from an initializer list of (theta0, theta1, omega0,
   // omega1, alpha0, alpha1)
   Path(::std::initializer_list<::std::array<double, 6>> list);
+  // TODO(austin): I'd like to not have 2 constructors, but it looks like C++
+  // doesn't want that.
+  Path(::std::vector<::std::array<double, 6>> list);
 
   // Returns the length of the path in radians.
   double length() const { return distances_.back(); }
@@ -33,6 +38,8 @@
     return Interpolate(alphas_, distance);
   }
 
+  const ::std::vector<double> &distances() const { return distances_; }
+
  private:
   // Interpolates the function represented by the list of points at the provided
   // distance.  Distances outside the range will get truncated.  This is a
@@ -48,10 +55,10 @@
     }
     // Keep a max and min, and pull them in towards eachother until they bound
     // our point exactly.
-    size_t before = ::std::distance(
+    size_t after = ::std::distance(
         distances_.begin(),
         ::std::lower_bound(distances_.begin(), distances_.end(), distance));
-    size_t after = before + 1;
+    size_t before = after - 1;
     return (distance - distances_[before]) * (points[after] - points[before]) /
                (distances_[after] - distances_[before]) +
            points[before];
@@ -68,6 +75,133 @@
   ::std::vector<double> 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(Path *path, double gridsize)
+      : path_(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)) {}
+
+  // Optimizes the trajectory.  The path will adhere to the constraints that
+  // || angular acceleration * alpha_unitizer || < 1, and the applied voltage <
+  // vmax.
+  void OptimizeTrajectory(const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer,
+                          double vmax) {
+    max_dvelocity_unfiltered_ = CurvatureOptimizationPass(alpha_unitizer, 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_ = BackwardsOptimizationPass(max_dvelocity_unfiltered_,
+                                               alpha_unitizer, vmax);
+    max_dvelocity_forward_pass_ = ForwardsOptimizationPass(
+        max_dvelocity_, alpha_unitizer, 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 < vmax.  Returns the velocities.
+  ::std::vector<double> CurvatureOptimizationPass(
+      const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double vmax);
+
+  // Computes the maximum forwards feasable acceleration at a given position
+  // while adhering to the 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 vmax,
+      const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer);
+
+  // Computes the maximum backwards feasable acceleration at a given position
+  // while adhering to the 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 vmax,
+      const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer);
+
+  // Executes the backwards path optimization pass.
+  ::std::vector<double> BackwardsOptimizationPass(
+      const ::std::vector<double> &max_dvelocity,
+      const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double vmax);
+
+  // Executes the forwards path optimization pass.
+  ::std::vector<double> ForwardsOptimizationPass(
+      const ::std::vector<double> &max_dvelocity,
+      const ::Eigen::Matrix<double, 2, 2> &alpha_unitizer, double 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() const { return max_dvelocity_; }
+  // 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_forward_pass() const {
+    return max_dvelocity_forward_pass_;
+  }
+
+ 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);
+  }
+
+  // The path to follow.
+  const Path *path_ = nullptr;
+  // 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_;
+  ::std::vector<double> max_dvelocity_forward_pass_;
+};
+
 }  // namespace arm
 }  // namespace superstructure
 }  // namespace control_loops
diff --git a/y2018/control_loops/superstructure/arm/trajectory_test.cc b/y2018/control_loops/superstructure/arm/trajectory_test.cc
index 5d5350b..a4e7822 100644
--- a/y2018/control_loops/superstructure/arm/trajectory_test.cc
+++ b/y2018/control_loops/superstructure/arm/trajectory_test.cc
@@ -32,6 +32,19 @@
   }
 }
 
+// Tests a path where picking the right point to interpolate for matters.
+TEST(TrajectoryTest, HarderPath) {
+  Path p({{{0.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
+          {{1.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
+          {{1.0, 1.0, 0.0, 1.0, 0.0, 0.0}}});
+
+  EXPECT_TRUE(p.Theta(0.5).isApprox(
+      (::Eigen::Matrix<double, 2, 1>() << 0.5, 0.0).finished()));
+
+  EXPECT_TRUE(p.Theta(1.5).isApprox(
+      (::Eigen::Matrix<double, 2, 1>() << 1.0, 0.5).finished()));
+}
+
 // Tests that a path with an even number of points works as expected.
 TEST(TrajectoryTest, EvenPointNumbers) {
   Path p({{{0.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
@@ -59,6 +72,47 @@
       (::Eigen::Matrix<double, 2, 1>() << 3.0, 0.0).finished()));
 }
 
+
+// Tests that we can compute the indices of the plan for a given distance correctly.
+TEST(TrajectoryTest, IndicesForDistanceTest) {
+  // Start with a stupid simple plan.
+  Path p({{{0.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
+          {{1.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
+          {{2.0, 0.0, 1.0, 0.0, 0.0, 0.0}},
+          {{3.0, 0.0, 1.0, 0.0, 0.0, 0.0}}});
+  Trajectory t(&p, 0.1);
+
+  // 0 - 3.0 every 0.1 should be 31 points.
+  EXPECT_EQ(t.num_plan_points(), 31);
+
+  // Verify that something centered in a grid cell returns the points on either side.
+  EXPECT_EQ(::std::make_pair(static_cast<size_t>(0), static_cast<size_t>(1)),
+            t.IndicesForDistance(0.05));
+  EXPECT_EQ(::std::make_pair(static_cast<size_t>(1), static_cast<size_t>(2)),
+            t.IndicesForDistance(0.15));
+
+  // Verify that something on an edge returns the expected result.
+  EXPECT_EQ(::std::make_pair(static_cast<size_t>(1), static_cast<size_t>(2)),
+            t.IndicesForDistance(0.1));
+
+  // Verify what small deviations result.
+  EXPECT_EQ(::std::make_pair(static_cast<size_t>(1), static_cast<size_t>(2)),
+            t.IndicesForDistance(0.1001));
+  EXPECT_EQ(::std::make_pair(static_cast<size_t>(0), static_cast<size_t>(1)),
+            t.IndicesForDistance(0.0999));
+
+  // Verify that blowing past the ends works.
+  EXPECT_EQ(::std::make_pair(static_cast<size_t>(29), static_cast<size_t>(30)),
+            t.IndicesForDistance(4.0));
+  EXPECT_EQ(::std::make_pair(static_cast<size_t>(0), static_cast<size_t>(1)),
+            t.IndicesForDistance(-0.1));
+
+  // Verify that the index to distance calculation also works.
+  EXPECT_EQ(0.0, t.DistanceForIndex(0));
+  EXPECT_EQ(0.1, t.DistanceForIndex(1));
+  EXPECT_EQ(3.0, t.DistanceForIndex(30));
+}
+
 }  // namespace testing
 }  // namespace arm
 }  // namespace superstructure