diff --git a/README.md b/README.md index c307841..805bf2d 100644 --- a/README.md +++ b/README.md @@ -78,13 +78,11 @@ HERDING_USE_GT=1 tools/run_webots.sh 5 strombom differential field `make help` lists every target and the overridable hyperparameters. **Mecanum note**: the `ShepherdDogMecanum.proto` uses physical roller -hinges in Webots (committed 2026-05-16). The Webots calibration shows -a ~60% strafe efficiency and ~28% backward bleed compared to textbook -mecanum; the gym kinematics in `HERDING_MEC_WEBOTS` are tuned to -match. **Mecanum BC/RL policies need to be retrained against this -preset** — see `mecanum_proto_gap.md` in `memory/` for the 3-command -flow. The v1 policies in `training/runs/{bc,rl}_mecanum_*` predate the -proto rewrite and will not herd reliably in Webots until retrained. +hinges in Webots. The Webots calibration shows ~60% strafe efficiency +and ~28% backward bleed compared to textbook mecanum; the gym +kinematics in `HERDING_MEC_WEBOTS` are tuned to match. **Mecanum BC/RL +policies need to be retrained against this preset** — see the retrain +flow in the Mecanum results section below. ## Documentation map @@ -215,16 +213,30 @@ information. ### Mecanum (differential is the headline) -The `ShepherdDogMecanum.proto` was rewritten on 2026-05-16 with 32 -physical roller hinges, giving true omnidirectional motion in Webots -(`tools/calibrate_mecanum.sh` confirms the X-pattern). The mecanum -calibration shows ~60% strafe efficiency vs textbook (vs ~89% on -forward), so v1 mecanum BC/RL policies trained on textbook gym -mecanum no longer herd reliably. The fix is staged but not run: -the gym now has `HERDING_MEC_WEBOTS` which matches Webots' physical -mecanum, and `training/bc/collect.py` / `training/rl/train.py` auto- -select this preset for mecanum runs. Retraining (≈ 2 h per combo, -4 combos) is the documented future step. +`ShepherdDogMecanum.proto` has 32 physical roller hinges giving true +omnidirectional motion in Webots — `tools/calibrate_mecanum.sh` +confirms the X-pattern. Calibration shows ~60% strafe efficiency vs +textbook (versus ~89% on forward), so the gym needs to match the +imperfect physical mecanum for the trained policy to compensate. +`HERDING_MEC_WEBOTS` is the matched preset; `training/bc/collect.py` +and `training/rl/train.py` auto-select it for mecanum runs. Mecanum +policies were trained on the textbook gym, so they need to be +retrained against `HERDING_MEC_WEBOTS` (≈ 2 h per combo, 4 combos): + +```bash +python -m training.bc.collect \ + --drive-mode mecanum --world field --use-webots-preset \ + --out training/bc/demos_mecanum_field.npz +python -m training.bc.pretrain \ + --demos training/bc/demos_mecanum_field.npz \ + --out training/runs/bc_mecanum_field +python -m training.rl.train \ + --bc training/runs/bc_mecanum_field \ + --out training/runs/rl_mecanum_field \ + --drive-mode mecanum --world field --use-webots-preset +``` + +Repeat for `field_round`. ## License diff --git a/controllers/shepherd_dog/shepherd_dog.py b/controllers/shepherd_dog/shepherd_dog.py index 22cfe98..1af4ed3 100644 --- a/controllers/shepherd_dog/shepherd_dog.py +++ b/controllers/shepherd_dog/shepherd_dog.py @@ -1,42 +1,49 @@ """Shepherd Dog controller (Webots). -Mode is selected by ``HERDING_MODE`` (env var, or via the -``herding_runtime.cfg`` file the launcher writes since Webots strips -env vars on some setups): +Mode is selected by ``HERDING_MODE`` — read from the env var or from +the ``herding_runtime.cfg`` file the launcher writes (Webots strips +env vars from controller subprocesses on some setups): strombom → canonical Strömbom (2014) collect/drive heuristic wrapped in ActiveScanTeacher (opening rotation + - walk-to-centre when the tracker briefly empties). - sequential → single-target "pin-and-push", same wrapper. - bc → behaviour-cloned MLP, trained on Strömbom demos. - Default policy: training/runs/bc/policy.zip. - rl → KL-regularised PPO fine-tune of bc. Same obs/action - space as bc; refines time-to-pen via reward while - staying anchored to bc. - Default policy: training/runs/rl/policy.zip. + walk-to-centre when the tracker briefly empties) + sequential → single-target "pin-and-push", same wrapper + universal → mecanum-aware teacher (Strömbom + omega + recovery) + bc → behaviour-cloned MLP, trained on universal demos + rl → KL-regularised PPO fine-tune of `bc` + +Policy directories are resolved by `policy_loader` from +``training/runs/{bc,rl}_{drive}_{world}`` with a fallback to +``training/runs/{bc,rl}`` (legacy single-policy paths). Sheep perception ---------------- -The dog perceives sheep through its **front-mounted 140° LiDAR** +The dog perceives sheep through its front-mounted 140° LiDAR (``protos/ShepherdDog.proto``: 180 rays, 12 m max range). Each step: - 1. Reads ``lidar.getRangeImage()``. - 2. Runs ``herding.perception.lidar_perception.detections_from_scan`` - to cluster returns into world-frame ``(x, y)`` sheep estimates. - 3. Folds those into a ``SheepTracker`` which maintains last-seen - positions for sheep currently out of FOV and latches "penned" - once a track crosses the gate plane south. + 1. Read ``lidar.getRangeImage()``. + 2. Cluster returns into world-frame ``(x, y)`` estimates + (``herding.perception.lidar_perception.detections_from_scan``). + 3. Fold detections into a ``SheepTracker``, which maintains + last-seen positions for sheep currently out of FOV, requires + consensus across multiple frames before promoting a candidate + to a real track, and latches "penned" once a track crosses + the gate plane south. -Sheep ``emitter`` messages are read **for diagnostic logging only** -(GT_penned counter + auto-finish sentinel); they are never used to -drive the policy. Perception for control comes entirely from LiDAR. +Setting ``HERDING_USE_GT=1`` bypasses the tracker and feeds emitter +ground-truth positions to the policy — useful as a perception +ablation for the analytic baselines. + +Sheep emitter messages are otherwise read for diagnostic logging +only (``GT_penned`` counter + auto-finish sentinel); the control +loop never depends on them. Auto-finish ----------- -When the dog observes (via GT, read off the receiver) that all sheep -are penned, it writes ``training/.run_done`` and the launcher -(``tools/run_webots.sh``) detects it and closes Webots. This keeps -batch evaluation runs bounded. +When every emitter-reported sheep is penned, the controller writes +``training/.run_done``. The launcher (``tools/run_webots.sh``) +detects the sentinel and closes Webots so headless sweep runs are +bounded. """ import math @@ -111,6 +118,24 @@ MODE = (os.environ.get("HERDING_MODE") or _runtime_cfg.get("HERDING_MODE") or "bc").lower() +_VALID_MODES = ("bc", "rl", "strombom", "sequential", "universal", "calibrate") +if MODE not in _VALID_MODES: + print(f"[dog] unknown HERDING_MODE={MODE!r}; defaulting to strombom.") + MODE = "strombom" + +# Drive mode: "differential" (2-wheel) or "mecanum" (4-wheel omnidirectional). +DRIVE_MODE = (os.environ.get("HERDING_DRIVE") + or _runtime_cfg.get("HERDING_DRIVE") + or "differential").lower() +if DRIVE_MODE not in ("differential", "mecanum"): + print(f"[dog] unknown HERDING_DRIVE={DRIVE_MODE!r}; defaulting to differential.") + DRIVE_MODE = "differential" + +# World shape — used to disambiguate the trained policy directory. +WORLD = (os.environ.get("HERDING_WORLD") + or _runtime_cfg.get("HERDING_WORLD") + or "field").lower() + # Diagnostic: bypass LiDAR tracker and use GT emitter positions directly. # Set HERDING_USE_GT=1 to isolate perception sim-to-real gap from policy quality. USE_GT_PERCEPTION = bool(int( @@ -119,50 +144,34 @@ USE_GT_PERCEPTION = bool(int( )) -def _resolve_policy_dir(mode: str) -> str: - """Where to look for the trained policy for the given mode. +def _resolve_policy_dir(mode: str, drive: str, world: str) -> str: + """Where to look for the trained policy for the given mode/drive/world. Priority: 1. HERDING_POLICY_DIR env var or runtime-cfg entry, if it points to a real directory. - 2. Drive-mode-specific default: - bc → training/runs/bc_differential (or bc_mecanum) - rl → training/runs/rl_differential (or rl_mecanum) - 3. Legacy path (no drive suffix): - bc → training/runs/bc - rl → training/runs/rl + 2. Canonical: training/runs/{bc,rl}__ + 3. Drive-only: training/runs/{bc,rl}_ + 4. Bare-mode: training/runs/{bc,rl} + The first existing directory wins; if none exist, the canonical + path is returned so the loader's error message is informative. """ env_dir = (os.environ.get("HERDING_POLICY_DIR") or _runtime_cfg.get("HERDING_POLICY_DIR")) if env_dir and os.path.isdir(env_dir): return env_dir - drive = DRIVE_MODE - mode_default = { - "bc": os.path.join(_PROJECT_ROOT, "training", "runs", - f"bc_{drive}"), - "rl": os.path.join(_PROJECT_ROOT, "training", "runs", - f"rl_{drive}"), - } - primary = mode_default.get(mode, mode_default["bc"]) - if os.path.isdir(primary): - return primary - # Fallback: legacy paths without drive suffix. - legacy = { - "bc": os.path.join(_PROJECT_ROOT, "training", "runs", "bc"), - "rl": os.path.join(_PROJECT_ROOT, "training", "runs", "rl"), - } - fallback = legacy.get(mode, legacy["bc"]) - if os.path.isdir(fallback): - return fallback - return env_dir or primary + base = "rl" if mode == "rl" else "bc" + runs = os.path.join(_PROJECT_ROOT, "training", "runs") + for cand in (f"{base}_{drive}_{world}", f"{base}_{drive}", base): + path = os.path.join(runs, cand) + if os.path.isdir(path): + return path + return os.path.join(runs, f"{base}_{drive}_{world}") -_VALID_MODES = ("bc", "rl", "strombom", "sequential", "universal", "calibrate") -if MODE not in _VALID_MODES: - print(f"[dog] unknown HERDING_MODE={MODE!r}; defaulting to strombom.") - MODE = "strombom" +print(f"[dog] mode={MODE} drive={DRIVE_MODE} world={WORLD}") -POLICY_DIR = _resolve_policy_dir(MODE) +POLICY_DIR = _resolve_policy_dir(MODE, DRIVE_MODE, WORLD) policy_handle = None if MODE in ("bc", "rl"): print(f"[dog] resolved POLICY_DIR={POLICY_DIR} exists={os.path.isdir(POLICY_DIR)}") @@ -173,16 +182,6 @@ if MODE in ("bc", "rl"): except Exception as exc: print(f"[dog] policy load failed ({exc!r}); falling back to strombom.") MODE = "strombom" -print(f"[dog] running in mode={MODE}") - -# Drive mode: "differential" (2-wheel) or "mecanum" (4-wheel omnidirectional). -DRIVE_MODE = (os.environ.get("HERDING_DRIVE") - or _runtime_cfg.get("HERDING_DRIVE") - or "differential").lower() -if DRIVE_MODE not in ("differential", "mecanum"): - print(f"[dog] unknown HERDING_DRIVE={DRIVE_MODE!r}; defaulting to differential.") - DRIVE_MODE = "differential" -print(f"[dog] drive mode={DRIVE_MODE}") # --------------------------------------------------------------------------- diff --git a/herding/perception/sheep_tracker.py b/herding/perception/sheep_tracker.py index 15f5793..8e0f0a2 100644 --- a/herding/perception/sheep_tracker.py +++ b/herding/perception/sheep_tracker.py @@ -1,32 +1,26 @@ """Multi-target tracker for LiDAR-detected sheep. -Greedy nearest-neighbour data association across frames, with a wider -re-acquisition gate for stale tracks (sheep flee during occlusion and -reappear off-position), plus memory of last-seen positions for sheep -out of FOV. Output is ``{name: (x, y)}`` — Strömbom / Sequential -consume it directly. +Three-stage greedy nearest-neighbour data association: -When **predictive mode** is enabled (the default), tracks carry a -constant-velocity state ``(vx, vy)`` estimated from the last two -observations. While a track is occluded its position is extrapolated -using this velocity for up to ``PREDICT_STEPS`` frames, keeping the -teacher's CoM estimate stable during brief losses. After prediction -expires, the track falls back to its last-seen position (static memory) -until ``FORGET_STEPS`` deletes it entirely. +1. **Consensus promotion**. New detections start as *candidate* tracks + invisible to ``get_positions``. They must accumulate ``consensus_k`` + matches within ``consensus_radius_m`` to promote; candidates that + fail to re-confirm within ``consensus_max_age`` steps die. This + filters one-shot LiDAR phantoms — wall returns, multi-cluster sheep + splits, fast-moving sheep position jumps — at the cost of a small + acquisition latency (~50 ms at the default ``consensus_k=3``). + ``consensus_k=1`` disables the stage. +2. **Constant-velocity prediction**. Each track carries a smoothed + ``(vx, vy)``. While a track is occluded its position is + extrapolated for up to ``PREDICT_STEPS`` frames, then falls back to + last-seen static memory until ``FORGET_STEPS`` deletes it. +3. **Pen latching**. A track whose estimated position crosses the gate + plane south of ``is_penned_position`` is marked penned, excluded + from ``get_positions``, and kept indefinitely. -A track is marked penned once its estimated position crosses the gate -plane south (``is_penned_position``). Penned tracks are excluded from -``get_positions`` and kept indefinitely. - -**Consensus promotion** (``consensus_k > 1``): every new detection -starts as a *candidate* track that is invisible to ``get_positions``. -It must be matched ``consensus_k`` times within a tight radius -(``consensus_radius_m``) before being promoted to a regular track. -Candidates that fail to re-confirm within ``consensus_max_age`` steps -are deleted. The cost is a small acquisition latency -(``consensus_k * timestep`` ≈ 65 ms) in exchange for rejecting the -one-shot LiDAR phantom returns Webots produces from real-world 3D -geometry. ``consensus_k=1`` disables the stage entirely (default). +Output of :meth:`SheepTracker.get_positions` is ``{name: (x, y)}`` — +Strömbom, Sequential and the BC observation builder consume it +directly. """ from __future__ import annotations diff --git a/tools/run_webots.sh b/tools/run_webots.sh index 9f68ee7..f3f1ebd 100755 --- a/tools/run_webots.sh +++ b/tools/run_webots.sh @@ -60,19 +60,26 @@ DST="$ROOT/worlds/${WORLD}_test.wbt" if [[ -n "${HERDING_POLICY_DIR:-}" ]]; then RESOLVED_POLICY_DIR="$HERDING_POLICY_DIR" else - # Try drive-mode-specific path first, then legacy path. + # The training pipeline writes policies to: + # training/runs/{bc,rl}__ + # Try that first; fall back to the drive-only and finally the + # bare-mode legacy paths so older policy checkouts still load. if [[ "$MODE" == "rl" ]]; then - DRIVED="$ROOT/training/runs/rl_${DRIVE}" - LEGACY="$ROOT/training/runs/rl" + BASE="rl" else - DRIVED="$ROOT/training/runs/bc_${DRIVE}" - LEGACY="$ROOT/training/runs/bc" - fi - if [[ -d "$DRIVED" ]]; then - RESOLVED_POLICY_DIR="$DRIVED" - else - RESOLVED_POLICY_DIR="$LEGACY" + BASE="bc" fi + for CAND in \ + "$ROOT/training/runs/${BASE}_${DRIVE}_${WORLD}" \ + "$ROOT/training/runs/${BASE}_${DRIVE}" \ + "$ROOT/training/runs/${BASE}" + do + if [[ -d "$CAND" ]]; then + RESOLVED_POLICY_DIR="$CAND" + break + fi + done + : "${RESOLVED_POLICY_DIR:=$ROOT/training/runs/${BASE}_${DRIVE}_${WORLD}}" fi cp "$SRC" "$DST" diff --git a/training/README.md b/training/README.md index 0f04965..e859733 100644 --- a/training/README.md +++ b/training/README.md @@ -1,80 +1,105 @@ -# Training and Evaluation Details +# Training and evaluation details -This file is the command-level companion to the root README. It focuses -on data collection, BC, PPO fine-tuning, evaluation flags, and generated -artifacts; use the root README for the high-level architecture and -Webots demo quick start. +Command-level companion to the root README. Covers demo collection, +behaviour cloning, PPO fine-tuning, and evaluation flags; use the root +README for the high-level architecture and Webots quick start. -Two stages, strictly sequential: +The pipeline is two strictly-sequential stages per `(drive, world)` +combo: ``` -sim demos (Strömbom on tracker output, K=4 frame stack) +sim demos (universal teacher on tracker output, K=4 frame stack) │ ▼ -bc/pretrain.py ──► runs/bc (Strömbom-imitated MLP) +bc/pretrain.py ──► runs/bc__ (MLP) │ ▼ KL-regularised PPO fine-tune │ -runs/rl (deployed `rl` mode — beats BC and Strömbom) +runs/rl__ (deployed `rl` mode) ``` ## Files ``` herding_env.py — Gymnasium env (LiDAR raycast + tracker by default) +bc/collect.py — universal-teacher sim demos bc/pretrain.py — MSE + cosine BC of (obs, action) demos into MlpPolicy -rl/train.py — KL-regularised PPO fine-tune of a BC checkpoint +rl/train.py — KL-regularised PPO fine-tune of a BC checkpoint +rl/train_lstm.py — RecurrentPPO variant (ablation) eval.py — multi-seed analytic / learned policy comparison -runs/ — checkpoints (whitelisted entries in top-level .gitignore) - -(Unit + integration tests live in the top-level ``tests/`` directory; -run with ``python -m pytest tests/``.) +runs/ — checkpoints (gitignored except for policy.zip) ``` +Unit + integration tests live in the top-level `tests/`. Run with +`make test` or `python -m pytest tests/`. + ## End-to-end pipeline -The simplest way to run everything is the Makefile at the project -root: ``make`` does the full chain, ``make rl`` rebuilds whatever's -needed up to that point, etc. The individual stages below are kept -explicit for cases where you want to tune a single step. +The simplest way to train one combo is the project-root Makefile: ```bash -# 1. Sim demos with the active-scan + Strömbom teacher under LiDAR -# perception. K=4 frame stack so the MLP has temporal context. -python -m training.bc.collect --teacher strombom \ - --out training/bc/demos.npz --seeds-per-n 15 --subsample 3 --frame-stack 4 +make DRIVE=differential WORLD=field # demos → bc → rl → eval +make DRIVE=differential WORLD=field_round +make train_all # all four combos sequentially +``` -# 2. Behaviour-clone. -python -m training.bc.pretrain --demos training/bc/demos.npz \ - --out training/runs/bc --epochs 60 --net-arch 512,512 +The individual stages below are kept explicit for cases where you +want to tune a single step. + +```bash +# 1. Sim demos with the active-scan + universal teacher under LiDAR +# perception. K=4 frame stack so the MLP has temporal context. +python -m training.bc.collect \ + --teacher universal --drive-mode differential --world field \ + --out training/bc/demos_differential_field.npz \ + --seeds-per-n 15 --subsample 3 --frame-stack 4 + +# 2. Behaviour-clone the demos. +python -m training.bc.pretrain \ + --demos training/bc/demos_differential_field.npz \ + --out training/runs/bc_differential_field \ + --epochs 60 --net-arch 512,512 # 3. KL-regularised PPO fine-tune of bc. python -m training.rl.train \ - --bc training/runs/bc --out training/runs/rl \ + --bc training/runs/bc_differential_field \ + --out training/runs/rl_differential_field \ + --drive-mode differential --world field \ --total-timesteps 1000000 # 4. Multi-seed eval (env-side, fast). -python -m training.eval --policy training/runs/rl \ +python -m training.eval --policy training/runs/rl_differential_field \ + --drive-mode differential --world field \ --max-flock 10 --max-steps 15000 --n-seeds 10 ``` `bc/pretrain.py` saves the **best-val_cos** snapshot, not the final -epoch — multi-modal teachers make training noisy and the last epoch is -often worse than an earlier one. +epoch — multi-modal teachers make training noisy and the last epoch +is often worse than an earlier one. `rl/train.py` loads BC weights into both a trainable policy and a frozen reference, fixes `log_std` small, and adds `β · KL(π‖π_ref)` to the loss so the policy can only move within a trust region around BC. See the file header for hyperparameter rationale. -## Available analytic teachers +## Mecanum retraining + +For mecanum runs, pass `--use-webots-preset`. Both `collect.py` and +`train.py` detect `--drive-mode mecanum` and switch to the +`HERDING_MEC_WEBOTS` preset, which matches the physical-roller +Webots proto's strafe efficiency (~0.4) and forward bleed (~−0.28). +Training without this preset produces a policy that herds in textbook +gym mecanum but not in Webots. + +## Analytic teachers | Name | What it does | Notes | |---|---|---| -| `strombom` | Strömbom 2014 — collect when flock is scattered, drive CoM otherwise | Default; works for n=1–10 under tight cohesion | -| `sequential` | Pick the sheep closest to the pen and drive only it | Alternative; needs loose-cohesion regime | +| `strombom` | Strömbom 2014 — collect when flock is scattered, drive CoM otherwise | Round-world aware (radially-inward fallback when natural target lies outside the curved boundary) | +| `sequential` | Three-phase: collect, drive, then single-target push for the last 1–2 stragglers | Alternative to strombom | +| `universal` | Strömbom core + mecanum omega + last-straggler recovery | Used as the BC demo teacher | -Both are wrapped at demo-collection time in +All three are wrapped at demo-collection time in `herding/control/active_scan.py:ActiveScanTeacher`, which adds an opening in-place rotation, walk-to-centre when the LiDAR sees nothing, and near-sheep speed modulation (same modulation @@ -83,8 +108,11 @@ inference). ## Evaluating analytic teachers directly +```bash +python -m training.eval --policy strombom \ + --drive-mode differential --world field \ + --max-flock 10 --max-steps 15000 --n-seeds 10 +python -m training.eval --policy sequential \ + --drive-mode differential --world field_round \ + --max-flock 10 --max-steps 15000 --n-seeds 10 ``` -python -m training.eval --policy strombom --max-flock 10 --max-steps 15000 --n-seeds 10 -python -m training.eval --policy sequential --max-flock 10 --max-steps 15000 --n-seeds 10 -``` - diff --git a/training/bc/collect.py b/training/bc/collect.py index dade3e4..aad6b72 100644 --- a/training/bc/collect.py +++ b/training/bc/collect.py @@ -8,8 +8,8 @@ the same partial-obs view the student will have at deployment. Usage:: - python -m training.bc.collect --teacher strombom \\ - --out training/bc/demos.npz --frame-stack 4 + python -m training.bc.collect --teacher universal --drive-mode differential \\ + --world field --out training/bc/demos_differential_field.npz --frame-stack 4 """ from __future__ import annotations @@ -125,7 +125,9 @@ def collect_one(n_sheep: int, seed: int, max_steps: int, subsample: int, def main(): parser = argparse.ArgumentParser() - parser.add_argument("--out", default="training/bc/demos.npz") + parser.add_argument("--out", required=True, + help="Output .npz path (convention: " + "training/bc/demos__.npz).") parser.add_argument("--n-sheep-list", default="1,2,3,5,8,10") parser.add_argument("--seeds-per-n", type=int, default=15) parser.add_argument("--max-steps", type=int, default=30000) diff --git a/training/bc/pretrain.py b/training/bc/pretrain.py index e5b836a..ecb3303 100644 --- a/training/bc/pretrain.py +++ b/training/bc/pretrain.py @@ -12,8 +12,8 @@ Output zip is loadable by ``PPO.load(...)`` and consumed by Usage:: python -m training.bc.pretrain \\ - --demos training/bc/demos.npz \\ - --out training/runs/bc + --demos training/bc/demos_differential_field.npz \\ + --out training/runs/bc_differential_field """ from __future__ import annotations @@ -70,8 +70,11 @@ def policy_forward_mean(policy, obs_batch): def main(): parser = argparse.ArgumentParser() - parser.add_argument("--demos", default="training/bc/demos.npz") - parser.add_argument("--out", default="training/runs/bc") + parser.add_argument("--demos", required=True, + help="Path to demos .npz collected by training.bc.collect.") + parser.add_argument("--out", required=True, + help="Output directory (convention: " + "training/runs/bc__).") parser.add_argument("--epochs", type=int, default=60) parser.add_argument("--batch-size", type=int, default=256) parser.add_argument("--lr", type=float, default=1e-3)