diff --git a/testbed/.gitignore b/testbed/.gitignore new file mode 100644 index 0000000..e1736e9 --- /dev/null +++ b/testbed/.gitignore @@ -0,0 +1,5 @@ +.venv/ +__pycache__/ +*.pyc +*.egg-info/ +.pytest_cache/ diff --git a/testbed/__init__.py b/testbed/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/testbed/room/__init__.py b/testbed/room/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/testbed/room/render.py b/testbed/room/render.py new file mode 100644 index 0000000..eec91da --- /dev/null +++ b/testbed/room/render.py @@ -0,0 +1,421 @@ +"""Headless first-person renderer for the testbed room. + +A small raycaster (Pillow-only, no GPU, no display) that produces an honest +frame: it derives every pixel from the actual world state in ``Room``. Used by +the ``vision`` and ``depth`` sensor tools. + +Camera model: yaw = heading around Y, pitch = camera tilt. 90° horizontal FOV. +""" + +from __future__ import annotations + +import math + +from PIL import Image + +from testbed.room.world import ROOM_HEIGHT, ROOM_SIZE, Box, Room + +# Palette +FOG = (14, 14, 20) +FLOOR_A = (56, 56, 62) +FLOOR_B = (64, 64, 70) +FLOOR_LINE = (42, 42, 48) +CEIL = (40, 40, 46) +SKY = (12, 16, 30) +WALL = (96, 96, 106) +WALL_TOP_BAND = (70, 70, 78) + +BEACON_OFF = ((0.85, 1.0, 1.0), (0.42, 0.82, 0.92), (0.10, 0.45, 0.62)) +BEACON_ON = ((1.0, 0.98, 0.78), (1.0, 0.75, 0.35), (0.78, 0.43, 0.0)) + +MAX_VIEW = 20.0 +_EPS = 1e-9 + + +class Raycaster: + """Renders the room from the capsule's camera. Stateless per frame.""" + + def __init__(self, width: int = 160, height: int = 120): + self.width = width + self.height = height + self.tan_fx = 1.0 # 90° horizontal FOV + self.tan_fy = self.tan_fx * height / width + + def render( + self, room: Room, max_depth: float = 10.0 + ) -> tuple[Image.Image, list[list[float]]]: + """Render the current view. Returns (RGB image, aligned depth grid).""" + w, h = self.width, self.height + cap = room.capsule + px = cap.x + pz = cap.z + eye = cap.eye_height + yaw = math.radians(cap.yaw_deg) + pitch = math.radians(cap.pitch_deg) + tan_pitch = math.tan(pitch) + + fx = math.sin(yaw) + fz = math.cos(yaw) + rx = math.cos(yaw) + rz = -math.sin(yaw) + + pixels = bytearray(w * h * 3) + depth = [[MAX_VIEW] * w for _ in range(h)] + + slabs = self._slabs(room) + + for c in range(w): + u = (2.0 * (c + 0.5) / w - 1.0) * self.tan_fx + dx = fx + rx * u + dz = fz + rz * u + inv = 1.0 / math.hypot(dx, dz) + dx *= inv + dz *= inv + + dist, surf = self._cast(px, pz, dx, dz, slabs) + + if surf is None: + t_wall = math.inf + s_coord = 0.0 + else: + t_wall = dist + s_coord = surf["s"] + + row_top = self._row_of(ROOM_HEIGHT, t_wall, eye, tan_pitch) + row_bot = self._row_of(0.0, t_wall, eye, tan_pitch) + r_top = max(0, int(math.ceil(min(row_top, row_bot)))) + r_bot = min(h, int(math.floor(max(row_top, row_bot)))) + + base = c * 3 + for r in range(h): + v = self.tan_fy * (1.0 - 2.0 * (r + 0.5) / h) + down = v + tan_pitch + o = (r * w + c) * 3 + if r_top <= r < r_bot and t_wall < MAX_VIEW: + col = self._shade_wall(surf, dist, s_coord) + if r == r_top: + col = _lerp(col, WALL_TOP_BAND, 0.55) + t = dist + elif down < -_EPS: + t = eye / -down + col = self._shade_floor(px + dx * t, pz + dz * t, t) + elif down > _EPS: + t = (ROOM_HEIGHT - eye) / down + col = _fog(CEIL, t) + else: + col = SKY + t = math.inf + if t < MAX_VIEW: + depth[r][c] = t + pixels[o] = col[0] + pixels[o + 1] = col[1] + pixels[o + 2] = col[2] + + self._draw_beacon(room, pixels, depth, fx, fz, rx, rz, eye, tan_pitch) + + img = Image.frombuffer("RGB", (w, h), bytes(pixels), "raw", "RGB", 0, 1) + grid = self._depth_grid(depth, max_depth) + return img, grid + + # ---------- helpers ---------- + + def _slabs(self, room: Room) -> list[dict]: + """Room walls as thin slabs + the obstacle boxes, all axis-aligned.""" + e = 0.01 + slabs: list[dict] = [] + slabs.append( + { + "x0": -e, + "x1": ROOM_SIZE + e, + "z0": -e, + "z1": e, + "kind": "wall", + "id": "wall_north", + "top": ROOM_HEIGHT, + "color": WALL, + "face": "z0", + } + ) + slabs.append( + { + "x0": -e, + "x1": ROOM_SIZE + e, + "z0": ROOM_SIZE - e, + "z1": ROOM_SIZE + e, + "kind": "wall", + "id": "wall_south", + "top": ROOM_HEIGHT, + "color": WALL, + "face": "z1", + } + ) + slabs.append( + { + "x0": -e, + "x1": e, + "z0": -e, + "z1": ROOM_SIZE + e, + "kind": "wall", + "id": "wall_west", + "top": ROOM_HEIGHT, + "color": WALL, + "face": "x0", + } + ) + slabs.append( + { + "x0": ROOM_SIZE - e, + "x1": ROOM_SIZE + e, + "z0": -e, + "z1": ROOM_SIZE + e, + "kind": "wall", + "id": "wall_east", + "top": ROOM_HEIGHT, + "color": WALL, + "face": "x1", + } + ) + for box in room.boxes: + slabs.append( + { + "x0": box.x_min(), + "x1": box.x_max(), + "z0": box.z_min(), + "z1": box.z_max(), + "kind": "box", + "id": box.id, + "top": box.height, + "color": box.color, + "face": "box", + "box": box, + } + ) + return slabs + + def _cast(self, ox: float, oz: float, dx: float, dz: float, slabs: list[dict]): + """Nearest 2D hit of the ray against all slabs. Returns (dist, surface).""" + best = math.inf + best_surf = None + for s in slabs: + t = _ray_aabb(ox, oz, dx, dz, s["x0"], s["x1"], s["z0"], s["z1"]) + if t is not None and t < best: + best = t + best_surf = s + if best_surf is None: + return best, None + hit_x = ox + dx * best + hit_z = oz + dz * best + face = best_surf["face"] + if face == "z0" or face == "z1": + s_coord = hit_x + elif face == "x0" or face == "x1": + s_coord = hit_z + else: + # Box: pick the face by comparing t to each plane. + s_coord = ( + hit_x if best_surf["box"].half_w <= best_surf["box"].half_d else hit_z + ) + return best, {"surf": best_surf, "s": s_coord, "hit_x": hit_x, "hit_z": hit_z} + + def _row_of(self, world_h: float, t: float, eye: float, tan_pitch: float) -> float: + """Screen row (float) where a height `world_h` at distance `t` lands.""" + if math.isinf(t): + return 0.0 if world_h > eye else float(self.height) + vv = (world_h - eye) / t - tan_pitch + return (1.0 - vv / self.tan_fy) / 2.0 * self.height + + def _shade_wall( + self, surf: dict | None, dist: float, s: float + ) -> tuple[int, int, int]: + if surf is None: + return SKY + s_def = surf["surf"] + color = s_def["color"] + face = s_def["face"] + shade = 1.0 + if face == "x0": + shade = 0.9 + elif face == "x1": + shade = 1.08 + elif face == "z1": + shade = 1.0 + elif face == "z0": + shade = 0.95 + # Concrete panel grid: subtle stripes every 1 m. + if int(abs(s) * 1.0) % 2 == 0: + shade *= 1.06 + col = ( + min(255, int(color[0] * shade)), + min(255, int(color[1] * shade)), + min(255, int(color[2] * shade)), + ) + return _fog(col, dist) + + def _shade_floor(self, wx: float, wz: float, t: float) -> tuple[int, int, int]: + cell = int(math.floor(wx)) + int(math.floor(wz)) + col = FLOOR_A if cell % 2 == 0 else FLOOR_B + fx = wx - math.floor(wx) + fz = wz - math.floor(wz) + if min(fx, 1.0 - fx, fz, 1.0 - fz) < 0.07: + col = FLOOR_LINE + return _fog(col, t) + + def _draw_beacon( + self, + room: Room, + pixels: bytearray, + depth: list[list[float]], + fx: float, + fz: float, + rx: float, + rz: float, + eye: float, + tan_pitch: float, + ) -> None: + """Billboard the beacon with a soft glow, occluded by the depth buffer.""" + w, h = self.width, self.height + b = room.beacon + rel_x = b.x - room.capsule.x + rel_z = b.z - room.capsule.z + along = rel_x * fx + rel_z * fz + if along < 0.25: + return + right = rel_x * rx + rel_z * rz + col_c = (right / along / self.tan_fx + 1.0) / 2.0 * w + vv = (b.height - eye) / along - tan_pitch + row_c = (1.0 - vv / self.tan_fy) / 2.0 * h + r_px = b.radius / along / self.tan_fx * w / 2.0 + if r_px < 1.2: + r_px = 1.2 + glow = r_px * 3.4 + + pulse = 1.0 + if b.active: + pulse = 0.9 + 0.1 * math.sin(room.tick * 0.6) + core, mid, outer = BEACON_ON if b.active else BEACON_OFF + core = tuple(pulse * v for v in core) + mid = tuple(pulse * v for v in mid) + outer = tuple(pulse * v for v in outer) + + c0 = max(0, int(col_c - glow)) + c1 = min(w, int(col_c + glow) + 1) + r0 = max(0, int(row_c - glow)) + r1 = min(h, int(row_c + glow) + 1) + inv_r2 = 1.0 / (r_px * r_px) + for r in range(r0, r1): + for c in range(c0, c1): + if depth[r][c] <= along - 0.06: + continue # occluded by a nearer surface + dc = c - col_c + dr = r - row_c + d2 = (dc * dc + dr * dr) * inv_r2 + if d2 > glow * glow * inv_r2: + continue + if d2 <= 1.0: + a = 1.0 - d2 * 0.55 + col = core + elif d2 <= 4.0: + f = (d2 - 1.0) / 3.0 + a = 0.85 * (1.0 - f) + col = tuple(core[i] + (mid[i] - core[i]) * f for i in range(3)) + else: + f = (d2 - 4.0) / (glow * glow * inv_r2 - 4.0) + a = 0.5 * (1.0 - f) + col = tuple(mid[i] + (outer[i] - mid[i]) * f for i in range(3)) + o = (r * w + c) * 3 + base = (pixels[o], pixels[o + 1], pixels[o + 2]) + out = ( + int(base[0] * (1 - a) + col[0] * 255 * a), + int(base[1] * (1 - a) + col[1] * 255 * a), + int(base[2] * (1 - a) + col[2] * 255 * a), + ) + pixels[o] = min(255, out[0]) + pixels[o + 1] = min(255, out[1]) + pixels[o + 2] = min(255, out[2]) + + def _depth_grid( + self, depth: list[list[float]], max_depth: float + ) -> list[list[float]]: + """Downsample the per-pixel depth to 1/4 resolution (40x30 at 160x120).""" + w, h = self.width, self.height + gw, gh = w // 4, h // 4 + grid = [[0.0] * gw for _ in range(gh)] + for r in range(gh): + for c in range(gw): + total = 0.0 + n = 0 + for rr in range(r * 4, r * 4 + 4): + row = depth[rr] + for cc in range(c * 4, c * 4 + 4): + t = row[cc] + if t >= MAX_VIEW or t > max_depth: + continue + total += t + n += 1 + grid[r][c] = round(total / n, 2) if n else 0.0 + return grid + + +def _ray_aabb( + ox: float, + oz: float, + dx: float, + dz: float, + x0: float, + x1: float, + z0: float, + z1: float, +) -> float | None: + """Slab test in 2D. Returns entry distance or None.""" + if abs(dx) < _EPS: + if ox < x0 - _EPS or ox > x1 + _EPS: + return None + tx0, tx1 = -math.inf, math.inf + else: + tx0, tx1 = (x0 - ox) / dx, (x1 - ox) / dx + if tx0 > tx1: + tx0, tx1 = tx1, tx0 + if abs(dz) < _EPS: + if oz < z0 - _EPS or oz > z1 + _EPS: + return None + tz0, tz1 = -math.inf, math.inf + else: + tz0, tz1 = (z0 - oz) / dz, (z1 - oz) / dz + if tz0 > tz1: + tz0, tz1 = tz1, tz0 + tin = max(tx0, tz0) + tout = min(tx1, tz1) + if tout < 0.0 or tin > tout: + return None + if tin > _EPS: + return tin + return None if tout <= _EPS else tout + + +def _fog(color: tuple[int, int, int], t: float) -> tuple[int, int, int]: + k = min(1.0, t / MAX_VIEW) + k = k**1.4 + return ( + int(color[0] + (FOG[0] - color[0]) * k), + int(color[1] + (FOG[1] - color[1]) * k), + int(color[2] + (FOG[2] - color[2]) * k), + ) + + +def _lerp( + a: tuple[int, int, int], b: tuple[int, int, int], k: float +) -> tuple[int, int, int]: + return ( + int(a[0] + (b[0] - a[0]) * k), + int(a[1] + (b[1] - a[1]) * k), + int(a[2] + (b[2] - a[2]) * k), + ) + + +def render_preview( + room: Room, path: str, width: int = 320, height: int = 240 +) -> Image.Image: + """Debug helper: render the current view and save it to a PNG file.""" + img, _ = Raycaster(width, height).render(room) + img.save(path) + return img diff --git a/testbed/room/world.py b/testbed/room/world.py new file mode 100644 index 0000000..bfdfcde --- /dev/null +++ b/testbed/room/world.py @@ -0,0 +1,343 @@ +"""Room world: the environment owned by the testbed bridge. + +Single source of truth for all world state. Nothing here is reachable by an +agent except through the sensor tools in ``testbed.bridge``. +""" + +from __future__ import annotations + +import math +from dataclasses import dataclass, field + +ROOM_SIZE = 16.0 +ROOM_HEIGHT = 3.0 + + +@dataclass +class Box: + """Axis-aligned box obstacle.""" + + id: str + cx: float + cz: float + half_w: float + half_d: float + height: float + color: tuple[int, int, int] + + def x_min(self) -> float: + return self.cx - self.half_w + + def x_max(self) -> float: + return self.cx + self.half_w + + def z_min(self) -> float: + return self.cz - self.half_d + + def z_max(self) -> float: + return self.cz + self.half_d + + +@dataclass +class Beacon: + """The single interactable object: a glowing pillar.""" + + id: str = "beacon" + x: float = 12.5 + z: float = 12.5 + height: float = 1.6 + radius: float = 0.35 + active: bool = False + activated_tick: int = -1 + reach: float = 1.6 + + +@dataclass +class Capsule: + """The agent's body: position, heading (yaw), camera pitch, velocity.""" + + x: float = 1.5 + z: float = 1.5 + yaw_deg: float = 45.0 + pitch_deg: float = 0.0 + radius: float = 0.35 + eye_height: float = 0.55 + health: float = 100.0 + speed: float = 0.0 + + def forward(self) -> tuple[float, float]: + """Unit vector in XZ plane along the current heading.""" + rad = math.radians(self.yaw_deg) + return math.sin(rad), math.cos(rad) + + +@dataclass +class Hit: + """A collision: what was hit, where, and with what force.""" + + other: str + normal_x: float + normal_z: float + impulse: float + + +class Room: + """A 16x16 room: floor, walls, a few boxes, one glowing beacon. + + Tick semantics: ``tick_mode`` is ``event`` — the world advances only when + a tool is called. Every tool call bumps ``tick`` by one, so observations + and events share a monotonic clock (protocol §8). + """ + + def __init__(self) -> None: + self.tick = 0 + self.capsule = Capsule() + self.beacon = Beacon() + self.boxes: list[Box] = [ + Box( + id="crate_red", + cx=6.0, + cz=6.0, + half_w=1.0, + half_d=1.0, + height=1.4, + color=(178, 64, 54), + ), + Box( + id="crate_blue", + cx=11.0, + cz=3.5, + half_w=0.9, + half_d=0.9, + height=1.6, + color=(64, 96, 178), + ), + Box( + id="crate_olive", + cx=4.0, + cz=11.0, + half_w=0.7, + half_d=0.7, + height=1.2, + color=(128, 128, 60), + ), + ] + # Pending audio events, drained by the hear sensor. + self._audio: list[dict] = [] + + # ---------- ticks ---------- + + def advance_tick(self) -> int: + """Bump the world clock; returns the new tick.""" + self.tick += 1 + return self.tick + + # ---------- audio ---------- + + def queue_audio(self, kind: str, direction_deg: float, intensity: float) -> None: + self._audio.append( + { + "kind": kind, + "direction_deg": round(direction_deg % 360.0, 1), + "intensity": round(intensity, 3), + } + ) + + def drain_audio(self) -> list[dict]: + sounds = self._audio + self._audio = [] + return sounds + + def hear_now(self) -> list[dict]: + """The hear sensor: queued events plus the beacon hum if in range.""" + sounds = self.drain_audio() + dx = self.beacon.x - self.capsule.x + dz = self.beacon.z - self.capsule.z + dist = math.hypot(dx, dz) + if dist <= 6.0: + bearing = math.degrees(math.atan2(dx, dz)) - self.capsule.yaw_deg + intensity = max(0.0, 1.0 - dist / 6.0) + if self.beacon.active: + intensity = min(1.0, intensity + 0.25) + sounds.append( + { + "kind": "beacon_hum", + "direction_deg": round(bearing % 360.0, 1), + "intensity": round(intensity, 3), + } + ) + return sounds + + # ---------- movement ---------- + + def move_forward(self, distance: float) -> tuple[float, Hit | None]: + """Push the capsule forward along its heading, resolving collisions. + + Moves in 0.1 m substeps. Returns (distance actually moved, hit or None). + """ + moved = 0.0 + hit: Hit | None = None + remaining = max(0.0, distance) + if remaining <= 0.0: + return 0.0, None + fx, fz = self.capsule.forward() + while remaining > 0.0 and hit is None: + step = min(0.1, remaining) + ok, h = self._try_displace(fx * step, fz * step) + if not ok and h is not None: + h.impulse = remaining + hit = h + break + moved += step + remaining -= step + self.capsule.speed = moved / 0.1 if moved > 0 else 0.0 + return moved, hit + + def _try_displace(self, dx: float, dz: float) -> tuple[bool, Hit | None]: + """Move by (dx, dz) with circle-vs-AABB resolution. Returns (moved, hit).""" + cap = self.capsule + nx = cap.x + dx + nz = cap.z + dz + + # Room walls: clamp to bounds (the capsule cannot leave the room). + r = cap.radius + if nx < r or nx > ROOM_SIZE - r or nz < r or nz > ROOM_SIZE - r: + hit = self._wall_hit(nx, nz) + return False, hit + + blocked: Hit | None = None + for _ in range(8): + contact = self._box_contact(nx, nz, r) + if contact is None: + break + nx, nz, normal_x, normal_z, other = contact + blocked = Hit( + other=other, normal_x=normal_x, normal_z=normal_z, impulse=0.0 + ) + if blocked is not None: + return False, blocked + + cap.x = nx + cap.z = nz + return True, None + + def _wall_hit(self, nx: float, nz: float) -> Hit: + r = self.capsule.radius + if nx < r: + return Hit(other="wall_west", normal_x=1.0, normal_z=0.0, impulse=0.0) + if nx > ROOM_SIZE - r: + return Hit(other="wall_east", normal_x=-1.0, normal_z=0.0, impulse=0.0) + if nz < r: + return Hit(other="wall_north", normal_x=0.0, normal_z=1.0, impulse=0.0) + return Hit(other="wall_south", normal_x=0.0, normal_z=-1.0, impulse=0.0) + + def _box_contact( + self, cx: float, cz: float, r: float + ) -> tuple[float, float, float, float, str] | None: + """If the circle at (cx, cz) overlaps a box, push it out and report contact.""" + for box in self.boxes: + min_x, max_x = box.x_min(), box.x_max() + min_z, max_z = box.z_min(), box.z_max() + near_x = min(max(cx, min_x), max_x) + near_z = min(max(cz, min_z), max_z) + dx = cx - near_x + dz = cz - near_z + d2 = dx * dx + dz * dz + if d2 >= r * r: + continue + if d2 > 1e-12: + d = math.sqrt(d2) + push = (r - d) / d + return ( + cx + dx * push, + cz + dz * push, + dx / d, + dz / d, + box.id, + ) + # Center inside the box: push along the axis of least penetration. + ox = min(cx - min_x + r, max_x - cx + r) + oz = min(cz - min_z + r, max_z - cz + r) + if ox < oz: + nx = cx + ox if cx < box.cx else cx - ox + return (nx, cz, 1.0 if cx < box.cx else -1.0, 0.0, box.id) + nz = cz + oz if cz < box.cz else cz - oz + return (cx, nz, 0.0, 1.0 if cz < box.cz else -1.0, box.id) + return None + + def turn(self, yaw_delta: float = 0.0, pitch_delta: float = 0.0) -> None: + cap = self.capsule + cap.yaw_deg = (cap.yaw_deg + yaw_delta) % 360.0 + cap.pitch_deg = min(85.0, max(-85.0, cap.pitch_deg + pitch_delta)) + + def face(self, yaw_deg: float, pitch_deg: float) -> None: + self.capsule.yaw_deg = yaw_deg % 360.0 + self.capsule.pitch_deg = min(85.0, max(-85.0, pitch_deg)) + + def look_at_beacon(self) -> tuple[float, float]: + """Orient camera toward the beacon. Returns (yaw_deg, pitch_deg).""" + cap = self.capsule + dx = self.beacon.x - cap.x + dz = self.beacon.z - cap.z + dist = math.hypot(dx, dz) or 1.0 + yaw = math.degrees(math.atan2(dx, dz)) + pitch = math.degrees(math.atan2(self.beacon.height - cap.eye_height, dist)) + self.face(yaw, pitch) + return yaw, pitch + + def distance_to_beacon(self) -> float: + return math.hypot( + self.beacon.x - self.capsule.x, self.beacon.z - self.capsule.z + ) + + def interact_beacon(self) -> tuple[bool, str]: + """Try to activate the beacon. Returns (success, message).""" + dist = self.distance_to_beacon() + if dist > self.beacon.reach: + return ( + False, + f"too far: {dist:.1f} m from the beacon (need <= {self.beacon.reach} m)", + ) + if self.beacon.active: + return True, "the beacon is already active and glowing warm yellow" + self.beacon.active = True + self.beacon.activated_tick = self.tick + return True, "the beacon lights up: a warm yellow glow floods the room" + + # ---------- introspection helpers ---------- + + def surface_distances(self, x: float, z: float) -> dict[str, float]: + """Distances to the nearest wall on each side (used by demo steering).""" + return { + "west": x, + "east": ROOM_SIZE - x, + "north": z, + "south": ROOM_SIZE - z, + } + + def describe(self) -> dict: + return { + "room": { + "name": "testbed_room_01", + "width": ROOM_SIZE, + "depth": ROOM_SIZE, + "height": ROOM_HEIGHT, + }, + "obstacles": [ + { + "id": b.id, + "x": round(b.cx, 2), + "z": round(b.cz, 2), + "width": round(b.half_w * 2, 2), + "depth": round(b.half_d * 2, 2), + "height": round(b.height, 2), + } + for b in self.boxes + ], + "beacon": { + "id": self.beacon.id, + "x": round(self.beacon.x, 2), + "z": round(self.beacon.z, 2), + "height": round(self.beacon.height, 2), + }, + }