jalf/TIR_PROJ
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Webots sim-to-real fixes, DAgger pipeline, 360° proto variant

Browse Source 
Today's session worked across the full Webots delivery stack — found and
fixed a cluster of bugs blocking the BC/RL transfer, then explored
training-side mitigations for the residual perception gap.

Bug fixes:
- Makefile FP_RATE default 2.0 → 0.0: BC demos used fp_rate=0 but RL
  fine-tune defaulted to fp_rate=2, poisoning the BC obs distribution
  and stalling PPO at 0% success across 1.46M+ steps.
- controllers/{shepherd_dog,sheep}/runtime.ini: Webots was launching
  controllers under system python3 (no numpy) and they were crashing
  silently. Pinned to the conda tir env.
- herding/config.py HERDING_WEBOTS preset: pen_latch_depth 0.5 → 2.0,
  max_new_tracks_per_step 3 → 1, static_reject 0.8 → 1.2. Stops phantom
  FPs near the gate from latching as permanently-penned tracks.
- herding/perception/sheep_tracker.py: penned tracks now decay at
  forget_steps × 8 instead of living forever. Adds get_positions
  min_freshness filter for deploy-time use.

Training/eval matches deployment:
- training/bc/collect.py: --dagger-policy flag for DAgger rollouts
  (policy drives, teacher labels) + --use-webots-preset for matched
  140° tracker + DR config.
- controllers/shepherd_dog/shepherd_dog.py: scan-fallback (0, 0.6) when
  BC/RL sees empty sheep_positions — recovers from FOV gaps.

Tooling:
- tools/dagger_round.sh: one-shot DAgger round (collect + concat + bc).
- tools/webots_sweep_gt.sh: full sweep with HERDING_USE_GT=1 for the
  perception-gap diagnosis matrix.
- protos/ShepherdDog360.proto: 360° FOV variant for the FOV-ablation
  comparison. Canonical proto stays at 140° per project spec.

