Johnny Fernandes
a584a034e9
Repo hygiene pass after a long working session.
Files removed:
* stage1_train.log — runtime training log (~125 KB), shouldn't have
been tracked.
* training/bc/demos.npz — orphan default-name demos file from before
the world+drive-suffixed naming convention took over; no script
references it.
* training/runs/bc_dagger{1,2}_differential_field/policy.zip — failed
DAgger experiment artifacts. Per `memory/dagger_results.md` the
whole DAgger experiment hit 0/5 on Webots transfer; these checkpoints
have no consumers.
Untracked-but-deleted (no git change) — also cleaned from disk:
* Root-level runtime logs (43 *.log files, all unused — gitignored now).
* training/bc/{combined,dagger}*.npz (5 huge demo blobs, 2.6 GB
reclaimed; not committed).
* training/bc/v1/ (2.6 GB backup of pre-DAgger demos; reclaimed).
* training/runs/at_20260426_*/ (orphan timestamped runs; reclaimed).
* All __pycache__/.
Dead code removed:
* `herding/control/strombom.py::compute_action_debug` — no callers
anywhere in the repo.
* `herding/control/sequential.py::compute_action_debug` — same.
* `herding/control/universal.py::compute_action_diff` — same.
.gitignore extended to cover:
* All *.log files (training/eval/webots logs are runtime artifacts).
* training/bc/*.npz (re-collectable on demand by `make bc_demos`).
* training/bc/v1/.
* .pytest_cache, *.pyc, .claude/.
README refreshed:
* Mecanum + round-world coverage in the headline.
* Quick-start updated for DRIVE/WORLD-suffixed Makefile targets,
GT-bypass example, and the mecanum-retrain caveat.
* Layout reflects the actual current tree (config.py, both protos,
both worlds, all tools).
* Results table replaced with the Webots end-to-end numbers from
the 2026-05-16 sweep (8/8 diff combos + LiDAR/GT comparison).
Verification: 126 pytest cases still pass (was 126 going in — no
test-coverage regression from the dead-code removal).
Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
83 lines
3.0 KiB
Python
83 lines
3.0 KiB
Python
"""Adaptive sequential shepherd-dog controller.
|
||
|
||
Three-phase strategy:
|
||
|
||
1. **Collect** (flock scattered): Strömbom collect — park behind the
|
||
furthest sheep and push it toward the CoM. Identical to the
|
||
Strömbom heuristic; keeps the flock together.
|
||
|
||
2. **Drive** (flock compact, >STRAGGLER_THRESHOLD active): Strömbom
|
||
drive — park behind the CoM relative to the pen and push the whole
|
||
group through the gate.
|
||
|
||
3. **Targeted** (≤STRAGGLER_THRESHOLD sheep remain active): single-
|
||
target push on the sheep closest to the pen entry. Safe to isolate
|
||
individual sheep once the flock is nearly exhausted.
|
||
|
||
The original pure pin-and-push (Phase 3 only) caused flock scatter in
|
||
Webots physics whenever the dog tried to isolate a sheep while others
|
||
were still spread across the field. Phases 1–2 handle the bulk of
|
||
herding with flock-aware Strömbom logic; Phase 3 cleans up stragglers.
|
||
"""
|
||
|
||
import math
|
||
|
||
from herding.world.geometry import GATE_Y, PEN_ENTRY, in_pen
|
||
|
||
|
||
F_FACTOR = 4.0 # collect/drive threshold: radius > F_FACTOR·√n
|
||
DELTA_COLLECT = 1.5 # standoff behind the furthest sheep (collect)
|
||
DELTA_DRIVE = 2.0 # standoff behind CoM (drive)
|
||
DELTA_TARGET = 1.5 # standoff behind single target sheep (targeted)
|
||
STRAGGLER_THRESHOLD = 2 # switch to targeted push when ≤ this many active
|
||
|
||
|
||
def _unit(x: float, y: float):
|
||
d = math.hypot(x, y)
|
||
if d < 1e-6:
|
||
return 0.0, 0.0
|
||
return x / d, y / d
|
||
|
||
|
||
def _is_active(x: float, y: float) -> bool:
|
||
return (not in_pen(x, y)) and y > GATE_Y
|
||
|
||
|
||
def compute_action(dog_xy, sheep_positions, pen_target=PEN_ENTRY):
|
||
"""Return ``(vx, vy, mode)`` — same signature as Strömbom."""
|
||
active = [(x, y) for (x, y) in sheep_positions.values() if _is_active(x, y)]
|
||
if not active:
|
||
return 0.0, 0.0, "idle"
|
||
|
||
n = len(active)
|
||
com_x = sum(p[0] for p in active) / n
|
||
com_y = sum(p[1] for p in active) / n
|
||
dists = [math.hypot(p[0] - com_x, p[1] - com_y) for p in active]
|
||
radius = max(dists)
|
||
|
||
if n <= STRAGGLER_THRESHOLD:
|
||
# Targeted: push the sheep closest to the pen entry individually.
|
||
sx, sy = min(active,
|
||
key=lambda p: math.hypot(p[0] - pen_target[0],
|
||
p[1] - pen_target[1]))
|
||
ux, uy = _unit(sx - pen_target[0], sy - pen_target[1])
|
||
tx, ty = sx + DELTA_TARGET * ux, sy + DELTA_TARGET * uy
|
||
mode = "targeted"
|
||
|
||
elif radius > F_FACTOR * math.sqrt(n):
|
||
# Collect: aim behind the furthest sheep from the CoM.
|
||
idx = max(range(n), key=lambda i: dists[i])
|
||
sx, sy = active[idx]
|
||
ux, uy = _unit(sx - com_x, sy - com_y)
|
||
tx, ty = sx + DELTA_COLLECT * ux, sy + DELTA_COLLECT * uy
|
||
mode = "collect"
|
||
|
||
else:
|
||
# Drive: push the whole compact flock toward the gate.
|
||
ux, uy = _unit(com_x - pen_target[0], com_y - pen_target[1])
|
||
tx, ty = com_x + DELTA_DRIVE * ux, com_y + DELTA_DRIVE * uy
|
||
mode = "drive"
|
||
|
||
ax, ay = _unit(tx - dog_xy[0], ty - dog_xy[1])
|
||
return ax, ay, mode
|