From 1b41e9637f5fbc636e1aca37d50fb9f2cebb82b2 Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Sat, 29 Aug 2026 07:53:25 -0400 Subject: [PATCH] ford: balance path pose and curvature unwind Assisted-by: Codex --- openpilot/selfdrive/controls/lib/ford_path.py | 36 ++++--- .../controls/tests/test_ford_path.py | 94 ++++++++++++++----- 2 files changed, 93 insertions(+), 37 deletions(-) diff --git a/openpilot/selfdrive/controls/lib/ford_path.py b/openpilot/selfdrive/controls/lib/ford_path.py index 3c6859b647..0403a850b1 100644 --- a/openpilot/selfdrive/controls/lib/ford_path.py +++ b/openpilot/selfdrive/controls/lib/ford_path.py @@ -12,10 +12,11 @@ DBC_CURVATURE_RATE = (-0.001024, 0.001023) _PATH_OFFSET_DISTANCE = 7.0 _PATH_MIN_LOOKAHEAD = 7.0 _CURVATURE_RATE_HORIZONS = (3.5, 5.0, 7.0) -_CENTERING_CURVATURE_BASEBAND = (0.003, 0.006) +_FAST_POSE_CURVATURE_BAND = (0.009, 0.012) +_CENTERING_CURVATURE_SHARE = 0.65 _TRACKING_ERROR_DEADZONE = 0.0005 _TRACKING_ERROR_LIMIT = 0.012 -_PATH_RATES = (4.0, 1.0, math.inf, 0.002) +_PATH_RATES = (4.0, 1.0, math.inf, math.inf) @dataclass(frozen=True) @@ -94,19 +95,18 @@ def _encode_path(model, desired_curvature: float, v_ego: float, current_curvatur model_curvature = _curvature(path) model_curvature_rate = _curvature_rate(path) action_curvature = _finite(desired_curvature) - requested_curvature = max((model_curvature, action_curvature), key=abs) + 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 - tracking_demand = 0.0 wheel_beyond_target = False if current_curvature is not None: target_curvature = action_curvature measured_curvature = _finite(current_curvature) tracking_error = target_curvature - measured_curvature correction = math.copysign(max(abs(tracking_error) - _TRACKING_ERROR_DEADZONE, 0.0), tracking_error) - tracking_demand = abs(correction) wheel_beyond_target = target_curvature * measured_curvature > 0.0 and \ abs(target_curvature) + _TRACKING_ERROR_DEADZONE < abs(measured_curvature) correction_limit = _TRACKING_ERROR_LIMIT @@ -122,21 +122,27 @@ def _encode_path(model, desired_curvature: float, v_ego: float, current_curvatur path_offset += correction_offset path_angle += correction_angle - spatial_demand = abs(model_curvature_rate) * lookahead / 3.0 - overflow_demand = max(abs(requested_curvature) - DBC_CURVATURE[1], 0.0) - maneuver_demand = max(spatial_demand, overflow_demand, tracking_demand) - maneuver_share = 1.0 if wheel_beyond_target else \ - float(np.interp(maneuver_demand, _CENTERING_CURVATURE_BASEBAND, (0.0, 1.0))) - path_offset *= maneuver_share - path_angle *= maneuver_share - centering_curvature = action_curvature + 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_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 return FordPath( valid=True, path_offset=float(np.clip(path_offset, *DBC_OFFSET)), path_angle=float(np.clip(path_angle, *DBC_ANGLE)), - curvature=float(np.clip(centering_curvature * (1.0 - maneuver_share), *DBC_CURVATURE)), - curvature_rate=float(np.clip(model_curvature_rate * maneuver_share, *DBC_CURVATURE_RATE)), + curvature=float(np.clip(centering_curvature, *DBC_CURVATURE)), + curvature_rate=0.0, ) diff --git a/openpilot/selfdrive/controls/tests/test_ford_path.py b/openpilot/selfdrive/controls/tests/test_ford_path.py index fc27610852..c1752552f6 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_path.py +++ b/openpilot/selfdrive/controls/tests/test_ford_path.py @@ -35,16 +35,58 @@ 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 test_steady_arc_uses_c2_without_fast_pose_fields(): +def test_steady_arc_keeps_c2_with_small_continuous_pose_authority(): path = _command(_path(0.008), 0.008, v_ego=8.0) assert path.valid - assert abs(path.path_offset) < 1e-9 - assert abs(path.path_angle) < 1e-9 - assert np.isclose(path.curvature, 0.008, atol=5e-5) + assert 0.0 < path.path_offset < 0.02 + assert 0.0 < path.path_angle < 0.01 + assert np.isclose(path.curvature, 0.0052, atol=5e-5) assert abs(path.curvature_rate) < 1e-5 +def test_gentle_changing_curve_keeps_continuous_pose_authority(): + path = _command(_path(0.004, 0.00015), 0.004, v_ego=8.0, current_curvature=0.004) + + assert abs(path.path_offset) > 0.0001 + assert abs(path.path_angle) > 0.0001 + assert 0.0 < path.curvature < 0.004 + + +def test_c2_unloads_before_near_horizon_curve_exit(): + path = _command(_path(0.004, -0.0005), 0.004, v_ego=8.0, current_curvature=0.004) + + assert path.curvature == 0.0 + assert path.curvature_rate == 0.0 + assert path.path_angle < 0.02 + + +def test_tight_curve_unwind_keeps_fast_pose_without_loading_c2(): + steady = _command(_path(0.015), 0.015, v_ego=10.0, current_curvature=0.012) + unwinding = _command(_path(0.015, -0.0005), 0.015, v_ego=10.0, current_curvature=0.012) + + assert steady.curvature == 0.0 + assert unwinding.curvature == 0.0 + assert abs(_equivalent_curvature(unwinding)) > 0.9 * abs(_equivalent_curvature(steady)) + + +def test_action_curvature_wins_over_opposing_model_geometry(): + path = _command(_path(0.008), -0.004, v_ego=7.0, current_curvature=0.0) + + assert path.path_angle < 0.0 + assert _equivalent_curvature(path) < -0.003 + + +def test_changing_path_keeps_c3_zero_without_software_drain(): + controller = FordPathController() + for _ in range(100): + changing = controller.update(_path(0.004, 0.001), 0.004, v_ego=15.0, current_curvature=0.004) + flat = controller.update(_path(0.004), 0.004, v_ego=15.0, current_curvature=0.004) + + assert changing.curvature_rate == 0.0 + assert flat.curvature_rate == 0.0 + + def test_sunnypilot_path_message_round_trip(): message = custom.CarControlSP.new_message() message.fordLateralPath.pathOffset = 0.3 @@ -153,20 +195,28 @@ def test_action_demand_exposes_forward_path_authority(): assert _equivalent_curvature(command) >= 0.004 -def test_minor_curve_uses_c2_when_tracking_is_close(): +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) + + assert command.curvature == 0.0 + assert command.path_offset > 0.5 + assert command.path_angle > 0.2 + + +def test_minor_curve_blends_c2_with_small_pose_authority_when_tracking_is_close(): command = FordPathController(dt=1.0).update(_path(0.005), 0.005, v_ego=8.0, current_curvature=0.0048) - assert abs(command.path_offset) < 1e-9 - assert abs(command.path_angle) < 1e-9 - assert command.curvature > 0.0049 + assert 0.0 < command.path_offset < 0.01 + assert 0.0 < command.path_angle < 0.01 + assert command.curvature > 0.003 -def test_minor_changing_curve_does_not_emit_ungated_c3(): +def test_minor_changing_curve_keeps_future_geometry_in_pose_not_c3(): command = FordPathController(dt=1.0).update(_path(0.005, 0.0003), 0.005, v_ego=8.0, current_curvature=0.0048) - assert abs(command.path_offset) < 1e-9 - assert abs(command.path_angle) < 1e-9 - assert command.curvature > 0.0049 + assert abs(command.path_offset) < 0.02 + assert abs(command.path_angle) < 0.01 + assert command.curvature > 0.003 assert command.curvature_rate == 0.0 @@ -175,8 +225,8 @@ def test_c2_uses_stable_action_curvature_not_independent_model_fit(): first = controller.update(_path(0.004), 0.002, v_ego=8.0, current_curvature=0.002) second = controller.update(_path(0.006), 0.002, v_ego=8.0, current_curvature=0.002) - assert np.isclose(first.curvature, 0.002) - assert np.isclose(second.curvature, 0.002) + assert np.isclose(first.curvature, 0.0013) + assert np.isclose(second.curvature, 0.0013) def test_action_curvature_corrects_stale_opposing_model_at_low_speed(): @@ -188,12 +238,12 @@ def test_action_curvature_corrects_stale_opposing_model_at_low_speed(): assert _equivalent_curvature(command) >= 0.004 -def test_small_action_sign_noise_does_not_reverse_a_strong_model_path(): +def test_action_sign_wins_over_opposing_model_path(): command = FordPathController(dt=1.0).update(_path(0.04), -0.0005, v_ego=6.0, current_curvature=0.02) - assert command.path_offset > 0.0 - assert command.path_angle > 0.0 - assert _equivalent_curvature(command) > 0.02 + assert command.path_offset < 0.0 + assert command.path_angle < 0.0 + assert _equivalent_curvature(command) < 0.0 def test_measured_curvature_after_path_exit_commands_countersteer(): @@ -235,8 +285,8 @@ def test_curvature_error_increases_forward_pose_command_while_behind(): assert behind.path_offset > tracking.path_offset + 0.01 assert behind.path_angle > tracking.path_angle + 0.015 - assert behind.curvature == 0.0 - assert tracking.curvature > 0.007 + assert np.isclose(behind.curvature, tracking.curvature) + assert tracking.curvature > 0.005 assert np.isclose(behind.curvature_rate, tracking.curvature_rate) @@ -254,8 +304,8 @@ def test_action_c2_remains_active_for_centering(): centering = FordPathController(dt=1.0).update(_path(0.002), 0.002, v_ego=15.0, current_curvature=0.002) assert centering.curvature > 0.001 - assert abs(centering.path_offset) < 1e-9 - assert abs(centering.path_angle) < 1e-9 + assert centering.path_offset > 0.0 + assert centering.path_angle > 0.0 def test_tight_turn_from_stop_builds_bounded_forward_pose_authority():