Checkpoint 8
This commit is contained in:
Johnny Fernandes
+101
-30
@@ -1,7 +1,8 @@
|
||||
"""Fast 2D LiDAR simulator for the Gymnasium env.
|
||||
|
||||
Raycasts against sheep (discs) and static world geometry (axis-aligned
|
||||
walls + gate posts) so the env reproduces the false-positive cluster
|
||||
Raycasts against sheep (discs) and static world geometry. For rectangular
|
||||
fields this is axis-aligned walls + gate posts; for round fields it is a
|
||||
circular wall + gate posts. The env reproduces the false-positive cluster
|
||||
distribution Webots produces from real 3D geometry.
|
||||
|
||||
Returns a range array matching the Webots Lidar device:
|
||||
@@ -15,49 +16,96 @@ import math
|
||||
|
||||
import numpy as np
|
||||
|
||||
from herding.world.geometry import (
|
||||
FIELD_SHAPE, FIELD_ROUND_R,
|
||||
FIELD_X, FIELD_Y,
|
||||
GATE_X, GATE_Y,
|
||||
PEN_X, PEN_Y,
|
||||
)
|
||||
|
||||
# Match protos/ShepherdDog.proto Lidar device.
|
||||
LIDAR_N_RAYS = 180
|
||||
LIDAR_FOV = 2.44 # rad ≈ 140°
|
||||
|
||||
# Match protos/ShepherdDog.proto Lidar device — extended to 360° for
|
||||
# full situational awareness. The original Webots device is 140° FOV /
|
||||
# 180 rays; we use 360 rays for full-circle coverage.
|
||||
LIDAR_N_RAYS = 360
|
||||
LIDAR_FOV = 2.0 * math.pi # 360° full circle
|
||||
LIDAR_MAX_RANGE = 12.0
|
||||
LIDAR_NOISE = 0.005 # m, gaussian std
|
||||
|
||||
# Sheep cross-section in the LiDAR plane (horizontal cylinder approx).
|
||||
SHEEP_RADIUS = 0.30
|
||||
POST_RADIUS = 0.25
|
||||
|
||||
|
||||
# --- Static world geometry — mirrors worlds/field.wbt ---
|
||||
|
||||
# Vertical walls: (x, y_min, y_max).
|
||||
_VERTICAL_WALLS = (
|
||||
# ---------------------------------------------------------------------------
|
||||
# Rectangular-field static geometry
|
||||
# ---------------------------------------------------------------------------
|
||||
_VERTICAL_WALLS_RECT = (
|
||||
( 15.0, -15.0, 15.0), # field east
|
||||
(-15.0, -15.0, 15.0), # field west
|
||||
( 10.0, -22.0, -15.0), # pen west
|
||||
( 13.0, -22.0, -15.0), # pen east
|
||||
)
|
||||
|
||||
# Horizontal walls: (y, x_min, x_max). South wall has a 3 m gap at the gate.
|
||||
_HORIZONTAL_WALLS = (
|
||||
_HORIZONTAL_WALLS_RECT = (
|
||||
( 15.0, -15.0, 15.0), # field north
|
||||
(-15.0, -15.0, 10.0), # field south-west of gate
|
||||
(-15.0, 13.0, 15.0), # field south-east of gate
|
||||
(-22.0, 10.0, 13.0), # pen south
|
||||
)
|
||||
|
||||
# Gate posts + field corner pillars, treated as discs at LiDAR height.
|
||||
_POSTS_XY = np.array([
|
||||
_POSTS_RECT = np.array([
|
||||
( 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
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Round-field static geometry
|
||||
# ---------------------------------------------------------------------------
|
||||
# Circular wall with gate gap. Gate posts at the edges of the gate gap.
|
||||
_gate_cx = 0.5 * (GATE_X[0] + GATE_X[1])
|
||||
_POSTS_ROUND = np.array([
|
||||
(GATE_X[0], GATE_Y),
|
||||
(GATE_X[1], GATE_Y),
|
||||
], dtype=np.float64)
|
||||
|
||||
# Pen walls for round field
|
||||
_VERTICAL_WALLS_ROUND = (
|
||||
(GATE_X[0], PEN_Y[0], GATE_Y), # pen west
|
||||
(GATE_X[1], PEN_Y[0], GATE_Y), # pen east
|
||||
)
|
||||
_HORIZONTAL_WALLS_ROUND = (
|
||||
(PEN_Y[0], GATE_X[0], GATE_X[1]), # pen south
|
||||
)
|
||||
|
||||
|
||||
def _build_static_geometry():
|
||||
"""Select the correct static geometry for the active field shape."""
|
||||
if FIELD_SHAPE == "field_round":
|
||||
return (
|
||||
_VERTICAL_WALLS_ROUND,
|
||||
_HORIZONTAL_WALLS_ROUND,
|
||||
_POSTS_ROUND,
|
||||
FIELD_ROUND_R,
|
||||
)
|
||||
return (
|
||||
_VERTICAL_WALLS_RECT,
|
||||
_HORIZONTAL_WALLS_RECT,
|
||||
_POSTS_RECT,
|
||||
None, # no circular wall
|
||||
)
|
||||
|
||||
|
||||
_VERTS, _HORIZS, _POSTS, _CIRC_R = _build_static_geometry()
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Ray helpers
|
||||
# ---------------------------------------------------------------------------
|
||||
def ray_angles(n: int = LIDAR_N_RAYS, fov: float = LIDAR_FOV) -> np.ndarray:
|
||||
"""Local-frame ray angles, CCW from forward, sweeping +fov/2 → -fov/2.
|
||||
|
||||
Matches Webots' default Lidar sweep direction.
|
||||
"""
|
||||
"""Local-frame ray angles, CCW from forward, sweeping +fov/2 → -fov/2."""
|
||||
return np.linspace(fov / 2.0, -fov / 2.0, n, dtype=np.float64)
|
||||
|
||||
|
||||
@@ -78,7 +126,7 @@ def _raycast_static(
|
||||
safe_sin = np.where(np.abs(sin_w) < 1e-9, 1e-9, sin_w)
|
||||
|
||||
# Vertical walls (x = const)
|
||||
for wx, ymin, ymax in _VERTICAL_WALLS:
|
||||
for wx, ymin, ymax in _VERTS:
|
||||
t = (wx - ox) / safe_cos
|
||||
y_at = oy + t * sin_w
|
||||
valid = (t > EPS) & (y_at >= ymin - EPS) & (y_at <= ymax + EPS)
|
||||
@@ -86,19 +134,47 @@ def _raycast_static(
|
||||
np.minimum(best, cand, out=best)
|
||||
|
||||
# Horizontal walls (y = const)
|
||||
for wy, xmin, xmax in _HORIZONTAL_WALLS:
|
||||
for wy, xmin, xmax in _HORIZS:
|
||||
t = (wy - oy) / safe_sin
|
||||
x_at = ox + t * cos_w
|
||||
valid = (t > EPS) & (x_at >= xmin - EPS) & (x_at <= xmax + EPS)
|
||||
cand = np.where(valid, t, np.inf)
|
||||
np.minimum(best, cand, out=best)
|
||||
|
||||
# Circular wall (round field only)
|
||||
if _CIRC_R is not None:
|
||||
# Ray: P(t) = O + t·D. ||P(t)||² = R²
|
||||
# t² - 2t(O·D) + (||O||² - R²) = 0
|
||||
# a = 1 (rays are unit), b = -2(O·D), c = ||O||² - R²
|
||||
a = 1.0 # cos_w² + sin_w² = 1
|
||||
b = -(ox * cos_w + oy * sin_w)
|
||||
c = ox * ox + oy * oy - _CIRC_R * _CIRC_R
|
||||
disc = b * b - a * c
|
||||
valid_disc = disc >= 0.0
|
||||
sqrt_disc = np.sqrt(np.maximum(disc, 0.0))
|
||||
# Two intersection candidates: t = (-b ± sqrt(disc)) / a
|
||||
t1 = -b - sqrt_disc
|
||||
t2 = -b + sqrt_disc
|
||||
# We want the smallest positive t.
|
||||
t1_valid = valid_disc & (t1 > EPS)
|
||||
t2_valid = valid_disc & (t2 > EPS)
|
||||
t_circ = np.where(t1_valid, t1, np.where(t2_valid, t2, np.inf))
|
||||
|
||||
# Exclude rays that hit the gate gap: the hit point must not lie
|
||||
# in the gate column (between GATE_X and above GATE_Y).
|
||||
hx = ox + t_circ * cos_w
|
||||
hy = oy + t_circ * sin_w
|
||||
in_gate = ((hx > GATE_X[0]) & (hx < GATE_X[1]) &
|
||||
(hy > GATE_Y - 2.0) & (hy < GATE_Y + 2.0))
|
||||
t_circ = np.where(in_gate, np.inf, t_circ)
|
||||
np.minimum(best, t_circ, out=best)
|
||||
|
||||
# Posts (treat as discs)
|
||||
if _POSTS_XY.size:
|
||||
px = _POSTS_XY[:, 0] - ox
|
||||
py = _POSTS_XY[:, 1] - oy
|
||||
t_post = np.outer(px, cos_w) + np.outer(py, sin_w) # (P, N)
|
||||
d2 = (px ** 2 + py ** 2)[:, None] # (P, 1)
|
||||
if _POSTS.size:
|
||||
px = _POSTS[:, 0] - ox
|
||||
py = _POSTS[:, 1] - oy
|
||||
t_post = np.outer(px, cos_w) + np.outer(py, sin_w)
|
||||
d2 = (px ** 2 + py ** 2)[:, None]
|
||||
perp2 = d2 - t_post ** 2
|
||||
R2 = POST_RADIUS ** 2
|
||||
hit = (perp2 < R2) & (t_post > 0.0)
|
||||
@@ -121,16 +197,12 @@ def simulate_scan(
|
||||
|
||||
``sheep_xy`` is every sheep (penned or active) in the scene.
|
||||
"""
|
||||
n_rays = _ANGLES.shape[0]
|
||||
|
||||
ch, sh = math.cos(dog_heading), math.sin(dog_heading)
|
||||
cos_w = ch * _COS - sh * _SIN
|
||||
sin_w = sh * _COS + ch * _SIN
|
||||
|
||||
# Walls + posts
|
||||
best = _raycast_static(dog_x, dog_y, cos_w, sin_w)
|
||||
|
||||
# Sheep discs
|
||||
if sheep_xy:
|
||||
sx = np.asarray([p[0] for p in sheep_xy], dtype=np.float64) - dog_x
|
||||
sy = np.asarray([p[1] for p in sheep_xy], dtype=np.float64) - dog_y
|
||||
@@ -144,7 +216,6 @@ def simulate_scan(
|
||||
nearest = candidate.min(axis=0)
|
||||
np.minimum(best, nearest, out=best)
|
||||
|
||||
# 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)
|
||||
|
||||
|
||||
Reference in New Issue
Block a user