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.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 4da3dee..68664ac 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -567,6 +567,7 @@
::std::chrono::nanoseconds dt, ::Eigen::Matrix<double, 2, 1> *state) {
double dt_float = ::aos::time::DurationInSeconds(dt);
+ const double last_distance = (*state)(0);
// TODO(austin): This feels like something that should be pulled out into
// a library for re-use.
*state = RungeKutta(
@@ -575,6 +576,12 @@
return (::Eigen::Matrix<double, 2, 1>() << x(1), xva(2)).finished();
},
*state, dt_float);
+ // Force the distance to move forwards, to guarantee that we actually finish
+ // the planning.
+ constexpr double kMinDistanceIncrease = 1e-7;
+ if ((*state)(0) < last_distance + kMinDistanceIncrease) {
+ (*state)(0) = last_distance + kMinDistanceIncrease;
+ }
::Eigen::Matrix<double, 3, 1> result = FFAcceleration((*state)(0));
(*state)(1) = result(1);
@@ -589,6 +596,13 @@
result.back()(1) = 0.0;
while (!is_at_end(state)) {
+ if (state_is_faulted(state)) {
+ LOG(WARNING)
+ << "Found invalid state in generating spline and aborting. This is "
+ "likely due to a spline with extremely high jerk/changes in "
+ "curvature with an insufficiently small step size.";
+ return {};
+ }
result.emplace_back(GetNextXVA(dt, &state));
}
return result;
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(); }
diff --git a/frc971/control_loops/python/graph.py b/frc971/control_loops/python/graph.py
index 7e98a38..60d1e65 100644
--- a/frc971/control_loops/python/graph.py
+++ b/frc971/control_loops/python/graph.py
@@ -41,14 +41,15 @@
traj = Trajectory(distanceSpline)
traj.Plan()
XVA = traj.GetPlanXVA(dT)
- self.draw_x_axis(cr, start, height, zero, XVA, end)
- self.drawVelocity(cr, XVA, start, height, skip, zero, end)
- self.drawAcceleration(cr, XVA, start, height, skip, zero,
- AXIS_MARGIN_SPACE, end)
- self.drawVoltage(cr, XVA, start, height, skip, traj, zero, end)
- cr.set_source_rgb(0, 0, 0)
- cr.move_to(-1.0 * AXIS_MARGIN_SPACE, zero + height / 2.0)
- cr.line_to(AXIS_MARGIN_SPACE - SCREEN_SIZE, zero + height / 2.0)
+ if len(XVA[0]) > 0:
+ self.draw_x_axis(cr, start, height, zero, XVA, end)
+ self.drawVelocity(cr, XVA, start, height, skip, zero, end)
+ self.drawAcceleration(cr, XVA, start, height, skip, zero,
+ AXIS_MARGIN_SPACE, end)
+ self.drawVoltage(cr, XVA, start, height, skip, traj, zero, end)
+ cr.set_source_rgb(0, 0, 0)
+ cr.move_to(-1.0 * AXIS_MARGIN_SPACE, zero + height / 2.0)
+ cr.line_to(AXIS_MARGIN_SPACE - SCREEN_SIZE, zero + height / 2.0)
cr.stroke()
def connectLines(self, cr, points, color):