ford: add coherent path pose experiment

Assisted-by: Codex

openpilot/common/params_keys.h

@@ -192,6 +192,9 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> 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"}},

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()

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))

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)
 

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())