From 96d7850888fed72bf4e9d977227a76af3ff1604f Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Tue, 9 Jun 2026 21:34:27 -0700 Subject: [PATCH] feat(long): fix accelersonality rubbernband --- .../lib/accel_personality/accel_controller.py | 90 +++++++++++++++---- .../lib/accel_personality/constants.py | 24 ++++- .../tests/test_accel_controller.py | 74 +++++++++++++++ 3 files changed, 170 insertions(+), 18 deletions(-) diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py index fce121b8b7..e31635b3dd 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py @@ -11,10 +11,12 @@ Three independent, per-tier levers keyed by the cereal AccelerationPersonality o 2. Accel rise rate - get_rise_rate(), slews the accel ceiling upward. 3. Early soft braking - smooth_target_accel(), front-loads a gentle decel BEFORE the plan brakes, never commanding less braking than the plan (never-weaken invariant), with hard-brake / FCW / - should_stop / closing-lead / e2e bypass back to the stock plan. + should_stop / closing-lead / e2e bypass back to the stock plan. Engagement is hysteretic and + suppressed when a lead is pulling away (anti rubber-band), and a stop-hold latch prevents the + stop -> creep -> stop "double stop" on small lead twitches. Disabled or Normal == stock by construction: Normal tier uses the stock ceiling/rise literals, and a -disabled controller forces Normal and passes the target through untouched. +disabled controller forces Normal, passes the target through untouched, and never latches stop-hold. """ from collections.abc import Sequence @@ -29,7 +31,9 @@ from openpilot.sunnypilot import get_sanitize_int_param from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import \ NORMAL, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, A_CRUISE_MAX_V, RISE_RATE, SMOOTH_DECEL_BP, \ SMOOTH_DECEL_V, BRAKE_DEEPENING_JERK, BRAKE_RELEASE_JERK, ACCEL_RISE_JERK, SMOOTH_DECEL_LOOKAHEAD_T, \ - MIN_SMOOTH_BRAKE_NEED, HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_NEED, CLOSING_LEAD_VREL, CLOSING_LEAD_TTC + MIN_SMOOTH_BRAKE_NEED, HARD_BRAKE_TARGET_ACCEL, HARD_BRAKE_NEED, CLOSING_LEAD_VREL, CLOSING_LEAD_TTC, \ + SMOOTH_ENTER, SMOOTH_EXIT, EARLY_BRAKE_PULLAWAY_VREL, EARLY_BRAKE_SPEED_BP, EARLY_BRAKE_SPEED_V, \ + STOP_HOLD_EGO_V, STOP_HOLD_LEAD_V, STOP_HOLD_RELEASE_LEAD_V, STOP_HOLD_RELEASE_DREL, STOP_HOLD_ACCEL _ZERO_ACCEL_EPS = 1e-6 @@ -43,12 +47,19 @@ class AccelController: self._enabled: bool = self._params.get_bool("AccelPersonalityEnabled") self._personality = NORMAL # cereal AccelerationPersonality ordinal self._v_ego = 0.0 + self._lead_status = False + self._v_rel = 0.0 + self._v_lead = 0.0 + self._d_rel = 999.0 self._lead_closing = False self._last_target_accel = 0.0 self._brake_need = 0.0 self._decel_target = 0.0 self._smooth_active = False + self._smooth_latched = False self._bypassed = False + self._stop_held = False + self._stop_d_rel = 0.0 self._read_params() def _read_params(self) -> None: @@ -63,19 +74,35 @@ class AccelController: if self._frame % int(1. / DT_MDL) == 0: self._read_params() self._v_ego = sm['carState'].vEgo - self._lead_closing = self._compute_lead_closing(sm) + lead = sm['radarState'].leadOne + self._lead_status = bool(lead.status) + self._v_rel = float(lead.vRel) if lead.status else 0.0 + self._v_lead = float(lead.vLead) if lead.status else 0.0 + self._d_rel = float(lead.dRel) if lead.status else 999.0 + self._lead_closing = self._compute_lead_closing() + self._update_stop_hold() self._frame += 1 - @staticmethod - def _compute_lead_closing(sm: messaging.SubMaster) -> bool: - lead = sm['radarState'].leadOne - if not lead.status: + def _compute_lead_closing(self) -> bool: + if not self._lead_status or self._v_rel >= 0.0: return False - v_rel = float(lead.vRel) - if v_rel >= 0.0: - return False - ttc = float(lead.dRel) / max(-v_rel, 1e-3) - return v_rel <= CLOSING_LEAD_VREL or ttc <= CLOSING_LEAD_TTC + ttc = self._d_rel / max(-self._v_rel, 1e-3) + return self._v_rel <= CLOSING_LEAD_VREL or ttc <= CLOSING_LEAD_TTC + + def _update_stop_hold(self) -> None: + # Latch a stop behind a near-stopped lead; release only on a real departure (not a 1m twitch). + if not self._enabled: + self._stop_held = False + return + + if not self._stop_held: + if self._v_ego < STOP_HOLD_EGO_V and self._lead_status and self._v_lead < STOP_HOLD_LEAD_V: + self._stop_held = True + self._stop_d_rel = self._d_rel + else: + departed = (self._v_lead > STOP_HOLD_RELEASE_LEAD_V) or (self._d_rel - self._stop_d_rel > STOP_HOLD_RELEASE_DREL) + if departed or self._v_ego > 1.5 or not self._lead_status: + self._stop_held = False # --- positive accel levers --- @@ -95,23 +122,44 @@ class AccelController: raw_target_accel = float(raw_target_accel) self._brake_need = self._compute_brake_need(raw_target_accel, accel_trajectory, t_idxs) self._decel_target = 0.0 + out = self._smooth_core(raw_target_accel, should_stop, reset, stock_brake) + return self._apply_stop_hold(out) + def _smooth_core(self, raw_target_accel: float, should_stop: bool, reset: bool, stock_brake: bool) -> float: if reset or not self._enabled: self._bypassed = False + self._smooth_latched = False return self._passthrough(raw_target_accel) # e2e/blended path: never reshape braking (the planner already min-blends e2e/mpc, and vision stops # are the model's job per the lead->ACC policy). if stock_brake and (raw_target_accel < 0.0 or self._brake_need >= MIN_SMOOTH_BRAKE_NEED): self._bypassed = False + self._smooth_latched = False return self._passthrough(raw_target_accel) self._bypassed = self._emergency_bypass(raw_target_accel, should_stop) if self._bypassed: + self._smooth_latched = False return self._passthrough(raw_target_accel) - if self._brake_need < MIN_SMOOTH_BRAKE_NEED: - # no decel predicted: jerk-limit the (positive) accel, but never weaken an active brake + # A present lead pulling away makes a predicted decel spurious in a following context -> suppress + # the anticipatory brake (kills the low-speed rubber-band). The plan's real brake still passes below. + eff_brake_need = self._brake_need + if self._lead_status and self._v_rel > EARLY_BRAKE_PULLAWAY_VREL: + eff_brake_need = 0.0 + # Taper the anticipatory brake out at low speed (stop-and-go rubber-band zone). + eff_brake_need *= float(np.interp(self._v_ego, EARLY_BRAKE_SPEED_BP, EARLY_BRAKE_SPEED_V)) + + # hysteresis: engage only on a clear predicted decel, hold until it clearly clears (no toggling) + if self._smooth_latched: + if eff_brake_need < SMOOTH_EXIT: + self._smooth_latched = False + elif eff_brake_need >= SMOOTH_ENTER: + self._smooth_latched = True + + if not self._smooth_latched: + # no anticipatory brake: jerk-limit the (positive) accel, but never weaken an active brake self._smooth_active = False slewed = self._slew(raw_target_accel) out = min(slewed, raw_target_accel) if raw_target_accel < 0.0 else slewed @@ -119,11 +167,18 @@ class AccelController: # decel predicted: front-load a gentle EARLY target, but NEVER weaker than the plan. self._smooth_active = True - self._decel_target = self.get_decel_target(self._brake_need) + self._decel_target = self.get_decel_target(eff_brake_need) commanded = min(raw_target_accel, self._decel_target) # early-soft onset, can only brake >= plan slewed = self._slew(commanded) return self._finalize(min(slewed, raw_target_accel)) # post-slew clamp: never weaker than plan + def _apply_stop_hold(self, out: float) -> float: + # Latched stop: hold gently (no creep), but never weaken a deeper plan brake. + if self._stop_held: + out = self._clean_accel(min(out, STOP_HOLD_ACCEL)) + self._last_target_accel = out + return out + def _compute_brake_need(self, raw_target_accel: float, accel_trajectory: Sequence[float], t_idxs: Sequence[float]) -> float: min_accel = float(raw_target_accel) for accel, t in zip(accel_trajectory, t_idxs, strict=False): @@ -193,3 +248,6 @@ class AccelController: def bypassed(self) -> bool: return self._bypassed + + def stop_held(self) -> bool: + return self._stop_held diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index 4be7477116..11e7a35eab 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -44,7 +44,7 @@ RISE_RATE = { # Gentle, human-like progression: lead the brake early and softly rather than late and hard. SMOOTH_DECEL_BP = [0.0, 0.4, 0.8, 1.2, 1.6, 2.0, 2.4] SMOOTH_DECEL_V = { - ECO: [0.00, -0.10, -0.24, -0.44, -0.68, -0.92, -1.15], + ECO: [0.00, -0.08, -0.20, -0.38, -0.60, -0.82, -1.05], NORMAL: [0.00, -0.13, -0.30, -0.55, -0.84, -1.12, -1.40], SPORT: [0.00, -0.17, -0.40, -0.72, -1.05, -1.35, -1.65], } @@ -53,7 +53,7 @@ SMOOTH_DECEL_V = { # Deepening only shapes the EARLY front-loaded brake; the never-weaken clamp lets a real plan brake # through immediately, so a soft deepening rate never delays genuine braking. BRAKE_DEEPENING_JERK = { - ECO: 0.6, + ECO: 0.5, NORMAL: 0.8, SPORT: 1.0, } @@ -80,3 +80,23 @@ HARD_BRAKE_NEED = 2.6 # accel thresholds above (mirrors the route 000003da lesson - shaping must yield to closing dynamics). CLOSING_LEAD_VREL = -8.0 # m/s, lead approaching faster than this CLOSING_LEAD_TTC = 4.0 # s, time-to-collision below this + +# --- early-soft-braking consistency (anti rubber-band, route 00000423 finding) --- +# Hysteresis on engagement so the anticipatory brake doesn't toggle on transient predicted-decel dips. +SMOOTH_ENTER = 0.40 # brake_need to ENGAGE early-soft braking +SMOOTH_EXIT = 0.15 # brake_need to DISENGAGE once engaged +# A present lead pulling away makes the model's predicted decel spurious in a following context; +# suppress the anticipatory brake then (the plan's real brake still passes via never-weaken). +EARLY_BRAKE_PULLAWAY_VREL = 0.5 # m/s, lead opening faster than this -> no anticipatory brake +# Taper the anticipatory brake out at low speed: in stop-and-go it only adds rubber-band and the stops +# are owned by stop-hold / should_stop anyway (route 00000423: the flips lived < 7 m/s). The plan's real +# brake still passes via never-weaken, so no braking is lost - only the spurious early shaping. +EARLY_BRAKE_SPEED_BP = [6.0, 9.0] # m/s +EARLY_BRAKE_SPEED_V = [0.0, 1.0] # anticipatory-brake gain + +# --- stop-hold / anti-creep (no double-stop / roll, route 00000423 finding) --- +STOP_HOLD_EGO_V = 0.5 # ego considered stopped below this (m/s) +STOP_HOLD_LEAD_V = 1.0 # lead considered stopped below this (m/s) +STOP_HOLD_RELEASE_LEAD_V = 1.5 # lead clearly departing -> release latch (m/s); above a 1m twitch +STOP_HOLD_RELEASE_DREL = 2.0 # ... or the gap opened this much from the stop (m) +STOP_HOLD_ACCEL = -0.3 # gentle hold while latched (no creep); never weakens a deeper plan brake diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py index 626cb0994c..c6cd5581f8 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py @@ -185,6 +185,80 @@ def test_e2e_brake_passthrough(): assert not ctrl.smooth_active() +# --- rubber-band fixes: pull-away suppression + hysteresis --- + +def test_lead_pullaway_suppresses_early_brake(): + # lead pulling away (vRel>0.5) -> model-predicted decel is spurious -> no anticipatory brake + ctrl = make_controller(personality=ECO) + ctrl.update(make_sm(v_ego=10.0, lead_status=True, v_rel=1.5, d_rel=20.0)) + out = ctrl.smooth_target_accel(0.0, flat_traj(-1.5), T_IDXS, should_stop=False) + assert not ctrl.smooth_active() + assert out >= -_EPS + + +def test_low_speed_tapers_anticipatory_brake(): + # at low speed the anticipatory brake is tapered out (rubber-band zone); plan brake still passes + ctrl = make_controller(personality=ECO) + ctrl.update(make_sm(v_ego=3.0)) # < 6 m/s -> gain 0 + out = ctrl.smooth_target_accel(0.0, flat_traj(-1.5), T_IDXS, should_stop=False) + assert not ctrl.smooth_active() + assert out >= -_EPS + # same predicted decel at speed engages + ctrl.update(make_sm(v_ego=15.0)) + out = ctrl.smooth_target_accel(0.0, flat_traj(-1.5), T_IDXS, should_stop=False) + assert ctrl.smooth_active() + + +def test_smooth_hysteresis_no_toggle(): + ctrl = make_controller(personality=NORMAL) + ctrl.update(make_sm(v_ego=20.0)) + # brake_need 0.25 (between EXIT 0.15 and ENTER 0.40): does NOT engage from idle + ctrl.smooth_target_accel(0.0, flat_traj(-0.25), T_IDXS, should_stop=False) + assert not ctrl.smooth_active() + # clear decel engages + ctrl.smooth_target_accel(0.0, flat_traj(-0.6), T_IDXS, should_stop=False) + assert ctrl.smooth_active() + # drops to 0.25 (still > EXIT): stays engaged (hysteresis, no toggle) + ctrl.smooth_target_accel(0.0, flat_traj(-0.25), T_IDXS, should_stop=False) + assert ctrl.smooth_active() + + +# --- stop-hold / anti-creep (double-stop fix) --- + +def test_stop_hold_prevents_creep(): + ctrl = make_controller(personality=ECO) + ctrl.update(make_sm(v_ego=0.0, lead_status=True, v_rel=0.0, d_rel=5.0)) # stopped behind stopped lead + assert ctrl.stop_held() + out = ctrl.smooth_target_accel(0.3, flat_traj(0.3), T_IDXS, should_stop=False) # plan wants to creep + assert out <= 0.0 # creep suppressed + + +def test_stop_hold_ignores_lead_twitch(): + ctrl = make_controller(personality=ECO) + ctrl.update(make_sm(v_ego=0.0, lead_status=True, v_rel=0.0, d_rel=5.0)) + assert ctrl.stop_held() + ctrl.update(make_sm(v_ego=0.0, lead_status=True, v_rel=1.0, d_rel=5.8)) # twitch: vLead 1.0<1.5, dRel +0.8<2.0 + assert ctrl.stop_held() + + +def test_stop_hold_releases_on_real_departure(): + ctrl = make_controller(personality=ECO) + ctrl.update(make_sm(v_ego=0.0, lead_status=True, v_rel=0.0, d_rel=5.0)) + assert ctrl.stop_held() + ctrl.update(make_sm(v_ego=0.0, lead_status=True, v_rel=2.0, d_rel=7.5)) # lead clearly departing (vLead 2.0) + assert not ctrl.stop_held() + out = ctrl.smooth_target_accel(0.5, flat_traj(0.5), T_IDXS, should_stop=False) + assert out > 0.0 # launch allowed + + +def test_stop_hold_disabled_is_stock(): + ctrl = make_controller(enabled=False) + ctrl.update(make_sm(v_ego=0.0, lead_status=True, v_rel=0.0, d_rel=5.0)) + assert not ctrl.stop_held() # never latches when disabled (off == stock) + out = ctrl.smooth_target_accel(0.3, flat_traj(0.3), T_IDXS, should_stop=False) + assert out == pytest.approx(0.3, abs=_EPS) + + # --- param sanitation --- def test_out_of_range_personality_clamps():