Checkpoint 7

herding/perception/lidar_sim.py

@@ -1,16 +1,12 @@
 """Fast 2D LiDAR simulator for the Gymnasium env.
 
-Raycasts against:
-  * **Sheep** — discs of radius ``SHEEP_RADIUS``.
-  * **Static world geometry** — axis-aligned wall segments and gate
-    posts taken from ``worlds/field.wbt``. Without these, demos
-    collected in-env would never include the false-positive clusters
-    Webots produces from the stone walls and gate-post boxes, and the
-    BC student trained on those demos collapses on deployment.
+Raycasts against sheep (discs) and static world geometry (axis-aligned
+walls + gate posts) so the env reproduces the false-positive cluster
+distribution Webots produces from real 3D geometry.
 
-Returns a range array matching the Webots Lidar device on the dog
-(see ``protos/ShepherdDog.proto``: 180 rays, 140° FOV centred on
-forward, 12 m max range, 5 mm noise).
+Returns a range array matching the Webots Lidar device:
+180 rays, 140° FOV centred on forward, 12 m max range, 5 mm noise.
+See ``protos/ShepherdDog.proto``.
 """
 
 from __future__ import annotations
@@ -26,19 +22,13 @@ LIDAR_FOV = 2.44       # rad ≈ 140°
 LIDAR_MAX_RANGE = 12.0
 LIDAR_NOISE = 0.005    # m, gaussian std
 
-# Sheep modelled as a vertical cylinder; this is the horizontal-section
-# radius the LiDAR plane intersects. Tuned to the proto sheep (~0.45 m
-# body length). The exact value is not load-bearing — the perception
-# clusterer is range-tolerant.
+# Sheep cross-section in the LiDAR plane (horizontal cylinder approx).
 SHEEP_RADIUS = 0.30
 
 
-# ---------------------------------------------------------------------------
-# Static world geometry — must match worlds/field.wbt
-# ---------------------------------------------------------------------------
+# --- Static world geometry — mirrors worlds/field.wbt ---
 
-# Vertical walls: (x, y_min, y_max). Field east/west walls and the two
-# pen side walls are visible through the open gate.
+# Vertical walls: (x, y_min, y_max).
 _VERTICAL_WALLS = (
     ( 15.0, -15.0,  15.0),  # field east
     (-15.0, -15.0,  15.0),  # field west
@@ -46,8 +36,7 @@ _VERTICAL_WALLS = (
     ( 13.0, -22.0, -15.0),  # pen east
 )
 
-# Horizontal walls: (y, x_min, x_max). South wall is split by the 3 m
-# gate at x ∈ [10, 13]; the pen south wall closes the back of the pen.
+# Horizontal walls: (y, x_min, x_max). South wall has a 3 m gap at the gate.
 _HORIZONTAL_WALLS = (
     ( 15.0, -15.0,  15.0),  # field north
     (-15.0, -15.0,  10.0),  # field south-west of gate
@@ -55,31 +44,23 @@ _HORIZONTAL_WALLS = (
     (-22.0,  10.0,  13.0),  # pen south
 )
 
-# Gate posts and field corner pillars treated as vertical cylinders at
-# LiDAR height. Radius 0.25 m comes from the 0.44 × 0.44 m boxes in the
-# wbt — close enough to a circular cross-section for this purpose.
+# Gate posts + field corner pillars, treated as discs at LiDAR height.
 _POSTS_XY = np.array([
-    ( 10.0, -15.0),  # west gate post
-    ( 13.0, -15.0),  # east gate post
-    ( 15.0,  15.0),  # NE field corner
-    ( 15.0, -15.0),  # SE field corner
-    (-15.0,  15.0),  # NW field corner
-    (-15.0, -15.0),  # SW field corner
+    ( 10.0, -15.0), ( 13.0, -15.0),
+    ( 15.0,  15.0), ( 15.0, -15.0),
+    (-15.0,  15.0), (-15.0, -15.0),
 ], dtype=np.float64)
 POST_RADIUS = 0.25
 
 
 def ray_angles(n: int = LIDAR_N_RAYS, fov: float = LIDAR_FOV) -> np.ndarray:
-    """Local-frame ray angles, sweeping from +fov/2 to -fov/2.
+    """Local-frame ray angles, CCW from forward, sweeping +fov/2 → -fov/2.
 
-    Convention: angle is measured CCW from the dog's forward axis. Ray 0
-    points to the dog's left, last ray to the right. Webots' default
-    Lidar sweep matches this.
+    Matches Webots' default Lidar sweep direction.
     """
     return np.linspace(fov / 2.0, -fov / 2.0, n, dtype=np.float64)
 
 
-# Cached so we don't rebuild every step.
 _ANGLES = ray_angles()
 _COS = np.cos(_ANGLES)
 _SIN = np.sin(_ANGLES)
@@ -88,13 +69,7 @@ _SIN = np.sin(_ANGLES)
 def _raycast_static(
     ox: float, oy: float, cos_w: np.ndarray, sin_w: np.ndarray,
 ) -> np.ndarray:
-    """Per-ray distance to nearest wall or post hit (∞ if none).
-
-    Walls are axis-aligned line segments; for each ray we compute t at
-    which it crosses the wall's constant-coord plane and check the
-    other coord lies in the segment. Posts are circles; same disc
-    intersection as for sheep.
-    """
+    """Per-ray distance to the nearest wall or post hit (∞ if none)."""
     n_rays = cos_w.shape[0]
     best = np.full(n_rays, np.inf, dtype=np.float64)
 
@@ -144,10 +119,7 @@ def simulate_scan(
 ) -> np.ndarray:
     """Return a (N,) float32 range array. No-hit entries equal ``max_range``.
 
-    ``sheep_xy`` is the list of (x, y) world positions of every sheep in
-    the scene (penned and active). Static world geometry (walls and
-    posts) is also raycast so demos contain the same false-positive
-    clusters Webots produces.
+    ``sheep_xy`` is every sheep (penned or active) in the scene.
     """
     n_rays = _ANGLES.shape[0]
 
@@ -172,8 +144,7 @@ def simulate_scan(
         nearest = candidate.min(axis=0)
         np.minimum(best, nearest, out=best)
 
-    # Clip to LIDAR_MAX_RANGE; entries that never got a hit stay at inf
-    # → clipped down to max_range like the real Webots device.
+    # Entries with no hit stay at inf → clipped to max_range, matching Webots.
     ranges = np.minimum(best, max_range).astype(np.float32)
     return _add_noise(ranges, noise, rng, max_range)