feat(simulation): add Worker AI polygon runtime and terrain navigation

simulation/ai-polygon/navigation/footprint.py

@@ -0,0 +1,107 @@
+"""Metre-square swept-body check on CMU's observed terrain and chosen path.
+
+CMU's circular path table proposes paths. This final adapter check preserves
+the actual square chassis, including the initial turn, without widening it to
+its circumscribed circle for straight travel. No scene geometry enters here.
+"""
+
+import math
+
+import numpy as np
+
+MAX_STEP_M = 0.10
+FRAME_DEADLINE_SECONDS = 0.8
+
+
+def _obstacles(terrain, pose):
+    terrain = np.asarray(terrain, dtype=float)
+    obstacles = terrain[terrain[:, 3] > MAX_STEP_M + 1e-4, :2] - np.asarray(pose[:2])
+    x, y, z, w = pose[3:]
+    yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
+    return obstacles @ np.array([[math.cos(yaw), -math.sin(yaw)], [math.sin(yaw), math.cos(yaw)]])
+
+
+def _clearances(obstacles):
+    return np.maximum(np.abs(obstacles[:, 0]) - 0.5, np.abs(obstacles[:, 1]) - 0.5)
+
+
+def _clear(obstacles, position, angle):
+    delta = obstacles - position
+    c, s = math.cos(angle), math.sin(angle)
+    along = delta[:, 0] * c + delta[:, 1] * s
+    across = -delta[:, 0] * s + delta[:, 1] * c
+    initial = _clearances(obstacles)
+    if np.any(initial <= 0):
+        return False  # Never excuse an overlap of the actual metre-square body.
+    clearance = np.maximum(np.abs(along) - 0.5, np.abs(across) - 0.5)
+    # An observed point may already be inside the 5 cm reserve behind the body.
+    # Permit only motion that never decreases that initial clearance. This
+    # cannot authorize moving toward it, reversing into it or corner penetration.
+    return bool(np.all(clearance + 1e-6 >= np.minimum(initial, 0.05)))
+
+
+def command_footprint_clear(speed, yaw_rate, terrain, pose):
+    """Collision monitor over deadman latency plus a conservative braking arc.
+
+    CMU replans the route continuously. A later blocked corner must not prevent
+    safe progress on its prefix; this checks the command that can actually be
+    applied before the source-frame deadline, plus braking and 0.5 s reserve.
+    """
+    obstacles = _obstacles(terrain, pose)
+    horizon = FRAME_DEADLINE_SECONDS + 0.5 + abs(speed) / 0.4 + abs(yaw_rate) / 1.6
+    for t in np.arange(0, horizon + 0.025, 0.025):
+        angle = yaw_rate * t
+        position = (
+            np.array([speed * math.sin(angle) / yaw_rate, speed * (1 - math.cos(angle)) / yaw_rate])
+            if abs(yaw_rate) > 1e-6
+            else np.array([speed * t, 0])
+        )
+        if not _clear(obstacles, position, angle):
+            return False
+    return True
+
+
+def regulate_command(speed, yaw_rate, terrain, pose):
+    """Reduce speed along CMU's same arc when its full-speed stop is unsafe.
+
+    Scaling both components preserves curvature. The shortened stopping envelope
+    is a prefix of the original arc, so search for its largest admitted scale.
+    Never choose another turn/direction, ignore a hazard or creep arbitrarily.
+    """
+    if command_footprint_clear(speed, yaw_rate, terrain, pose):
+        return speed, yaw_rate, 1.0
+    low, high = 0.2, 1.0
+    if not command_footprint_clear(speed * low, yaw_rate * low, terrain, pose):
+        return 0.0, 0.0, 0.0
+    for _ in range(7):
+        middle = (low + high) / 2
+        if command_footprint_clear(speed * middle, yaw_rate * middle, terrain, pose):
+            low = middle
+        else:
+            high = middle
+    return speed * low, yaw_rate * low, low
+
+
+def swept_footprint_clear(path, terrain, pose):
+    path = np.asarray(path, dtype=float)[:, :2]
+    terrain = np.asarray(terrain, dtype=float)
+    if len(path) < 2 or terrain.ndim != 2 or terrain.shape[1] != 4:
+        return False
+    obstacles = _obstacles(terrain, pose)
+
+    previous_angle = 0.0
+    for start, end in zip(path[:-1], path[1:], strict=True):
+        delta = end - start
+        length = np.linalg.norm(delta)
+        if length < 1e-6:
+            continue
+        angle = math.atan2(delta[1], delta[0])
+        turn = math.atan2(math.sin(angle - previous_angle), math.cos(angle - previous_angle))
+        for fraction in np.linspace(0, 1, max(2, math.ceil(abs(turn) / 0.035) + 1)):
+            if not _clear(obstacles, start, previous_angle + fraction * turn):
+                return False
+        for fraction in np.linspace(0, 1, max(2, math.ceil(length / 0.025) + 1)):
+            if not _clear(obstacles, start + fraction * delta, angle):
+                return False
+        previous_angle = angle
+    return True