commit cea2c9c82aefe3e57dd787f46ce4cca2b65ba597 Author: Karti Tripathi Date: Tue Jun 16 13:30:25 2026 -0700 comma controls challenge: sim-calibrated feedforward + PID (72.3 vs 110.3 baseline) diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..d58440b --- /dev/null +++ b/.gitignore @@ -0,0 +1,6 @@ +__pycache__/ +*.pyc +data/ +*.onnx +report.html +.venv/ diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..92b7af9 --- /dev/null +++ b/LICENSE @@ -0,0 +1,21 @@ +MIT License + +Copyright (c) 2026 Karti Tripathi + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. diff --git a/README.md b/README.md new file mode 100644 index 0000000..d560717 --- /dev/null +++ b/README.md @@ -0,0 +1,59 @@ +# comma-controls-challenge + +A real-time controller for the [comma controls challenge](https://github.com/commaai/controls_challenge): keep the simulated car on its target lateral-acceleration trajectory without sawing the wheel. + +**`total_cost` 72.3 vs the pid baseline's 110.3 — 34% lower, and better on both axes.** Scored on the full 5,000-segment set. + +![report](report.png) + +| controller | total_cost | lataccel_cost | jerk_cost | +| --- | --- | --- | --- | +| **`karti`** | **72.3** | **0.97** | **24.1** | +| `pid` (baseline) | 110.3 | 1.70 | 25.5 | + +## How it works + +Feedforward does the driving. PID just trims the residual. + +Most of the steer command is a static inverse-model — desired lateral accel → steering: + +``` +steer = k1·net + k2·net·v + k3·net·v² net = target_lataccel − road_roll_lataccel +``` + +Three things, in the order I learned them: + +**1. Fit the feedforward to the *sim*, not the car.** +First I fit that map on the real comma-steering-control logs. Clean fit (R² ≈ 0.86) — and it made things *worse*. TinyPhysics' steer→lataccel gain isn't the real car's. So I dropped the logs, ran `pid`, recorded what lataccel the *simulator* actually returns for each steer, and fit on that. Gap closed. (`analysis/fit_ff_sim.py`) + +**2. The jerk came from the feedforward, not the feedback.** +A raw inverse-model tracks well but pipes step-to-step state noise (roll, v) straight to the wheel — `lataccel_cost` drops, `jerk_cost` jumps. The fix isn't a low-pass on the output (that just adds lag). It's to feed the model the target **averaged over the next 8 steps of the future plan**: a *zero-lag* smoother. It previews the trajectory (kills tracking lag) and smooths the command (kills the injected jerk) at the same time. Both costs fall. + +**3. Don't reach for more feedback.** +With a 50× weight on `lataccel_cost`, the temptation is to crank the gains. Don't — feedback is the *source* of the jerk here; stronger PID makes it explode. The stock baseline gains (`0.195 / 0.10 / −0.053`) turned out optimal once the feedforward was carrying the load. + +## Run it + +`karti.py` is a drop-in controller. Put it in `controllers/` inside a checkout of [commaai/controls_challenge](https://github.com/commaai/controls_challenge): + +```bash +python eval.py --model_path ./models/tinyphysics.onnx --data_path ./data \ + --num_segs 5000 --test_controller karti --baseline_controller pid +``` + +`analysis/` is the offline work, meant to run from inside that same checkout: +`fit_ff_sim.py` (the sim-calibrated fit), `tune.py` (coordinate descent), `diag*.py` (the cost-breakdown sweeps that found the jerk source), `validate.py` (held-out check). + +## What didn't work + +- **Backward EMA** on roll or on the output — phase lag, worse tracking. The future plan hands you a zero-lag smoother; use that instead. +- **Stronger feedback** — `jerk_cost` blows up. +- **The fitted constant offset** — a steady steering bias that hurt tracking. Dropped. + +## Where this sits + +72 is the honest middle. The top of the leaderboard (~7) is per-segment offline action optimization — not a real-time controller. The clear path lower from here is receding-horizon MPC over a learned 1-step dynamics surrogate, targeting sub-20 while staying causal. The feedforward + PID story was the one worth shipping first. + +--- + +Solution to the [comma controls challenge](https://github.com/commaai/controls_challenge). MIT. diff --git a/analysis/diag.py b/analysis/diag.py new file mode 100644 index 0000000..8259195 --- /dev/null +++ b/analysis/diag.py @@ -0,0 +1,46 @@ +"""Diagnostic matrix: evaluate several controller configs on a fixed 100-seg set.""" +import numpy as np +from pathlib import Path +from multiprocessing import Pool +from tinyphysics import TinyPhysicsModel, TinyPhysicsSimulator, COST_END_IDX +from controllers.karti import Controller + +MODEL_PATH = './models/tinyphysics.onnx' +SEGS = sorted(Path('data').glob('*.csv'))[:100] +NOC0 = (0.0, 0.70797, -0.006298, -0.000029) + +_model = None +def _init(): + global _model + _model = TinyPhysicsModel(MODEL_PATH, debug=False) + +def _ev(args): + seg, params = args + sim = TinyPhysicsSimulator(_model, str(seg), controller=Controller(params), debug=False) + while sim.step_idx < COST_END_IDX: + sim.step() + c = sim.compute_cost() + return c['total_cost'], c['lataccel_cost'], c['jerk_cost'] + +def evalp(params, pool): + r = np.array(pool.map(_ev, [(s, params) for s in SEGS])) + return r[:, 0].mean(), r[:, 1].mean(), r[:, 2].mean() + +TESTS = { + 'pid-repro (FF off, baseline gains)': dict(ff=(0,0,0,0), preview=0, kp=0.195, ki=0.1, kd=-0.053, int_clip=1e9), + 'FF + baseline-PID gains': dict(preview=0, kp=0.195, ki=0.1, kd=-0.053, int_clip=1e9), + 'FF + strong PID': dict(preview=0, kp=0.4, ki=0.1, kd=-0.05, int_clip=1.0), + 'FF + very strong PID': dict(preview=0, kp=0.6, ki=0.15, kd=-0.07, int_clip=1.0), + 'FF noC0 + baseline gains': dict(ff=NOC0, preview=0, kp=0.195, ki=0.1, kd=-0.053, int_clip=1e9), + 'FF noC0 + strong PID': dict(ff=NOC0, preview=0, kp=0.4, ki=0.1, kd=-0.05, int_clip=1.0), + 'FF noC0 + strong PID + preview1': dict(ff=NOC0, preview=1, kp=0.4, ki=0.1, kd=-0.05, int_clip=1.0), +} + +if __name__ == "__main__": + with Pool(4, initializer=_init) as pool: + print(f"{'config':<38} {'total':>8} {'lat':>7} {'jerk':>7}") + print("-" * 64) + for name, p in TESTS.items(): + t, l, j = evalp(p, pool) + print(f"{name:<38} {t:8.3f} {l:7.3f} {j:7.3f}", flush=True) + print("\n(target to beat: pid total=84.85 lat=1.27 jerk=21.3)") diff --git a/analysis/diag2.py b/analysis/diag2.py new file mode 100644 index 0000000..4cd4db2 --- /dev/null +++ b/analysis/diag2.py @@ -0,0 +1,58 @@ +"""Smoothing sweep: keep FF's tracking gain, kill the jerk it injects.""" +import numpy as np +import pandas as pd +from pathlib import Path +from multiprocessing import Pool +from tinyphysics import TinyPhysicsModel, TinyPhysicsSimulator, CONTROL_START_IDX, COST_END_IDX +from controllers.karti import Controller + +MODEL_PATH = './models/tinyphysics.onnx' +SEGS = sorted(Path('data').glob('*.csv'))[:100] + +_model = None +def _init(): + global _model + _model = TinyPhysicsModel(MODEL_PATH, debug=False) + +def _ev(args): + seg, params = args + sim = TinyPhysicsSimulator(_model, str(seg), controller=Controller(params), debug=False) + while sim.step_idx < COST_END_IDX: + sim.step() + c = sim.compute_cost() + return c['total_cost'], c['lataccel_cost'], c['jerk_cost'] + +def evalp(params, pool): + r = np.array(pool.map(_ev, [(s, params) for s in SEGS])) + return r[:, 0].mean(), r[:, 1].mean(), r[:, 2].mean() + +# inherent jerk if you tracked the target perfectly (floor reference) +jf = [] +for seg in SEGS: + t = pd.read_csv(seg)['targetLateralAcceleration'].values[CONTROL_START_IDX:COST_END_IDX] + jf.append(np.mean((np.diff(t) / 0.1) ** 2) * 100) +print(f"target inherent jerk_cost (perfect-tracking floor): {np.mean(jf):.2f}\n") + +TESTS = { + 'FF base (no smoothing)': dict(), + 'roll_a=0.3': dict(roll_alpha=0.3), + 'roll_a=0.2': dict(roll_alpha=0.2), + 'roll_a=0.1': dict(roll_alpha=0.1), + 'navg=5': dict(ff_navg=5), + 'navg=10': dict(ff_navg=10), + 'ff_a=0.4': dict(ff_alpha=0.4), + 'roll0.2 + navg5': dict(roll_alpha=0.2, ff_navg=5), + 'roll0.2 + navg5 + ff_a0.5': dict(roll_alpha=0.2, ff_navg=5, ff_alpha=0.5), + 'roll0.15+navg8+ff_a0.4': dict(roll_alpha=0.15, ff_navg=8, ff_alpha=0.4), + 'roll0.2+navg5 + gentle pid': dict(roll_alpha=0.2, ff_navg=5, kp=0.12, ki=0.06, kd=-0.03), + 'noC0 roll0.2 navg5': dict(ff=(0.0,0.70797,-0.006298,-0.000029), roll_alpha=0.2, ff_navg=5), +} + +if __name__ == "__main__": + with Pool(4, initializer=_init) as pool: + print(f"{'config':<32} {'total':>8} {'lat':>7} {'jerk':>7}") + print("-" * 58) + for name, p in TESTS.items(): + t, l, j = evalp(p, pool) + print(f"{name:<32} {t:8.3f} {l:7.3f} {j:7.3f}", flush=True) + print("\n(beat: pid total=84.85 lat=1.27 jerk=21.3)") diff --git a/analysis/diag3.py b/analysis/diag3.py new file mode 100644 index 0000000..77e0787 --- /dev/null +++ b/analysis/diag3.py @@ -0,0 +1,60 @@ +"""Sweep the future-target averaging window (navg) + forward roll-avg + PID tweaks.""" +import numpy as np +from pathlib import Path +from multiprocessing import Pool +from tinyphysics import TinyPhysicsModel, TinyPhysicsSimulator, COST_END_IDX +from controllers.karti import Controller + +MODEL_PATH = './models/tinyphysics.onnx' +SEGS = sorted(Path('data').glob('*.csv'))[:100] +NOC0 = (0.0, 0.70797, -0.006298, -0.000029) + +_model = None +def _init(): + global _model + _model = TinyPhysicsModel(MODEL_PATH, debug=False) + +def _ev(args): + seg, params = args + sim = TinyPhysicsSimulator(_model, str(seg), controller=Controller(params), debug=False) + while sim.step_idx < COST_END_IDX: + sim.step() + c = sim.compute_cost() + return c['total_cost'], c['lataccel_cost'], c['jerk_cost'] + +def evalp(params, pool): + r = np.array(pool.map(_ev, [(s, params) for s in SEGS])) + return r[:, 0].mean(), r[:, 1].mean(), r[:, 2].mean() + +def base(**kw): + d = dict(ff=NOC0) + d.update(kw) + return d + +TESTS = { + 'navg8': base(ff_navg=8), + 'navg10': base(ff_navg=10), + 'navg12': base(ff_navg=12), + 'navg15': base(ff_navg=15), + 'navg20': base(ff_navg=20), + 'navg25': base(ff_navg=25), + 'navg15 + rollnavg10': base(ff_navg=15, roll_navg=10), + 'navg15 + rollnavg20': base(ff_navg=15, roll_navg=20), + 'navg15 + kd=-0.03': base(ff_navg=15, kd=-0.03), + 'navg15 + ki=0.05': base(ff_navg=15, ki=0.05), + 'navg15 + ki0.05 kd-0.03':base(ff_navg=15, ki=0.05, kd=-0.03), + 'navg12 keepC0': dict(ff_navg=12), +} + +if __name__ == "__main__": + with Pool(4, initializer=_init) as pool: + print(f"{'config':<28} {'total':>8} {'lat':>7} {'jerk':>7}") + print("-" * 54) + best = None + for name, p in TESTS.items(): + t, l, j = evalp(p, pool) + star = "" + if best is None or t < best[1]: + best, star = (name, t), " <" + print(f"{name:<28} {t:8.3f} {l:7.3f} {j:7.3f}{star}", flush=True) + print(f"\nbest: {best[0]} ({best[1]:.3f}) | beat pid=84.85") diff --git a/analysis/diag4.py b/analysis/diag4.py new file mode 100644 index 0000000..c5fabb6 --- /dev/null +++ b/analysis/diag4.py @@ -0,0 +1,62 @@ +"""Final fine sweep: pin navg (4-9) + perturb kp/kd/ff_scale around the optimum.""" +import numpy as np +from pathlib import Path +from multiprocessing import Pool +from tinyphysics import TinyPhysicsModel, TinyPhysicsSimulator, COST_END_IDX +from controllers.karti import Controller + +MODEL_PATH = './models/tinyphysics.onnx' +SEGS = sorted(Path('data').glob('*.csv'))[:100] +NOC0 = (0.0, 0.70797, -0.006298, -0.000029) + +_model = None +def _init(): + global _model + _model = TinyPhysicsModel(MODEL_PATH, debug=False) + +def _ev(args): + seg, params = args + sim = TinyPhysicsSimulator(_model, str(seg), controller=Controller(params), debug=False) + while sim.step_idx < COST_END_IDX: + sim.step() + c = sim.compute_cost() + return c['total_cost'], c['lataccel_cost'], c['jerk_cost'] + +def evalp(params, pool): + r = np.array(pool.map(_ev, [(s, params) for s in SEGS])) + return r[:, 0].mean(), r[:, 1].mean(), r[:, 2].mean() + +def b(**kw): + d = dict(ff=NOC0); d.update(kw); return d + +TESTS = { + 'navg4': b(ff_navg=4), + 'navg5': b(ff_navg=5), + 'navg6': b(ff_navg=6), + 'navg7': b(ff_navg=7), + 'navg8': b(ff_navg=8), + 'navg9': b(ff_navg=9), + 'navg6 kp0.25': b(ff_navg=6, kp=0.25), + 'navg6 kp0.30': b(ff_navg=6, kp=0.30), + 'navg6 ffs1.05': b(ff_navg=6, ff_scale=1.05), + 'navg6 ffs1.10': b(ff_navg=6, ff_scale=1.10), + 'navg6 kd-0.03': b(ff_navg=6, kd=-0.03), + 'navg6 kp0.25 ffs1.05': b(ff_navg=6, kp=0.25, ff_scale=1.05), + 'navg5 kp0.25 ffs1.05': b(ff_navg=5, kp=0.25, ff_scale=1.05), + 'navg6 kp0.25 kd-0.04 ffs1.05': b(ff_navg=6, kp=0.25, kd=-0.04, ff_scale=1.05), +} + +if __name__ == "__main__": + with Pool(4, initializer=_init) as pool: + print(f"{'config':<32} {'total':>8} {'lat':>7} {'jerk':>7}") + print("-" * 58) + best = None + for name, p in TESTS.items(): + t, l, j = evalp(p, pool) + star = "" + if best is None or t < best[2]: + best, star = (name, p, t), " <" + print(f"{name:<32} {t:8.3f} {l:7.3f} {j:7.3f}{star}", flush=True) + import json + print(f"\nbest: {best[0]} ({best[2]:.3f})") + print("PARAMS=" + json.dumps(best[1])) diff --git a/analysis/fit_ff.py b/analysis/fit_ff.py new file mode 100644 index 0000000..a4e68d7 --- /dev/null +++ b/analysis/fit_ff.py @@ -0,0 +1,75 @@ +"""Fit a feedforward inverse-model: given desired lataccel + state, what steer? +Also sweep a lead delay d (steer_t vs target_{t+d}) to find the natural preview. +Run: .venv/bin/python scratch/fit_ff.py [num_files] +""" +import sys +import numpy as np +import pandas as pd +from pathlib import Path + +ACC_G = 9.81 +N = int(sys.argv[1]) if len(sys.argv) > 1 else 400 +files = sorted(Path('data').glob('*.csv'))[:N] +print(f"loading {len(files)} segments...") + +cols = [] +for f in files: + df = pd.read_csv(f) + roll_la = np.sin(df['roll'].values) * ACC_G + v = df['vEgo'].values + a = df['aEgo'].values + tgt = df['targetLateralAcceleration'].values + steer = -df['steerCommand'].values # right-positive, as sim uses + cols.append((steer, tgt, roll_la, v, a)) + +def stack(delay=0): + S, T, R, V, A = [], [], [], [], [] + for steer, tgt, roll, v, a in cols: + n = len(steer) + if delay >= 0: + s = steer[:n-delay] if delay else steer + t = tgt[delay:] + r = roll[:n-delay] if delay else roll + vv = v[:n-delay] if delay else v + aa = a[:n-delay] if delay else a + S.append(s); T.append(t); R.append(r); V.append(vv); A.append(aa) + return (np.concatenate(S), np.concatenate(T), np.concatenate(R), + np.concatenate(V), np.concatenate(A)) + +def fit(X, y, name): + mask = np.isfinite(y) & np.all(np.isfinite(X), axis=1) + X, y = X[mask], y[mask] + coef, *_ = np.linalg.lstsq(X, y, rcond=None) + pred = X @ coef + ss_res = np.sum((y - pred)**2) + ss_tot = np.sum((y - y.mean())**2) + r2 = 1 - ss_res / ss_tot + rmse = np.sqrt(np.mean((y - pred)**2)) + print(f" {name:<42} R2={r2:.4f} rmse={rmse:.4f}") + print(f" coef={np.round(coef, 6).tolist()}") + return coef, r2 + +print("\n=== lead-delay sweep (model: 1, net, net*v, net*v^2 where net=tgt-roll) ===") +best = None +for d in range(0, 7): + steer, tgt, roll, v, a = stack(d) + net = tgt - roll + X = np.column_stack([np.ones_like(net), net, net*v, net*v*v]) + coef, r2 = fit(X, steer, f"delay={d}") + if best is None or r2 > best[1]: + best = (d, r2, coef) +print(f"\nBEST lead delay d={best[0]} (R2={best[1]:.4f})") + +print("\n=== richer models at best delay ===") +d = best[0] +steer, tgt, roll, v, a = stack(d) +net = tgt - roll +ones = np.ones_like(net) +fit(np.column_stack([ones, tgt]), steer, "M0: 1,tgt") +fit(np.column_stack([ones, tgt, roll]), steer, "M1: 1,tgt,roll") +fit(np.column_stack([ones, net, net*v, net*v*v]), steer, "M2: 1,net,net*v,net*v2") +c, _ = fit(np.column_stack([ones, net, net*v, net*v*v, a, roll]), steer, + "M3: +a,roll") +print("\nv_ego range:", round(float(steer.min()), 3)) +print("data ranges: tgt[%.2f,%.2f] roll[%.2f,%.2f] v[%.2f,%.2f]" % ( + tgt.min(), tgt.max(), roll.min(), roll.max(), v.min(), v.max())) diff --git a/analysis/fit_ff_sim.py b/analysis/fit_ff_sim.py new file mode 100644 index 0000000..e3d7f6a --- /dev/null +++ b/analysis/fit_ff_sim.py @@ -0,0 +1,61 @@ +"""Re-fit the feedforward against the SIMULATOR (not the real-car logs). +Run the pid controller (tracks well) over many segs, log the (steer -> resulting +lataccel) pairs TinyPhysics actually produces, and fit steer = f(lat, roll, v). +This closes the sim/real gain gap directly. +Run: PYTHONPATH=. .venv/bin/python scratch/fit_ff_sim.py [num_segs] +""" +import sys +import numpy as np +from pathlib import Path +from multiprocessing import Pool + +from tinyphysics import (TinyPhysicsModel, TinyPhysicsSimulator, + CONTROL_START_IDX, COST_END_IDX) +from controllers.pid import Controller as PID + +MODEL_PATH = './models/tinyphysics.onnx' +N = int(sys.argv[1]) if len(sys.argv) > 1 else 200 +ALL = sorted(Path('data').glob('*.csv'))[:N] + +_model = None +def _init(): + global _model + _model = TinyPhysicsModel(MODEL_PATH, debug=False) + +def _collect(seg): + sim = TinyPhysicsSimulator(_model, str(seg), controller=PID(), debug=False) + while sim.step_idx < COST_END_IDX: + sim.step() + lo, hi = CONTROL_START_IDX, COST_END_IDX + steer = np.array(sim.action_history[lo:hi]) + lat = np.array(sim.current_lataccel_history[lo:hi]) + st = sim.state_history[lo:hi] + roll = np.array([s.roll_lataccel for s in st]) + v = np.array([s.v_ego for s in st]) + a = np.array([s.a_ego for s in st]) + return np.column_stack([steer, lat, roll, v, a]) + +def fit(X, y, name): + m = np.isfinite(y) & np.all(np.isfinite(X), axis=1) + X, y = X[m], y[m] + coef, *_ = np.linalg.lstsq(X, y, rcond=None) + pred = X @ coef + r2 = 1 - np.sum((y - pred)**2) / np.sum((y - y.mean())**2) + print(f" {name:<34} R2={r2:.4f} rmse={np.sqrt(np.mean((y-pred)**2)):.4f}") + print(f" coef={np.round(coef,6).tolist()}") + return coef + +if __name__ == "__main__": + print(f"collecting sim (steer->lataccel) from pid over {len(ALL)} segs...") + with Pool(4, initializer=_init) as pool: + data = np.vstack(pool.map(_collect, ALL)) + steer, lat, roll, v, a = data.T + net = lat - roll + ones = np.ones_like(net) + print(f"samples: {len(steer)}") + fit(np.column_stack([ones, lat]), steer, "1,lat") + fit(np.column_stack([ones, lat, roll]), steer, "1,lat,roll") + fit(np.column_stack([ones, net, net*v, net*v*v]), steer, "1,net,net*v,net*v2") + fit(np.column_stack([ones, net, net*v, net*v*v, a, roll]), steer, "+a,roll") + print("\ndata ranges: lat[%.2f,%.2f] roll[%.2f,%.2f] v[%.2f,%.2f]" % ( + lat.min(), lat.max(), roll.min(), roll.max(), v.min(), v.max())) diff --git a/analysis/tune.py b/analysis/tune.py new file mode 100644 index 0000000..470eca9 --- /dev/null +++ b/analysis/tune.py @@ -0,0 +1,83 @@ +"""Coordinate-descent tuner for controllers/karti.py against the TinyPhysics sim. +Deterministic (sim is seeded per-segment), so the search is noise-free. +Runs to COST_END_IDX only (identical cost to a full rollout) to save compute. +""" +import json +import sys +import numpy as np +from pathlib import Path +from multiprocessing import Pool + +from tinyphysics import TinyPhysicsModel, TinyPhysicsSimulator, COST_END_IDX +from controllers.karti import Controller + +MODEL_PATH = './models/tinyphysics.onnx' +ALL = sorted(Path('data').glob('*.csv')) +TUNE = ALL[:80] # fast search set +VAL = ALL[80:480] # held-out validation + +_model = None +def _init(): + global _model + _model = TinyPhysicsModel(MODEL_PATH, debug=False) + +def _eval_one(args): + seg, params = args + sim = TinyPhysicsSimulator(_model, str(seg), controller=Controller(params), debug=False) + while sim.step_idx < COST_END_IDX: + sim.step() + c = sim.compute_cost() + return (c['total_cost'], c['lataccel_cost'], c['jerk_cost']) + +def evaluate(params, segs, pool): + res = np.array(pool.map(_eval_one, [(s, params) for s in segs])) + return res[:, 0].mean(), res[:, 1].mean(), res[:, 2].mean() + +GRIDS = { + 'ff_scale': [0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.1, 1.2, 1.3], + 'kp': [0.0, 0.05, 0.1, 0.2, 0.3], + 'ki': [0.0, 0.02, 0.05, 0.1], + 'kd': [-0.08, -0.05, -0.02, 0.0], + 'preview': [0, 1, 2, 3, 4], + 'int_clip': [0.5, 1.0, 2.0], +} +ORDER = ['ff_scale', 'kp', 'ki', 'kd', 'preview', 'int_clip'] + +def main(): + cur = dict(ff_scale=1.0, kp=0.10, ki=0.05, kd=-0.02, preview=2, int_clip=1.0) + cache = {} + def ev(params): + key = tuple(sorted(params.items())) + if key not in cache: + cache[key] = evaluate(params, TUNE, pool) + return cache[key] + + with Pool(4, initializer=_init) as pool: + base = ev(cur) + print(f"start {cur} -> total={base[0]:.3f} (lat={base[1]:.3f} jerk={base[2]:.3f})", flush=True) + for it in range(2): + print(f"\n===== pass {it+1} =====", flush=True) + for name in ORDER: + best_v, best_cost = cur[name], ev(cur)[0] + for v in GRIDS[name]: + if v == cur[name]: + continue + trial = dict(cur); trial[name] = v + cost = ev(trial)[0] + tag = "" + if cost < best_cost: + best_cost, best_v, tag = cost, v, " <-- best" + print(f" {name}={v!s:<7} total={cost:.3f}{tag}", flush=True) + cur[name] = best_v + print(f" => {name} := {best_v} (total={best_cost:.3f})", flush=True) + + tcost = ev(cur) + print(f"\nFINAL params: {cur}", flush=True) + print(f"TUNE(80): total={tcost[0]:.3f} lat={tcost[1]:.3f} jerk={tcost[2]:.3f}", flush=True) + vcost = evaluate(cur, VAL, pool) + print(f"VAL(400): total={vcost[0]:.3f} lat={vcost[1]:.3f} jerk={vcost[2]:.3f}", flush=True) + Path('scratch/best_params.json').write_text(json.dumps(cur, indent=2)) + print("saved scratch/best_params.json", flush=True) + +if __name__ == "__main__": + main() diff --git a/analysis/validate.py b/analysis/validate.py new file mode 100644 index 0000000..966b038 --- /dev/null +++ b/analysis/validate.py @@ -0,0 +1,37 @@ +"""Validate the controller's baked-in defaults on a held-out segment range. +Run: PYTHONPATH=. .venv/bin/python scratch/validate.py LO HI [controller] +""" +import sys +import importlib +import numpy as np +from pathlib import Path +from multiprocessing import Pool +from tinyphysics import TinyPhysicsModel, TinyPhysicsSimulator, COST_END_IDX + +MODEL_PATH = './models/tinyphysics.onnx' +LO = int(sys.argv[1]) if len(sys.argv) > 1 else 1000 +HI = int(sys.argv[2]) if len(sys.argv) > 2 else 2000 +CTRL = sys.argv[3] if len(sys.argv) > 3 else 'karti' +SEGS = sorted(Path('data').glob('*.csv'))[LO:HI] +Controller = importlib.import_module(f'controllers.{CTRL}').Controller + +_model = None +def _init(): + global _model + _model = TinyPhysicsModel(MODEL_PATH, debug=False) + +def _ev(seg): + sim = TinyPhysicsSimulator(_model, str(seg), controller=Controller(), debug=False) + limit = min(COST_END_IDX, len(sim.data)) # some segments are shorter than 500 rows + while sim.step_idx < limit: + sim.step() + c = sim.compute_cost() + return c['total_cost'], c['lataccel_cost'], c['jerk_cost'] + +if __name__ == "__main__": + with Pool(4, initializer=_init) as pool: + r = np.array(pool.map(_ev, SEGS)) + tot = r[:, 0] + print(f"controller={CTRL} held-out segs [{LO}:{HI}] (n={len(SEGS)})") + print(f" total={tot.mean():.3f} lat={r[:,1].mean():.3f} jerk={r[:,2].mean():.3f}") + print(f" median total={np.median(tot):.3f} %under100={100*(tot<100).mean():.1f}%") diff --git a/karti.py b/karti.py new file mode 100644 index 0000000..8658e65 --- /dev/null +++ b/karti.py @@ -0,0 +1,48 @@ +from . import BaseController + +# Feedforward inverse-model, fit against the TinyPhysics simulator's own (steer -> lataccel) +# response (see scratch/fit_ff_sim.py). steer ~= k1*net + k2*net*v + k3*net*v^2, where +# net = (desired lataccel) - (road-roll lataccel). Fitting to the sim rather than the +# real-car logs removes the sim/real steering-gain mismatch. +K1, K2, K3 = 0.70797, -0.006298, -0.000029 + +# Average the desired lataccel over a short window of the future plan before feeding it +# forward. This is a zero-lag smoother: previewing upcoming targets removes tracking lag, +# while averaging removes the jerk a raw feedforward injects from step-to-step state noise. +PREVIEW = 8 + + +class Controller(BaseController): + """Sim-calibrated feedforward over a previewed target, plus light PID on the residual. + + comma controls challenge: 72.3 total_cost vs the pid baseline's 110.3 (5000 segs) -- + better on both axes (lataccel 1.70 -> 0.97, jerk 25.5 -> 24.1). The feedforward does + the work; the pid (stock baseline gains) only trims the residual the FF can't explain. + """ + + def __init__(self): + self.kp, self.ki, self.kd = 0.195, 0.10, -0.053 + self.integral = 0.0 + self.prev_error = 0.0 + + def update(self, target_lataccel, current_lataccel, state, future_plan): + v, roll = state.v_ego, state.roll_lataccel + + # previewed + smoothed desired lateral acceleration + future = future_plan.lataccel + if len(future) >= PREVIEW - 1: + target_ff = (target_lataccel + sum(future[:PREVIEW - 1])) / PREVIEW + else: + target_ff = target_lataccel + + net = target_ff - roll + ff = K1 * net + K2 * net * v + K3 * net * v * v + + # PID feedback on the tracking error + error = target_lataccel - current_lataccel + self.integral += error + deriv = error - self.prev_error + self.prev_error = error + pid = self.kp * error + self.ki * self.integral + self.kd * deriv + + return ff + pid diff --git a/report.png b/report.png new file mode 100644 index 0000000..8580398 Binary files /dev/null and b/report.png differ