From ed56f3ff7cc9f242d9e8ea56d41e6d291e06b0cc Mon Sep 17 00:00:00 2001 From: Isaac Barham Date: Mon, 31 Aug 2026 17:31:40 -0400 Subject: [PATCH] Ford: restore curvature as primary path control Keep upstream-style desired curvature active in C2 for steady path following. Use C0/C1 only for demand beyond C2 and measured tracking error, preserving fast turn and unwind authority without replacing C2. Assisted-by: Codex --- openpilot/selfdrive/controls/lib/ford_path.py | 37 +---------- .../controls/tests/test_ford_path.py | 61 ++++++++++++------- 2 files changed, 41 insertions(+), 57 deletions(-) diff --git a/openpilot/selfdrive/controls/lib/ford_path.py b/openpilot/selfdrive/controls/lib/ford_path.py index a7548af334..17f66abd8c 100644 --- a/openpilot/selfdrive/controls/lib/ford_path.py +++ b/openpilot/selfdrive/controls/lib/ford_path.py @@ -12,10 +12,6 @@ DBC_CURVATURE = (-0.02, 0.02) DBC_CURVATURE_RATE = (-0.001024, 0.001023) _PATH_HORIZON = 7.0 -_CURVATURE_RATE_HORIZONS = (3.5, 5.0, 7.0) -_FAST_POSE_CURVATURE_BAND = (0.009, 0.012) -_CENTERING_CURVATURE_SHARE = 1.0 -_CURVATURE_NOISE_FLOOR = 0.0005 _PATH_RATES = (4.0, 1.0, math.inf, math.inf) @@ -32,10 +28,6 @@ def _finite(value: float) -> float: return float(value) if math.isfinite(value) else 0.0 -def _sample(distance: float, distances: list[float], values: list[float]) -> float: - return float(np.interp(distance, distances, values)) - - def _model_path(model) -> tuple[list[float], list[float], list[float]] | None: try: x = [float(value) for value in model.position.x] @@ -61,40 +53,15 @@ def _model_path(model) -> tuple[list[float], list[float], list[float]] | None: return distance, y, unwrapped_heading -def _curvature_rate(path: tuple[list[float], list[float], list[float]]) -> float: - distance, _, heading = path - rates = [] - for requested_horizon in _CURVATURE_RATE_HORIZONS: - horizon = min(requested_horizon, distance[-1]) - start = _sample(0.0, distance, heading) - midpoint = _sample(0.5 * horizon, distance, heading) - end = _sample(horizon, distance, heading) - rates.append(4.0 * (start - 2.0 * midpoint + end) / horizon ** 2) - - magnitude = sum(abs(rate) for rate in rates) - if magnitude == 0.0: - return 0.0 - return sorted(rates)[1] * abs(sum(rates)) / magnitude - - def _encode_path(model, desired_curvature: float, v_ego: float, current_curvature: float | None) -> FordPath: - path = _model_path(model) - if path is None: + if _model_path(model) is None: return FordPath() - model_curvature_rate = _curvature_rate(path) action_curvature = _finite(desired_curvature) measured_curvature = float(np.clip(_finite(current_curvature), -MAX_CURVATURE, MAX_CURVATURE)) \ if current_curvature is not None else 0.0 - future_curvature = action_curvature + model_curvature_rate * max(_finite(v_ego), _PATH_HORIZON) - sustained_curvature = 0.0 - if action_curvature * future_curvature > 0.0 and abs(future_curvature) > _CURVATURE_NOISE_FLOOR: - sustained_curvature = math.copysign(min(abs(action_curvature), abs(future_curvature)), action_curvature) - maneuver_demand = abs(action_curvature) - maneuver_share = float(np.interp(maneuver_demand, _FAST_POSE_CURVATURE_BAND, (0.0, 1.0))) - centering_curvature = sustained_curvature * _CENTERING_CURVATURE_SHARE * (1.0 - maneuver_share) - + centering_curvature = float(np.clip(action_curvature, *DBC_CURVATURE)) fast_curvature = (action_curvature - centering_curvature) + (action_curvature - measured_curvature) path_offset = 0.5 * fast_curvature * _PATH_HORIZON ** 2 path_angle = fast_curvature * _PATH_HORIZON diff --git a/openpilot/selfdrive/controls/tests/test_ford_path.py b/openpilot/selfdrive/controls/tests/test_ford_path.py index 50d1034640..68d5e9518e 100644 --- a/openpilot/selfdrive/controls/tests/test_ford_path.py +++ b/openpilot/selfdrive/controls/tests/test_ford_path.py @@ -49,6 +49,22 @@ def test_steady_gentle_arc_uses_full_c2_without_fast_pose(): assert abs(path.curvature_rate) < 1e-5 +def test_upstream_c2_remains_primary_for_tight_curve_when_tracking(): + path = _command(_path(0.015), 0.015, v_ego=20.0, current_curvature=0.015) + + assert np.isclose(path.curvature, 0.015) + assert np.allclose(_fast_curvatures(path), (0.0, 0.0)) + + +def test_fast_fields_only_encode_c2_excess_and_tracking_error(): + tracking = _command(_path(0.04), 0.04, v_ego=8.0, current_curvature=0.04) + behind = _command(_path(0.04), 0.04, v_ego=8.0, current_curvature=0.03) + + assert np.isclose(tracking.curvature, DBC_CURVATURE[1]) + assert np.allclose(_fast_curvatures(tracking), (0.04 - DBC_CURVATURE[1],) * 2) + assert np.allclose(_fast_curvatures(behind), (0.05 - DBC_CURVATURE[1],) * 2) + + def test_gentle_changing_curve_stays_on_full_c2_when_tracking(): path = _command(_path(0.004, 0.00015), 0.004, v_ego=8.0, current_curvature=0.004) @@ -57,21 +73,21 @@ def test_gentle_changing_curve_stays_on_full_c2_when_tracking(): assert np.isclose(path.curvature, 0.004) -def test_c2_unloads_before_near_horizon_curve_exit(): +def test_c2_follows_action_instead_of_independent_model_unwind(): path = _command(_path(0.004, -0.0005), 0.004, v_ego=8.0, current_curvature=0.004) - assert path.curvature == 0.0 + assert np.isclose(path.curvature, 0.004) assert path.curvature_rate == 0.0 - assert path.path_angle < 0.03 + assert path.path_angle == 0.0 -def test_tight_curve_unwind_keeps_fast_pose_without_loading_c2(): +def test_model_unwind_does_not_remove_upstream_c2_baseline(): 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)) + assert np.isclose(steady.curvature, 0.015) + assert np.isclose(unwinding.curvature, 0.015) + assert np.isclose(_equivalent_curvature(unwinding), _equivalent_curvature(steady)) def test_action_curvature_wins_over_opposing_model_geometry(): @@ -107,12 +123,12 @@ def test_sunnypilot_path_message_round_trip(): assert path.valid -def test_tight_arc_uses_signed_forward_pose_without_slow_c2(): +def test_tight_arc_uses_c2_limit_and_signed_fast_residual(): left = _command(_path(0.04), 0.04, v_ego=8.0) right = _command(_path(-0.04), -0.04, v_ego=8.0) - assert left.curvature == 0.0 - assert right.curvature == 0.0 + assert left.curvature == DBC_CURVATURE[1] + assert right.curvature == DBC_CURVATURE[0] assert abs(left.curvature_rate) < 1e-4 assert abs(right.curvature_rate) < 1e-4 assert left.path_angle > 0.06 @@ -218,7 +234,7 @@ def test_fast_fields_encode_one_virtual_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.curvature == DBC_CURVATURE[1] assert command.path_offset > 0.5 assert command.path_angle > 0.2 @@ -313,8 +329,8 @@ def test_curvature_error_increases_forward_pose_command_while_behind(): def test_major_turn_undertracking_uses_bounded_fast_feedback_authority(): command = FordPathController(dt=1.0).update(_path(0.02), 0.02, v_ego=8.0, current_curvature=0.01) - assert command.curvature == 0.0 - assert command.path_angle >= 0.2 + assert command.curvature == DBC_CURVATURE[1] + assert np.isclose(command.path_angle, 0.07) def test_driver_well_beyond_requested_curvature_gets_countersteer(): @@ -341,7 +357,7 @@ def test_tight_turn_from_stop_builds_bounded_forward_pose_authority(): outputs = [controller.update(_path(0.04), 0.04, v_ego=0.0, current_curvature=0.0) for _ in range(20)] path = outputs[-1] - assert path.curvature == 0.0 + assert path.curvature == DBC_CURVATURE[1] assert path.path_offset > 0.7 assert path.path_angle > 0.15 assert np.max(np.abs(np.diff([output.path_offset for output in outputs]))) <= 0.04 + 1e-9 @@ -369,22 +385,23 @@ def test_invalid_model_ramps_pose_to_zero_while_remaining_in_extended_mode(): assert not controller.update(_path(0.0), 0.0, v_ego=12.0, active=False).valid -def test_fast_pose_holds_requested_arc_at_measured_target(): +def test_fast_pose_holds_only_demand_beyond_c2_at_measured_target(): command = _command(_path(0.03), 0.03, v_ego=20.0, current_curvature=0.03) offset_curvature, angle_curvature = _fast_curvatures(command) - assert np.isclose(offset_curvature, 0.03) - assert np.isclose(angle_curvature, 0.03) + assert np.isclose(command.curvature, DBC_CURVATURE[1]) + assert np.isclose(offset_curvature, 0.01) + assert np.isclose(angle_curvature, 0.01) -def test_fast_pose_adds_remaining_error_to_arc_feedforward(): +def test_fast_pose_adds_tracking_error_to_demand_beyond_c2(): entering = _command(_path(0.03), 0.03, current_curvature=0.0) halfway = _command(_path(0.03), 0.03, current_curvature=0.015) tracking = _command(_path(0.03), 0.03, current_curvature=0.03) - assert np.allclose(_fast_curvatures(entering), (0.06, 0.06)) - assert np.allclose(_fast_curvatures(halfway), (0.045, 0.045)) - assert np.allclose(_fast_curvatures(tracking), (0.03, 0.03)) + assert np.allclose(_fast_curvatures(entering), (0.04, 0.04)) + assert np.allclose(_fast_curvatures(halfway), (0.025, 0.025)) + assert np.allclose(_fast_curvatures(tracking), (0.01, 0.01)) def test_small_overshoot_reduces_hold_without_reversing_active_arc(): @@ -393,7 +410,7 @@ def test_small_overshoot_reduces_hold_without_reversing_active_arc(): assert 0.0 < overshoot.path_offset < tracking.path_offset assert 0.0 < overshoot.path_angle < tracking.path_angle - assert np.allclose(_fast_curvatures(overshoot), (0.029, 0.029)) + assert np.allclose(_fast_curvatures(overshoot), (0.009, 0.009)) def test_path_exit_uses_measured_curvature_to_unwind():