feat(long): generalize jerk-cost relax to onset

sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py

@@ -7,6 +7,8 @@ See the LICENSE.md file in the root directory for more details.
 Acceleration Personality (ECO / NORMAL / SPORT). Tunes only MPC INPUTS, never the output:
   * positive-accel ceiling + speed-dependent per-cycle open-rate -> tier-scaled take-off from a stop
     (the open-rate is fast near v=0 so launch is never delayed, tapering to a steady-state rate at speed);
+  * jerk-cost relaxation (scales the core MPC's jerk_factor) -> smooth accel/decel onset: near a stop, on
+    any fresh accel<->decel direction change, and when the tracked lead is itself braking hard;
   * add-only, speed-dependent follow-gap widen on the MPC t_follow -> earlier/gentler braking, roomier gap;
   * sticky should_stop hysteresis -> no stop-and-go gas-brake-gas-brake.
 Add-only gap => desired distance >= stock => braking >= stock. Disabled => stock everywhere (byte-stock).
@@ -21,8 +23,36 @@ from openpilot.common.realtime import DT_MDL
 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, STOCK_A_CRUISE_MAX_V, \
-  RISE_RATE_BP, RISE_RATE_V, STOCK_RISE_RATE, JERK_SCALE_BP, JERK_SCALE_V, TF_WIDEN_V_BP, TF_WIDEN_BASE_V, \
-  TF_WIDEN_TIER, TF_WIDEN_MAX, TF_SLEW_PER_S, TF_DECEL_HOLD_A
+  RISE_RATE_BP, RISE_RATE_V, STOCK_RISE_RATE, JERK_SCALE_BP, JERK_SCALE_V, ONSET_DEADBAND, ONSET_RAMP_S, \
+  ONSET_FLOOR, LEAD_BRAKE_ALEAD_BP, LEAD_BRAKE_FACTOR_V, TF_WIDEN_V_BP, TF_WIDEN_BASE_V, TF_WIDEN_TIER, \
+  TF_WIDEN_MAX, TF_SLEW_PER_S, TF_DECEL_HOLD_A
+
+
+class _OnsetRelax:
+  # Detects a fresh accel<->decel direction change on aEgo (a real, causal signal -- never the MPC's own
+  # solved target) and relaxes toward a tier-scaled floor immediately, then eases linearly back to 1.0 over
+  # ONSET_RAMP_S. Feeds the MPC's cost weights for the cycles that follow; never touches this cycle's output.
+  def __init__(self):
+    self._prev_sign = 0
+    self._ramp = 1.0
+
+  def reset(self) -> None:
+    self._prev_sign = 0
+    self._ramp = 1.0
+
+  def update(self, a_ego: float, floor: float) -> float:
+    sign = 0
+    if a_ego > ONSET_DEADBAND:
+      sign = 1
+    elif a_ego < -ONSET_DEADBAND:
+      sign = -1
+
+    if sign != 0 and sign != self._prev_sign:
+      self._ramp = floor
+      self._prev_sign = sign
+    else:
+      self._ramp = min(1.0, self._ramp + (1.0 - floor) * (DT_MDL / ONSET_RAMP_S))
+    return self._ramp
 
 
 class AccelController:
@@ -36,6 +66,9 @@ class AccelController:
     self._a_ego = 0.0
     self._widen = 0.0                     # current slewed follow-gap widen (s), add-only
     self._t_follow = 0.0                  # last t_follow handed to the MPC (telemetry)
+    self._onset_relax = _OnsetRelax()
+    self._onset_factor = 1.0
+    self._lead_brake_factor = 1.0
     self._read_params()
 
   def _read_params(self) -> None:
@@ -50,11 +83,28 @@ class AccelController:
       self._read_params()
     self._v_ego = float(sm['carState'].vEgo)
     self._a_ego = float(sm['carState'].aEgo)
+
+    if self._enabled:
+      self._onset_factor = self._onset_relax.update(self._a_ego, ONSET_FLOOR[self._personality])
+      self._lead_brake_factor = self._get_lead_brake_factor(sm['radarState'].leadOne)
+    else:
+      self._onset_relax.reset()
+      self._onset_factor = 1.0
+      self._lead_brake_factor = 1.0
+
     self._frame += 1
 
+  def _get_lead_brake_factor(self, lead) -> float:
+    if not lead.status:
+      return 1.0
+    return float(np.interp(lead.aLeadK, LEAD_BRAKE_ALEAD_BP, LEAD_BRAKE_FACTOR_V[self._personality]))
+
   def reset(self) -> None:
     # Drop the accumulated widen (e.g. on disengage / standstill re-init) so it re-ramps cleanly.
     self._widen = 0.0
+    self._onset_relax.reset()
+    self._onset_factor = 1.0
+    self._lead_brake_factor = 1.0
 
   def get_max_accel(self, v_ego: float) -> float:
     # Disabled -> stock ceiling (off == stock, independent of the NORMAL profile so NORMAL is free to differ).
@@ -69,11 +119,13 @@ class AccelController:
     return float(np.interp(v_ego, RISE_RATE_BP, RISE_RATE_V[self._personality]))
 
   def get_jerk_scale(self, v_ego: float) -> float:
-    # Disabled -> 1.0 -> byte-stock jerk cost. Enabled: relaxes the core MPC's jerk_factor near a stop
-    # (tier-scaled), ramping back to 1.0 (stock) by the v=5 knot so cruise/follow jerk is unchanged.
+    # Disabled -> 1.0 -> byte-stock jerk cost. Enabled: takes the most-relaxed of three tier-scaled factors
+    # -- near a stop (v_ego), a fresh accel<->decel onset (any speed), and a hard-braking lead -- each never
+    # exceeding 1.0 (stock), so this only ever relaxes jerk cost, never tightens it beyond stock.
     if not self._enabled:
       return 1.0
-    return float(np.interp(v_ego, JERK_SCALE_BP, JERK_SCALE_V[self._personality]))
+    near_stop = float(np.interp(v_ego, JERK_SCALE_BP, JERK_SCALE_V[self._personality]))
+    return min(near_stop, self._onset_factor, self._lead_brake_factor)
 
   def get_t_follow(self, t_follow: float, v_ego: float) -> float:
     # MPC t_follow hook. Adds a slewed, decel-held, speed-dependent comfort widen on top of the stock

sunnypilot/selfdrive/controls/lib/accel_personality/constants.py

@@ -58,6 +58,25 @@ JERK_SCALE_V = {
   SPORT:  [0.30, 1.0],
 }
 
+# --- Onset jerk-cost relaxation (MPC INPUT: general accel<->decel-direction change, not just launch) ------
+# The v_ego-indexed table above only relaxes near a stop. This relaxes on ANY fresh accel<->decel direction
+# change (aEgo crossing the deadband in a new sign) regardless of speed: drop to a tier-scaled floor right
+# away, then ease linearly back to 1.0 (stock) over ONSET_RAMP_S. Same shape as the launch ramp, general to
+# any onset (e.g. releasing off a lead, starting to brake). Disabled -> 1.0.
+ONSET_DEADBAND = 0.15          # m/s^2: ignore aEgo noise this small around a zero-crossing
+ONSET_RAMP_S = 0.4             # s: ease back to stock over this long
+ONSET_FLOOR = {ECO: 0.75, NORMAL: 0.65, SPORT: 0.50}
+
+# --- Lead-braking jerk-cost relaxation (MPC INPUT: react faster to a hard-braking lead) --------------------
+# When the tracked lead is itself decelerating hard, relax jerk cost so the MPC's reaction isn't paced by a
+# jerk budget tuned for routine following. No lead, or lead not braking -> 1.0. Disabled -> 1.0.
+LEAD_BRAKE_ALEAD_BP = [-3.0, -0.5]      # m/s^2, lead's own aLeadK (ascending, as np.interp requires)
+LEAD_BRAKE_FACTOR_V = {
+  ECO:    [0.75, 1.0],
+  NORMAL: [0.60, 1.0],
+  SPORT:  [0.45, 1.0],
+}
+
 # --- Follow-gap widen (add-only, fed to the MPC t_follow) ------------------------------------------------
 # Add a small speed-dependent widen to the stock t_follow (the driver's gap-button value). Wider gap ->
 # MPC brakes earlier + gentler onto a slowing lead and settles a roomier cruise gap. Invariants:

sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py

@@ -19,8 +19,9 @@ from openpilot.common.realtime import DT_MDL
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_controller import AccelController
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import \
   ECO, NORMAL, SPORT, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, RISE_RATE_V, \
-  STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, JERK_SCALE_BP, JERK_SCALE_V, TF_WIDEN_V_BP, TF_WIDEN_BASE_V, \
-  TF_WIDEN_TIER, TF_WIDEN_MAX, TF_SLEW_PER_S, TF_DECEL_HOLD_A, AccelerationPersonality
+  STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, JERK_SCALE_BP, JERK_SCALE_V, ONSET_DEADBAND, ONSET_RAMP_S, \
+  ONSET_FLOOR, LEAD_BRAKE_ALEAD_BP, LEAD_BRAKE_FACTOR_V, TF_WIDEN_V_BP, TF_WIDEN_BASE_V, TF_WIDEN_TIER, \
+  TF_WIDEN_MAX, TF_SLEW_PER_S, TF_DECEL_HOLD_A, AccelerationPersonality
 
 _EPS = 1e-6
 _TF_STOCK = 1.45          # a representative stock t_follow (standard personality); the widen is add-only on top
@@ -41,8 +42,13 @@ class FakeParams:
     self.store[key] = val
 
 
-def make_sm(v_ego=20.0, a_ego=0.0):
-  return {'carState': SimpleNamespace(vEgo=v_ego, aEgo=a_ego)}
+def make_lead(status=False, aLeadK=0.0):
+  return SimpleNamespace(status=status, aLeadK=aLeadK)
+
+
+def make_sm(v_ego=20.0, a_ego=0.0, lead=None):
+  return {'carState': SimpleNamespace(vEgo=v_ego, aEgo=a_ego),
+          'radarState': SimpleNamespace(leadOne=lead or make_lead())}
 
 
 def make_controller(enabled=True, personality=NORMAL):
@@ -178,6 +184,114 @@ def test_jerk_scale_table_matches_constants():
       assert ctrl.get_jerk_scale(v) == pytest.approx(np.interp(v, JERK_SCALE_BP, JERK_SCALE_V[personality]))
 
 
+# --- onset relax: fresh accel<->decel direction change, any speed ------------------------------------------
+
+def test_onset_disabled_is_stock():
+  ctrl = make_controller(enabled=False, personality=SPORT)
+  ctrl.update(make_sm(v_ego=20.0, a_ego=2.0))
+  assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0)
+
+
+def test_onset_no_effect_at_steady_state():
+  ctrl = make_controller(personality=NORMAL)
+  for _ in range(10):
+    ctrl.update(make_sm(v_ego=20.0, a_ego=0.0))
+  assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0)   # within the deadband, no direction to flip from
+
+
+def test_onset_drops_to_floor_on_fresh_direction_change():
+  ctrl = make_controller(personality=NORMAL)
+  ctrl.update(make_sm(v_ego=20.0, a_ego=1.0))    # establish "accelerating"
+  ctrl.update(make_sm(v_ego=20.0, a_ego=-1.0))   # fresh flip to decelerating
+  assert ctrl.get_jerk_scale(20.0) == pytest.approx(ONSET_FLOOR[NORMAL])
+
+
+def test_onset_eases_back_to_stock_over_ramp_s():
+  ctrl = make_controller(personality=NORMAL)
+  ctrl.update(make_sm(v_ego=20.0, a_ego=1.0))
+  ctrl.update(make_sm(v_ego=20.0, a_ego=-1.0))
+  assert ctrl.get_jerk_scale(20.0) == pytest.approx(ONSET_FLOOR[NORMAL])
+  n = int(ONSET_RAMP_S / DT_MDL)
+  for _ in range(n):
+    ctrl.update(make_sm(v_ego=20.0, a_ego=-1.0))   # sustained decel, no further flip
+  assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0, abs=1e-3)
+
+
+def test_onset_never_relaxes_below_its_own_floor():
+  ctrl = make_controller(personality=SPORT)
+  ctrl.update(make_sm(v_ego=20.0, a_ego=1.0))
+  for _ in range(50):
+    ctrl.update(make_sm(v_ego=20.0, a_ego=-1.0 if _ % 2 == 0 else -1.2))   # repeated same-direction wiggle
+  assert ctrl.get_jerk_scale(20.0) >= ONSET_FLOOR[SPORT] - _EPS
+
+
+def test_onset_tier_ordering():
+  ordered = []
+  for personality in (ECO, NORMAL, SPORT):
+    ctrl = make_controller(personality=personality)
+    ctrl.update(make_sm(v_ego=20.0, a_ego=1.0))
+    ctrl.update(make_sm(v_ego=20.0, a_ego=-1.0))
+    ordered.append(ctrl.get_jerk_scale(20.0))
+  assert ordered[2] < ordered[1] < ordered[0]   # SPORT relaxes most, ECO least
+
+
+def test_onset_ignores_deadband_noise():
+  ctrl = make_controller(personality=NORMAL)
+  ctrl.update(make_sm(v_ego=20.0, a_ego=0.3))
+  for _ in range(10):
+    ctrl.update(make_sm(v_ego=20.0, a_ego=ONSET_DEADBAND - 0.02))   # tiny wiggle, never crosses -deadband
+  assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0, abs=1e-3)
+
+
+# --- lead-braking relax: hard-braking lead relaxes jerk cost regardless of speed ---------------------------
+
+def test_lead_brake_no_lead_is_stock():
+  ctrl = make_controller(personality=SPORT)
+  ctrl.update(make_sm(v_ego=20.0, lead=make_lead(status=False, aLeadK=-5.0)))
+  assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0)
+
+
+def test_lead_brake_relaxes_with_lead_decel():
+  for personality, floor in ((ECO, 0.75), (NORMAL, 0.60), (SPORT, 0.45)):
+    ctrl = make_controller(personality=personality)
+    ctrl.update(make_sm(v_ego=20.0, lead=make_lead(status=True, aLeadK=-3.0)))
+    assert ctrl.get_jerk_scale(20.0) == pytest.approx(floor)
+
+
+def test_lead_brake_gentle_lead_decel_is_stock():
+  ctrl = make_controller(personality=SPORT)
+  ctrl.update(make_sm(v_ego=20.0, lead=make_lead(status=True, aLeadK=-0.2)))
+  assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0)   # above the -0.5 gate -> no relax
+
+
+def test_lead_brake_matches_constants_table():
+  for personality in (ECO, NORMAL, SPORT):
+    ctrl = make_controller(personality=personality)
+    for a_lead in (-0.5, -1.5, -3.0):
+      ctrl.update(make_sm(v_ego=20.0, lead=make_lead(status=True, aLeadK=a_lead)))
+      expected = np.interp(a_lead, LEAD_BRAKE_ALEAD_BP, LEAD_BRAKE_FACTOR_V[personality])
+      assert ctrl.get_jerk_scale(20.0) == pytest.approx(expected)
+
+
+# --- combined: get_jerk_scale takes the most-relaxed of all three factors ----------------------------------
+
+def test_combined_takes_most_relaxed_factor():
+  ctrl = make_controller(personality=NORMAL)
+  ctrl.update(make_sm(v_ego=20.0, a_ego=1.0))
+  # onset flip (floor=0.65) + hard-braking lead (floor=0.60 at aLeadK=-3.0) -> min of the two, near-stop is 1.0 at v=20
+  ctrl.update(make_sm(v_ego=20.0, a_ego=-1.0, lead=make_lead(status=True, aLeadK=-3.0)))
+  assert ctrl.get_jerk_scale(20.0) == pytest.approx(min(ONSET_FLOOR[NORMAL], LEAD_BRAKE_FACTOR_V[NORMAL][0]))
+
+
+def test_reset_clears_onset_and_lead_brake_state():
+  ctrl = make_controller(personality=SPORT)
+  ctrl.update(make_sm(v_ego=20.0, a_ego=1.0))
+  ctrl.update(make_sm(v_ego=20.0, a_ego=-1.0, lead=make_lead(status=True, aLeadK=-3.0)))
+  assert ctrl.get_jerk_scale(20.0) < 1.0 - _EPS
+  ctrl.reset()
+  assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0)
+
+
 # --- t_follow: add-only speed widen -----------------------------------------------------------------------
 
 def test_t_follow_zero_below_gate():