modeld_v2: soften the turn plan floor for Light Speed 2

Routes 85 and 89 ran entirely on the big model, Light Speed 2. Below 4 m/s the 0.85 floor set the steering command in 56-85% of turn frames, and every hard driver push against a turn (>3 Nm, 4 of 19 turns) came while it did. - Lower the floor to 0.75x the plan (same 4-8 m/s fade). - Gate the floor by the model's own state for the requested turn (meta.desireState), full above 0.8 and off below 0.5. Pulling away after a wait at a stop, the model's turn state was ~0.6 while the plan still had the turn, the floor turned in early, and the driver pushed back in 48% of those frames vs 9% in rolling turns, where the state sits at ~1.0. - Only apply the floor when the plan points into the turn. A stale turn desire on exit clamped a steer the other way for 0.4 s in route 89; turns where the plan agrees with the turn are unchanged. - Log the action head's curvature before the floor and the plan curvature the floor compares against in modelDataV2SP (actionCurvature, turnPlanCurvature) so the next tuning is exact. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
2026-11-07 01:13:45 +08:00 · 2026-11-05 22:16:41 -05:00
parent f9c100561a
commit 94a138cab5
4 changed files with 112 additions and 10 deletions
@@ -475,6 +475,8 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
  blendfulLeftProb @7 :Float32;
  blendfulRightProb @8 :Float32;
  blendfulLaneWidth @9 :Float32;
+  actionCurvature @10 :Float32;  # action head curvature before the turn plan floor
+  turnPlanCurvature @11 :Float32;  # plan curvature the turn floor compares against, 0 when no turn is requested

  enum TurnDirection {
    none @0;
@@ -49,7 +49,7 @@ from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.modeld_v2.helpers import load_model
 from openpilot.sunnypilot.modeld_v2.frame_fusion import fused_with_warp
 from openpilot.sunnypilot.modeld_v2.model_adapters import get_model_adapter
-from openpilot.sunnypilot.modeld_v2.turn_plan_floor import apply_turn_plan_floor, turn_plan_floor_scale
+from openpilot.sunnypilot.modeld_v2.turn_plan_floor import apply_turn_plan_floor, turn_plan_floor_scale, turn_state_gate
 from openpilot.sunnypilot.modeld_v2.plan_long import PlanLong
 from openpilot.sunnypilot.modeld_v2.navigation_input import (NavigationInputBuffer, NavigationStatePublisher, SignalRouteGuard,
                                                          destination_route_context, navigation_capability)
@@ -110,6 +110,8 @@ class ModelState(ModelStateBase):
    self.MIN_LAT_CONTROL_SPEED = 0.3
    self.PLANPLUS_CONTROL: float = 1.0
    self.chestnut = chestnut
+    self.action_curvature = 0.  # last command before the turn plan floor, logged for tuning
+    self.turn_plan_curvature = 0.

    pkl_path = _find_driving_pkl(model_bundle)
    assert pkl_path is not None, f"No driving pkl found for {'chestnut' if chestnut else 'small model'} — all models must be compiled with compile_modeld.py"
@@ -253,18 +255,26 @@ class ModelState(ModelStateBase):
      curvature_plan = (plan + (self.PLANPLUS_CONTROL - 1.0) * model_output['planplus'][0]
                        if 'planplus' in model_output and self.PLANPLUS_CONTROL != 1.0 else plan)
      desired_curvature = get_curvature_from_output(model_output, curvature_plan, v_ego, lat_action_t, self.mlsim)
+      self.action_curvature, self.turn_plan_curvature = desired_curvature, 0.
    else:
      desired_accel = model_output['action'][0, 1]
      if plan_long is not None:
        desired_accel = plan_long.update(model_output['plan'][0], model_output['plan_stds'][0], self.constants.T_IDXS, long_action_t,
                                         action_accel=desired_accel)
      desired_curvature = model_output['action'][0, 0] / (max(1.0, v_ego))**2
-      if turn_sign and (floor_scale := turn_plan_floor_scale(v_ego)) > 0.:
-        # the action head under-commits in slow turns while the model's own plan has the turn; use the plan as a floor
+      self.action_curvature, self.turn_plan_curvature = desired_curvature, 0.
+      if turn_sign:
        plan = model_output['plan'][0]
-        plan_curvature = get_curvature_from_plan(plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], plan[:, Plan.ORIENTATION_RATE][:, 2],
-                                                 self.constants.T_IDXS, v_ego, lat_action_t)
-        desired_curvature = apply_turn_plan_floor(desired_curvature, plan_curvature, turn_sign, floor_scale)
+        self.turn_plan_curvature = get_curvature_from_plan(plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], plan[:, Plan.ORIENTATION_RATE][:, 2],
+                                                           self.constants.T_IDXS, v_ego, lat_action_t)
+        floor_scale = turn_plan_floor_scale(v_ego)
+        if 'desire_state' in model_output:
+          # fade the floor out when the model itself has lost the turn, e.g. pulling away after a wait at a stop
+          turn_state = model_output['desire_state'][0, int(log.Desire.turnRight if turn_sign > 0 else log.Desire.turnLeft)]
+          floor_scale *= turn_state_gate(turn_state)
+        if floor_scale > 0.:
+          # the action head under-commits in slow turns while the model's own plan has the turn; use the plan as a floor
+          desired_curvature = apply_turn_plan_floor(desired_curvature, self.turn_plan_curvature, turn_sign, floor_scale)
    if blendful is not None:
      desired_curvature += blendful(desired_curvature)

