mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-09-11 04:43:43 +08:00
tools: evaluate native Ford path polynomial
Assisted-by: Codex
This commit is contained in:
Isaac Barham
@@ -0,0 +1,498 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Offline evaluation of Ford's native four-field path polynomial.
|
||||
|
||||
The experiment deliberately does not alter the live controller. It rebases the
|
||||
model path into the vehicle pose expected at actuation time, fits one cubic over
|
||||
the remaining short path, and converts the cubic into the LMC2 C0/C1/C2/C3
|
||||
signals. A first-order C2 response envelope is included to expose commands that
|
||||
would look good only if the PSCM curvature channel were instantaneous.
|
||||
"""
|
||||
|
||||
import argparse
|
||||
from collections import defaultdict
|
||||
from dataclasses import dataclass
|
||||
import glob
|
||||
import math
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.tools.lib.logreader import LogReader
|
||||
|
||||
|
||||
DBC_OFFSET = (-5.12, 5.11)
|
||||
DBC_ANGLE = (-0.5, 0.5235)
|
||||
DBC_CURVATURE = (-0.02, 0.02)
|
||||
DBC_CURVATURE_RATE = (-0.001024, 0.001023)
|
||||
MAX_LATERAL_ACCEL = 3.0 + 9.81 * 0.06
|
||||
MAX_LATERAL_JERK = 3.0 + 9.81 * 0.06
|
||||
@dataclass(frozen=True)
|
||||
class ModelPath:
|
||||
x: np.ndarray
|
||||
y: np.ndarray
|
||||
heading: np.ndarray
|
||||
distance: np.ndarray
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Sample:
|
||||
route: str
|
||||
time: float
|
||||
speed: float
|
||||
curvature: float
|
||||
steering_pressed: bool
|
||||
path: ModelPath
|
||||
sent_c0: float
|
||||
sent_c1: float
|
||||
sent_c2: float
|
||||
sent_c3: float
|
||||
@dataclass(frozen=True)
|
||||
class NativePath:
|
||||
c0: float
|
||||
c1: float
|
||||
c2: float
|
||||
c3: float
|
||||
fit_rmse: float
|
||||
path_rms: float
|
||||
|
||||
|
||||
def _model_path(model) -> ModelPath | None:
|
||||
try:
|
||||
x = np.asarray(model.position.x, dtype=float)
|
||||
y = np.asarray(model.position.y, dtype=float)
|
||||
heading = np.unwrap(np.asarray(model.orientation.z, dtype=float))
|
||||
except (AttributeError, TypeError, ValueError):
|
||||
return None
|
||||
if len(x) < 4 or len(x) != len(y) or len(x) != len(heading):
|
||||
return None
|
||||
if not np.isfinite(np.concatenate((x, y, heading))).all():
|
||||
return None
|
||||
distance = np.concatenate(([0.0], np.cumsum(np.hypot(np.diff(x), np.diff(y)))))
|
||||
unique_distance, unique = np.unique(distance, return_index=True)
|
||||
if len(unique_distance) < 4 or unique_distance[-1] <= 0.0:
|
||||
return None
|
||||
return ModelPath(x[unique], y[unique], heading[unique], unique_distance)
|
||||
|
||||
|
||||
def _arc_pose(distance: float, curvature: float) -> tuple[float, float, float]:
|
||||
heading = curvature * distance
|
||||
if abs(curvature) < 1e-9:
|
||||
return distance, 0.0, 0.0
|
||||
return math.sin(heading) / curvature, (1.0 - math.cos(heading)) / curvature, heading
|
||||
|
||||
|
||||
def _relative_points(path: ModelPath, vehicle_pose: tuple[float, float, float], start: float,
|
||||
horizon: float, count: int = 25) -> tuple[np.ndarray, np.ndarray]:
|
||||
sample_distance = np.linspace(start, min(start + horizon, path.distance[-1]), count)
|
||||
desired_x = np.interp(sample_distance, path.distance, path.x)
|
||||
desired_y = np.interp(sample_distance, path.distance, path.y)
|
||||
vehicle_x, vehicle_y, vehicle_heading = vehicle_pose
|
||||
dx = desired_x - vehicle_x
|
||||
dy = desired_y - vehicle_y
|
||||
cosine = math.cos(vehicle_heading)
|
||||
sine = math.sin(vehicle_heading)
|
||||
return cosine * dx + sine * dy, -sine * dx + cosine * dy
|
||||
|
||||
|
||||
def _fit_points(path: ModelPath, speed: float, current_curvature: float, delay: float,
|
||||
horizon: float) -> tuple[np.ndarray, np.ndarray] | None:
|
||||
advance = min(max(speed, 0.0) * delay, path.distance[-1])
|
||||
available = min(horizon, path.distance[-1] - advance)
|
||||
if available <= 0.25:
|
||||
return None
|
||||
x, y = _relative_points(path, _arc_pose(advance, current_curvature), advance, available)
|
||||
forward = (x >= -0.25) & (x <= horizon)
|
||||
x = x[forward]
|
||||
y = y[forward]
|
||||
if len(x) < 4 or np.ptp(x) <= 0.25:
|
||||
return None
|
||||
return x, y
|
||||
|
||||
|
||||
def _wire_coefficients(c0: float, c1: float, c2: float, c3: float) -> tuple[float, float, float, float]:
|
||||
slope = math.tan(c1)
|
||||
slope_norm = 1.0 + slope ** 2
|
||||
a2 = 0.5 * c2 * slope_norm ** 1.5
|
||||
a3 = (c3 + 12.0 * slope * a2 ** 2 / slope_norm ** 3) * slope_norm ** 2 / 6.0
|
||||
return c0, slope, a2, a3
|
||||
|
||||
|
||||
def _wire_rmse(command: tuple[float, float, float, float], x: np.ndarray, y: np.ndarray) -> float:
|
||||
a0, a1, a2, a3 = _wire_coefficients(*command)
|
||||
reconstructed = a0 + a1 * x + a2 * x ** 2 + a3 * x ** 3
|
||||
return float(np.sqrt(np.mean((reconstructed - y) ** 2)))
|
||||
|
||||
|
||||
def fit_native_path(path: ModelPath, speed: float, current_curvature: float, *, delay: float,
|
||||
horizon: float) -> NativePath:
|
||||
"""Fit the delay-aligned path and return physical LMC2 fields.
|
||||
|
||||
C2 and C3 are curvature and curvature rate at the vehicle-frame origin, not
|
||||
the raw quadratic and cubic polynomial coefficients.
|
||||
"""
|
||||
points = _fit_points(path, speed, current_curvature, delay, horizon)
|
||||
if points is None:
|
||||
return NativePath(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)
|
||||
x, y = points
|
||||
|
||||
# Scaling x before the least-squares solve keeps tight-turn fits well
|
||||
# conditioned while preserving an ordinary cubic in vehicle coordinates.
|
||||
scale = max(float(np.max(np.abs(x))), 1.0)
|
||||
normalized_x = x / scale
|
||||
design = np.column_stack((np.ones(len(x)), normalized_x, normalized_x ** 2, normalized_x ** 3))
|
||||
scaled, *_ = np.linalg.lstsq(design, y, rcond=None)
|
||||
a0, a1, a2, a3 = (float(scaled[index] / scale ** index) for index in range(4))
|
||||
slope = a1
|
||||
slope_norm = 1.0 + slope ** 2
|
||||
curvature = 2.0 * a2 / slope_norm ** 1.5
|
||||
curvature_rate = 6.0 * a3 / slope_norm ** 2 - 12.0 * slope * a2 ** 2 / slope_norm ** 3
|
||||
command = (float(np.clip(a0, *DBC_OFFSET)),
|
||||
float(np.clip(math.atan(slope), *DBC_ANGLE)),
|
||||
float(np.clip(curvature, *DBC_CURVATURE)),
|
||||
float(np.clip(curvature_rate, *DBC_CURVATURE_RATE)))
|
||||
return NativePath(
|
||||
*command,
|
||||
_wire_rmse(command, x, y),
|
||||
float(np.sqrt(np.mean(y ** 2))),
|
||||
)
|
||||
|
||||
|
||||
def fit_c2_aware_path(path: ModelPath, speed: float, current_curvature: float, *, delay: float,
|
||||
horizon: float, target_c2: float, effective_c2: float,
|
||||
use_c3: bool = True) -> NativePath:
|
||||
"""Fit fast fields around the C2 curvature the PSCM is expected to realize."""
|
||||
points = _fit_points(path, speed, current_curvature, delay, horizon)
|
||||
if points is None:
|
||||
return NativePath(0.0, 0.0, target_c2, 0.0, 0.0, 0.0)
|
||||
x, y = points
|
||||
|
||||
slope = 0.0
|
||||
a0 = a1 = a3 = 0.0
|
||||
for _ in range(3):
|
||||
a2 = 0.5 * effective_c2 * (1.0 + slope ** 2) ** 1.5
|
||||
design = np.column_stack((np.ones(len(x)), x, x ** 3))
|
||||
(a0, a1, a3), *_ = np.linalg.lstsq(design, y - a2 * x ** 2, rcond=None)
|
||||
slope = float(a1)
|
||||
|
||||
slope_norm = 1.0 + slope ** 2
|
||||
c3 = 6.0 * float(a3) / slope_norm ** 2 - 12.0 * slope * a2 ** 2 / slope_norm ** 3
|
||||
c3 = float(np.clip(c3, *DBC_CURVATURE_RATE)) if use_c3 else 0.0
|
||||
# Once C2 and C3 are fixed to what the hardware can realize, refit C0/C1 so
|
||||
# their fast feedback preserves as much of the same path as possible.
|
||||
_, _, fixed_a2, fixed_a3 = _wire_coefficients(0.0, math.atan(slope), effective_c2, c3)
|
||||
(a0, a1), *_ = np.linalg.lstsq(np.column_stack((np.ones(len(x)), x)),
|
||||
y - fixed_a2 * x ** 2 - fixed_a3 * x ** 3, rcond=None)
|
||||
c0 = float(np.clip(a0, *DBC_OFFSET))
|
||||
c1 = float(np.clip(math.atan(float(a1)), *DBC_ANGLE))
|
||||
effective_command = (c0, c1, effective_c2, c3)
|
||||
return NativePath(c0, c1, target_c2, c3, _wire_rmse(effective_command, x, y),
|
||||
float(np.sqrt(np.mean(y ** 2))))
|
||||
|
||||
|
||||
def _route(path: str) -> str:
|
||||
return Path(path).name.split("--", 1)[0]
|
||||
|
||||
|
||||
def load_samples(paths: list[str], stride: int = 2) -> list[Sample]:
|
||||
grouped: dict[str, list[str]] = defaultdict(list)
|
||||
for path in paths:
|
||||
grouped[_route(path)].append(path)
|
||||
|
||||
samples = []
|
||||
for route, route_paths in sorted(grouped.items()):
|
||||
events = []
|
||||
for path in sorted(route_paths):
|
||||
events.extend(LogReader(path))
|
||||
events.sort(key=lambda event: event.logMonoTime)
|
||||
if not events:
|
||||
continue
|
||||
start_time = events[0].logMonoTime
|
||||
model_path = None
|
||||
curvature = 0.0
|
||||
lat_active = path_valid = False
|
||||
sent = (0.0, 0.0, 0.0, 0.0)
|
||||
car_state_count = 0
|
||||
for event in events:
|
||||
which = event.which()
|
||||
if which == "modelV2":
|
||||
model_path = _model_path(event.modelV2)
|
||||
elif which == "controlsState":
|
||||
curvature = float(event.controlsState.curvature)
|
||||
elif which == "carControl":
|
||||
lat_active = bool(event.carControl.latActive)
|
||||
elif which == "carControlSP":
|
||||
command = event.carControlSP.fordLateralPath
|
||||
path_valid = bool(command.valid)
|
||||
sent = (float(command.pathOffset), float(command.pathAngle),
|
||||
float(command.curvature), float(command.curvatureRate))
|
||||
elif which == "carState" and lat_active and path_valid and model_path is not None:
|
||||
car_state_count += 1
|
||||
if car_state_count % stride:
|
||||
continue
|
||||
samples.append(Sample(
|
||||
route, (event.logMonoTime - start_time) * 1e-9, float(event.carState.vEgo), curvature,
|
||||
bool(event.carState.steeringPressed), model_path, *sent,
|
||||
))
|
||||
return samples
|
||||
|
||||
|
||||
def _percentile(values: np.ndarray, percentile: float, mask: np.ndarray | None = None) -> float:
|
||||
selected = values if mask is None else values[mask]
|
||||
return float(np.percentile(np.abs(selected), percentile)) if len(selected) else math.nan
|
||||
|
||||
|
||||
def _route_rate(samples: list[Sample], values: np.ndarray) -> np.ndarray:
|
||||
rate = np.zeros(len(values))
|
||||
for index in range(1, len(values)):
|
||||
dt = samples[index].time - samples[index - 1].time
|
||||
if samples[index].route == samples[index - 1].route and 0.005 <= dt <= 0.2:
|
||||
rate[index] = (values[index] - values[index - 1]) / dt
|
||||
return rate
|
||||
|
||||
|
||||
def _c2_response(samples: list[Sample], target: np.ndarray, tau_load: float,
|
||||
tau_unload: float) -> np.ndarray:
|
||||
effective = np.zeros(len(target))
|
||||
previous_route = None
|
||||
previous_time = 0.0
|
||||
state = 0.0
|
||||
for index, sample in enumerate(samples):
|
||||
if sample.route != previous_route:
|
||||
state = 0.0
|
||||
previous_time = sample.time
|
||||
dt = float(np.clip(sample.time - previous_time, 0.005, 0.2))
|
||||
loading = target[index] * state >= 0.0 and abs(target[index]) > abs(state)
|
||||
tau = tau_load if loading else tau_unload
|
||||
state += (1.0 - math.exp(-dt / tau)) * (target[index] - state)
|
||||
effective[index] = state
|
||||
previous_route, previous_time = sample.route, sample.time
|
||||
return effective
|
||||
|
||||
|
||||
def _limit_c2_command(samples: list[Sample], target: np.ndarray) -> np.ndarray:
|
||||
"""Mirror the CAN-FD Ford curvature acceleration/jerk limiter."""
|
||||
limited = np.zeros(len(target))
|
||||
previous_route = None
|
||||
previous_time = 0.0
|
||||
previous = 0.0
|
||||
for index, sample in enumerate(samples):
|
||||
if sample.route != previous_route:
|
||||
previous = 0.0
|
||||
previous_time = sample.time
|
||||
dt = float(np.clip(sample.time - previous_time, 0.005, 0.2))
|
||||
speed = max(sample.speed, 1.0)
|
||||
value = float(np.clip(target[index], -MAX_LATERAL_ACCEL / speed ** 2,
|
||||
MAX_LATERAL_ACCEL / speed ** 2))
|
||||
step = MAX_LATERAL_JERK / speed ** 2 * dt
|
||||
value = float(np.clip(value, previous - step, previous + step))
|
||||
limited[index] = float(np.clip(value, *DBC_CURVATURE))
|
||||
previous = limited[index]
|
||||
previous_route, previous_time = sample.route, sample.time
|
||||
return limited
|
||||
|
||||
|
||||
def _limit_fast_fields(samples: list[Sample], c0_target: np.ndarray,
|
||||
c1_target: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||||
c0 = np.zeros(len(samples))
|
||||
c1 = np.zeros(len(samples))
|
||||
previous_route = None
|
||||
previous_time = 0.0
|
||||
previous_c0 = previous_c1 = 0.0
|
||||
for index, sample in enumerate(samples):
|
||||
if sample.route != previous_route:
|
||||
previous_c0 = previous_c1 = 0.0
|
||||
previous_time = sample.time
|
||||
dt = float(np.clip(sample.time - previous_time, 0.005, 0.2))
|
||||
c0[index] = np.clip(c0_target[index], previous_c0 - 4.0 * dt, previous_c0 + 4.0 * dt)
|
||||
c1[index] = np.clip(c1_target[index], previous_c1 - 1.0 * dt, previous_c1 + 1.0 * dt)
|
||||
previous_c0, previous_c1 = c0[index], c1[index]
|
||||
previous_route, previous_time = sample.route, sample.time
|
||||
return c0, c1
|
||||
|
||||
|
||||
def evaluate(samples: list[Sample], *, delay: float, horizon: float,
|
||||
tau_load: float, tau_unload: float, horizon_time: float = 0.0,
|
||||
assumed_tau_load: float | None = None, assumed_tau_unload: float | None = None,
|
||||
use_c3: bool = True, c2_limit: float = DBC_CURVATURE[1]) -> dict[str, float]:
|
||||
horizons = np.asarray([float(np.clip(sample.speed * horizon_time, 1.0, horizon))
|
||||
if horizon_time > 0.0 else horizon for sample in samples])
|
||||
commands = [fit_native_path(sample.path, sample.speed, sample.curvature,
|
||||
delay=delay, horizon=sample_horizon)
|
||||
for sample, sample_horizon in zip(samples, horizons, strict=True)]
|
||||
c0 = np.asarray([command.c0 for command in commands])
|
||||
c1 = np.asarray([command.c1 for command in commands])
|
||||
raw_c2 = np.asarray([command.c2 for command in commands])
|
||||
c2 = np.clip(raw_c2, -c2_limit, c2_limit)
|
||||
c3 = np.asarray([command.c3 for command in commands])
|
||||
fit_rmse = np.asarray([command.fit_rmse for command in commands])
|
||||
path_rms = np.asarray([command.path_rms for command in commands])
|
||||
transmitted_c2 = _limit_c2_command(samples, c2)
|
||||
effective_c2 = _c2_response(samples, transmitted_c2, tau_load, tau_unload)
|
||||
estimated_c2 = _c2_response(samples, transmitted_c2,
|
||||
tau_load if assumed_tau_load is None else assumed_tau_load,
|
||||
tau_unload if assumed_tau_unload is None else assumed_tau_unload)
|
||||
compensated = [fit_c2_aware_path(sample.path, sample.speed, sample.curvature,
|
||||
delay=delay, horizon=sample_horizon, target_c2=target,
|
||||
effective_c2=estimated, use_c3=use_c3)
|
||||
for sample, sample_horizon, target, estimated in
|
||||
zip(samples, horizons, c2, estimated_c2, strict=True)]
|
||||
compensated_c0 = np.asarray([command.c0 for command in compensated])
|
||||
compensated_c1 = np.asarray([command.c1 for command in compensated])
|
||||
compensated_c3 = np.asarray([command.c3 for command in compensated])
|
||||
limited_c0, limited_c1 = _limit_fast_fields(samples, compensated_c0, compensated_c1)
|
||||
estimated_compensated_rmse = np.asarray([command.fit_rmse for command in compensated])
|
||||
compensated_rmse = []
|
||||
for sample, sample_horizon, command, effective in zip(samples, horizons, compensated, effective_c2, strict=True):
|
||||
points = _fit_points(sample.path, sample.speed, sample.curvature, delay, sample_horizon)
|
||||
compensated_rmse.append(0.0 if points is None else _wire_rmse(
|
||||
(command.c0, command.c1, effective, command.c3), *points))
|
||||
compensated_rmse = np.asarray(compensated_rmse)
|
||||
limited_compensated_rmse = []
|
||||
for sample, sample_horizon, c0_value, c1_value, c3_value, effective in \
|
||||
zip(samples, horizons, limited_c0, limited_c1, compensated_c3, effective_c2, strict=True):
|
||||
points = _fit_points(sample.path, sample.speed, sample.curvature, delay, sample_horizon)
|
||||
limited_compensated_rmse.append(0.0 if points is None else _wire_rmse(
|
||||
(c0_value, c1_value, effective, c3_value), *points))
|
||||
limited_compensated_rmse = np.asarray(limited_compensated_rmse)
|
||||
missing_c2 = transmitted_c2 - effective_c2
|
||||
# Compare channels by their lateral contribution at the fit horizon. This
|
||||
# includes C3: treating it as zero would incorrectly blame C0/C1 for a
|
||||
# curvature transition the native polynomial assigns to curvature rate.
|
||||
fast = (2.0 * compensated_c0 / horizons ** 2 +
|
||||
2.0 * np.tan(compensated_c1) / horizons +
|
||||
compensated_c3 * horizons / 3.0)
|
||||
lagging = np.abs(missing_c2) > 0.0005
|
||||
unloading = lagging & (np.abs(c2) < 0.75 * np.abs(effective_c2))
|
||||
pressed = np.asarray([sample.steering_pressed for sample in samples])
|
||||
speed = np.asarray([sample.speed for sample in samples])
|
||||
sent_c2 = np.asarray([sample.sent_c2 for sample in samples])
|
||||
sent_transmitted_c2 = _limit_c2_command(samples, sent_c2)
|
||||
sent_effective_c2 = _c2_response(samples, sent_transmitted_c2, tau_load, tau_unload)
|
||||
sent_lpf_rmse = []
|
||||
for sample, sample_horizon, effective in zip(samples, horizons, sent_effective_c2, strict=True):
|
||||
points = _fit_points(sample.path, sample.speed, sample.curvature, delay, sample_horizon)
|
||||
sent_lpf_rmse.append(0.0 if points is None else _wire_rmse(
|
||||
(sample.sent_c0, sample.sent_c1, effective, sample.sent_c3), *points))
|
||||
sent_lpf_rmse = np.asarray(sent_lpf_rmse)
|
||||
raw_c2_rate = _route_rate(samples, c2)
|
||||
c2_rate = _route_rate(samples, transmitted_c2)
|
||||
sent_c2_rate = _route_rate(samples, sent_c2)
|
||||
compensated_c0_rate = _route_rate(samples, compensated_c0)
|
||||
compensated_c1_rate = _route_rate(samples, compensated_c1)
|
||||
compensated_c3_rate = _route_rate(samples, compensated_c3)
|
||||
normalized_fit = np.divide(fit_rmse, path_rms, out=np.zeros_like(fit_rmse), where=path_rms > 1e-4)
|
||||
return {
|
||||
"samples": float(len(samples)),
|
||||
"delay": delay,
|
||||
"horizon": horizon,
|
||||
"horizon_time": horizon_time,
|
||||
"assumed_tau_load": tau_load if assumed_tau_load is None else assumed_tau_load,
|
||||
"assumed_tau_unload": tau_unload if assumed_tau_unload is None else assumed_tau_unload,
|
||||
"use_c3": float(use_c3),
|
||||
"c2_limit": c2_limit,
|
||||
"actual_horizon_p50": _percentile(horizons, 50),
|
||||
"actual_horizon_p95": _percentile(horizons, 95),
|
||||
"fit_rmse_p50": _percentile(fit_rmse, 50),
|
||||
"fit_rmse_p95": _percentile(fit_rmse, 95),
|
||||
"normalized_fit_p95": _percentile(normalized_fit, 95),
|
||||
"c2_aware_rmse_p50": _percentile(compensated_rmse, 50),
|
||||
"c2_aware_rmse_p95": _percentile(compensated_rmse, 95),
|
||||
"c2_aware_estimated_rmse_p95": _percentile(estimated_compensated_rmse, 95),
|
||||
"c2_aware_limited_rmse_p95": _percentile(limited_compensated_rmse, 95),
|
||||
"sent_lpf_rmse_p50": _percentile(sent_lpf_rmse, 50),
|
||||
"sent_lpf_rmse_p95": _percentile(sent_lpf_rmse, 95),
|
||||
"c0_p95": _percentile(c0, 95),
|
||||
"c1_p95": _percentile(c1, 95),
|
||||
"c2_p95": _percentile(c2, 95),
|
||||
"c3_p95": _percentile(c3, 95),
|
||||
"c0_clip_rate": float(np.mean((c0 <= DBC_OFFSET[0]) | (c0 >= DBC_OFFSET[1]))),
|
||||
"c1_clip_rate": float(np.mean((c1 <= DBC_ANGLE[0]) | (c1 >= DBC_ANGLE[1]))),
|
||||
"c2_clip_rate": float(np.mean((c2 <= DBC_CURVATURE[0]) | (c2 >= DBC_CURVATURE[1]))),
|
||||
"c3_clip_rate": float(np.mean((c3 <= DBC_CURVATURE_RATE[0]) | (c3 >= DBC_CURVATURE_RATE[1]))),
|
||||
"c2_aware_c0_p95": _percentile(compensated_c0, 95),
|
||||
"c2_aware_c1_p95": _percentile(compensated_c1, 95),
|
||||
"c2_aware_c3_p95": _percentile(compensated_c3, 95),
|
||||
"c2_aware_c0_rate_p95": _percentile(compensated_c0_rate, 95),
|
||||
"c2_aware_c1_rate_p95": _percentile(compensated_c1_rate, 95),
|
||||
"c2_aware_c0_rate_limit_rate": float(np.mean(np.abs(compensated_c0_rate) > 4.0)),
|
||||
"c2_aware_c1_rate_limit_rate": float(np.mean(np.abs(compensated_c1_rate) > 1.0)),
|
||||
"c2_aware_c3_rate_p95": _percentile(compensated_c3_rate, 95),
|
||||
"raw_c2_rate_p95": _percentile(raw_c2_rate, 95),
|
||||
"c2_rate_p95": _percentile(c2_rate, 95),
|
||||
"sent_c2_rate_p95": _percentile(sent_c2_rate, 95),
|
||||
"c2_lag_p95": _percentile(missing_c2, 95),
|
||||
"lag_samples": float(np.count_nonzero(lagging)),
|
||||
"lag_fast_support_rate": float(np.mean(fast[lagging] * missing_c2[lagging] > 0.0)) if np.any(lagging) else math.nan,
|
||||
"lag_fast_coverage_p50": _percentile(np.divide(fast, missing_c2, out=np.zeros_like(fast),
|
||||
where=np.abs(missing_c2) > 1e-6), 50, lagging),
|
||||
"unload_samples": float(np.count_nonzero(unloading)),
|
||||
"unload_fast_counter_rate": float(np.mean(fast[unloading] * effective_c2[unloading] < 0.0)) if np.any(unloading) else math.nan,
|
||||
"unload_residual_c2_p95": _percentile(effective_c2 - c2, 95, unloading),
|
||||
"pressed_c0_p95": _percentile(c0, 95, pressed),
|
||||
"pressed_c1_p95": _percentile(c1, 95, pressed),
|
||||
"low_speed_fit_p95": _percentile(fit_rmse, 95, speed < 5.0),
|
||||
"road_speed_fit_p95": _percentile(fit_rmse, 95, speed >= 15.0),
|
||||
}
|
||||
|
||||
|
||||
def _expand(patterns: list[str]) -> list[str]:
|
||||
return sorted({path for pattern in patterns for path in glob.glob(pattern)})
|
||||
|
||||
|
||||
def _self_test() -> None:
|
||||
distance = np.linspace(0.0, 20.0, 81)
|
||||
coefficients = (0.2, 0.03, 0.004, -0.00005)
|
||||
y = sum(coefficient * distance ** power for power, coefficient in enumerate(coefficients))
|
||||
slope = coefficients[1] + 2.0 * coefficients[2] * distance + 3.0 * coefficients[3] * distance ** 2
|
||||
heading = np.arctan(slope)
|
||||
path = ModelPath(distance, y, heading, np.concatenate(([0.0], np.cumsum(np.hypot(np.diff(distance), np.diff(y))))))
|
||||
command = fit_native_path(path, 0.0, 0.0, delay=0.1, horizon=7.0)
|
||||
assert abs(command.c0 - coefficients[0]) < 2e-3
|
||||
assert abs(command.c1 - math.atan(coefficients[1])) < 2e-3
|
||||
expected_c2 = 2.0 * coefficients[2] / (1.0 + coefficients[1] ** 2) ** 1.5
|
||||
assert abs(command.c2 - expected_c2) < 2e-4
|
||||
assert command.fit_rmse < 1e-4
|
||||
|
||||
|
||||
def main() -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--logs", action="append", help="rlog glob", default=[])
|
||||
parser.add_argument("--delay", type=float, default=0.1)
|
||||
parser.add_argument("--horizon", type=float, action="append")
|
||||
parser.add_argument("--time-horizon", type=float, default=0.0,
|
||||
help="if nonzero, use clamp(speed * seconds, 1 m, --horizon)")
|
||||
parser.add_argument("--tau-load", type=float, default=0.75)
|
||||
parser.add_argument("--tau-unload", type=float, default=1.3)
|
||||
parser.add_argument("--assumed-tau-load", type=float)
|
||||
parser.add_argument("--assumed-tau-unload", type=float)
|
||||
parser.add_argument("--zero-c3", action="store_true")
|
||||
parser.add_argument("--c2-limit", type=float, action="append",
|
||||
help="C2 cap to test; defaults to gentle 0.006 and full 0.02")
|
||||
parser.add_argument("--self-test", action="store_true")
|
||||
args = parser.parse_args()
|
||||
if args.self_test:
|
||||
_self_test()
|
||||
paths = _expand(args.logs)
|
||||
if not paths:
|
||||
if args.self_test:
|
||||
return 0
|
||||
parser.error("at least one usable --logs glob is required")
|
||||
samples = load_samples(paths)
|
||||
if not samples:
|
||||
parser.error("logs contain no active Ford path samples")
|
||||
print(f"loaded_logs={len(paths)} samples={len(samples)} tau_load={args.tau_load} tau_unload={args.tau_unload}")
|
||||
for horizon in args.horizon or [3.5, 5.0, 7.0, 10.0]:
|
||||
for c2_limit in args.c2_limit or [0.006, DBC_CURVATURE[1]]:
|
||||
result = evaluate(samples, delay=args.delay, horizon=horizon,
|
||||
tau_load=args.tau_load, tau_unload=args.tau_unload,
|
||||
horizon_time=args.time_horizon,
|
||||
assumed_tau_load=args.assumed_tau_load,
|
||||
assumed_tau_unload=args.assumed_tau_unload,
|
||||
use_c3=not args.zero_c3,
|
||||
c2_limit=c2_limit)
|
||||
print(" ".join(f"{key}={value:.8g}" for key, value in result.items()))
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
Reference in New Issue
Block a user