mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-04 21:03:48 +08:00
Ford: continuously blend model pose with C2
Use the model's forward offset and heading for fast path authority while C2 retains ordinary path following. Coordinate all transmitted coefficients through one bounded handoff and add measured-curvature catch-up without overshoot countersteer.\n\nAssisted-by: Codex
This commit is contained in:
Isaac Barham
@@ -160,7 +160,8 @@ class Controls(ControlsExt):
|
||||
actuators.steeringAngleDeg = float(lateral_output)
|
||||
if self.CP.brand == "ford":
|
||||
self.ford_path = self.ford_path_controller.update(model_v2 if self.sm.valid['modelV2'] else None,
|
||||
self.desired_curvature, v_ego=CS.vEgo, active=CC.latActive)
|
||||
self.desired_curvature, current_curvature=self.curvature,
|
||||
v_ego=CS.vEgo, active=CC.latActive)
|
||||
actuators.curvature = float(self.ford_path.curvature)
|
||||
# Ensure no NaNs/Infs
|
||||
for p in ACTUATOR_FIELDS:
|
||||
|
||||
@@ -9,10 +9,10 @@ DBC_ANGLE = (-0.5, 0.5235)
|
||||
DBC_CURVATURE = (-0.02, 0.02)
|
||||
DBC_CURVATURE_RATE = (-0.001024, 0.001023)
|
||||
|
||||
_PATH_LOOKAHEAD_TIME = 1.0
|
||||
_PATH_MIN_LOOKAHEAD = 7.0
|
||||
_MANEUVER_ENTER_CURVATURE = 0.018
|
||||
_MANEUVER_EXIT_CURVATURE = 0.012
|
||||
_POSE_BLEND_CURVATURE = (0.006, 0.012)
|
||||
_TRACKING_ERROR_DEADZONE = 0.0005
|
||||
_TRACKING_ERROR_LIMIT = 0.012
|
||||
_PATH_OFFSET_RATE = 4.0
|
||||
_PATH_ANGLE_RATE = 1.0
|
||||
|
||||
@@ -34,6 +34,11 @@ def _sample(distance: float, distances: list[float], values: list[float]) -> flo
|
||||
return float(np.interp(distance, distances, values))
|
||||
|
||||
|
||||
def _blend_share(demand: float) -> float:
|
||||
lower, upper = _POSE_BLEND_CURVATURE
|
||||
return float(np.clip((demand - lower) / (upper - lower), 0.0, 1.0))
|
||||
|
||||
|
||||
def _model_path(model) -> tuple[list[float], list[float], list[float]] | None:
|
||||
try:
|
||||
x = [float(value) for value in model.position.x]
|
||||
@@ -59,56 +64,71 @@ def _model_path(model) -> tuple[list[float], list[float], list[float]] | None:
|
||||
return distance, y, unwrapped_heading
|
||||
|
||||
|
||||
def _encode_path(model_offset: float, model_angle: float, desired_curvature: float, maneuver: bool) -> FordPath:
|
||||
action_curvature = _finite(desired_curvature)
|
||||
def _encode_path(path: tuple[list[float], list[float], list[float]], desired_curvature: float,
|
||||
current_curvature: float, v_ego: float) -> FordPath:
|
||||
distance, offset, heading = path
|
||||
offset_horizon = min(_PATH_MIN_LOOKAHEAD, distance[-1])
|
||||
angle_horizon = min(max(v_ego, _PATH_MIN_LOOKAHEAD), distance[-1])
|
||||
model_offset = _sample(offset_horizon, distance, offset)
|
||||
model_angle = _sample(angle_horizon, distance, heading)
|
||||
|
||||
offset_curvature = 2.0 * model_offset / offset_horizon ** 2
|
||||
angle_curvature = model_angle / angle_horizon
|
||||
pose_share = _blend_share(max(abs(offset_curvature), abs(angle_curvature), abs(desired_curvature)))
|
||||
|
||||
tracking_error = desired_curvature - current_curvature
|
||||
if tracking_error * desired_curvature > 0.0:
|
||||
tracking_error = math.copysign(max(abs(tracking_error) - _TRACKING_ERROR_DEADZONE, 0.0), tracking_error)
|
||||
tracking_error = float(np.clip(tracking_error, -_TRACKING_ERROR_LIMIT, _TRACKING_ERROR_LIMIT))
|
||||
else:
|
||||
tracking_error = 0.0
|
||||
|
||||
# C2 owns normal path following. As model pose demand grows, transfer the
|
||||
# same path continuously to the faster C0/C1 fields. Measured shortfall is
|
||||
# expressed in those same pose units and cannot initiate the transfer.
|
||||
path_offset = pose_share * (model_offset + 0.5 * tracking_error * offset_horizon ** 2)
|
||||
path_angle = pose_share * (model_angle + tracking_error * angle_horizon)
|
||||
curvature = desired_curvature * (1.0 - pose_share)
|
||||
return FordPath(
|
||||
valid=True,
|
||||
path_offset=float(np.clip(model_offset, *DBC_OFFSET)) if maneuver else 0.0,
|
||||
path_angle=float(np.clip(model_angle, *DBC_ANGLE)) if maneuver else 0.0,
|
||||
curvature=0.0 if maneuver else float(np.clip(action_curvature, *DBC_CURVATURE)),
|
||||
path_offset=float(np.clip(path_offset, *DBC_OFFSET)),
|
||||
path_angle=float(np.clip(path_angle, *DBC_ANGLE)),
|
||||
curvature=float(np.clip(curvature, *DBC_CURVATURE)),
|
||||
curvature_rate=0.0,
|
||||
)
|
||||
|
||||
|
||||
class FordPathController:
|
||||
"""Convert the model path directly into one vehicle-independent Ford path command."""
|
||||
"""Blend normal C2 following into the model's forward C0/C1 pose."""
|
||||
|
||||
def __init__(self, dt: float = 0.01):
|
||||
self.dt = dt
|
||||
self._last_path = FordPath(valid=True)
|
||||
self._maneuver = False
|
||||
|
||||
def _limit(self, target: FordPath) -> FordPath:
|
||||
path_offset = float(np.clip(target.path_offset,
|
||||
self._last_path.path_offset - _PATH_OFFSET_RATE * self.dt,
|
||||
self._last_path.path_offset + _PATH_OFFSET_RATE * self.dt))
|
||||
path_angle = float(np.clip(target.path_angle,
|
||||
self._last_path.path_angle - _PATH_ANGLE_RATE * self.dt,
|
||||
self._last_path.path_angle + _PATH_ANGLE_RATE * self.dt))
|
||||
# Never stack the normal C2 path on a maneuver pose while C0/C1 are slewing out.
|
||||
curvature = target.curvature if path_offset == 0.0 and path_angle == 0.0 else 0.0
|
||||
self._last_path = FordPath(True, path_offset, path_angle, curvature, 0.0)
|
||||
offset_delta = target.path_offset - self._last_path.path_offset
|
||||
angle_delta = target.path_angle - self._last_path.path_angle
|
||||
scale = min(
|
||||
1.0,
|
||||
_PATH_OFFSET_RATE * self.dt / abs(offset_delta) if offset_delta else 1.0,
|
||||
_PATH_ANGLE_RATE * self.dt / abs(angle_delta) if angle_delta else 1.0,
|
||||
)
|
||||
self._last_path = FordPath(
|
||||
True,
|
||||
self._last_path.path_offset + scale * offset_delta,
|
||||
self._last_path.path_angle + scale * angle_delta,
|
||||
self._last_path.curvature + scale * (target.curvature - self._last_path.curvature),
|
||||
0.0,
|
||||
)
|
||||
return self._last_path
|
||||
|
||||
def update(self, model, desired_curvature: float, *, v_ego: float = 0.0, active: bool = True) -> FordPath:
|
||||
def update(self, model, desired_curvature: float, *, current_curvature: float = 0.0,
|
||||
v_ego: float = 0.0, active: bool = True) -> FordPath:
|
||||
if not active:
|
||||
self._last_path = FordPath(valid=True)
|
||||
self._maneuver = False
|
||||
return FordPath()
|
||||
path = _model_path(model) if model is not None else None
|
||||
if path is None:
|
||||
self._maneuver = False
|
||||
return self._limit(FordPath(valid=True))
|
||||
|
||||
distance, offset, heading = path
|
||||
lookahead = min(max(_finite(v_ego) * _PATH_LOOKAHEAD_TIME, _PATH_MIN_LOOKAHEAD), distance[-1])
|
||||
model_offset = _sample(lookahead, distance, offset)
|
||||
model_angle = _sample(lookahead, distance, heading)
|
||||
demand = max(abs(_finite(desired_curvature)), abs(model_angle / lookahead))
|
||||
if self._maneuver:
|
||||
self._maneuver = demand >= _MANEUVER_EXIT_CURVATURE
|
||||
else:
|
||||
self._maneuver = demand >= _MANEUVER_ENTER_CURVATURE
|
||||
|
||||
return self._limit(_encode_path(model_offset, model_angle, desired_curvature, self._maneuver))
|
||||
return self._limit(_encode_path(path, _finite(desired_curvature), _finite(current_curvature),
|
||||
max(_finite(v_ego), 0.0)))
|
||||
|
||||
@@ -25,35 +25,66 @@ def _path(curvature: float, speed: float = 8.0):
|
||||
)
|
||||
|
||||
|
||||
def _model_pose(model, lookahead: float) -> tuple[float, float]:
|
||||
x = np.asarray(model.position.x)
|
||||
y = np.asarray(model.position.y)
|
||||
heading = np.asarray(model.orientation.z)
|
||||
distance = np.concatenate(([0.0], np.cumsum(np.hypot(np.diff(x), np.diff(y)))))
|
||||
horizon = min(lookahead, distance[-1])
|
||||
return float(np.interp(horizon, distance, y)), float(np.interp(horizon, distance, heading))
|
||||
def _changing_path(start_curvature: float, end_curvature: float, speed: float = 8.0):
|
||||
t = np.linspace(0.0, 3.0, 61)
|
||||
distance = speed * t
|
||||
curvature = np.interp(distance, [distance[0], min(distance[-1], 7.0)], [start_curvature, end_curvature])
|
||||
heading = np.zeros_like(distance)
|
||||
x = np.zeros_like(distance)
|
||||
y = np.zeros_like(distance)
|
||||
for i in range(1, len(distance)):
|
||||
ds = distance[i] - distance[i - 1]
|
||||
heading[i] = heading[i - 1] + 0.5 * (curvature[i] + curvature[i - 1]) * ds
|
||||
average_heading = 0.5 * (heading[i] + heading[i - 1])
|
||||
x[i] = x[i - 1] + ds * math.cos(average_heading)
|
||||
y[i] = y[i - 1] + ds * math.sin(average_heading)
|
||||
return SimpleNamespace(
|
||||
position=SimpleNamespace(t=t.tolist(), x=x.tolist(), y=y.tolist()),
|
||||
orientation=SimpleNamespace(z=heading.tolist()),
|
||||
)
|
||||
|
||||
|
||||
def _command(model, desired_curvature: float, *, v_ego: float = 8.0):
|
||||
return FordPathController(dt=1.0).update(model, desired_curvature, v_ego=v_ego)
|
||||
def _command(model, desired_curvature: float, *, current_curvature: float = 0.0, v_ego: float = 8.0):
|
||||
return FordPathController(dt=1.0).update(model, desired_curvature, current_curvature=current_curvature, v_ego=v_ego)
|
||||
|
||||
|
||||
def _equivalent_curvature(command) -> float:
|
||||
return 2.0 * command.path_offset / 7.0 ** 2 + 2.0 * command.path_angle / 7.0 + command.curvature
|
||||
|
||||
|
||||
def test_gentle_path_uses_only_c2():
|
||||
command = _command(_path(0.008, speed=20.0), 0.008, v_ego=20.0)
|
||||
command = _command(_path(0.004, speed=20.0), 0.004, v_ego=20.0)
|
||||
assert command.valid
|
||||
assert command.path_offset == 0.0
|
||||
assert command.path_angle == 0.0
|
||||
assert np.isclose(command.curvature, 0.008)
|
||||
assert np.isclose(command.curvature, 0.004)
|
||||
assert command.curvature_rate == 0.0
|
||||
|
||||
|
||||
def test_large_maneuver_uses_model_pose_and_immediately_zeros_c2():
|
||||
model = _path(0.04)
|
||||
expected_offset, expected_angle = _model_pose(model, 8.0)
|
||||
command = _command(model, 0.04)
|
||||
assert np.isclose(command.path_offset, expected_offset)
|
||||
assert np.isclose(command.path_angle, expected_angle)
|
||||
def test_spatially_growing_path_adds_fast_pose_before_action_becomes_large():
|
||||
controller = FordPathController(dt=1.0)
|
||||
command = controller.update(_changing_path(0.0, 0.04), 0.012, current_curvature=0.0, v_ego=8.0)
|
||||
assert command.path_offset > 0.0
|
||||
assert command.path_angle > 0.0
|
||||
assert command.curvature < 0.012
|
||||
assert command.curvature_rate == 0.0
|
||||
|
||||
|
||||
def test_growing_model_pose_adds_authority_but_c3_is_never_transmitted():
|
||||
constant = _command(_path(0.012), 0.012)
|
||||
growing = _command(_changing_path(0.0, 0.04), 0.012)
|
||||
assert growing.path_offset > constant.path_offset
|
||||
assert growing.path_angle > constant.path_angle
|
||||
assert constant.curvature_rate == 0.0
|
||||
assert growing.curvature_rate == 0.0
|
||||
|
||||
|
||||
def test_large_maneuver_uses_fast_pose_and_zeros_c2():
|
||||
command = _command(_path(0.04), 0.04)
|
||||
assert command.path_offset > 0.5
|
||||
assert command.path_angle > 0.2
|
||||
assert command.curvature == 0.0
|
||||
assert command.curvature_rate == 0.0
|
||||
|
||||
|
||||
def test_model_pose_can_trigger_maneuver_when_action_is_late():
|
||||
@@ -70,78 +101,88 @@ def test_action_can_trigger_maneuver_before_model_pose_grows():
|
||||
assert command.curvature == 0.0
|
||||
|
||||
|
||||
def test_large_offset_does_not_trigger_maneuver_without_heading_or_action_demand():
|
||||
model = _path(0.008)
|
||||
model.position.y = (np.asarray(model.position.y) + np.linspace(0.0, 4.0, len(model.position.y))).tolist()
|
||||
command = _command(model, 0.008)
|
||||
assert command.path_offset == 0.0
|
||||
assert command.path_angle == 0.0
|
||||
assert np.isclose(command.curvature, 0.008)
|
||||
def test_nearby_demands_blend_continuously_without_a_mode_threshold():
|
||||
low = _command(_path(0.0119), 0.0119)
|
||||
high = _command(_path(0.0121), 0.0121)
|
||||
assert abs(high.path_offset - low.path_offset) < 0.05
|
||||
assert abs(high.path_angle - low.path_angle) < 0.03
|
||||
assert abs(high.curvature - low.curvature) < 0.001
|
||||
|
||||
|
||||
def test_maneuver_hysteresis_prevents_mode_chatter():
|
||||
controller = FordPathController(dt=1.0)
|
||||
entry = controller.update(_path(0.02), 0.02, v_ego=8.0)
|
||||
held = controller.update(_path(0.015), 0.015, v_ego=8.0)
|
||||
exited = controller.update(_path(0.01), 0.01, v_ego=8.0)
|
||||
assert entry.path_angle != 0.0 and entry.curvature == 0.0
|
||||
assert held.path_angle != 0.0 and held.curvature == 0.0
|
||||
assert exited.path_offset == 0.0 and exited.path_angle == 0.0
|
||||
assert np.isclose(exited.curvature, 0.01)
|
||||
def test_leaving_c2_normal_band_does_not_drop_total_authority():
|
||||
normal = _command(_path(0.006), 0.006)
|
||||
transition = _command(_path(0.0061), 0.0061)
|
||||
assert transition.curvature <= normal.curvature
|
||||
assert _equivalent_curvature(transition) >= _equivalent_curvature(normal)
|
||||
|
||||
|
||||
def test_low_speed_uses_seven_meter_lookahead_for_both_pose_fields():
|
||||
model = _path(0.04, speed=2.0)
|
||||
expected_offset, expected_angle = _model_pose(model, 7.0)
|
||||
command = _command(model, 0.04, v_ego=2.0)
|
||||
assert np.isclose(command.path_offset, expected_offset)
|
||||
assert np.isclose(command.path_angle, expected_angle)
|
||||
def test_low_speed_still_uses_available_model_pose():
|
||||
command = _command(_path(0.04, speed=2.0), 0.04, v_ego=2.0)
|
||||
assert command.path_offset > 0.0
|
||||
assert command.path_angle > 0.0
|
||||
|
||||
|
||||
def test_speed_uses_one_second_lookahead_for_both_pose_fields():
|
||||
model = _path(0.02, speed=15.0)
|
||||
expected_offset, expected_angle = _model_pose(model, 15.0)
|
||||
command = _command(model, 0.02, v_ego=15.0)
|
||||
assert np.isclose(command.path_offset, expected_offset)
|
||||
assert np.isclose(command.path_angle, expected_angle)
|
||||
def test_higher_speed_extends_heading_horizon_without_moving_offset_horizon():
|
||||
model = _changing_path(0.0, 0.04, speed=20.0)
|
||||
slow = _command(model, 0.012, v_ego=7.0)
|
||||
fast = _command(model, 0.012, v_ego=20.0)
|
||||
assert np.isclose(fast.path_offset, slow.path_offset)
|
||||
assert fast.path_angle > slow.path_angle
|
||||
|
||||
|
||||
def test_short_model_uses_available_endpoint():
|
||||
model = _path(0.04, speed=1.0)
|
||||
expected_offset, expected_angle = _model_pose(model, math.inf)
|
||||
command = _command(model, 0.04, v_ego=1.0)
|
||||
assert np.isclose(command.path_offset, expected_offset)
|
||||
assert np.isclose(command.path_angle, expected_angle)
|
||||
assert command.valid
|
||||
assert command.path_offset > 0.0
|
||||
assert command.path_angle > 0.0
|
||||
|
||||
|
||||
def test_turn_entry_does_not_retain_previous_gentle_c2():
|
||||
controller = FordPathController()
|
||||
for _ in range(20):
|
||||
assert controller.update(_path(0.004), 0.004, v_ego=8.0).curvature > 0.0
|
||||
turning = controller.update(_path(0.04), 0.04, v_ego=8.0)
|
||||
assert turning.curvature == 0.0
|
||||
|
||||
|
||||
def test_turn_exit_never_stacks_pose_and_c2():
|
||||
def test_turn_entry_coordinates_c2_release_with_fast_pose_attack():
|
||||
controller = FordPathController(dt=0.01)
|
||||
for _ in range(20):
|
||||
controller.update(_path(0.04), 0.04, v_ego=8.0)
|
||||
outputs = [controller.update(_path(0.008), 0.008, v_ego=8.0) for _ in range(100)]
|
||||
assert all(command.curvature == 0.0 or (command.path_offset == 0.0 and command.path_angle == 0.0) for command in outputs)
|
||||
assert np.isclose(outputs[-1].curvature, 0.008)
|
||||
assert controller.update(_path(0.004), 0.004, v_ego=8.0).curvature > 0.0
|
||||
outputs = [controller.update(_path(0.04), 0.04, current_curvature=0.01, v_ego=8.0) for _ in range(100)]
|
||||
assert 0.0 < outputs[0].curvature < 0.004
|
||||
assert outputs[0].path_offset > 0.0
|
||||
assert outputs[0].path_angle > 0.0
|
||||
assert outputs[-1].curvature == 0.0
|
||||
|
||||
|
||||
def test_inactive_and_invalid_model_reset_maneuver_latch():
|
||||
controller = FordPathController(dt=1.0)
|
||||
controller.update(_path(0.02), 0.02, v_ego=8.0)
|
||||
controller.update(None, 0.0, v_ego=8.0)
|
||||
after_invalid = controller.update(_path(0.015), 0.015, v_ego=8.0)
|
||||
assert after_invalid.path_angle == 0.0 and np.isclose(after_invalid.curvature, 0.015)
|
||||
def test_turn_exit_allows_c2_to_take_over_while_fast_pose_drains():
|
||||
controller = FordPathController(dt=0.01)
|
||||
for _ in range(20):
|
||||
controller.update(_path(0.04), 0.04, current_curvature=0.02, v_ego=8.0)
|
||||
outputs = [controller.update(_path(0.004), 0.004, current_curvature=0.004, v_ego=8.0) for _ in range(100)]
|
||||
assert 0.0 < outputs[0].curvature < 0.004
|
||||
assert outputs[0].path_offset != 0.0 or outputs[0].path_angle != 0.0
|
||||
assert outputs[-1].path_offset == 0.0
|
||||
assert outputs[-1].path_angle == 0.0
|
||||
|
||||
controller.update(_path(0.02), 0.02, v_ego=8.0)
|
||||
controller.update(_path(0.0), 0.0, v_ego=8.0, active=False)
|
||||
after_inactive = controller.update(_path(0.015), 0.015, v_ego=8.0)
|
||||
assert after_inactive.path_angle == 0.0 and np.isclose(after_inactive.curvature, 0.015)
|
||||
|
||||
def test_100hz_handoff_preserves_total_authority_without_entry_drop_or_exit_overshoot():
|
||||
controller = FordPathController(dt=0.01)
|
||||
normal = controller.update(_path(0.006), 0.006, current_curvature=0.006, v_ego=8.0)
|
||||
entries = [controller.update(_path(0.04), 0.04, current_curvature=0.01, v_ego=8.0) for _ in range(100)]
|
||||
entry_authority = np.asarray([_equivalent_curvature(command) for command in entries])
|
||||
assert np.all(np.diff(entry_authority) >= -1e-9)
|
||||
assert entry_authority[0] >= _equivalent_curvature(normal)
|
||||
|
||||
exits = [controller.update(_path(0.004), 0.004, current_curvature=0.004, v_ego=8.0) for _ in range(100)]
|
||||
exit_authority = np.asarray([_equivalent_curvature(command) for command in exits])
|
||||
assert np.all(np.diff(exit_authority) <= 1e-9)
|
||||
assert np.all(exit_authority >= 0.004 - 1e-9)
|
||||
|
||||
|
||||
def test_measured_undertracking_adds_fast_authority_without_overshoot_countersteer():
|
||||
model = _path(0.04)
|
||||
under = _command(model, 0.04, current_curvature=0.005)
|
||||
on_target = _command(model, 0.04, current_curvature=0.04)
|
||||
over = _command(model, 0.04, current_curvature=0.05)
|
||||
assert under.path_offset > on_target.path_offset
|
||||
assert under.path_angle > on_target.path_angle
|
||||
assert over.path_offset == on_target.path_offset
|
||||
assert over.path_angle == on_target.path_angle
|
||||
|
||||
|
||||
def test_s_turn_reverses_model_pose_without_slow_c2():
|
||||
|
||||
Reference in New Issue
Block a user