@@ -594,6 +604,8 @@ def main(demo=False):
      mdv2sp_send.modelDataV2SP.blendfulLeftProb = blendful.l_prob
      mdv2sp_send.modelDataV2SP.blendfulRightProb = blendful.r_prob
      mdv2sp_send.modelDataV2SP.blendfulLaneWidth = float(blendful.lane_width.x)
+      mdv2sp_send.modelDataV2SP.actionCurvature = float(model.action_curvature)
+      mdv2sp_send.modelDataV2SP.turnPlanCurvature = float(model.turn_plan_curvature)
      drivingdata_send.drivingModelData.meta.laneChangeState = DH.lane_change_state
      drivingdata_send.drivingModelData.meta.laneChangeDirection = DH.lane_change_direction

@@ -6,8 +6,8 @@ See the LICENSE.md file in the root directory for more details.
 """
 import pytest

-from openpilot.sunnypilot.modeld_v2.turn_plan_floor import TURN_PLAN_FLOOR_BP, TURN_PLAN_FLOOR_V, apply_turn_plan_floor, \
-  turn_plan_floor_scale
+from openpilot.sunnypilot.modeld_v2.turn_plan_floor import TURN_PLAN_FLOOR_BP, TURN_PLAN_FLOOR_V, TURN_STATE_BP, apply_turn_plan_floor, \
+  turn_plan_floor_scale, turn_state_gate

 RIGHT, LEFT, NONE = 1., -1., 0.
 LOW_SPEED_SCALE = TURN_PLAN_FLOOR_V[0]
@@ -38,6 +38,12 @@ def test_floor_only_pushes_toward_the_requested_side():
  assert apply_turn_plan_floor(-0.02, -0.10, RIGHT, LOW_SPEED_SCALE) == -0.02


+@pytest.mark.parametrize("sign", [RIGHT, LEFT])
+def test_stale_turn_never_weakens_a_steer_the_plan_also_wants(sign):
+  # a right turn still requested while plan and action both steer left (exit or a right straight into a left)
+  assert apply_turn_plan_floor(-sign * 0.12, -sign * 0.10, sign, LOW_SPEED_SCALE) == -sign * 0.12
+
+
@pytest.mark.parametrize("sign", [RIGHT, LEFT])
 def test_zero_scale_is_off_even_for_a_counter_steer(sign):
  # above the fade-out speed the action head steers alone, including small corrections against the turn
@@ -51,3 +57,71 @@ def test_scale_is_full_at_low_speed_and_fades_out():
  assert turn_plan_floor_scale((low + high) / 2) == pytest.approx(LOW_SPEED_SCALE / 2)
  assert turn_plan_floor_scale(high) == 0.
  assert turn_plan_floor_scale(30.) == 0.
+
+
+def _run_action(turn_sign, v_ego=2., k=0.1, action_share=0.5, turn_state=None):
+  # a right turn the plan holds at curvature k while the action head asks for only part of it
+  from typing import Any
+  import numpy as np
+  from openpilot.cereal import log
+  from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
+  from openpilot.sunnypilot.modeld_v2.modeld import ModelState
+
+  class State:
+    pass
+  state: Any = State()
+  state.LONG_SMOOTH_SECONDS, state.LAT_SMOOTH_SECONDS, state.MIN_LAT_CONTROL_SPEED, state.generation = 0.3, 0., 0.3, 12
+  state.constants = ModelConstants
+  t = np.array(ModelConstants.T_IDXS)
+  plan = np.zeros((len(t), ModelConstants.PLAN_WIDTH), dtype=np.float32)
+  plan[:, Plan.T_FROM_CURRENT_EULER.start + 2] = k * v_ego * t
+  plan[:, Plan.ORIENTATION_RATE.start + 2] = k * v_ego
+  out = {'plan': plan[None], 'action': np.array([[action_share * k * v_ego ** 2, 0.]], dtype=np.float32)}
+  if turn_state is not None:
+    desire_state = np.zeros((1, ModelConstants.DESIRE_PRED_WIDTH), dtype=np.float32)
+    desire_state[0, int(log.Desire.turnRight if turn_sign > 0 else log.Desire.turnLeft)] = turn_state
+    out['desire_state'] = desire_state
+  action = ModelState.get_action_from_model(state, out, log.ModelDataV2.Action(), 0.3, 0.3, v_ego, turn_sign=turn_sign)
+  return state, action
+
+
+def test_logs_the_action_before_the_floor_and_the_plan_it_compares_against():
+  state, action = _run_action(RIGHT)
+  assert state.action_curvature == pytest.approx(0.05)
+  assert state.turn_plan_curvature == pytest.approx(0.1, rel=1e-3)
+  assert action.desiredCurvature == pytest.approx(LOW_SPEED_SCALE * state.turn_plan_curvature, rel=1e-5)
+
+
+def test_no_turn_logs_no_plan_curvature():
+  state, action = _run_action(NONE)
+  assert state.turn_plan_curvature == 0.
+  assert action.desiredCurvature == pytest.approx(state.action_curvature)
+
+
+def test_turn_state_gate_is_off_below_and_full_above():
+  low, high = TURN_STATE_BP
+  assert turn_state_gate(0.) == 0.
+  assert turn_state_gate(low) == 0.
+  assert turn_state_gate((low + high) / 2) == pytest.approx(0.5)
+  assert turn_state_gate(high) == 1.
+  assert turn_state_gate(1.) == 1.
+
+
+@pytest.mark.parametrize("sign", [RIGHT, LEFT])
+def test_model_sure_of_the_turn_keeps_the_full_floor(sign):
+  state, action = _run_action(sign, k=sign * 0.1, turn_state=0.95)
+  assert action.desiredCurvature == pytest.approx(LOW_SPEED_SCALE * state.turn_plan_curvature, rel=1e-5)
+
+
+@pytest.mark.parametrize("sign", [RIGHT, LEFT])
+def test_model_that_lost_the_turn_leaves_the_action_alone(sign):
+  # pulling away after a wait at a stop: the model's turn state has faded, so the action head steers
+  state, action = _run_action(sign, k=sign * 0.1, turn_state=0.3)
+  assert action.desiredCurvature == pytest.approx(state.action_curvature)
+
+
+def test_half_sure_model_gets_a_partial_floor():
+  low, high = TURN_STATE_BP
+  state, action = _run_action(RIGHT, action_share=0.2, turn_state=(low + high) / 2)
+  assert state.action_curvature < 0.5 * LOW_SPEED_SCALE * state.turn_plan_curvature  # so the half floor is what steers
+  assert action.desiredCurvature == pytest.approx(0.5 * LOW_SPEED_SCALE * state.turn_plan_curvature, rel=1e-5)
@@ -10,14 +10,27 @@ import numpy as np
 # The action head falls short of the plan by a roughly fixed lateral acceleration (~0.2 m/s^2), which costs the most
 # curvature at low speed: logged turns put it at ~0.65-0.8x the plan below 4 m/s and ~0.85-0.9x above 6 m/s.
 # The plan reads tight against driven turns, so the floor stays below 1 and fades out where the action head is close.
+# 0.85 was too strong on Light Speed 2 (routes 85/89): below 4 m/s the floor set the command most of the time and every
+# hard driver push against the turn came while it did, mostly turning in early when pulling away from a stop.
 TURN_PLAN_FLOOR_BP = [4., 8.]
-TURN_PLAN_FLOOR_V = [0.85, 0.]
+TURN_PLAN_FLOOR_V = [0.75, 0.]
+
+# The model's own state for the requested turn (meta.desireState) gates the floor. In rolling turns it sits at ~1.0
+# while the floor acts. After a wait at a stop it fades to ~0 and is only ~0.6 when pulling away, yet the plan still has
+# the turn, so the floor turned in early: the driver pushed back in 48% of those frames vs 9% in rolling turns
+# (routes 85/89, Light Speed 2). Below 0.5 the action head steers alone.
+TURN_STATE_BP = [0.5, 0.8]
+TURN_STATE_V = [0., 1.]


 def turn_plan_floor_scale(v_ego: float) -> float:
  return float(np.interp(v_ego, TURN_PLAN_FLOOR_BP, TURN_PLAN_FLOOR_V))


+def turn_state_gate(turn_state: float) -> float:
+  return float(np.interp(turn_state, TURN_STATE_BP, TURN_STATE_V))
+
+
 def apply_turn_plan_floor(action_curvature: float, plan_curvature: float, turn_sign: float, scale: float) -> float:
  """Keep the steering command from falling below a scaled share of the model's own plan during a requested turn.

@@ -25,6 +38,7 @@ def apply_turn_plan_floor(action_curvature: float, plan_curvature: float, turn_s
  The floor only acts in the turn direction: it can strengthen a turn the action head under-commits to, but never
  weakens or reverses the action. A scale of 0 turns the floor off.
  """
-  if turn_sign == 0. or scale <= 0.:
+  if turn_sign == 0. or scale <= 0. or turn_sign * plan_curvature <= 0.:
+    # with the plan steering out of the turn (a stale desire on exit, or a right straight into a left) the action steers
    return action_curvature
  return turn_sign * max(turn_sign * action_curvature, scale * turn_sign * plan_curvature)