From 8774a462ac6ee06fff9e4abc96c047bdc57e7974 Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Sun, 30 Aug 2026 08:37:40 -0400 Subject: [PATCH] ford: add coherent path pose experiment Assisted-by: Codex --- openpilot/common/params_keys.h | 3 ++ openpilot/selfdrive/controls/controlsd.py | 2 +- openpilot/selfdrive/controls/lib/ford_path.py | 41 +++++++++++++------ .../controls/tests/test_ford_path.py | 38 +++++++++++++++++ .../layouts/settings/vehicle/brands/ford.py | 11 ++++- 5 files changed, 81 insertions(+), 14 deletions(-) diff --git a/openpilot/common/params_keys.h b/openpilot/common/params_keys.h index e76d2890d3..02d22ce8f6 100644 --- a/openpilot/common/params_keys.h +++ b/openpilot/common/params_keys.h @@ -192,6 +192,9 @@ inline static std::unordered_map keys = { {"StandstillTimer", {PERSISTENT | BACKUP, BOOL, "0"}}, {"TrueVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}}, + // Ford params + {"FordCoherentPath", {PERSISTENT | BACKUP, BOOL, "1"}}, + // MADS params {"Mads", {PERSISTENT | BACKUP, BOOL, "1"}}, {"MadsMainCruiseAllowed", {PERSISTENT | BACKUP, BOOL, "1"}}, diff --git a/openpilot/selfdrive/controls/controlsd.py b/openpilot/selfdrive/controls/controlsd.py index a5f0455a66..f6b3317231 100755 --- a/openpilot/selfdrive/controls/controlsd.py +++ b/openpilot/selfdrive/controls/controlsd.py @@ -53,7 +53,7 @@ class Controls(ControlsExt): self.steer_limited_by_safety = False self.curvature = 0.0 self.desired_curvature = 0.0 - self.ford_path_controller = FordPathController() + self.ford_path_controller = FordPathController(coherent_pose=self.params.get_bool("FordCoherentPath")) self.ford_path = FordPath() self.pose_calibrator = PoseCalibrator() diff --git a/openpilot/selfdrive/controls/lib/ford_path.py b/openpilot/selfdrive/controls/lib/ford_path.py index 0403a850b1..80a897610d 100644 --- a/openpilot/selfdrive/controls/lib/ford_path.py +++ b/openpilot/selfdrive/controls/lib/ford_path.py @@ -83,23 +83,26 @@ def _curvature(path: tuple[list[float], list[float], list[float]]) -> float: return (_sample(horizon, distance, heading) - _sample(0.0, distance, heading)) / horizon -def _encode_path(model, desired_curvature: float, v_ego: float, current_curvature: float | None) -> FordPath: +def _encode_path(model, desired_curvature: float, v_ego: float, current_curvature: float | None, + coherent_pose: bool = False) -> FordPath: path = _model_path(model) if path is None: return FordPath() distance, offset, heading = path lookahead = max(_finite(v_ego), _PATH_MIN_LOOKAHEAD) - path_offset = _sample(_PATH_OFFSET_DISTANCE, distance, offset) - path_angle = _sample(lookahead, distance, heading) + pose_horizon = min(lookahead, distance[-1]) if coherent_pose else lookahead + offset_horizon = pose_horizon if coherent_pose else _PATH_OFFSET_DISTANCE + path_offset = _sample(offset_horizon, distance, offset) + path_angle = _sample(pose_horizon, distance, heading) model_curvature = _curvature(path) model_curvature_rate = _curvature_rate(path) action_curvature = _finite(desired_curvature) requested_curvature = action_curvature if action_curvature * model_curvature < 0.0 else \ max((model_curvature, action_curvature), key=abs) maneuver_residual = requested_curvature - model_curvature - path_offset += 0.5 * maneuver_residual * _PATH_OFFSET_DISTANCE ** 2 - path_angle += maneuver_residual * lookahead + path_offset += 0.5 * maneuver_residual * offset_horizon ** 2 + path_angle += maneuver_residual * pose_horizon correction = 0.0 wheel_beyond_target = False if current_curvature is not None: @@ -113,8 +116,8 @@ def _encode_path(model, desired_curvature: float, v_ego: float, current_curvatur if correction * target_curvature < 0.0: correction_limit = 0.5 * abs(target_curvature) correction = float(np.clip(correction, -correction_limit, correction_limit)) - correction_offset = 0.5 * correction * _PATH_OFFSET_DISTANCE ** 2 - correction_angle = correction * lookahead + correction_offset = 0.5 * correction * offset_horizon ** 2 + correction_angle = correction * pose_horizon if wheel_beyond_target: path_offset = correction_offset path_angle = correction_angle @@ -122,7 +125,7 @@ def _encode_path(model, desired_curvature: float, v_ego: float, current_curvatur path_offset += correction_offset path_angle += correction_angle - future_curvature = action_curvature + model_curvature_rate * lookahead + future_curvature = action_curvature + model_curvature_rate * pose_horizon sustained_curvature = 0.0 if action_curvature * future_curvature > 0.0 and abs(future_curvature) > _TRACKING_ERROR_DEADZONE: sustained_curvature = math.copysign(min(abs(action_curvature), abs(future_curvature)), action_curvature) @@ -130,12 +133,16 @@ def _encode_path(model, desired_curvature: float, v_ego: float, current_curvatur maneuver_share = float(np.interp(maneuver_demand, _FAST_POSE_CURVATURE_BAND, (0.0, 1.0))) centering_curvature = 0.0 if wheel_beyond_target else \ sustained_curvature * _CENTERING_CURVATURE_SHARE * (1.0 - maneuver_share) - path_offset -= 0.5 * centering_curvature * _PATH_OFFSET_DISTANCE ** 2 - path_angle -= centering_curvature * lookahead + path_offset -= 0.5 * centering_curvature * offset_horizon ** 2 + path_angle -= centering_curvature * pose_horizon pose_gain = 0.20 + 0.80 * maneuver_share path_offset *= pose_gain path_angle *= pose_gain + if coherent_pose: + # C0/C1 describe one line which, together with C2, reaches the same + # forward position and heading at a shared horizon. + path_offset -= path_angle * pose_horizon return FordPath( valid=True, @@ -149,11 +156,21 @@ def _encode_path(model, desired_curvature: float, v_ego: float, current_curvatur class FordPathController: """Convert the model path directly into one vehicle-independent Ford path command.""" - def __init__(self, dt: float = 0.01): + def __init__(self, dt: float = 0.01, coherent_pose: bool = False): self.dt = dt + self.coherent_pose = coherent_pose self._last_path = FordPath(valid=True) def _limit(self, target: FordPath) -> FordPath: + if self.coherent_pose: + deltas = (target.path_offset - self._last_path.path_offset, + target.path_angle - self._last_path.path_angle) + scale = min((1.0, *(rate * self.dt / abs(delta) for delta, rate in zip(deltas, _PATH_RATES[:2], strict=True) if delta != 0.0))) + path_offset = self._last_path.path_offset + scale * deltas[0] + path_angle = self._last_path.path_angle + scale * deltas[1] + self._last_path = FordPath(True, path_offset, path_angle, target.curvature, target.curvature_rate) + return self._last_path + values = [] for field, rate in zip(fields(FordPath)[1:], _PATH_RATES, strict=True): previous = getattr(self._last_path, field.name) @@ -169,4 +186,4 @@ class FordPathController: return FordPath() if model is None: return self._limit(FordPath(valid=True)) - return self._limit(_encode_path(model, desired_curvature, v_ego, current_curvature)) + return self._limit(_encode_path(model, desired_curvature, v_ego, current_curvature, self.coherent_pose)) diff --git a/openpilot/selfdrive/controls/tests/test_ford_path.py b/openpilot/selfdrive/controls/tests/test_ford_path.py index c1752552f6..7f9c44e6fd 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_path.py +++ b/openpilot/selfdrive/controls/tests/test_ford_path.py @@ -35,6 +35,44 @@ def _command(model, desired_curvature: float, *, v_ego: float = 0.0, current_cur return FordPathController(dt=1.0).update(model, desired_curvature, v_ego=v_ego, current_curvature=current_curvature) +def _coherent_command(model, desired_curvature: float, *, v_ego: float = 0.0, current_curvature: float | None = None): + return FordPathController(dt=1.0, coherent_pose=True).update( + model, desired_curvature, v_ego=v_ego, current_curvature=current_curvature, + ) + + +def test_coherent_pose_matches_one_forward_position_and_heading(): + horizon = 8.0 + model = _path(0.04) + command = _coherent_command(model, 0.04, v_ego=horizon, current_curvature=0.04) + expected_heading = 0.04 * horizon + expected_offset = (1.0 - math.cos(expected_heading)) / 0.04 + + commanded_heading = command.path_angle + command.curvature * horizon + commanded_offset = command.path_offset + command.path_angle * horizon + 0.5 * command.curvature * horizon ** 2 + + assert np.isclose(commanded_heading, expected_heading, atol=2e-3) + assert np.isclose(commanded_offset, expected_offset, atol=2e-3) + + +def test_existing_pose_encoder_remains_available_as_fallback(): + model = _path(0.04) + fallback = FordPathController(dt=1.0, coherent_pose=False).update(model, 0.04, v_ego=8.0, current_curvature=0.04) + current_default = FordPathController(dt=1.0).update(model, 0.04, v_ego=8.0, current_curvature=0.04) + + assert fallback == current_default + + +def test_coherent_pose_slews_offset_and_angle_together(): + model = _path(0.04) + target = FordPathController(dt=1.0, coherent_pose=True).update(model, 0.04, v_ego=8.0, current_curvature=0.04) + limited = FordPathController(dt=0.01, coherent_pose=True).update(model, 0.04, v_ego=8.0, current_curvature=0.04) + + assert np.isclose(limited.path_offset / target.path_offset, limited.path_angle / target.path_angle) + assert abs(limited.path_offset) <= 0.04 + assert abs(limited.path_angle) <= 0.01 + + def test_steady_arc_keeps_c2_with_small_continuous_pose_authority(): path = _command(_path(0.008), 0.008, v_ego=8.0) diff --git a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/vehicle/brands/ford.py b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/vehicle/brands/ford.py index 8871087e03..d21b12c992 100644 --- a/openpilot/selfdrive/ui/sunnypilot/layouts/settings/vehicle/brands/ford.py +++ b/openpilot/selfdrive/ui/sunnypilot/layouts/settings/vehicle/brands/ford.py @@ -5,11 +5,20 @@ 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 openpilot.selfdrive.ui.sunnypilot.layouts.settings.vehicle.brands.base import BrandSettings +from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.system.ui.lib.multilang import tr +from openpilot.system.ui.sunnypilot.widgets.list_view import toggle_item_sp class FordSettings(BrandSettings): def __init__(self): super().__init__() + self.coherent_path_toggle = toggle_item_sp( + tr("Coherent Ford Path (Experimental)"), + tr("Solve the fast path offset and angle at one shared lookahead. Disable to use the previous Ford path controller."), + param="FordCoherentPath", + ) + self.items = [self.coherent_path_toggle] def update_settings(self): - pass + self.coherent_path_toggle.action_item.set_enabled(ui_state.is_offroad())