Remove Takeoff Twitch

Limit the curvature guard to a short pull-away window and reject invalid model plans. Original PR author: whoisdomi <dcantu83@gmail.com>.
2026-09-03 14:43:48 +08:00 · 2026-08-26 10:24:25 -05:00
parent f20b256473
commit e20ea5d0d1
2 changed files with 179 additions and 12 deletions
@@ -169,12 +169,27 @@ CURVATURE_HOLD_OPPOSITE_RELEASE = 0.01  # 1/m
 CURVATURE_HOLD_CONFIRM_MIN = 0.003  # 1/m (~7 deg) of wound curvature before capture
 CURVATURE_HOLD_CONFIRM_SWEPT = 0.6  # rad of heading swept this blinker cycle; past this the push is exit-shaping, not initiation

+# Suppress low-speed action spikes while the model's spatial path remains straight.
+TWITCH_GUARD_MAX_SPEED = 4.0
+TWITCH_GUARD_FADE_SPEED = 3.0
+TWITCH_GUARD_DURATION = 1.5
+TWITCH_GUARD_PLAN_RATIO = 4.0
+TWITCH_GUARD_FLOOR = 0.002
+TWITCH_GUARD_STRAIGHT_LO = 0.005
+TWITCH_GUARD_STRAIGHT_HI = 0.014
+TWITCH_GUARD_MIN_REACH = 12.0
+

 def _plan_circle_curvature(xs, ys, lookahead: float) -> float:
-  # curvature of the circle through the origin, tangent to the car's heading, passing
-  # through the plan point ~lookahead meters ahead: kappa = 2y / (x^2 + y^2)
+  # Fit curvature through the plan point at the requested lookahead.
  px, py = 0.0, 0.0
-  for x, y in zip(xs, ys):
+  for x, y in zip(xs, ys, strict=False):
+    try:
+      x, y = float(x), float(y)
+    except (TypeError, ValueError, OverflowError):
+      return 0.0
+    if not (math.isfinite(x) and math.isfinite(y)):
+      return 0.0
    px, py = x, y
    if math.hypot(x, y) >= lookahead:
      break
@@ -185,11 +200,7 @@ def _plan_circle_curvature(xs, ys, lookahead: float) -> float:


 def _plan_dual_probe(model_v2, d_near: float, d_far: float) -> float:
-  # Min-magnitude of a near and a far circle fit. The far probe alone assumes the turn
-  # starts immediately, which over-winds wide turns whose arc begins several meters out
-  # (wide multi-lane lefts): the near probe reads ~straight there and only grows as the
-  # car approaches the arc, so the readout self-scales to the turn geometry. Sign
-  # disagreement means no coherent turn ahead: contribute nothing.
+  # Use the smaller magnitude of near and far probes to avoid early turn bias.
  xs, ys = model_v2.position.x, model_v2.position.y
  near = _plan_circle_curvature(xs, ys, d_near)
  far = _plan_circle_curvature(xs, ys, d_far)
@@ -222,8 +233,52 @@ def get_plan_turn_onset_dist(model_v2) -> float:


 def get_plan_reach(model_v2) -> float:
-  xs = model_v2.position.x
-  return xs[-1] if len(xs) else 0.0
+  try:
+    xs = model_v2.position.x
+    return float(xs[-1]) if len(xs) else 0.0
+  except (AttributeError, IndexError, TypeError, ValueError, OverflowError):
+    return 0.0
+
+
+def _plan_positions_are_finite(model_v2) -> bool:
+  try:
+    xs, ys = model_v2.position.x, model_v2.position.y
+    return len(xs) == len(ys) and all(
+      math.isfinite(float(x)) and math.isfinite(float(y)) for x, y in zip(xs, ys, strict=True)
+    )
+  except (AttributeError, TypeError, ValueError, OverflowError):
+    return False
+
+
+def limit_curvature_to_plan(model_v2, curvature: float, v_ego: float) -> float:
+  if not (math.isfinite(curvature) and math.isfinite(v_ego)):
+    return curvature
+  if v_ego >= TWITCH_GUARD_MAX_SPEED or curvature == 0.0:
+    return curvature
+  if not _plan_positions_are_finite(model_v2):
+    return curvature
+  reach = get_plan_reach(model_v2)
+  if not math.isfinite(reach) or reach < TWITCH_GUARD_MIN_REACH:
+    return curvature
+  plan = abs(_plan_circle_curvature(model_v2.position.x, model_v2.position.y,
+                                    CURVATURE_HOLD_PLAN_LOOKAHEAD_FAR))
+  if not math.isfinite(plan):
+    return curvature
+  straightness = (plan - TWITCH_GUARD_STRAIGHT_LO) / (TWITCH_GUARD_STRAIGHT_HI - TWITCH_GUARD_STRAIGHT_LO)
+  limit = max(TWITCH_GUARD_PLAN_RATIO * plan * min(max(straightness, 0.0), 1.0), TWITCH_GUARD_FLOOR)
+  if abs(curvature) <= limit:
+    return curvature
+  fade = (TWITCH_GUARD_MAX_SPEED - v_ego) / (TWITCH_GUARD_MAX_SPEED - TWITCH_GUARD_FADE_SPEED)
+  fade = min(max(fade, 0.0), 1.0)
+  return curvature + (math.copysign(limit, curvature) - curvature) * fade
+
+
+def update_twitch_guard(remaining: float, v_ego: float, standstill: bool) -> float:
+  if not (math.isfinite(remaining) and math.isfinite(v_ego)):
+    return 0.0
+  if standstill or abs(v_ego) <= 0.3:
+    return TWITCH_GUARD_DURATION
+  return max(remaining - DT_CTRL, 0.0)


 def get_control_lateral_smooth_seconds(brand: str, v_ego: float, vehicle_smooth_seconds: float) -> float:
@@ -346,6 +401,7 @@ class Controls:
    self.turn_hold_handoff_t = 0.0
    self.turn_hold_done = False
    self.turn_blinker_swept = 0.0
+    self.twitch_guard_remaining = 0.0
    self.kona_non_scc_lateral_active = False

    self.pose_calibrator = PoseCalibrator()
@@ -401,6 +457,7 @@ class Controls:

  def state_control(self):
    CS = self.sm['carState']
+    self.twitch_guard_remaining = update_twitch_guard(self.twitch_guard_remaining, CS.vEgo, CS.standstill)

    # Update VehicleModel
    lp = self.sm['liveParameters']
@@ -460,7 +517,11 @@ class Controls:
    # EcuDisableFailed is set when car started in READY mode (ECU disable was rejected)
    # Disable longitudinal so stock ACC works instead
    self.update_ecu_disable_failed()
-    CC.longActive = CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and not self.sm['starpilotCarState'].pauseLongitudinal and self.CP.openpilotLongitudinalControl and not self.ecu_disable_failed
+    CC.longActive = (
+      CC.enabled and not any(e.overrideLongitudinal for e in self.sm['onroadEvents']) and
+      not self.sm['starpilotCarState'].pauseLongitudinal and self.CP.openpilotLongitudinalControl and
+      not self.ecu_disable_failed
+    )

    actuators = CC.actuators
    actuators.longControlState = self.LoC.long_control_state
@@ -499,6 +560,9 @@ class Controls:
    # here is positive for RIGHT turns (pauseturn log: left turn at +148 deg steering
    # angle logs desiredCurvature -0.07), so the blinker maps right=+1, left=-1.
    blinker_dir = float(CS.rightBlinker) - float(CS.leftBlinker)
+    if (CC.latActive and self.twitch_guard_remaining > 0.0 and
+        blinker_dir == 0.0 and self.turn_hold_curvature == 0.0):
+      new_desired_curvature = limit_curvature_to_plan(model_v2, new_desired_curvature, CS.vEgo)
    # heading swept in the blinker's direction over the whole blinker cycle (any speed):
    # discriminates a turn not yet made from one being exited (see the re-arm below)
    if blinker_dir == 0.0:
@@ -1,13 +1,40 @@
+import math
+import types
+
 from cereal import car

 import pytest

-from openpilot.selfdrive.controls.controlsd import get_control_lateral_smooth_seconds, turn_lead_allowed
+from openpilot.common.realtime import DT_CTRL
+from openpilot.selfdrive.controls.controlsd import (
+  TWITCH_GUARD_DURATION,
+  TWITCH_GUARD_FLOOR,
+  TWITCH_GUARD_MAX_SPEED,
+  get_control_lateral_smooth_seconds,
+  limit_curvature_to_plan,
+  turn_lead_allowed,
+  update_twitch_guard,
+)


 LateralControlMode = car.CarControl.Actuators.LateralControlMode


+def _plan(xs, ys):
+  return types.SimpleNamespace(position=types.SimpleNamespace(x=xs, y=ys))
+
+
+def _arc_plan(radius, n=200):
+  return _plan([radius * math.sin(i / n) for i in range(n)],
+               [radius * (1.0 - math.cos(i / n)) for i in range(n)])
+
+
+STRAIGHT_PLAN = _plan([i * 0.5 for i in range(200)], [0.0] * 200)
+STANDSTILL_STUB_PLAN = _plan([0.0, 0.3], [0.0, 0.0])
+TURN_PLAN = _arc_plan(30.0)
+GENTLE_BEND_PLAN = _arc_plan(143.0)
+
+
 def test_turn_lead_is_suppressed_only_during_applied_angle_control():
  assert not turn_lead_allowed("rivian", LateralControlMode.angle)
  assert turn_lead_allowed("rivian", LateralControlMode.torque)