Artifacts: v1 BC/RL policies for all 4 (drive × world) combos trained
in clean gym (success: diff/field 90-100%, diff/round 58%, mec/field
60-100%, mec/round 50-100%). DAgger r1/r2 BCs for diff/field show
12%→38% progression on gym HERDING_WEBOTS proxy but did not close
to actual Webots LiDAR (0/5 throughout). Next: LSTM policy or
learned tracker per the project-state memory.

Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
This commit is contained in:
Johnny Fernandes
2026-05-16 17:21:02 +00:00
parent c61df91950
commit dd5ac669e5
34 changed files with 2336 additions and 188 deletions
Download Patch File Download Diff File Expand all files Collapse all files
herding/perception/lidar_perception.py
+52 -18
View File
@@ -21,9 +21,13 @@ The downstream tracker handles association across frames.
from __future__ import annotations
import math
from typing import TYPE_CHECKING
import numpy as np
if TYPE_CHECKING:
from herding.config import DetectionConfig, LidarConfig
from herding.world.geometry import (
FIELD_SHAPE, FIELD_ROUND_R,
FIELD_X, FIELD_Y, GATE_X, GATE_Y,
@@ -79,21 +83,22 @@ def _in_field_region(cx: float, cy: float) -> bool:
FIELD_Y[0] - 0.2 < cy < FIELD_Y[1] + 0.2)
def _near_wall(cx: float, cy: float) -> bool:
def _near_wall(cx: float, cy: float, wall_reject: float = WALL_REJECT) -> bool:
"""True if the detection is too close to a wall to be a sheep."""
if FIELD_SHAPE == "field_round":
r = math.hypot(cx, cy)
return r > FIELD_ROUND_R - WALL_REJECT
return r > FIELD_ROUND_R - wall_reject
return (
cx > FIELD_X[1] - WALL_REJECT or cx < FIELD_X[0] + WALL_REJECT or
cy > FIELD_Y[1] - WALL_REJECT or
(cy < FIELD_Y[0] + WALL_REJECT and not (PEN_X[0] <= cx <= PEN_X[1]))
cx > FIELD_X[1] - wall_reject or cx < FIELD_X[0] + wall_reject or
cy > FIELD_Y[1] - wall_reject or
(cy < FIELD_Y[0] + wall_reject and not (PEN_X[0] <= cx <= PEN_X[1]))
)
def _split_cluster_by_range(
points: list[tuple[float, float]],
range_vals: list[float],
split_range_gap: float = SPLIT_RANGE_GAP,
) -> list[list[tuple[float, float]]]:
"""Split a cluster at range-profile local maxima (gaps between sheep).
@@ -108,7 +113,7 @@ def _split_cluster_by_range(
# Find the maximum range (the dip/gap between sheep).
r_max = max(range_vals)
# If the range variation is small, it's a single target.
if r_max - r_min < SPLIT_RANGE_GAP:
if r_max - r_min < split_range_gap:
return [points]
# Find the split point: the index with the maximum range.
split_idx = range_vals.index(r_max)
@@ -124,7 +129,7 @@ def _split_cluster_by_range(
(right, range_vals[split_idx + 1:]),
]:
if len(sub_pts) >= 1:
result.extend(_split_cluster_by_range(sub_pts, sub_ranges))
result.extend(_split_cluster_by_range(sub_pts, sub_ranges, split_range_gap))
return result if result else [points]
@@ -132,14 +137,43 @@ def detections_from_scan(
ranges: np.ndarray,
dog_x: float, dog_y: float, dog_heading: float,
max_range: float = LIDAR_MAX_RANGE,
detection_cfg: "DetectionConfig | None" = None,
lidar_cfg: "LidarConfig | None" = None,
) -> list[tuple[float, float]]:
"""Return list of (x, y) world-frame sheep position estimates."""
"""Return list of (x, y) world-frame sheep position estimates.
Pass ``detection_cfg`` to override clustering/filtering thresholds, or
``lidar_cfg`` to inform the function of a non-default FOV (the number of
rays and FOV are inferred from the length of ``ranges`` and
``lidar_cfg.fov_rad`` respectively).
"""
# Resolve parameters — fall back to module-level constants when no cfg.
if detection_cfg is not None:
gap_thr = detection_cfg.gap_threshold
max_span = detection_cfg.max_cluster_span
hit_eps = detection_cfg.range_hit_eps
split_gap = detection_cfg.split_range_gap
wall_rej = detection_cfg.wall_reject
static_rej = detection_cfg.static_reject
else:
gap_thr = GAP_THRESHOLD
max_span = MAX_CLUSTER_SPAN
hit_eps = RANGE_HIT_EPS
split_gap = SPLIT_RANGE_GAP
wall_rej = WALL_REJECT
static_rej = STATIC_REJECT
sheep_r = lidar_cfg.sheep_radius if lidar_cfg is not None else SHEEP_RADIUS
fov = lidar_cfg.fov_rad if lidar_cfg is not None else LIDAR_FOV
if lidar_cfg is not None:
max_range = lidar_cfg.max_range
ranges = np.asarray(ranges, dtype=np.float32)
n_rays = ranges.shape[0]
if n_rays == 0:
return []
angles = ray_angles(n_rays, LIDAR_FOV)
hit = ranges < max_range - RANGE_HIT_EPS
angles = ray_angles(n_rays, fov)
hit = ranges < max_range - hit_eps
world_a = dog_heading + angles
px = dog_x + ranges * np.cos(world_a)
@@ -159,7 +193,7 @@ def detections_from_scan(
prev_xy = None
continue
pt = (float(px[i]), float(py[i]), float(ranges[i]))
if prev_xy is not None and math.hypot(pt[0] - prev_xy[0], pt[1] - prev_xy[1]) > GAP_THRESHOLD:
if prev_xy is not None and math.hypot(pt[0] - prev_xy[0], pt[1] - prev_xy[1]) > gap_thr:
clusters.append(current)
current = []
current.append(pt)
@@ -174,7 +208,7 @@ def detections_from_scan(
# Multi-peak splitting.
if len(cluster) >= 4:
sub_clusters = _split_cluster_by_range(points_xy, range_vals)
sub_clusters = _split_cluster_by_range(points_xy, range_vals, split_gap)
else:
sub_clusters = [points_xy]
@@ -185,24 +219,24 @@ def detections_from_scan(
ys = [p[1] for p in sub]
cx, cy = sum(xs) / len(xs), sum(ys) / len(ys)
span = math.hypot(max(xs) - min(xs), max(ys) - min(ys))
if span > MAX_CLUSTER_SPAN:
if span > max_span:
continue
# Rays hit the front edge of the sheep; offset outward by
# SHEEP_RADIUS along the dog→cluster direction.
# sheep_radius along the dog→cluster direction.
dx, dy = cx - dog_x, cy - dog_y
d = math.hypot(dx, dy)
if d > 1e-3:
cx += SHEEP_RADIUS * dx / d
cy += SHEEP_RADIUS * dy / d
cx += sheep_r * dx / d
cy += sheep_r * dy / d
in_main = _in_field_region(cx, cy)
in_gate_strip = (PEN_X[0] - 0.2 < cx < PEN_X[1] + 0.2 and
GATE_Y - 1.0 < cy < GATE_Y + 0.2)
if not (in_main or in_gate_strip):
continue
if any(math.hypot(cx - fx, cy - fy) < STATIC_REJECT
if any(math.hypot(cx - fx, cy - fy) < static_rej
for fx, fy in _STATIC_FEATURES):
continue
if _near_wall(cx, cy):
if _near_wall(cx, cy, wall_rej):
continue
detections.append((cx, cy))
return detections