From 94a138cab55a37716dc0dff1c38fed552aa0fbc3 Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Thu, 5 Nov 2026 22:16:41 -0500 Subject: [PATCH] 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 --- openpilot/cereal/custom.capnp | 2 + openpilot/sunnypilot/modeld_v2/modeld.py | 24 ++++-- .../modeld_v2/tests/test_turn_plan_floor.py | 78 ++++++++++++++++++- .../sunnypilot/modeld_v2/turn_plan_floor.py | 18 ++++- 4 files changed, 112 insertions(+), 10 deletions(-) diff --git a/openpilot/cereal/custom.capnp b/openpilot/cereal/custom.capnp index 430aa140d9..ddd03f5daf 100644 --- a/openpilot/cereal/custom.capnp +++ b/openpilot/cereal/custom.capnp @@ -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; diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index e1643f4d1a..2214571be9 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/openpilot/sunnypilot/modeld_v2/modeld.py @@ -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 diff --git a/openpilot/sunnypilot/modeld_v2/tests/test_turn_plan_floor.py b/openpilot/sunnypilot/modeld_v2/tests/test_turn_plan_floor.py index 001cc38e12..e91fe26276 100644 --- a/openpilot/sunnypilot/modeld_v2/tests/test_turn_plan_floor.py +++ b/openpilot/sunnypilot/modeld_v2/tests/test_turn_plan_floor.py @@ -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) diff --git a/openpilot/sunnypilot/modeld_v2/turn_plan_floor.py b/openpilot/sunnypilot/modeld_v2/turn_plan_floor.py index e30199f42d..c975583ffb 100644 --- a/openpilot/sunnypilot/modeld_v2/turn_plan_floor.py +++ b/openpilot/sunnypilot/modeld_v2/turn_plan_floor.py @@ -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)