From b2b7d21b7b685a2785d1beede3d223f0bb954807 Mon Sep 17 00:00:00 2001 From: Jason Wen Date: Mon, 6 Jan 2025 19:28:39 -0500 Subject: [PATCH] Revert "Fix low-speed allow_throttle behavior in long planner (#33894)" --- .../controls/lib/longitudinal_mpc_lib/long_mpc.py | 4 +--- selfdrive/controls/lib/longitudinal_planner.py | 10 +++++----- 2 files changed, 6 insertions(+), 8 deletions(-) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 65e1421d77..fa6d5542bd 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -347,8 +347,7 @@ class LongitudinalMpc: lead_1_obstacle = lead_xv_1[:,0] + get_stopped_equivalence_factor(lead_xv_1[:,1]) self.params[:,0] = ACCEL_MIN - # negative accel constraint causes problems because negative speed is not allowed - self.params[:,1] = max(0.0, self.max_a) + self.params[:,1] = self.max_a # Update in ACC mode or ACC/e2e blend if self.mode == 'acc': @@ -357,7 +356,6 @@ class LongitudinalMpc: # Fake an obstacle for cruise, this ensures smooth acceleration to set speed # when the leads are no factor. v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05) - # TODO does this make sense when max_a is negative? v_upper = v_ego + (T_IDXS * self.max_a * 1.05) v_cruise_clipped = np.clip(v_cruise * np.ones(N+1), v_lower, diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index f1637d960c..5f47cebb1f 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -22,7 +22,7 @@ A_CRUISE_MAX_VALS = [1.6, 1.2, 0.8, 0.6] A_CRUISE_MAX_BP = [0., 10.0, 25., 40.] CONTROL_N_T_IDX = ModelConstants.T_IDXS[:CONTROL_N] ALLOW_THROTTLE_THRESHOLD = 0.5 -MIN_ALLOW_THROTTLE_SPEED = 2.5 +ACCEL_LIMIT_MARGIN = 0.05 # Lookup table for turns _A_TOTAL_MAX_V = [1.7, 3.2] @@ -151,12 +151,12 @@ class LongitudinalPlanner: self.v_model_error = get_speed_error(sm['modelV2'], v_ego) x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'], self.v_model_error) # Don't clip at low speeds since throttle_prob doesn't account for creep - self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= MIN_ALLOW_THROTTLE_SPEED + self.allow_throttle = throttle_prob > ALLOW_THROTTLE_THRESHOLD or v_ego <= 5.0 if not self.allow_throttle: - clipped_accel_coast = max(accel_coast, accel_limits_turns[0]) - clipped_accel_coast_interp = interp(v_ego, [MIN_ALLOW_THROTTLE_SPEED, MIN_ALLOW_THROTTLE_SPEED*2], [accel_limits_turns[1], clipped_accel_coast]) - accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast_interp) + # MPC breaks when accel limits would cause negative velocity within the MPC horizon, so we clip the max accel limit at vEgo/T_MAX plus a bit of margin + clipped_accel_coast = max(accel_coast, accel_limits_turns[0], -v_ego / T_IDXS_MPC[-1] + ACCEL_LIMIT_MARGIN) + accel_limits_turns[1] = min(accel_limits_turns[1], clipped_accel_coast) if force_slow_decel: v_cruise = 0.0