Prevent infinite loops in Trajectory planning

There were some cases where splines with really extreme jerks and too
high of timesteps would end up in infiite loops because the planning
would end up with a backwards velocity. Handle those cases more
explicitly.

Change-Id: Ib091d8cbebc1874282a65b4b7b8a390dff9c9054
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index a5b9807..6be898a 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -138,6 +138,13 @@
     return state(0) > length() - 1e-4;
   }
 
+  // Returns true if the state is invalid or unreasonable in some way.
+  bool state_is_faulted(::Eigen::Matrix<double, 2, 1> state) const {
+    // Consider things faulted if the current velocity implies we are going
+    // backwards or if any infinities/NaNs have crept in.
+    return state(1) < 0 || !state.allFinite();
+  }
+
   // Returns the length of the path in meters.
   double length() const { return spline_->length(); }