Next-Drive / drive_core.py
Lamapi's picture
Publish Next-Drive model
3a00e09 verified
Raw History Blame Contribute Delete
21.7 kB
"""Next Drive: top-down urban intersection simulator, expert planner, rule-based driver, learned driver and safety check.
Simulation and training demo only. No physical vehicle, no claim about real-road safety.
The ego car follows a fixed route (straight, left or right turn) and only chooses its longitudinal
action every 0.1 s from ACTIONS. Everything is deterministic given (seed, kind, density) and is ported
line by line to web/core.js so the Space can generate new drives.
"""
import math
import numpy as np
from nlrt import Mulberry32, forward
DT, VMAX, V_LIMIT, T_MAX = 0.1, 14.0, 13.9, 45.0
ACTIONS = [-6.0, -3.0, -1.5, 0.0, 1.0, 2.0]
ACTION_NAMES = ["brake hard", "brake", "ease off", "hold speed", "speed up", "accelerate"]
LANE, S_STOP, HALF = 1.75, 60.0, 2.2
ROUTES = ["straight", "left", "right"]
KINDS = ["light", "crosswalk", "lead_brake", "occlusion", "mixed"]
TURN = {"right": (-5.75, 4.0), "left": (-5.25, 7.0)}
N_FEAT = 21
# ------------------------------------------------------------------ geometry
def path_len(route):
if route == "straight":
return 130.0
y0, r = TURN[route]
return (y0 + 70.0) + r * math.pi / 2 + 55.0
def pose(route, s):
"""Position and heading on the route at arc length s (clamped to the route)."""
s = max(0.0, min(s, path_len(route)))
if route == "straight":
return LANE, -70.0 + s, math.pi / 2
y0, r = TURN[route]
L1 = y0 + 70.0
if s < L1:
return LANE, -70.0 + s, math.pi / 2
arc = r * math.pi / 2
if s < L1 + arc:
th = (s - L1) / r
if route == "right":
return LANE + r - r * math.cos(th), y0 + r * math.sin(th), math.pi / 2 - th
return LANE - r + r * math.cos(th), y0 + r * math.sin(th), math.pi / 2 + th
d = s - L1 - arc
if route == "right":
return LANE + r + d, y0 + r, 0.0
return LANE - r - d, y0 + r, math.pi
def circles(x, y, h):
c, s = math.cos(h), math.sin(h)
return [(x + d * c, y + d * s) for d in (-1.4, 0.0, 1.4)]
def hits_car(ego, other, margin=0.0):
for ax, ay in ego:
for bx, by in other:
dx, dy, rr = ax - bx, ay - by, 2.0 + margin
if dx * dx + dy * dy < rr * rr:
return True
return False
def hits_ped(ego, px, py, margin=0.0):
for ax, ay in ego:
dx, dy, rr = ax - px, ay - py, 1.35 + margin
if dx * dx + dy * dy < rr * rr:
return True
return False
# ------------------------------------------------------------------ traffic lights
def light_ns(sc, t):
G, Y, R = sc["G"], 3.0, sc["R"]
u = (t + sc["phase"]) % (G + Y + R)
if u < G:
return 0, G - u
if u < G + Y:
return 1, G + Y - u
return 2, G + Y + R - u
def light_ew(sc, t):
G, Y, R = sc["G"], 3.0, sc["R"]
u = (t + sc["phase"]) % (G + Y + R)
if u < G + Y:
return 2
r = u - G - Y
if r < 1.0:
return 2
if r < R - 4.0:
return 0
if r < R - 1.0:
return 1
return 2
# ------------------------------------------------------------------ scenarios
def make_scenario(seed, kind="mixed", density=1):
rng = Mulberry32((seed * 7919 + KINDS.index(kind) * 101 + density * 13) % 4294967296)
u = rng.random()
route = "straight" if u < 0.5 else ("left" if u < 0.75 else "right")
if kind in ("occlusion", "crosswalk") and rng.random() < 0.5:
route = "straight"
G, R = 7.0 + 6.0 * rng.random(), 8.0 + 5.0 * rng.random()
sc = {"seed": seed, "kind": kind, "density": density, "route": route, "G": G, "R": R,
"phase": rng.random() * (G + 3.0 + R), "cars": [], "peds": [], "parked": [], "lead": None}
for k in range(density + 1): # cross traffic, both directions
for d in (1, -1):
sc["cars"].append({"axis": "x", "dir": d, "lane": -LANE * d, "p0": -70.0 * d, "v0": 8.0 + 4.0 * rng.random(),
"t0": k * 7.0 + 5.0 * rng.random()})
if route == "left" or kind == "mixed": # oncoming traffic (southbound)
for k in range(density):
sc["cars"].append({"axis": "y", "dir": -1, "lane": -LANE, "p0": 70.0, "v0": 8.0 + 4.0 * rng.random(),
"t0": k * 6.0 + 4.0 * rng.random()})
if kind in ("crosswalk", "mixed"):
for k in range(density + 1):
d = 1 if rng.random() < 0.5 else -1
sc["peds"].append({"x0": -9.0 * d, "y0": -7.5 + 1.6 * (rng.random() - 0.5), "vx": d * (1.0 + 0.6 * rng.random()),
"vy": 0.0, "t0": 1.0 + 11.0 * rng.random(), "x_end": 9.0 * d})
if kind in ("lead_brake", "mixed"):
sc["lead"] = {"s0": 14.0 + 10.0 * rng.random(), "v0": 9.0 + 3.0 * rng.random(),
"tb": 2.5 + 5.0 * rng.random(), "db": 1.5 + 1.5 * rng.random()}
if kind in ("occlusion", "mixed"):
y = -46.0
gaps = []
while y < -16.0:
L = 4.6
sc["parked"].append([4.6, y + L / 2, 2.0, L])
y += L + 0.4 + (2.6 if rng.random() < 0.45 else 0.0)
gaps.append(y - 0.2)
n_emerge = density
for k in range(min(n_emerge, len(gaps))):
gy = gaps[rng.randint(len(gaps))] - 1.0
D = 12.0 + 10.0 * rng.random()
sc["peds"].append({"x0": 5.8, "y0": gy, "vx": -(1.2 + 0.4 * rng.random()), "vy": 0.0,
"t0": max(0.0, (gy + 70.0 - HALF - D) / 10.0), "x_end": -6.0})
return sc
# ------------------------------------------------------------------ world state
def init_world(sc):
w = {"t": 0.0, "s": 0.0, "v": 10.0, "a": 0.0, "cars": [], "peds": [], "lead": None}
for c in sc["cars"]:
w["cars"].append({"p": c["p0"], "v": c["v0"], "active": False, "done": False, **{k: c[k] for k in ("axis", "dir", "lane", "v0", "t0")}})
for p in sc["peds"]:
w["peds"].append({"x": p["x0"], "y": p["y0"], "vx": 0.0, "vy": 0.0, "done": False, **{k: p[k] for k in ("t0", "x_end")}, "svx": p["vx"]})
if sc["lead"]:
L = sc["lead"]
w["lead"] = {"s": L["s0"], "v": L["v0"], **L}
return w
def car_xyh(c):
if c["axis"] == "x":
return c["p"], c["lane"], 0.0 if c["dir"] > 0 else math.pi
return c["lane"], c["p"], math.pi / 2 if c["dir"] > 0 else -math.pi / 2
def step_agents(sc, w):
t = w["t"]
ew, (ns, _) = light_ew(sc, t), light_ns(sc, t)
lanes = {}
for c in w["cars"]:
if not c["active"] and not c["done"] and t >= c["t0"]:
c["active"] = True
if c["active"]:
lanes.setdefault((c["axis"], c["dir"]), []).append(c)
for key, cs in lanes.items():
cs.sort(key=lambda c: -c["p"] * c["dir"]) # front-most first
for i, c in enumerate(cs):
light = ew if c["axis"] == "x" else ns
front = c["p"] * c["dir"] + HALF
d_stop = -10.0 - front
a = min(2.0, 1.5 * (c["v0"] - c["v"]))
if light != 0 and d_stop > -1.0 and c["v"] * c["v"] / (2 * 3.0) < d_stop + 1.0:
a = min(a, -c["v"] * c["v"] / (2 * max(d_stop - 0.5, 0.3)))
if i > 0:
gap = (cs[i - 1]["p"] - c["p"]) * c["dir"] - 4.4
if gap < 6.0 + c["v"] * 1.2:
a = min(a, -3.0 if gap > 3.0 else -6.0)
c["v"] = max(0.0, c["v"] + max(-6.0, a) * DT)
c["p"] += c["dir"] * c["v"] * DT
if c["p"] * c["dir"] > 75.0:
c["active"], c["done"] = False, True
for p in w["peds"]:
if p["done"]:
continue
if t >= p["t0"]:
p["vx"] = p["svx"]
p["x"] += p["vx"] * DT
if (p["x"] - p["x_end"]) * p["svx"] > 0:
p["done"] = True
L = w["lead"]
if L is not None:
if L["tb"] <= t < L["tb"] + L["db"]:
a = -6.0
else:
a = min(2.0, 1.5 * (L["v0"] - L["v"]))
d_stop = S_STOP - (L["s"] + HALF)
if ns != 0 and d_stop > -1.0 and L["v"] * L["v"] / (2 * 3.0) < d_stop + 1.0:
a = min(a, -L["v"] * L["v"] / (2 * max(d_stop - 0.5, 0.3)))
L["v"] = max(0.0, L["v"] + max(-6.0, a) * DT)
L["s"] += L["v"] * DT
# ------------------------------------------------------------------ perception
def seg_hits_rect(x0, y0, x1, y1, rect):
cx, cy, wd, ht = rect
xmin, xmax, ymin, ymax = cx - wd / 2, cx + wd / 2, cy - ht / 2, cy + ht / 2
t0, t1 = 0.0, 1.0
for p, q in ((-(x1 - x0), x0 - xmin), (x1 - x0, xmax - x0), (-(y1 - y0), y0 - ymin), (y1 - y0, ymax - y0)):
if p == 0:
if q < 0:
return False
else:
r = q / p
if p < 0:
if r > t1:
return False
if r > t0:
t0 = r
else:
if r < t0:
return False
if r < t1:
t1 = r
return True
def observe(sc, w):
ex, ey, eh = pose(sc["route"], w["s"])
eye = (ex + 1.0 * math.cos(eh), ey + 1.0 * math.sin(eh))
agents = []
for c in w["cars"]:
if c["active"]:
x, y, h = car_xyh(c)
agents.append({"kind": "car", "x": x, "y": y, "h": h, "vx": c["v"] * math.cos(h), "vy": c["v"] * math.sin(h)})
L = w["lead"]
if L is not None and L["s"] < path_len(sc["route"]):
x, y, h = pose(sc["route"], L["s"])
agents.append({"kind": "lead", "x": x, "y": y, "h": h, "vx": L["v"] * math.cos(h), "vy": L["v"] * math.sin(h), "s": L["s"], "v": L["v"]})
visible = []
for p in w["peds"]:
if p["done"]:
continue
dx, dy = p["x"] - eye[0], p["y"] - eye[1]
vis = dx * dx + dy * dy < 3600.0 and not any(seg_hits_rect(eye[0], eye[1], p["x"], p["y"], r) for r in sc["parked"])
visible.append(vis)
if vis:
agents.append({"kind": "ped", "x": p["x"], "y": p["y"], "h": 0.0, "vx": p["vx"], "vy": p["vy"]})
phantoms = []
s_front = w["s"] + HALF
for r in sc["parked"]:
gy = r[1] + r[3] / 2 + 0.3
ahead = (gy + 70.0) - s_front
if 0.0 < ahead < 35.0 and seg_hits_rect(eye[0], eye[1], 4.2, gy, r):
phantoms.append({"x": 4.2, "y": gy, "vx": -1.4, "vy": 0.0, "ahead": ahead})
ls, rem = light_ns(sc, w["t"])
return {"t": w["t"], "s": w["s"], "v": w["v"], "a": w["a"], "route": sc["route"], "agents": agents,
"phantoms": phantoms, "light": ls, "remaining": rem, "sc": sc, "visible": visible}
# ------------------------------------------------------------------ prediction helpers
def rollout_hits(obs, plan, horizon=3.0, dt=0.25, margin_car=0.4, margin_ped=0.6):
"""plan(k, tau) -> accel. Returns (first collision time or None, red-light crossing time or None, s_end, soft penalty).
Soft penalty: speed above the limit, speed above what allows stopping before an occluded gap
(where a hidden pedestrian could step out), and following closer than 2 m + 1.2 s to the lead car."""
s, v = obs["s"], obs["v"]
route = obs["route"]
n = int(round(horizon / dt))
excess = 0.0
red_t = None
lead = [ag for ag in obs["agents"] if ag["kind"] == "lead"]
for k in range(1, n + 1):
tau = k * dt
a = plan(k, tau)
v = min(VMAX, max(0.0, v + a * dt))
s_prev = s
s = s + v * dt
if v > V_LIMIT:
excess += v - V_LIMIT
for ph in obs["phantoms"]:
d = ph["y"] + 70.0 - (s + HALF)
if d > -1.0:
excess += max(0.0, v - (math.sqrt(2 * 4.0 * max(d - 2.0, 0.0)) + 3.0))
for ag in lead:
gap = ag["s"] + ag["v"] * tau - s - 4.4
excess += 0.5 * max(0.0, 2.0 + 1.2 * v - gap)
if red_t is None and s > s_prev:
lt, _ = light_ns(obs["sc"], obs["t"] + tau)
crossing = s_prev + HALF < S_STOP - 0.5 <= s + HALF
creeping = S_STOP - 0.5 <= obs["s"] + HALF < S_STOP + 6.0 and obs["v"] < 3.0
if lt == 2 and (crossing or creeping):
red_t = tau
ex, ey, eh = pose(route, s)
ec = circles(ex, ey, eh)
for ag in obs["agents"]:
px, py = ag["x"] + ag["vx"] * tau, ag["y"] + ag["vy"] * tau
if (px - ex) * (px - ex) + (py - ey) * (py - ey) > 64.0:
continue
if ag["kind"] == "ped":
if hits_ped(ec, px, py, margin_ped):
return tau, red_t, s, excess
elif hits_car(ec, circles(px, py, ag["h"]), margin_car):
return tau, red_t, s, excess
return None, red_t, s, excess
# ------------------------------------------------------------------ drivers
def expert(obs):
"""Search over two-phase action sequences (1 s then 2 s), constant-velocity predictions, phantom pedestrians."""
best, best_cost = 0, float("inf")
for i, a1 in enumerate(ACTIONS):
for a2 in ACTIONS:
plan = lambda k, tau, a1=a1, a2=a2: a1 if tau <= 1.0 + 1e-9 else a2
hit, red, s_end, excess = rollout_hits(obs, plan)
cost = 0.0
if hit is not None:
cost += 1e5 - 1e4 * hit
if red is not None:
cost += 2e4
cost += -10.0 * (s_end - obs["s"])
cost += 40.0 if a1 == -6.0 else (8.0 if a1 == -3.0 else 0.0)
cost += 3.0 * abs(a1 - obs["a"])
cost += 50.0 * excess
if cost < best_cost:
best, best_cost = i, cost
return best
def rule_driver(obs):
"""IDM-style rules: lights, lead car, visible pedestrians near the route, yield before a left turn."""
v, s = obs["v"], obs["s"]
route = obs["route"]
s_front = s + HALF
obst = []
for ag in obs["agents"]:
if ag["kind"] == "lead":
obst.append((ag["s"] - s - 4.4, ag["v"]))
d_stop = S_STOP - s_front
if d_stop > 0 and (obs["light"] == 2 or (obs["light"] == 1 and v * v / (2 * 3.0) < d_stop)):
obst.append((d_stop - 0.5, 0.0))
for ag in obs["agents"]:
if ag["kind"] != "ped":
continue
for j in range(0, 51, 2):
px, py, _ = pose(route, s + j)
fx, fy = ag["x"] + ag["vx"] * 1.5, ag["y"] + ag["vy"] * 1.5
d1 = (ag["x"] - px) * (ag["x"] - px) + (ag["y"] - py) * (ag["y"] - py)
d2 = (fx - px) * (fx - px) + (fy - py) * (fy - py)
if min(d1, d2) < 6.76:
obst.append((j - HALF - 1.5, 0.0))
break
if route == "left":
y0, _ = TURN["left"]
d_turn = (y0 + 70.0) - s_front
if d_turn > -1.0:
for ag in obs["agents"]:
if ag["kind"] == "car" and ag["vy"] < -0.5 and -4.0 < ag["y"] < 45.0 and ag["x"] < 0:
obst.append((d_turn - 0.5, 0.0))
break
q = v / 12.0
q4 = (q * q) * (q * q)
a = 1.5 * (1.0 - q4)
for gap, vo in obst:
gap = max(gap, 0.1)
sstar = 2.0 + v * 1.2 + v * (v - vo) / (2 * math.sqrt(1.5 * 3.0))
a = min(a, 1.5 * (1.0 - q4 - (sstar / gap) * (sstar / gap)))
a = max(-6.0, min(2.0, a))
return min(range(len(ACTIONS)), key=lambda i: (abs(ACTIONS[i] - a), i))
def features(obs):
v, s = obs["v"], obs["s"]
d_stop = S_STOP - (s + HALF)
f = [v / 14.0, obs["a"] / 6.0, min(max(d_stop, 0.0), 60.0) / 60.0, 1.0 if d_stop < 0 else 0.0,
1.0 if obs["light"] == 0 else 0.0, 1.0 if obs["light"] == 1 else 0.0, 1.0 if obs["light"] == 2 else 0.0,
min(obs["remaining"], 15.0) / 15.0]
lead = [ag for ag in obs["agents"] if ag["kind"] == "lead"]
if lead:
f += [min(max(lead[0]["s"] - s - 4.4, 0.0), 50.0) / 50.0, max(-1.0, min(1.0, (lead[0]["v"] - v) / 14.0))]
else:
f += [1.0, 0.0]
for acc in (0.0, -3.0, 1.0):
hit, _, _, _ = rollout_hits(obs, lambda k, tau, acc=acc: acc, horizon=5.0)
f.append((hit if hit is not None else 5.0) / 5.0)
ex, ey, _ = pose(obs["route"], s)
peds = [math.sqrt((ag["x"] - ex) * (ag["x"] - ex) + (ag["y"] - ey) * (ag["y"] - ey)) for ag in obs["agents"] if ag["kind"] == "ped"]
f.append(min(min(peds), 40.0) / 40.0 if peds else 1.0)
f.append(min(min(p["ahead"] for p in obs["phantoms"]), 40.0) / 40.0 if obs["phantoms"] else 1.0)
busy = any(ag["kind"] == "car" and abs(ag["x"]) < 12.0 and abs(ag["y"]) < 12.0 for ag in obs["agents"])
f.append(1.0 if busy else 0.0)
f += [1.0 if obs["route"] == r else 0.0 for r in ROUTES]
f.append(min(v * v / 12.0, 60.0) / 60.0)
f.append(1.0 if obs["light"] != 0 and d_stop > 0 and v * v / 6.0 < d_stop + 1.0 else 0.0)
return f
def learned(obs, layers):
z = forward(layers, np.asarray([features(obs)]))[0]
return int(np.argmax(z))
def shield(obs, i):
"""Safety check: keep action i only if braking hard after 0.5 s still avoids predicted collisions / red-light entry."""
def unsafe(a):
hit, red, _, _ = rollout_hits(obs, lambda k, tau: a if tau <= 0.5 + 1e-9 else -6.0, margin_car=0.2, margin_ped=0.4)
if hit is not None:
return True
if red is not None:
_, red_b, _, _ = rollout_hits(obs, lambda k, tau: -6.0, margin_car=0.2, margin_ped=0.4)
return red_b is None
return False
if not unsafe(ACTIONS[i]):
return i, False
for j in range(i - 1, -1, -1):
if not unsafe(ACTIONS[j]):
return j, True
return 0, True
# ------------------------------------------------------------------ episode
def run_episode(sc, driver, layers=None, record=True, timer=None, on_obs=None):
w = init_world(sc)
route = sc["route"]
goal = path_len(route) - 5.0
out = {"collision": False, "collision_with": None, "red_violations": 0, "reached": False, "hard_brakes": 0,
"overrides": 0, "steps": 0, "time": 0.0, "decision_ms": [], "trace": [], "events": [], "actions": []}
prev_a = 0.0
for step in range(int(round(T_MAX / DT))):
obs = observe(sc, w)
if on_obs is not None:
on_obs(obs)
t0 = timer() if timer else 0.0
override = False
if driver == "expert":
i = expert(obs)
elif driver == "rule":
i = rule_driver(obs)
elif driver == "learned":
i = learned(obs, layers)
else:
i, override = shield(obs, learned(obs, layers))
if timer:
out["decision_ms"].append(1000.0 * (timer() - t0))
a = ACTIONS[i]
out["actions"].append(i)
if override:
out["overrides"] += 1
if a <= -6.0 and prev_a > -6.0:
out["hard_brakes"] += 1
out["events"].append({"t": round(w["t"], 2), "type": "hard_brake"})
prev_a = a
s_front_prev = w["s"] + HALF
w["v"] = min(VMAX, max(0.0, w["v"] + a * DT))
w["s"] += w["v"] * DT
w["a"] = a
step_agents(sc, w)
w["t"] = (step + 1) * DT
if s_front_prev < S_STOP <= w["s"] + HALF and light_ns(sc, w["t"])[0] == 2:
out["red_violations"] += 1
out["events"].append({"t": round(w["t"], 2), "type": "red_light"})
ex, ey, eh = pose(route, w["s"])
ec = circles(ex, ey, eh)
hit = None
for c in w["cars"]:
if c["active"] and hits_car(ec, circles(*car_xyh(c))):
hit = "car"
if w["lead"] is not None and w["lead"]["s"] < path_len(route) and hits_car(ec, circles(*pose(route, w["lead"]["s"]))):
hit = "lead car"
for p in w["peds"]:
if not p["done"] and hits_ped(ec, p["x"], p["y"]):
hit = "pedestrian"
if record:
out["trace"].append([round(w["t"], 2), round(ex, 3), round(ey, 3), round(eh, 4), round(w["v"], 3), i, int(override),
[[round(v_, 2) for v_ in car_xyh(c)] for c in w["cars"] if c["active"]] +
([[round(v_, 2) for v_ in pose(route, w["lead"]["s"])]] if w["lead"] is not None and w["lead"]["s"] < path_len(route) else []),
[[round(p["x"], 2), round(p["y"], 2)] for p in w["peds"] if not p["done"]],
obs["visible"], light_ns(sc, w["t"])[0], light_ew(sc, w["t"])])
out["steps"] = step + 1
out["time"] = w["t"]
if hit:
out["collision"], out["collision_with"] = True, hit
out["events"].append({"t": round(w["t"], 2), "type": "collision", "with": hit})
break
if w["s"] >= goal:
out["reached"] = True
break
return out
# ------------------------------------------------------------------ batch jobs (used with multiprocessing)
def summarize(r):
return {k: r[k] for k in ("collision", "collision_with", "red_violations", "reached", "hard_brakes", "overrides", "steps", "time")}
def collect_job(job):
"""job = (seed, kind, density, driver, layers). Runs the driver and labels every visited state with the expert."""
seed, kind, density, driver, layers = job
X, y = [], []
def label(obs):
X.append(features(obs))
y.append(expert(obs))
r = run_episode(make_scenario(seed, kind, density), driver, layers, record=False, on_obs=label)
return X, y, summarize(r)
def eval_job(job):
seed, kind, density, driver, layers = job
sc = make_scenario(seed, kind, density)
r = run_episode(sc, driver, layers, record=False)
return {"seed": seed, "kind": kind, "density": density, "driver": driver, "route": sc["route"], **summarize(r),
"events": r["events"][-4:]}