diff --git a/app/python_simulations.py b/app/python_simulations.py index 5767700b..521eec32 100644 --- a/app/python_simulations.py +++ b/app/python_simulations.py @@ -39,10 +39,14 @@ def run(workflow_id: str) -> None: return if workflow_id in {"lap-qss", "lap-transient"}: config = config_io.resolve("simulations/lap/lap_time_eval_config.yml", root=root) + data = load_yaml(config) + print(f"DynPy | {data['model_dof']} DOF | {workflow_id}", flush=True) + if data.get("qss", {}).get("linked_ggv"): + print("Using linked GGV. Solving the racing line and lap speed profile.", flush=True) summary = run_lap_time_evaluation( config, scenario="qss" if workflow_id == "lap-qss" else "transient" ) - data = load_yaml(config) + print("Rendering lap report.", flush=True) output = root / str(data["output"]["directory"]).format(model_dof=summary["model_dof"]) vehicle = load_yaml(root / data.get("vehicle", "vehicle.yml")) build_dynpy_report( diff --git a/simulations/lap/core/qss.py b/simulations/lap/core/qss.py index d8fc0536..62b42542 100644 --- a/simulations/lap/core/qss.py +++ b/simulations/lap/core/qss.py @@ -4,6 +4,7 @@ import csv from dataclasses import dataclass +from functools import cached_property from pathlib import Path from typing import Literal @@ -37,6 +38,29 @@ def __post_init__(self) -> None: if np.any(np.diff(speeds) <= 0.0): raise ValueError("GGV speed slices must be strictly increasing.") + @cached_property + def _speeds(self) -> FloatArray: + return np.asarray([item.speed_mps for item in self.slices]) + + @cached_property + def _branches(self) -> dict[float, tuple[tuple[FloatArray, FloatArray, FloatArray, FloatArray], ...]]: + branches = {} + for direction in (-1., 1.): + rows = [] + for original in self.slices: + item = _directional_slice(original, direction) + drive = np.isfinite(item.ay_mps2) & np.isfinite(item.ax_accel_mps2) + brake = np.isfinite(item.ay_mps2) & np.isfinite(item.ax_brake_mps2) + rows.append((item.ay_mps2[drive], item.ax_accel_mps2[drive], + item.ay_mps2[brake], item.ax_brake_mps2[brake])) + branches[direction] = tuple(rows) + return branches + + @cached_property + def _lateral_limits(self) -> dict[float, FloatArray]: + return {direction: np.asarray([_slice_lateral_limit(item, direction) for item in self.slices]) + for direction in (-1., 1.)} + @classmethod def from_csv(cls, path: str | Path) -> GGVMap: """Load the CSV written by ``GGV.save_ggv_csv``.""" @@ -124,23 +148,22 @@ def longitudinal_limit( if lateral > limit + 1e-9: return float("nan") fraction = lateral / limit if limit > 0.0 else 0.0 - # Match lateral utilization so unequal slice endpoints remain connected. - values = np.asarray( - [ - _slice_longitudinal_limit(item, fraction * _slice_lateral_limit(item, direction), mode, direction) - for item in self.slices - ], - dtype=float, - ) - speeds = np.asarray([item.speed_mps for item in self.slices], dtype=float) - return float(np.interp(speed_mps, speeds, values)) + # Only the two neighboring speed slices contribute to interpolation. + upper = min(int(np.searchsorted(self._speeds, speed_mps, side="right")), len(self.slices) - 1) + lower = max(upper - 1, 0) + values = [] + for index in (lower, upper): + row = self._branches[direction][index] + ay, boundary = row[:2] if mode == "drive" else row[2:] + target = fraction * self._lateral_limits[direction][index] + values.append(float(np.interp(target, ay, boundary)) + if ay.size and target <= ay[-1] + 1e-9 else float("nan")) + return float(np.interp(speed_mps, self._speeds[[lower, upper]], values)) def lateral_limit(self, speed_mps: float, *, direction: float = 1.) -> float: """Interpolate maximum sustainable absolute lateral acceleration.""" - limits = np.asarray([_slice_lateral_limit(item, direction) for item in self.slices]) - speeds = np.asarray([item.speed_mps for item in self.slices]) - return float(np.interp(speed_mps, speeds, limits)) + return float(np.interp(speed_mps, self._speeds, self._lateral_limits[-1. if direction < 0. else 1.])) @dataclass(frozen=True) @@ -171,9 +194,18 @@ def solve_qss_lap( ggv.max_speed_mps, float(max_speed_mps) if max_speed_mps is not None else ggv.max_speed_mps, ) - speed = np.asarray( - [_corner_speed_limit(kappa, ggv, speed_ceiling) for kappa in line.curvature_per_m] - ) + curvature = np.abs(line.curvature_per_m) + lo = np.full(curvature.shape, 0.1) + hi = np.full(curvature.shape, speed_ceiling) + for _ in range(40): + midpoint = 0.5 * (lo + hi) + limits = np.where(line.curvature_per_m < 0., + np.interp(midpoint, ggv._speeds, ggv._lateral_limits[-1.]), + np.interp(midpoint, ggv._speeds, ggv._lateral_limits[1.])) + feasible = midpoint**2 * curvature <= limits + lo = np.where(feasible, midpoint, lo) + hi = np.where(feasible, hi, midpoint) + speed = np.where(curvature <= 1e-10, speed_ceiling, lo) converged = False completed_sweeps = 0 count = speed.size @@ -227,24 +259,6 @@ def _directional_slice(item: _GGVSlice, direction: float) -> _GGVSlice: item.ax_accel_mps2[mask][order], item.ax_brake_mps2[mask][order]) -def _slice_longitudinal_limit( - item: _GGVSlice, - ay_mps2: float, - mode: Literal["drive", "brake"], - direction: float = 1., -) -> float: - item = _directional_slice(item, direction) - values = item.ax_accel_mps2 if mode == "drive" else item.ax_brake_mps2 - finite = np.isfinite(item.ay_mps2) & np.isfinite(values) - if not np.any(finite): - return float("nan") - ay = item.ay_mps2[finite] - boundary = values[finite] - if ay_mps2 > float(np.max(ay)) + 1e-9: - return float("nan") - return float(np.interp(ay_mps2, ay, boundary)) - - def _slice_lateral_limit(item: _GGVSlice, direction: float = 1.) -> float: item = _directional_slice(item, direction) finite = np.isfinite(item.ax_accel_mps2) | np.isfinite(item.ax_brake_mps2) @@ -253,16 +267,3 @@ def _slice_lateral_limit(item: _GGVSlice, direction: float = 1.) -> float: return float(np.max(item.ay_mps2[finite])) -def _corner_speed_limit(curvature_per_m: float, ggv: GGVMap, ceiling_mps: float) -> float: - if abs(curvature_per_m) <= 1e-10: - return ceiling_mps - lo = 0.1 - hi = ceiling_mps - for _ in range(40): - midpoint = 0.5 * (lo + hi) - required_ay = midpoint**2 * abs(curvature_per_m) - if required_ay <= ggv.lateral_limit(midpoint, direction=np.sign(curvature_per_m)): - lo = midpoint - else: - hi = midpoint - return lo diff --git a/tests/test_lap_sim.py b/tests/test_lap_sim.py index ec0e3975..a8b97a6b 100644 --- a/tests/test_lap_sim.py +++ b/tests/test_lap_sim.py @@ -80,6 +80,34 @@ def test_ggv_keeps_missing_speed_slices(mode): assert np.isfinite(ggv.longitudinal_limit(30.0, 1.0, mode)) +@pytest.mark.parametrize("mode", ["drive", "brake"]) +def test_cached_ggv_matches_full_slice_interpolation(mode): + from simulations.lap.core.qss import _directional_slice, _slice_lateral_limit + + rng = np.random.default_rng(17) + ay = np.arange(-6., 7.) + drive = rng.uniform(0., 5., (4, ay.size)) + brake = -rng.uniform(0., 8., drive.shape) + drive[1, :3] = np.nan + brake[2, -3:] = np.nan + ggv = GGVMap.from_arrays([5., 10., 20., 30.], ay, drive, brake) + for speed in [0., 5., 7., 10., 15., 20., 25., 30., 40.]: + for direction in [-1., 1.]: + limits = [_slice_lateral_limit(item, direction) for item in ggv.slices] + for fraction in [0., .1, .5, 1.]: + values = [] + for original, limit in zip(ggv.slices, limits): + item = _directional_slice(original, direction) + boundary = item.ax_accel_mps2 if mode == "drive" else item.ax_brake_mps2 + finite = np.isfinite(boundary) + target = fraction * limit + values.append(np.interp(target, item.ay_mps2[finite], boundary[finite]) + if target <= item.ay_mps2[finite][-1] else np.nan) + expected = np.interp(speed, [5., 10., 20., 30.], values) + lateral = direction * fraction * np.interp(speed, [5., 10., 20., 30.], limits) + np.testing.assert_allclose(ggv.longitudinal_limit(speed, lateral, mode), expected, rtol=1e-14) + + def test_qss_lap_uses_varying_speed_slice_boundaries(circular_corridor, varying_ggv): line = circular_corridor.line_from_offsets(np.zeros(circular_corridor.gate_count)) lap = solve_qss_lap(line, varying_ggv)