Cache GGV interpolation during QSS lap optimization

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(

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

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)