Next-Passage / passage_core.py
Lamapi's picture
Publish Next-Passage model
051c46f verified
Raw History Blame Contribute Delete
11.1 kB
"""Next Passage: time-constrained grid navigation with periodic doors and moving obstacles.
Simulation only. One cell per step (4-neighbour moves or wait).
* Doors open and close on a known periodic schedule.
* Obstacles patrol back and forth along corridors and stall at random (probability p_stall per step),
so their future positions are only approximately predictable. They do not avoid the robot.
Planners
* static : BFS shortest path on the static map (doors treated as open); waits if the next cell is blocked now.
* reactive : like static, but re-runs BFS around currently blocked cells whenever it is blocked.
* timed : Next Passage. Space-time A* over (cell, time) with wait actions, using the door schedule and
predicted obstacle motion (cells occupied at t and t-1 are avoided). Replans when an observed
obstacle deviates from prediction or the next step became invalid.
A line-by-line JavaScript port lives in web/core.js; parity is tested on fixed seeds.
"""
from nlrt import Mulberry32
W, H = 21, 15
MOVES = [(0, 0), (1, 0), (-1, 0), (0, 1), (0, -1)]
DENSITY = {"low": 3, "medium": 6, "high": 10}
class Heap:
"""Binary min-heap on tuples; identical algorithm in JS so pop order matches exactly."""
def __init__(self):
self.a = []
def push(self, item):
a = self.a
a.append(item)
i = len(a) - 1
while i > 0:
p = (i - 1) // 2
if a[p] <= a[i]:
break
a[p], a[i] = a[i], a[p]
i = p
def pop(self):
a = self.a
top = a[0]
last = a.pop()
if a:
a[0] = last
i, n = 0, len(a)
while True:
l, r, m = 2 * i + 1, 2 * i + 2, i
if l < n and a[l] < a[m]:
m = l
if r < n and a[r] < a[m]:
m = r
if m == i:
break
a[m], a[i] = a[i], a[m]
i = m
return top
def __len__(self):
return len(self.a)
def make_world(seed, density="medium", p_stall=0.15):
rng = Mulberry32(seed * 2654435761 % 4294967296)
walls = set()
for x in range(W):
walls.add((x, 0)); walls.add((x, H - 1))
for y in range(H):
walls.add((0, y)); walls.add((W - 1, y))
doors = []
for wx in (5, 10, 15):
gap_open = 1 + rng.randint(H - 2)
gap_door = 1 + rng.randint(H - 2)
while abs(gap_door - gap_open) < 3:
gap_door = 1 + rng.randint(H - 2)
for y in range(1, H - 1):
if y not in (gap_open, gap_door):
walls.add((wx, y))
period = 8 + rng.randint(8)
doors.append({"cell": [wx, gap_door], "period": period, "phase": rng.randint(period),
"closed": max(2, period // 2 - 1 + rng.randint(3))})
for _ in range(6): # pillars
x, y = 2 + rng.randint(W - 4), 2 + rng.randint(H - 4)
if x not in (4, 5, 6, 9, 10, 11, 14, 15, 16):
walls.add((x, y))
start, goal = [1, 1 + rng.randint(H - 2)], [W - 2, 1 + rng.randint(H - 2)]
walls.discard(tuple(start)); walls.discard(tuple(goal))
door_cells = {tuple(d["cell"]) for d in doors}
obstacles, used = [], set()
tries = 0
while len(obstacles) < DENSITY[density] and tries < 400:
tries += 1
horiz = rng.random() < 0.5
x, y = 1 + rng.randint(W - 2), 1 + rng.randint(H - 2)
length = 4 + rng.randint(7)
cells = []
for k in range(length):
c = (x + k, y) if horiz else (x, y + k)
if c in walls or c in door_cells or c in used or c[0] >= W - 1 or c[1] >= H - 1 \
or list(c) == start or list(c) == goal:
break
cells.append(c)
if len(cells) < 3:
continue
loop = cells + cells[-2:0:-1]
used.update(cells)
obstacles.append({"loop": [list(c) for c in loop], "offset": rng.randint(len(loop))})
return {"seed": seed, "density": density, "p_stall": p_stall, "walls": sorted(list(c) for c in walls),
"doors": doors, "obstacles": obstacles, "start": start, "goal": goal}
def door_closed(d, t):
return (t + d["phase"]) % d["period"] < d["closed"]
def free(world_walls, c):
return 0 <= c[0] < W and 0 <= c[1] < H and c not in world_walls
def bfs(walls, start, goal, blocked=frozenset()):
"""Return (path, expanded). Neighbour order follows MOVES[1:]."""
start, goal = tuple(start), tuple(goal)
prev = {start: None}
q, qi, expanded = [start], 0, 0
while qi < len(q):
c = q[qi]; qi += 1; expanded += 1
if c == goal:
path = []
while c is not None:
path.append(c); c = prev[c]
return path[::-1], expanded
for dx, dy in MOVES[1:]:
n = (c[0] + dx, c[1] + dy)
if n not in prev and free(walls, n) and n not in blocked:
prev[n] = c
q.append(n)
return None, expanded
def distance_map(walls, goal):
goal = tuple(goal)
dist = {goal: 0}
q, qi = [goal], 0
while qi < len(q):
c = q[qi]; qi += 1
for dx, dy in MOVES[1:]:
n = (c[0] + dx, c[1] + dy)
if n not in dist and free(walls, n):
dist[n] = dist[c] + 1
q.append(n)
return dist
def timed_astar(walls, doors, dist, start, t0, goal, obs_idx, loops, horizon=160, max_expand=60000):
"""Space-time A*. obs_idx: current loop index of each obstacle (prediction assumes no stalls)."""
goal = tuple(goal)
door_at = {tuple(d["cell"]): d for d in doors}
def occupied(t):
cells = set()
for i, L in enumerate(loops):
for tt in (t, t - 1):
if tt >= t0:
cells.add(tuple(L[(obs_idx[i] + tt - t0) % len(L)]))
return cells
occ_cache = {}
start = tuple(start)
if start not in dist:
return None, 0
heap = Heap()
seq = 0
heap.push((dist[start], 0, seq, start[0], start[1], t0))
parent = {(start, t0): None}
expanded = 0
while len(heap) and expanded < max_expand:
f, negg, _, x, y, t = heap.pop()
c = (x, y)
expanded += 1
if c == goal:
path, node = [], (c, t)
while node is not None:
path.append(list(node[0])); node = parent[node]
return path[::-1], expanded
if t - t0 >= horizon:
continue
nt = t + 1
if nt not in occ_cache:
occ_cache[nt] = occupied(nt)
occ = occ_cache[nt]
for dx, dy in MOVES:
n = (x + dx, y + dy)
if not free(walls, n) or n in occ or n not in dist:
continue
d = door_at.get(n)
if d is not None and door_closed(d, nt):
continue
key = (n, nt)
if key in parent:
continue
parent[key] = (c, t)
seq += 1
g = nt - t0
heap.push((g + dist[n], -g, seq, n[0], n[1], nt))
return None, expanded
def run_episode(world, planner, max_steps=None):
walls = {tuple(c) for c in world["walls"]}
doors = world["doors"]
door_at = {tuple(d["cell"]): d for d in doors}
loops = [o["loop"] for o in world["obstacles"]]
idx = [o["offset"] % len(o["loop"]) for o in world["obstacles"]]
dist = distance_map(walls, world["goal"])
static_path, nodes = bfs(walls, world["start"], world["goal"])
max_steps = max_steps or 3 * len(static_path) + 60
rng = Mulberry32(world["seed"] * 7 + 3)
pos, t = tuple(world["start"]), 0
goal = tuple(world["goal"])
plan, plan_i, replans, collisions, door_hits, hard_waits = list(static_path), 0, 0, 0, 0, 0
pred = list(idx)
trace = [{"t": 0, "robot": list(pos), "obs": [loops[i][idx[i]] for i in range(len(loops))]}]
events = []
if planner == "timed":
plan, n = timed_astar(walls, doors, dist, pos, 0, goal, idx, loops)
nodes += n; replans += 1; plan_i = 0
arrived = None
while t < max_steps:
if pos == goal:
arrived = t
break
obs_now = {tuple(loops[i][idx[i]]) for i in range(len(loops))}
nxt = pos
if planner in ("static", "reactive"):
if plan is not None and plan_i + 1 < len(plan):
cand = tuple(plan[plan_i + 1])
d = door_at.get(cand)
blocked = cand in obs_now or (d is not None and door_closed(d, t))
if not blocked:
nxt = cand
plan_i += 1
elif planner == "reactive":
block = set(obs_now) | {tuple(dd["cell"]) for dd in doors if door_closed(dd, t)}
newp, n = bfs(walls, pos, goal, frozenset(block - {pos}))
nodes += n; replans += 1
if newp is not None and len(newp) > 1:
plan, plan_i = newp, 1
nxt = tuple(newp[1])
else:
hard_waits += 1
else:
hard_waits += 1
else:
deviated = any(idx[i] != pred[i] for i in range(len(loops)))
invalid = plan is None or plan_i + 1 >= len(plan)
if deviated or invalid:
plan, n = timed_astar(walls, doors, dist, pos, t, goal, idx, loops)
nodes += n; replans += 1; plan_i = 0
pred = list(idx)
if plan is not None and plan_i + 1 < len(plan):
nxt = tuple(plan[plan_i + 1]); plan_i += 1
else:
hard_waits += 1
# obstacles move (stall with probability p_stall), then robot moves
old = [tuple(loops[i][idx[i]]) for i in range(len(loops))]
for i in range(len(loops)):
if rng.random() >= world["p_stall"]:
idx[i] = (idx[i] + 1) % len(loops[i])
pred[i] = (pred[i] + 1) % len(loops[i])
t += 1
new = [tuple(loops[i][idx[i]]) for i in range(len(loops))]
hit = any(new[i] == nxt or (old[i] == nxt and new[i] == pos) for i in range(len(loops)))
prev_pos, pos = pos, nxt
if hit:
collisions += 1
events.append({"t": t, "type": "collision", "cell": list(pos)})
d = door_at.get(pos)
if d is not None and door_closed(d, t):
door_hits += 1
events.append({"t": t, "type": "door_closed_on_robot", "cell": list(pos)})
trace.append({"t": t, "robot": list(pos), "obs": [list(c) for c in new]})
if arrived is None and pos == goal:
arrived = t
return {"arrived": arrived is not None, "arrival_time": arrived, "collisions": collisions, "door_hits": door_hits,
"replans": replans, "nodes_expanded": nodes, "waits_blocked": hard_waits, "static_length": len(static_path) - 1,
"max_steps": max_steps, "events": events, "trace": trace}