diff --git a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py index 536a74f6c2..e2c5bf7a6d 100644 --- a/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py +++ b/openpilot/selfdrive/ui/sunnypilot/mici/layouts/models.py @@ -125,9 +125,10 @@ class ModelsLayoutMici(NavScroller): "while the model still sets the cruising speed. Turns off the accel boost. " + "Needs openpilot longitudinal. Not available with the Default model.")) - self.path_steering_toggle = BigParamControl(tr("slow-turn path steering (alpha)"), "TurnInHold", - description=tr("Below about 15 mph, limits steering to the model's planned line at your actual speed instead of the speed the " + - "model expects, so a slow turn is not taken tighter than planned. Only ever steers less. Fades out by 15 mph. " + + self.path_steering_toggle = BigParamControl(tr("turn correction (alpha)"), "TurnInHold", + description=tr("In slow turns, corrects the model's steering toward the line FSD and you take, with a correction learned for " + + "that model from your drives. It can only move steering within the range the model's own plan allows. A model " + + "without a learned correction is only kept from turning tighter than its planned line at your speed. " + "Not available with the Default model.")) self.map_turn_toggle = BigParamControl(tr("signal navigation (experimental)"), "MapboxNextTurnEnabled", diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index c43e5330a0..c1d2d2376b 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/openpilot/sunnypilot/modeld_v2/modeld.py @@ -53,6 +53,7 @@ from openpilot.sunnypilot.modeld_v2.steering_smoothing import lat_smooth_seconds from openpilot.sunnypilot.modeld_v2.turn_desire_refresh import TurnDesireRefresh, desire_input from openpilot.sunnypilot.modeld_v2.turn_plan_floor import apply_turn_plan_floor from openpilot.sunnypilot.modeld_v2 import path_steering +from openpilot.sunnypilot.modeld_v2.turn_correction import apply_turn_correction, correction_for 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) @@ -106,6 +107,7 @@ class ModelState(ModelStateBase): model_bundle = get_active_bundle(chestnut=chestnut) self.generation = model_bundle.generation if model_bundle is not None else None self.model_name = model_bundle.displayName if model_bundle is not None else '' + self.turn_correction = correction_for(model_bundle.ref) if model_bundle is not None else None overrides = {override.key: override.value for override in model_bundle.overrides} if model_bundle else {} self.LAT_SMOOTH_SECONDS = float(overrides.get('lat', ".0")) @@ -267,18 +269,26 @@ class ModelState(ModelStateBase): action_accel=desired_accel) desired_curvature = model_output['action'][0, 0] / (max(1.0, v_ego))**2 self.action_curvature, self.turn_plan_curvature = desired_curvature, 0. - if turn_sign: - # the action head under-commits in slow turns while the model's own plan has the turn; use the plan as a floor - plan = model_output['plan'][0] + plan = model_output['plan'][0] + if path_steering_enabled and self.turn_correction is not None: + # this model has a learned turn correction toward the FSD/driver line; it replaces the floor and the path cap 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) - desired_curvature = apply_turn_plan_floor(desired_curvature, self.turn_plan_curvature, turn_sign) - if path_steering_enabled and path_steering.path_weight(v_ego) > 0.: - plan = model_output['plan'][0] path = path_steering.path_curvature(plan[:, Plan.POSITION][:, 0], plan[:, Plan.POSITION][:, 1], plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], plan[0, Plan.ORIENTATION_RATE][2], v_ego, a_ego, lat_action_t) - desired_curvature = path_steering.apply_path_steering(desired_curvature, path, v_ego) + desired_curvature = apply_turn_correction(self.turn_correction, desired_curvature, self.turn_plan_curvature, path, v_ego) + else: + if turn_sign: + # the action head under-commits in slow turns while the model's own plan has the turn; use the plan as a floor + 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) + desired_curvature = apply_turn_plan_floor(desired_curvature, self.turn_plan_curvature, turn_sign) + if path_steering_enabled and path_steering.path_weight(v_ego) > 0.: + path = path_steering.path_curvature(plan[:, Plan.POSITION][:, 0], plan[:, Plan.POSITION][:, 1], + plan[:, Plan.T_FROM_CURRENT_EULER][:, 2], plan[0, Plan.ORIENTATION_RATE][2], + v_ego, a_ego, lat_action_t) + desired_curvature = path_steering.apply_path_steering(desired_curvature, path, v_ego) if blendful is not None: desired_curvature += blendful(desired_curvature) diff --git a/openpilot/sunnypilot/modeld_v2/tests/test_turn_correction.py b/openpilot/sunnypilot/modeld_v2/tests/test_turn_correction.py new file mode 100644 index 0000000000..d62af8dc40 --- /dev/null +++ b/openpilot/sunnypilot/modeld_v2/tests/test_turn_correction.py @@ -0,0 +1,35 @@ +import pytest + +from openpilot.sunnypilot.modeld_v2 import turn_correction as tc + +C = tc.TurnCorrection('test', v_full=6., v_end=10., theta_path=0.1, theta_time=0.8) + + +def test_no_change_at_road_speed_and_full_below_v_full(): + assert tc.apply_turn_correction(C, 0.05, 0.10, 0.12, 12.) == 0.05 + full = tc.apply_turn_correction(C, 0.05, 0.10, 0.12, 3.) + assert full == pytest.approx(0.05 + 0.1 * 0.07 + 0.8 * 0.05) + half = tc.apply_turn_correction(C, 0.05, 0.10, 0.12, 8.) + assert half == pytest.approx(0.05 + 0.5 * (full - 0.05)) + + +def test_it_stays_within_the_models_own_range(): + # the plan's time axis says far more than both the action and the line at the car's speed: bounded at path + 10% + assert tc.apply_turn_correction(C, 0.10, 0.40, 0.14, 3.) == pytest.approx(0.14 + 0.1 * 0.04) + # a turn the action and the line both say is straight is not invented from the time axis + assert tc.apply_turn_correction(C, 0.0, 0.2, 0.0, 3.) == 0.0 + # nor reversed: action and line right, the time axis left + assert tc.apply_turn_correction(C, 0.1, -0.2, 0.1, 3.) == pytest.approx(0.1) + + +def test_it_can_lower_an_overcommitted_action_toward_the_line(): + lower = tc.TurnCorrection('test', 4.5, 7., theta_path=0.9, theta_time=0.) + assert tc.apply_turn_correction(lower, -0.21, -0.21, -0.14, 3.) == pytest.approx(-0.21 + 0.9 * 0.07) + + +def test_lookup_by_ref_and_local_prefix(monkeypatch): + monkeypatch.setattr(tc, 'CORRECTIONS', {'abc': C, 'local-ff_turns_600-': C}) + assert tc.correction_for('abc') is C + assert tc.correction_for('local-ff_turns_600-0123456789abcdef') is C + assert tc.correction_for('local-other-0123') is None + assert tc.correction_for('') is None diff --git a/openpilot/sunnypilot/modeld_v2/turn_correction.py b/openpilot/sunnypilot/modeld_v2/turn_correction.py new file mode 100644 index 0000000000..86fcf6ba9c --- /dev/null +++ b/openpilot/sunnypilot/modeld_v2/turn_correction.py @@ -0,0 +1,56 @@ +""" +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. +""" +from dataclasses import dataclass + +import numpy as np + +# Learned turn correction: per model, a small correction on top of its action toward the line FSD and the driver +# took, fitted by openpilot.distill's supervised/fit_turn_correction.py on open-loop frames of driver/FSD turns and +# closed-loop frames of the user's engaged drives (where the driver or FSD was steering, or a turn openpilot steered +# with no push-back), scored leave-one-route-out. It replaces the 0.75 turn plan floor and the path cap for a model +# that has one. With +# time = the plan's curvature at action_t on its time axis (get_curvature_from_plan) +# path = the plan's heading at the arc length the car covers in action_t (path_steering.path_curvature) +# command = action + s(v) * (theta_path * (path - action) + theta_time * (time - action)), s = 1 below v_full and 0 +# from v_end, and the command stays within the range action..path spans (plus 10%), so it can only move the action +# toward the model's own line, never invent or reverse a turn. +MARGIN = 0.1 + + +@dataclass(frozen=True) +class TurnCorrection: + name: str + v_full: float + v_end: float + theta_path: float + theta_time: float + + +# bundle ref (or a local build's ref prefix ending in '-') -> correction +CORRECTIONS: dict[str, TurnCorrection] = { + # Nov 9 2026, closed-loop frames of routes 98, 9a, 9e, 9f, b0 (26 turns, 3640 frames). Held out by route, mean + # |command - driven| 1-3 m/s 0.039 -> 0.021 (0.75 floor 0.029), 3-4.5 m/s 0.028 -> 0.014, 4.5-6 m/s 0.017 -> 0.008, + # 6-9 m/s 0.006 -> 0.004; peak command / peak driven per turn 0.74 (floor 0.83) -> 1.02 [10-90%: 0.84-1.26]. + 'b92fc4828664f8767bce5e92f1630ed319cbe509': TurnCorrection('Light Speed 2', v_full=6.0, v_end=10.0, + theta_path=0.0554, theta_time=0.8708), +} + + +def correction_for(ref: str) -> TurnCorrection | None: + if ref in CORRECTIONS: + return CORRECTIONS[ref] + return next((c for key, c in CORRECTIONS.items() if key.endswith('-') and ref.startswith(key)), None) + + +def apply_turn_correction(correction: TurnCorrection, action: float, time: float, path: float, v_ego: float) -> float: + weight = float(np.clip((correction.v_end - v_ego) / (correction.v_end - correction.v_full), 0., 1.)) + if weight == 0.: + return action + raw = action + weight * (correction.theta_path * (path - action) + correction.theta_time * (time - action)) + lo, hi = min(action, path), max(action, path) + margin = MARGIN * (hi - lo) + return float(np.clip(raw, lo - margin, hi + margin))