take off twitch

selfdrive/controls/controlsd.py

@@ -163,6 +163,18 @@ 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
 
+# Pull-away twitch guard. modeld converts the action head's lateral-ACCELERATION output to
+# curvature with a 1/max(1, v)^2 divide, so its residual at pull-away (~0.02 m/s^2, the
+# head's own noise floor) reads as curvature 0.015 — a 38 deg steering command — where the
+# same 0.02 m/s^2 at highway speed is 0.2 deg. Route 78511c37 twitched on 10 of 10 straight
+# takeoffs, torque 0.57-0.76 at 1.1-1.4 m/s. The model's own planned path is the tell: it
+# read 0.000-0.004 there (6-108x disagreement), while across 11 real low-speed turns in the
+# same drive the action stayed within 0.82-2.56x its plan probe.
+TWITCH_GUARD_MAX_SPEED = 4.0    # m/s; above this the 1/v^2 amplification is gone
+TWITCH_GUARD_FADE_SPEED = 3.0   # m/s; full strength below, faded out by MAX_SPEED
+TWITCH_GUARD_PLAN_RATIO = 3.0   # allowed |action| / |plan curvature| (worst real turn: 2.56)
+TWITCH_GUARD_FLOOR = 0.002      # 1/m (~5 deg of wheel); a near-zero probe must not clamp to nothing
+
 
 def _plan_circle_curvature(xs, ys, lookahead: float) -> float:
   # curvature of the circle through the origin, tangent to the car's heading, passing
@@ -220,6 +232,18 @@ def get_plan_reach(model_v2) -> float:
   return xs[-1] if len(xs) else 0.0
 
 
+def limit_curvature_to_plan(model_v2, curvature: float, v_ego: float) -> float:
+  # See TWITCH_GUARD_*. Magnitude only: the command is bounded, never reversed.
+  if v_ego >= TWITCH_GUARD_MAX_SPEED or curvature == 0.0:
+    return curvature
+  limit = max(TWITCH_GUARD_PLAN_RATIO * abs(get_plan_spatial_curvature(model_v2)), 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 get_control_lateral_smooth_seconds(brand: str, v_ego: float, vehicle_smooth_seconds: float) -> float:
   if brand != "rivian":
     return LAT_SMOOTH_SECONDS
@@ -481,6 +505,10 @@ 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)
+    # Pull-away twitch guard (see TWITCH_GUARD_*). Requires no turn intent in play, so the
+    # pre-wind ratchet, turn lead and exit opposite-release never see a reduced command.
+    if CC.latActive 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:

selfdrive/controls/tests/test_turn_lead.py

@@ -1,13 +1,31 @@
+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.selfdrive.controls.controlsd import (TWITCH_GUARD_FLOOR, TWITCH_GUARD_MAX_SPEED,
+                                                    get_control_lateral_smooth_seconds,
+                                                    limit_curvature_to_plan, turn_lead_allowed)
 
 
 LateralControlMode = car.CarControl.Actuators.LateralControlMode
 
 
+def _plan(xs, ys):
+  return types.SimpleNamespace(position=types.SimpleNamespace(x=xs, y=ys))
+
+
+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])
+
+
+def _arc_plan(radius):
+  return _plan([radius * math.sin(t * 0.006) for t in range(200)],
+               [radius * (1.0 - math.cos(t * 0.006)) for t in range(200)])
+
+
 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)
@@ -28,3 +46,42 @@ def test_non_rivian_control_smoothing_matches_starpilot(v_ego):
 ])
 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
+
+
+# turning authority must never be reduced: a real turn's action agrees with its own plan
+@pytest.mark.parametrize("ratio", [0.8, 1.0, 2.0, 2.6])
+def test_real_turns_tracking_their_own_plan_are_untouched(ratio):
+  plan = _arc_plan(7.0)
+  action = 0.1428 * ratio
+  assert limit_curvature_to_plan(plan, action, 1.2) == pytest.approx(action)
+
+
+def test_degenerate_plans_do_not_raise_and_still_bound_the_command():
+  for plan in (STANDSTILL_STUB_PLAN, _plan([], [])):
+    assert abs(limit_curvature_to_plan(plan, 0.0155, 0.4)) == pytest.approx(TWITCH_GUARD_FLOOR)
+
+
+def test_zero_command_stays_zero():
+  assert limit_curvature_to_plan(STRAIGHT_PLAN, 0.0, 1.2) == 0.0