modeld_v2: floor turn steering at 0.75x the model's plan

While a turn desire is active, the action head can under-commit on turn
entry even though the model's own plan already has the turn, so the car
swings wide into the far lane. Keep the curvature command from falling
below 0.75x the plan-derived curvature, in the turn direction only. The
plan-derived curvature reads ~15-20% tight against driven turns, hence
the scale.

Offline replay of 42 logged turns (Cinque Terre V3): a 0.7 floor halved
the median wide miss with no added inside drift on turns openpilot made
alone; 0.8 nearly closed it with more inside drift. 0.75 splits the two.
Not yet tested on the car.

openpilot/sunnypilot/modeld_v2/modeld.py

@@ -35,7 +35,7 @@ from openpilot.common.realtime import config_realtime_process, DT_MDL
 from openpilot.common.transformations.camera import DEVICE_CAMERAS
 from openpilot.common.transformations.model import get_warp_matrix
 from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
-from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value
+from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, get_curvature_from_plan, smooth_value
 from openpilot.selfdrive.modeld.modeld import ChestnutGpuState
 
 from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output
@@ -49,6 +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
 from openpilot.sunnypilot.modeld_v2.navigation_input import (NavigationInputBuffer, NavigationStatePublisher, SignalRouteGuard,
                                                           destination_route_context, navigation_capability)
 from openpilot.sunnypilot.models.helpers import get_active_bundle
@@ -238,7 +239,7 @@ class ModelState(ModelStateBase):
 
   def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
                             lat_action_t: float, long_action_t: float, v_ego: float,
-                            blendful: Callable[[float], float] | None = None) -> log.ModelDataV2.Action:
+                            blendful: Callable[[float], float] | None = None, turn_sign: float = 0.) -> log.ModelDataV2.Action:
     if 'action' not in model_output:
       plan = model_output['plan'][0]
       desired_accel = get_accel_from_plan(plan[:, Plan.VELOCITY][:, 0], plan[:, Plan.ACCELERATION][:, 0], self.constants.T_IDXS,
@@ -250,6 +251,12 @@ class ModelState(ModelStateBase):
     else:
       desired_accel = model_output['action'][0, 1]
       desired_curvature = model_output['action'][0, 0] / (max(1.0, v_ego))**2
+      if turn_sign:
+        # the action head can under-commit on turn entry while the model's own plan has the turn; use the plan as a floor
+        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)
     if blendful is not None:
       desired_curvature += blendful(desired_curvature)
 
@@ -545,8 +552,10 @@ def main(demo=False):
       if not sm['carControl'].latActive:
         prev_action.desiredCurvature = sm['controlsState'].curvature
       blendful_active = desire == log.Desire.none and DH.lane_change_state == log.LaneChangeState.off
+      turn_sign = 1. if desire == log.Desire.turnRight else -1. if desire == log.Desire.turnLeft else 0.  # curvature + is right
       action = model.get_action_from_model(model_output, prev_action, lat_action_t, long_action_t, v_ego,
-                                           partial(blendful.update, model_output, v_ego, blendful_active, enabled=blendful_enabled))
+                                           partial(blendful.update, model_output, v_ego, blendful_active, enabled=blendful_enabled),
+                                           turn_sign=turn_sign)
       prev_action = action
       fill_model_msg(drivingdata_send, modelv2_send, model_output, action,
                      publish_state, meta_main.frame_id, meta_extra.frame_id, frame_id,

openpilot/sunnypilot/modeld_v2/tests/test_turn_plan_floor.py

@@ -0,0 +1,36 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+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, apply_turn_plan_floor
+
+RIGHT, LEFT, NONE = 1., -1., 0.
+
+
+def test_no_turn_leaves_action_alone():
+  assert apply_turn_plan_floor(0.01, 0.2, NONE) == 0.01
+
+
+@pytest.mark.parametrize("sign", [RIGHT, LEFT])
+def test_weak_action_is_raised_to_scaled_plan(sign):
+  # curvature is + to the right, so a left turn is negative
+  assert apply_turn_plan_floor(sign * 0.03, sign * 0.10, sign) == pytest.approx(sign * TURN_PLAN_FLOOR * 0.10)
+
+
+@pytest.mark.parametrize("sign", [RIGHT, LEFT])
+def test_strong_action_is_kept(sign):
+  assert apply_turn_plan_floor(sign * 0.12, sign * 0.10, sign) == sign * 0.12
+
+
+@pytest.mark.parametrize("sign", [RIGHT, LEFT])
+def test_plan_against_the_turn_never_weakens_or_reverses(sign):
+  assert apply_turn_plan_floor(sign * 0.05, -sign * 0.10, sign) == sign * 0.05
+
+
+def test_floor_only_pushes_toward_the_requested_side():
+  # a right-turn request must never add left curvature, even if action and plan both point left
+  assert apply_turn_plan_floor(-0.02, -0.10, RIGHT) == -0.02

openpilot/sunnypilot/modeld_v2/turn_plan_floor.py

@@ -0,0 +1,23 @@
+"""
+Copyright (c) 2021-, Haibin Wen, sunnypilot, and a number of other contributors.
+
+This file is part of sunnypilot and is licensed under the MIT License.
+See the LICENSE.md file in the root directory for more details.
+"""
+
+# Share of the plan-derived curvature the command may not fall below during a requested turn. The plan-derived
+# curvature reads ~15-20% tighter than turns actually driven, so it is only trusted as a scaled-down floor.
+TURN_PLAN_FLOOR = 0.75
+
+
+def apply_turn_plan_floor(action_curvature: float, plan_curvature: float, turn_sign: float,
+                          scale: float = TURN_PLAN_FLOOR) -> float:
+  """Keep the steering command from falling below a scaled share of the model's own plan during a requested turn.
+
+  Curvature is + to the right. turn_sign is +1 for a right turn, -1 for a left turn and 0 when no turn is requested.
+  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.
+  """
+  if turn_sign == 0.:
+    return action_curvature
+  return turn_sign * max(turn_sign * action_curvature, scale * turn_sign * plan_curvature)