Ford: drive fast path from desired curvature

Make C0 and C1 a coherent virtual-curvature pair sourced from the constrained action target and measured tracking error. Keep model trend only for supplemental C2 unloading so model geometry cannot inflate fast steering authority across vehicles.

Assisted-by: Codex

openpilot/selfdrive/controls/lib/ford_path.py

@@ -77,29 +77,14 @@ def _curvature_rate(path: tuple[list[float], list[float], list[float]]) -> float
   return sorted(rates)[1] * abs(sum(rates)) / magnitude
 
 
-def _curvature(path: tuple[list[float], list[float], list[float]]) -> float:
-  distance, _, heading = path
-  horizon = min(_PATH_MIN_LOOKAHEAD, distance[-1])
-  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:
   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)
-  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
   correction = 0.0
   wheel_beyond_target = False
   if current_curvature is not None:
@@ -113,29 +98,21 @@ 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
-    if wheel_beyond_target:
-      path_offset = correction_offset
-      path_angle = correction_angle
-    else:
-      path_offset += correction_offset
-      path_angle += correction_angle
 
   future_curvature = action_curvature + model_curvature_rate * lookahead
   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)
-  maneuver_demand = max(abs(requested_curvature), abs(correction))
+  maneuver_demand = max(abs(action_curvature), abs(correction))
   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
 
   pose_gain = 0.20 + 0.80 * maneuver_share
-  path_offset *= pose_gain
-  path_angle *= pose_gain
+  fast_curvature = correction if wheel_beyond_target else \
+    (action_curvature - centering_curvature) * pose_gain + correction
+  path_offset = 0.5 * fast_curvature * _PATH_OFFSET_DISTANCE ** 2
+  path_angle = fast_curvature * lookahead
 
   return FordPath(
     valid=True,

openpilot/selfdrive/controls/tests/test_ford_path.py

@@ -195,8 +195,24 @@ def test_action_demand_exposes_forward_path_authority():
   assert _equivalent_curvature(command) >= 0.004
 
 
-def test_model_turn_exposes_fast_authority_before_action_catches_up():
-  command = FordPathController(dt=1.0).update(_path(0.04), 0.002, v_ego=8.0, current_curvature=0.002)
+def test_model_curvature_does_not_change_fast_command_for_same_action():
+  gentle_model = _command(_path(0.004), 0.004, v_ego=8.0, current_curvature=0.002)
+  aggressive_model = _command(_path(0.04), 0.004, v_ego=8.0, current_curvature=0.002)
+
+  assert np.isclose(aggressive_model.path_offset, gentle_model.path_offset)
+  assert np.isclose(aggressive_model.path_angle, gentle_model.path_angle)
+
+
+def test_fast_fields_encode_one_virtual_curvature():
+  command = _command(_path(0.004), 0.008, v_ego=8.0, current_curvature=0.0)
+
+  offset_curvature = 2.0 * command.path_offset / 7.0 ** 2
+  angle_curvature = command.path_angle / 8.0
+  assert np.isclose(offset_curvature, angle_curvature)
+
+
+def test_action_turn_exposes_fast_authority_without_large_model_arc():
+  command = FordPathController(dt=1.0).update(_path(0.002), 0.04, v_ego=8.0, current_curvature=0.002)
 
   assert command.curvature == 0.0
   assert command.path_offset > 0.5