@@ -37,3 +64,79 @@ def test_subaru_control_smoothing_uses_vehicle_schedule(v_ego, expected):
 ])
 def test_rivian_control_smoothing_remains_speed_scheduled(v_ego, expected):
  assert get_control_lateral_smooth_seconds("rivian", v_ego, 0.4) == pytest.approx(expected)
+
+
+@pytest.mark.parametrize("curvature", [0.0155, -0.0155])
+def test_twitch_against_a_straight_plan_is_clamped_to_the_floor(curvature):
+  guarded = limit_curvature_to_plan(STRAIGHT_PLAN, curvature, 1.2)
+  assert abs(guarded) == pytest.approx(TWITCH_GUARD_FLOOR)
+  assert math.copysign(1.0, guarded) == math.copysign(1.0, curvature)
+
+
+def test_command_already_below_the_floor_is_untouched():
+  assert limit_curvature_to_plan(STRAIGHT_PLAN, 0.0015, 1.2) == pytest.approx(0.0015)
+
+
+@pytest.mark.parametrize("v_ego", [TWITCH_GUARD_MAX_SPEED, 6.0, 30.0])
+def test_guard_is_inactive_above_its_speed_band(v_ego):
+  assert limit_curvature_to_plan(STRAIGHT_PLAN, 0.0155, v_ego) == pytest.approx(0.0155)
+
+
+def test_guard_fades_out_across_the_speed_band():
+  full = limit_curvature_to_plan(STRAIGHT_PLAN, 0.0155, 1.2)
+  half = limit_curvature_to_plan(STRAIGHT_PLAN, 0.0155, 3.5)
+  assert full < half < 0.0155
+
+
+@pytest.mark.parametrize("ratio", [0.8, 1.0, 2.0, 3.0])
+def test_real_turns_tracking_their_own_plan_are_untouched(ratio):
+  action = (1.0 / 30.0) * ratio
+  assert limit_curvature_to_plan(TURN_PLAN, action, 1.2) == pytest.approx(action)
+
+
+def test_a_barely_bending_plan_does_not_license_a_large_command():
+  guarded = limit_curvature_to_plan(GENTLE_BEND_PLAN, 0.0155, 1.2)
+  assert TWITCH_GUARD_FLOOR < guarded < 0.008
+
+
+@pytest.mark.parametrize("plan", [STANDSTILL_STUB_PLAN, _plan([], [])])
+def test_guard_stands_down_when_the_plan_is_too_short_to_judge(plan):
+  assert limit_curvature_to_plan(plan, 0.0155, 0.4) == pytest.approx(0.0155)
+
+
+def test_zero_command_stays_zero():
+  assert limit_curvature_to_plan(STRAIGHT_PLAN, 0.0, 1.2) == 0.0
+
+
+@pytest.mark.parametrize("bad_plan", [
+  _plan([0.0, math.nan, 20.0], [0.0, 0.0, 0.0]),
+  _plan([0.0, math.inf, 20.0], [0.0, 0.0, 0.0]),
+  _plan([0.0, 20.0], [0.0]),
+])
+def test_invalid_plan_data_leaves_curvature_untouched(bad_plan):
+  assert limit_curvature_to_plan(bad_plan, 0.0155, 1.2) == pytest.approx(0.0155)
+
+
+@pytest.mark.parametrize("value", [math.nan, math.inf, -math.inf])
+def test_nonfinite_guard_inputs_disarm(value):
+  assert update_twitch_guard(value, 1.0, False) == 0.0
+  assert update_twitch_guard(TWITCH_GUARD_DURATION, value, False) == 0.0
+
+
+def test_twitch_guard_arms_at_standstill_or_creep_speed():
+  assert update_twitch_guard(0.0, 0.0, True) == TWITCH_GUARD_DURATION
+  assert update_twitch_guard(0.0, 0.3, False) == TWITCH_GUARD_DURATION
+
+
+def test_twitch_guard_decays_after_pullaway_and_expires():
+  remaining = update_twitch_guard(0.0, 0.0, True)
+  remaining = update_twitch_guard(remaining, 1.0, False)
+  assert remaining == pytest.approx(TWITCH_GUARD_DURATION - DT_CTRL)
+
+  for _ in range(int(TWITCH_GUARD_DURATION / DT_CTRL) + 1):
+    remaining = update_twitch_guard(remaining, 1.0, False)
+  assert remaining == 0.0
+
+
+def test_twitch_guard_does_not_arm_while_moving():
+  assert update_twitch_guard(0.0, 1.0, False) == 0.0