diff --git a/.gitignore b/.gitignore index ce02a54..923d7e2 100644 --- a/.gitignore +++ b/.gitignore @@ -1,30 +1,15 @@ -# Editor / IDE -.claude/ -.venv/ - # Python __pycache__/ -*.pyc -# Webots controller scratch / debug -controllers/shepherd_dog/dog_behavior_log.csv -dog_debug.csv - -# Training artefacts: ignore by default, whitelist the two working BC policies -*.zip -*.pkl -*.npz -events.out.tfevents.* -training/runs/*/checkpoints/ -training/runs/*/tb/ -training/runs/*/evals/ -training/runs/*/best/ +# Training artefacts: ignore all run outputs except deployable policies +training/runs/** +!training/runs/ !training/runs/.gitkeep -!training/runs/bc_v3/policy.zip -!training/runs/rl_v1/policy.zip -!training/runs/rl_v2/policy.zip -!training/runs/rl_v2/best/best_model.zip +!training/runs/*/ +!training/runs/*/policy.zip # Webots launcher scratch -worlds/field_test.wbt +worlds/** +!worlds/field.wbt +!worlds/field_round.wbt herding_runtime.cfg diff --git a/Makefile b/Makefile index 3f4a6ab..308d72d 100644 --- a/Makefile +++ b/Makefile @@ -9,35 +9,79 @@ # make eval # 10-seed env eval of rl # make test # pytest suite # make webots N=10 MODE=rl # launch Webots in the chosen mode +# WEBOTS_HEADLESS=1 make webots # no 3D view, fast mode (still needs DISPLAY or xvfb-run) # make clean # delete bc_demos and run artefacts +# make clean_all # delete artefacts for all combinations # make help # print the target table # # Override any hyperparameter on the command line, for example: # make rl PPO_STEPS=2000000 KL=0.02 # make eval EVAL_SEEDS=20 +# +# Drive mode selects the locomotion model: +# make DRIVE=differential 2-wheel diff-drive (default) +# make DRIVE=mecanum 4-wheel omnidirectional +# +# World shape: +# make WORLD=field rectangular (default) +# make WORLD=field_round circular fence +# +# To train all 4 combinations: +# make train_all PY := python -BC_DEMOS := training/bc/demos.npz -BC_DIR := training/runs/bc -RL_DIR := training/runs/rl -BC_POLICY := $(BC_DIR)/policy.zip -RL_POLICY := $(RL_DIR)/policy.zip +# Drive mode and world shape — each combination gets its own artefacts. +DRIVE ?= differential +WORLD ?= field + +# Derived tag and paths. +TAG = $(DRIVE)_$(WORLD) +BC_DEMOS = training/bc/demos_$(TAG).npz +BC_DIR = training/runs/bc_$(TAG) +RL_DIR = training/runs/rl_$(TAG) +BC_POLICY = $(BC_DIR)/policy.zip +RL_POLICY = $(RL_DIR)/policy.zip # --- Demo collection --- -TEACHER ?= strombom -SEEDS_PER_N ?= 15 +TEACHER ?= universal +# Round field is fundamentally harder (narrow gate at south of a circle). +# Default to more demos there to give BC a fair shot at 60%+. +ifeq ($(WORLD),field_round) +SEEDS_PER_N ?= 40 +else +SEEDS_PER_N ?= 25 +endif SUBSAMPLE ?= 3 FRAME_STACK ?= 4 +DEMO_MAX_STEPS ?= 100000 # --- Behaviour cloning --- +ifeq ($(WORLD),field_round) +BC_EPOCHS ?= 100 +else BC_EPOCHS ?= 60 +endif BC_NET_ARCH ?= 512,512 # --- KL-PPO fine-tune --- -PPO_STEPS ?= 1000000 +# Round field: longer training, looser KL, no time penalty (success +# must be learned before speed is rewarded). +ifeq ($(WORLD),field_round) +PPO_STEPS ?= 4000000 +KL ?= 0.02 +TIME_W ?= 0.0 +else +PPO_STEPS ?= 2000000 KL ?= 0.05 +TIME_W ?= -0.05 +endif +IMITATE ?= 0.0 +# PPO rollouts at full difficulty so the training distribution matches +# eval (deployment). Anything lower causes a train/eval mismatch that +# can make RL eval worse than BC. +DIFFICULTY ?= 1.0 # --- Evaluation --- EVAL_SEEDS ?= 10 @@ -48,16 +92,23 @@ N ?= 10 MODE ?= rl -.PHONY: all bc_demos bc rl eval test webots clean help +.PHONY: all bc_demos bc rl eval test webots clean clean_all help \ + train_all train_diff_rect train_diff_round \ + train_mec_rect train_mec_round all: eval +# Export HERDING_WORLD so that geometry.py picks it up at import time. +export HERDING_WORLD = $(WORLD) + bc_demos: $(BC_DEMOS) $(BC_DEMOS): $(PY) -m training.bc.collect \ --teacher $(TEACHER) --out $(BC_DEMOS) \ --seeds-per-n $(SEEDS_PER_N) --subsample $(SUBSAMPLE) \ - --frame-stack $(FRAME_STACK) + --frame-stack $(FRAME_STACK) --drive-mode $(DRIVE) \ + --world $(WORLD) \ + --max-steps $(DEMO_MAX_STEPS) bc: $(BC_POLICY) $(BC_POLICY): $(BC_DEMOS) @@ -69,20 +120,44 @@ rl: $(RL_POLICY) $(RL_POLICY): $(BC_POLICY) $(PY) -m training.rl.train \ --bc $(BC_DIR) --out $(RL_DIR) \ - --total-timesteps $(PPO_STEPS) --kl-coef $(KL) + --total-timesteps $(PPO_STEPS) --kl-coef $(KL) \ + --imitate-weight $(IMITATE) --time-weight $(TIME_W) \ + --difficulty $(DIFFICULTY) \ + --drive-mode $(DRIVE) --world $(WORLD) eval: $(RL_POLICY) $(PY) -m training.eval --policy $(RL_DIR) \ - --max-flock 10 --max-steps $(EVAL_MAX_STEPS) --n-seeds $(EVAL_SEEDS) + --max-flock 10 --max-steps $(EVAL_MAX_STEPS) --n-seeds $(EVAL_SEEDS) \ + --drive-mode $(DRIVE) --world $(WORLD) test: $(PY) -m pytest tests/ webots: - tools/run_webots.sh $(N) $(MODE) + tools/run_webots.sh $(N) $(MODE) $(DRIVE) $(WORLD) clean: - rm -rf $(BC_DEMOS) $(BC_DIR) $(RL_DIR) + rm -f $(BC_DEMOS) + rm -rf $(BC_DIR) $(RL_DIR) + +clean_all: + rm -f training/bc/demos_*.npz + rm -rf training/runs/bc_* training/runs/rl_* + +# --- Train all 4 combinations --- +train_diff_rect: + $(MAKE) DRIVE=differential WORLD=field + +train_diff_round: + $(MAKE) DRIVE=differential WORLD=field_round + +train_mec_rect: + $(MAKE) DRIVE=mecanum WORLD=field + +train_mec_round: + $(MAKE) DRIVE=mecanum WORLD=field_round + +train_all: train_diff_rect train_diff_round train_mec_rect train_mec_round help: @echo "Targets:" @@ -92,12 +167,21 @@ help: @echo " make rl KL-PPO fine-tune (rebuilds bc if missing)" @echo " make eval $(EVAL_SEEDS)-seed env eval of rl" @echo " make test pytest suite" - @echo " make webots [N=$(N)] [MODE=$(MODE)]" + @echo " make webots [N=$(N)] [MODE=$(MODE)] [DRIVE=$(DRIVE)] [WORLD=$(WORLD)]" @echo " launch Webots in the chosen mode" - @echo " make clean delete bc_demos and run artefacts" + @echo " WEBOTS_HEADLESS=1 make webots … no 3D view + fast + --batch" + @echo " make clean delete artefacts for current DRIVE+WORLD" + @echo " make clean_all delete artefacts for all combinations" + @echo "" + @echo "Combinations:" + @echo " make DRIVE=differential WORLD=field diff + rectangular (default)" + @echo " make DRIVE=differential WORLD=field_round diff + circular" + @echo " make DRIVE=mecanum WORLD=field mecanum + rectangular" + @echo " make DRIVE=mecanum WORLD=field_round mecanum + circular" + @echo " make train_all all 4 in sequence" @echo "" @echo "Hyperparameter overrides (showing defaults):" - @echo " TEACHER=$(TEACHER) SEEDS_PER_N=$(SEEDS_PER_N) SUBSAMPLE=$(SUBSAMPLE) FRAME_STACK=$(FRAME_STACK)" + @echo " TEACHER=$(TEACHER) SEEDS_PER_N=$(SEEDS_PER_N) SUBSAMPLE=$(SUBSAMPLE) FRAME_STACK=$(FRAME_STACK) DEMO_MAX_STEPS=$(DEMO_MAX_STEPS)" @echo " BC_EPOCHS=$(BC_EPOCHS) BC_NET_ARCH=$(BC_NET_ARCH)" - @echo " PPO_STEPS=$(PPO_STEPS) KL=$(KL)" + @echo " PPO_STEPS=$(PPO_STEPS) KL=$(KL) IMITATE=$(IMITATE) TIME_W=$(TIME_W)" @echo " EVAL_SEEDS=$(EVAL_SEEDS) EVAL_MAX_STEPS=$(EVAL_MAX_STEPS)" diff --git a/README.md b/README.md index d307c3d..2860859 100644 --- a/README.md +++ b/README.md @@ -74,6 +74,15 @@ make webots N=10 MODE=strombom # analytic baseline `make help` lists every target and the overridable hyperparameters (e.g. `make rl PPO_STEPS=2000000 KL=0.02`). +## Documentation map + +- This README is the project overview: architecture, quick start, and + headline results. +- `training/README.md` has the command-level training and evaluation + details for demo collection, BC, PPO fine-tuning, and policy artifacts. +- `docs/project.md` is the original course proposal/goals document, kept + for traceability rather than as run instructions. + ## Layout ``` @@ -128,7 +137,7 @@ worlds/ field.wbt — main world (3 m gate, external pen) protos/ — Sheep / ShepherdDog robot definitions -docs/project.md — original project goals +docs/project.md — original course proposal/goals ``` ## Shared low-level control diff --git a/controllers/shepherd_dog/shepherd_dog.py b/controllers/shepherd_dog/shepherd_dog.py index 6c03376..88429e1 100644 --- a/controllers/shepherd_dog/shepherd_dog.py +++ b/controllers/shepherd_dog/shepherd_dog.py @@ -49,37 +49,8 @@ _PROJECT_ROOT = os.path.normpath(os.path.join(_HERE, "..", "..")) if _PROJECT_ROOT not in sys.path: sys.path.insert(0, _PROJECT_ROOT) -import numpy as np - -from controller import Robot - -from herding.control.active_scan import ActiveScanTeacher -from herding.control.modulation import modulate_speed_near_sheep -from herding.control.sequential import compute_action as sequential_action -from herding.control.strombom import compute_action as strombom_action -from herding.perception.obs import build_obs -from herding.perception.lidar_perception import detections_from_scan -from herding.perception.sheep_tracker import SheepTracker -from herding.world.diffdrive import velocity_to_wheels -from herding.world.geometry import ( - DOG_MAX_LINEAR, DOG_MAX_WHEEL_OMEGA, - DOG_SOUTH_LIMIT, DOG_WHEEL_RADIUS, - PEN_ENTRY, is_penned_position, -) - - -# --------------------------------------------------------------------------- -# Mode + policy resolution -# --------------------------------------------------------------------------- - +# --- Read runtime cfg early so env vars are set before geometry import --- def _load_runtime_config(): - """Read mode + policy_dir overrides from a runtime config file. - - Webots strips HERDING_* env vars in some configurations, so the - launcher writes a tiny ``herding_runtime.cfg`` (key=value lines) - in the project root and the controller reads it here. Env vars - win if both are present; the file is the fallback. - """ cfg_path = os.path.join(_PROJECT_ROOT, "herding_runtime.cfg") if not os.path.exists(cfg_path): return {} @@ -96,8 +67,37 @@ def _load_runtime_config(): return {} return out - _runtime_cfg = _load_runtime_config() +# Seed env vars from runtime cfg so downstream modules (geometry.py) see them. +for _rk, _rv in _runtime_cfg.items(): + if _rk.startswith("HERDING_") and _rk not in os.environ: + os.environ[_rk] = _rv + +import numpy as np + +from controller import Robot + +from herding.control.active_scan import ActiveScanTeacher +from herding.control.modulation import modulate_speed_near_sheep +from herding.control.sequential import compute_action as sequential_action +from herding.control.strombom import compute_action as strombom_action +from herding.control.universal import compute_action as universal_action +from herding.perception.obs import build_obs +from herding.perception.lidar_perception import detections_from_scan +from herding.perception.sheep_tracker import SheepTracker +from herding.world.diffdrive import velocity_to_mecanum_wheels, velocity_to_wheels +from herding.world.geometry import ( + DOG_MAX_LINEAR, DOG_MAX_WHEEL_OMEGA, + DOG_SOUTH_LIMIT, DOG_WHEEL_BASE, DOG_WHEEL_BASE_X, + DOG_WHEEL_BASE_Y, DOG_WHEEL_RADIUS, + PEN_ENTRY, is_penned_position, +) + + +# --------------------------------------------------------------------------- +# Mode + policy resolution (cfg already loaded above) +# --------------------------------------------------------------------------- + MODE = (os.environ.get("HERDING_MODE") or _runtime_cfg.get("HERDING_MODE") or "bc").lower() @@ -109,31 +109,39 @@ def _resolve_policy_dir(mode: str) -> str: Priority: 1. HERDING_POLICY_DIR env var or runtime-cfg entry, if it points to a real directory. - 2. Mode-specific default: - bc → training/runs/bc (Strömbom-imitated MLP) - rl → training/runs/rl (KL-PPO fine-tune of bc) - 3. Fall back to bc. - All checkpoints are frame-stacked K = 4; ``policy_loader`` reads - the stacking factor from the policy's observation space. + 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 """ 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", "bc"), - "rl": os.path.join(_PROJECT_ROOT, "training", "runs", "rl"), + "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 = mode_default["bc"] + # 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 -_VALID_MODES = ("bc", "rl", "strombom", "sequential") +_VALID_MODES = ("bc", "rl", "strombom", "sequential", "universal") if MODE not in _VALID_MODES: print(f"[dog] unknown HERDING_MODE={MODE!r}; defaulting to strombom.") MODE = "strombom" @@ -151,6 +159,15 @@ if MODE in ("bc", "rl"): 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}") + # --------------------------------------------------------------------------- # Control parameters @@ -171,7 +188,8 @@ def safety_clamp(vx: float, vy: float, dog_x: float, dog_y: float) -> tuple: return (vx, vy) -def drive(vx: float, vy: float, left_motor, right_motor, compass, motor_max: float): +def drive_diff(vx: float, vy: float, left_motor, right_motor, + compass, motor_max: float): if math.hypot(vx, vy) < 1e-3: left_motor.setVelocity(0.0) right_motor.setVelocity(0.0) @@ -189,6 +207,32 @@ def drive(vx: float, vy: float, left_motor, right_motor, compass, motor_max: flo right_motor.setVelocity(right) +def drive_mecanum(vx: float, vy: float, omega: float, + fl_motor, fr_motor, rl_motor, rr_motor, + compass, motor_max: float): + if math.hypot(vx, vy) < 1e-3 and abs(omega) < 1e-3: + fl_motor.setVelocity(0.0) + fr_motor.setVelocity(0.0) + rl_motor.setVelocity(0.0) + rr_motor.setVelocity(0.0) + return + n = compass.getValues() + h = math.atan2(n[0], n[1]) + w_fl, w_fr, w_rl, w_rr = velocity_to_mecanum_wheels( + vx, vy, omega, h, + max_linear=DOG_MAX_LINEAR, + wheel_radius=DOG_WHEEL_RADIUS, + lx=DOG_WHEEL_BASE_X / 2.0, ly=DOG_WHEEL_BASE_Y / 2.0, + max_wheel_omega=motor_max, + k_turn=4.0, + wheel_base=DOG_WHEEL_BASE, + ) + fl_motor.setVelocity(w_fl) + fr_motor.setVelocity(w_fr) + rl_motor.setVelocity(w_rl) + rr_motor.setVelocity(w_rr) + + # --------------------------------------------------------------------------- # Webots devices # --------------------------------------------------------------------------- @@ -196,13 +240,23 @@ def drive(vx: float, vy: float, left_motor, right_motor, compass, motor_max: flo robot = Robot() timestep = int(robot.getBasicTimeStep()) -left_motor = robot.getDevice("left wheel motor") -right_motor = robot.getDevice("right wheel motor") -left_motor.setPosition(float("inf")) -right_motor.setPosition(float("inf")) -left_motor.setVelocity(0.0) -right_motor.setVelocity(0.0) -MOTOR_MAX = min(left_motor.getMaxVelocity(), DOG_MAX_WHEEL_OMEGA) +if DRIVE_MODE == "mecanum": + fl_motor = robot.getDevice("front left wheel motor") + fr_motor = robot.getDevice("front right wheel motor") + rl_motor = robot.getDevice("rear left wheel motor") + rr_motor = robot.getDevice("rear right wheel motor") + for m in (fl_motor, fr_motor, rl_motor, rr_motor): + m.setPosition(float("inf")) + m.setVelocity(0.0) + MOTOR_MAX = min(fl_motor.getMaxVelocity(), DOG_MAX_WHEEL_OMEGA) +else: + left_motor = robot.getDevice("left wheel motor") + right_motor = robot.getDevice("right wheel motor") + left_motor.setPosition(float("inf")) + right_motor.setPosition(float("inf")) + left_motor.setVelocity(0.0) + right_motor.setVelocity(0.0) + MOTOR_MAX = min(left_motor.getMaxVelocity(), DOG_MAX_WHEEL_OMEGA) gps = robot.getDevice("gps"); gps.enable(timestep) compass = robot.getDevice("compass"); compass.enable(timestep) @@ -235,13 +289,15 @@ analytic_teacher = None if MODE in ("strombom", "sequential"): base_fn = strombom_action if MODE == "strombom" else sequential_action analytic_teacher = ActiveScanTeacher(base_fn) +elif MODE == "universal": + analytic_teacher = ActiveScanTeacher(universal_action) # GT positions from sheep emitters — used **only** for the auto-finish # sentinel and the GT_penned diagnostic line. Never fed into control. _gt_sheep: dict = {} _run_done = False -prev_action = (0.0, 0.0) +prev_action = (0.0, 0.0, 0.0) if DRIVE_MODE == "mecanum" else (0.0, 0.0) step_count = 0 while robot.step(timestep) != -1: @@ -273,26 +329,43 @@ while robot.step(timestep) != -1: single_obs = build_obs(dog_xy, dog_heading, sheep_xy_list, sheep_penned_list) # ---- Action selection ---- + omega = 0.0 if MODE in ("bc", "rl") and policy_handle is not None: action = policy_handle.predict(single_obs) vx, vy = float(action[0]), float(action[1]) + if DRIVE_MODE == "mecanum" and len(action) >= 3: + omega = float(action[2]) else: - vx, vy, _mode_str = analytic_teacher( + result = analytic_teacher( dog_xy, dog_heading, sheep_positions, PEN_ENTRY, + DRIVE_MODE, ) + if len(result) == 4: + vx, vy, omega, _mode_str = result + else: + vx, vy, _mode_str = result # Near-sheep speed modulation (shared by every mode). vx, vy = modulate_speed_near_sheep(vx, vy, dog_xy, sheep_positions) # EMA smoothing — kills frame-to-frame action jitter. - vx = ACTION_SMOOTH * prev_action[0] + (1.0 - ACTION_SMOOTH) * vx - vy = ACTION_SMOOTH * prev_action[1] + (1.0 - ACTION_SMOOTH) * vy + if DRIVE_MODE == "mecanum": + vx = ACTION_SMOOTH * prev_action[0] + (1.0 - ACTION_SMOOTH) * vx + vy = ACTION_SMOOTH * prev_action[1] + (1.0 - ACTION_SMOOTH) * vy + omega = ACTION_SMOOTH * prev_action[2] + (1.0 - ACTION_SMOOTH) * omega + else: + vx = ACTION_SMOOTH * prev_action[0] + (1.0 - ACTION_SMOOTH) * vx + vy = ACTION_SMOOTH * prev_action[1] + (1.0 - ACTION_SMOOTH) * vy # Safety: dog must never enter the pen. vx, vy = safety_clamp(vx, vy, dog_xy[0], dog_xy[1]) - prev_action = (vx, vy) + prev_action = (vx, vy, omega) if DRIVE_MODE == "mecanum" else (vx, vy) - drive(vx, vy, left_motor, right_motor, compass, MOTOR_MAX) + if DRIVE_MODE == "mecanum": + drive_mecanum(vx, vy, omega, fl_motor, fr_motor, rl_motor, rr_motor, + compass, MOTOR_MAX) + else: + drive_diff(vx, vy, left_motor, right_motor, compass, MOTOR_MAX) emitter.send(f"dog:{dog_xy[0]:.4f}:{dog_xy[1]:.4f}") # Cosmetic ear wiggle. @@ -321,7 +394,14 @@ while robot.step(timestep) != -1: gt_penned = sum(1 for x, y in _gt_sheep.values() if is_penned_position(x, y)) gt_total = len(_gt_sheep) - print(f"[dog mode={MODE}] step={step_count} " + print(f"[dog mode={MODE} drive={DRIVE_MODE}] step={step_count} " + f"GT_penned={gt_penned}/{gt_total} " + f"tracks_active={tracker.n_active()} " + f"tracks_penned={tracker.n_penned()} " + f"detections={len(detections)} " + f"action=({vx:+.2f}, {vy:+.2f}, {omega:+.2f})" + if DRIVE_MODE == "mecanum" else + f"[dog mode={MODE} drive={DRIVE_MODE}] step={step_count} " f"GT_penned={gt_penned}/{gt_total} " f"tracks_active={tracker.n_active()} " f"tracks_penned={tracker.n_penned()} " diff --git a/docs/project.md b/docs/project.md index 19160d6..59f55d2 100644 --- a/docs/project.md +++ b/docs/project.md @@ -1,5 +1,9 @@ # Group G25 - Formal & Title & Goals +This is the original course proposal/goals document. For current setup, +training, evaluation, and Webots run instructions, see `../README.md` +and `../training/README.md`. + ## Team members - Diogo Costa - Johnny Fernandes diff --git a/herding/control/active_scan.py b/herding/control/active_scan.py index a81e17d..df7f815 100644 --- a/herding/control/active_scan.py +++ b/herding/control/active_scan.py @@ -33,7 +33,11 @@ class ActiveScanTeacher: Call signature:: - vx, vy, mode = teacher(dog_xy, dog_heading, sheep_positions, pen_target) + vx, vy, omega, mode = teacher(dog_xy, dog_heading, sheep_positions, + pen_target, drive_mode="differential") + + ``omega`` is the yaw-rate intent (mecanum only); 0.0 for differential + drive and during blind exploration phases. """ def __init__(self, base_action_fn, initial_scan_steps: int = INITIAL_SCAN_STEPS): @@ -62,7 +66,8 @@ class ActiveScanTeacher: return 0.0, 0.0 return EXPLORE_SPEED * dx / d, EXPLORE_SPEED * dy / d - def __call__(self, dog_xy, dog_heading, sheep_positions, pen_target): + def __call__(self, dog_xy, dog_heading, sheep_positions, pen_target, + drive_mode="differential"): self.step += 1 n_visible = len(sheep_positions) @@ -75,7 +80,7 @@ class ActiveScanTeacher: if self.step <= self.initial_scan: vx, vy = self._scan_action(dog_heading) self.last_action = (vx, vy) - return vx, vy, "scan_initial" + return vx, vy, 0.0, "scan_initial" # Phase 2: walk-to-centre after a sustained empty tracker. if self.empty_streak >= EMPTY_DEBOUNCE_STEPS: @@ -87,16 +92,31 @@ class ActiveScanTeacher: vx, vy = ex, ey mode = "explore" self.last_action = (vx, vy) - return vx, vy, mode + return vx, vy, 0.0, mode # Phase 2b: brief tracker blink — hold the previous action. if n_visible == 0: vx, vy = self.last_action - return vx, vy, "hold" + return vx, vy, 0.0, "hold" # Phase 3: hand off to the underlying analytic teacher, then # apply the shared near-sheep speed modulation. - vx, vy, mode = self.base(dog_xy, sheep_positions, pen_target) + # Handle both old-style (dog_xy, sheep, pen) and new-style + # (dog_xy, heading, sheep, pen, drive_mode) teachers. + try: + result = self.base(dog_xy, dog_heading, sheep_positions, + pen_target, drive_mode) + except TypeError: + try: + result = self.base(dog_xy, dog_heading, sheep_positions, + pen_target) + except TypeError: + result = self.base(dog_xy, sheep_positions, pen_target) + if len(result) == 4: + vx, vy, omega, mode = result + else: + vx, vy, mode = result + omega = 0.0 vx, vy = modulate_speed_near_sheep(vx, vy, dog_xy, sheep_positions) self.last_action = (vx, vy) - return vx, vy, mode + return vx, vy, omega, mode diff --git a/herding/control/universal.py b/herding/control/universal.py new file mode 100644 index 0000000..de945db --- /dev/null +++ b/herding/control/universal.py @@ -0,0 +1,187 @@ +"""Universal shepherd teacher — Strömbom core + mecanum omega + straggler recovery. + +The core collect/drive logic is **identical** to :mod:`strombom` (same +``F_FACTOR``, ``DELTA_COLLECT``, ``DELTA_DRIVE`` thresholds and target +computation) so it inherits the proven ~100 % success rate at n ≤ 8. +Two additions make it useful as a universal teacher: + +1. **Omega for mecanum.** When ``drive_mode="mecanum"``, the teacher + outputs a non-zero ``omega`` channel so the dog **faces the + direction of travel**. During collect the dog faces the target + sheep; during drive it faces the pen. This gives the BC student a + real rotation signal to learn from. + +2. **Last-straggler recovery.** When exactly one sheep remains active + and it is near the gate, the dog positions itself behind that + straggler (opposite the gate) and pushes it straight through. This + handles the edge case where the last sheep circles the gate posts. + +Call signature:: + + vx, vy, omega, mode = compute_action( + dog_xy, dog_heading, sheep_positions, pen_target, + drive_mode="differential", + ) + +For differential drive ``omega`` is always 0.0 and can be ignored. +""" + +import math + +from herding.world.geometry import ( + PEN_ENTRY, GATE_X, GATE_Y, in_pen, +) + +# --------------------------------------------------------------------------- +# Tuning constants — match Strömbom exactly for proven success rates. +# --------------------------------------------------------------------------- + +F_FACTOR = 4.0 # collect/drive threshold scaled by √n +DELTA_COLLECT = 1.5 # standoff behind the furthest sheep +DELTA_DRIVE = 2.0 # standoff behind flock CoM + +# Omega gain for mecanum (how strongly the dog turns to face target) +OMEGA_GAIN = 0.6 + +# Recovery: push the last straggler straight through the gate. +RECOVERY_GATE_DIST = 6.0 # only when straggler is this close to gate centre +RECOVERY_PUSH_DIST = 1.2 # stand-off behind straggler, away from gate + + +# --------------------------------------------------------------------------- +# Helpers +# --------------------------------------------------------------------------- + +def _unit(x, y): + d = math.hypot(x, y) + if d < 1e-6: + return 0.0, 0.0 + return x / d, y / d + + +def _is_active(x, y) -> bool: + return (not in_pen(x, y)) and y > GATE_Y + + +def _angle_diff(a, b): + """Signed shortest angular difference a - b, in [-π, π].""" + return math.atan2(math.sin(a - b), math.cos(a - b)) + + +def _gate_center(): + """Centre of the gate opening.""" + return (0.5 * (GATE_X[0] + GATE_X[1]), GATE_Y) + + +# --------------------------------------------------------------------------- +# Core teacher +# --------------------------------------------------------------------------- + +def compute_action(dog_xy, dog_heading, sheep_positions, + pen_target=PEN_ENTRY, drive_mode="differential"): + """Return ``(vx, vy, omega, mode)``. + + Parameters + ---------- + dog_xy : (float, float) + Dog position in world frame. + dog_heading : float + Dog heading in world frame (rad), 0 = +x axis. + sheep_positions : dict[str, (float, float)] + Visible sheep positions. + pen_target : (float, float) + Centre of the pen gate (defaults to geometry.PEN_ENTRY). + drive_mode : str + ``"differential"`` or ``"mecanum"``. + + Returns + ------- + vx, vy : float + Velocity intent in [-1, 1]. + omega : float + Yaw intent in [-1, 1] (0 for differential). + mode : str + Phase label: ``"idle"``, ``"collect"``, ``"drive"``, ``"recovery"``. + """ + active = [(x, y) for (x, y) in sheep_positions.values() + if _is_active(x, y)] + if not active: + return 0.0, 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) + + # ---- Last-straggler recovery (single sheep circling near gate) ---- + gc = _gate_center() + if n == 1: + sx, sy = active[0] + d_to_gate = math.hypot(sx - gc[0], sy - gc[1]) + if d_to_gate < RECOVERY_GATE_DIST: + dx_g = sx - gc[0] + dy_g = sy - gc[1] + d_g = math.hypot(dx_g, dy_g) + if d_g > 0.3: + ux, uy = dx_g / d_g, dy_g / d_g + else: + ux, uy = 0.0, 1.0 + tx = sx + RECOVERY_PUSH_DIST * ux + ty = sy + RECOVERY_PUSH_DIST * uy + ax, ay = _unit(tx - dog_xy[0], ty - dog_xy[1]) + mode = "recovery" + face_target = (sx, sy) + omega = 0.0 + if drive_mode == "mecanum": + desired = math.atan2( + face_target[1] - dog_xy[1], + face_target[0] - dog_xy[0], + ) + err = _angle_diff(desired, dog_heading) + omega = max(-1.0, min(1.0, OMEGA_GAIN * err / math.pi)) + return ax, ay, omega, mode + + # ---- Standard Strömbom collect/drive (proven core) ---- + if radius > F_FACTOR * math.sqrt(n): + # Collect: aim behind the furthest sheep, opposite 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" + face_target = (sx, sy) + else: + # Drive: aim behind the CoM, opposite the pen. + 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" + face_target = pen_target + + ax, ay = _unit(tx - dog_xy[0], ty - dog_xy[1]) + + # ---- Omega (mecanum only) ---- + omega = 0.0 + if drive_mode == "mecanum" and mode != "idle": + desired_heading = math.atan2( + face_target[1] - dog_xy[1], + face_target[0] - dog_xy[0], + ) + err = _angle_diff(desired_heading, dog_heading) + omega = max(-1.0, min(1.0, OMEGA_GAIN * err / math.pi)) + + return ax, ay, omega, mode + + +def compute_action_diff(dog_xy, dog_heading, sheep_positions, + pen_target=PEN_ENTRY): + """Compatibility wrapper returning ``(vx, vy, mode)`` — same as Strömbom. + + Use this when plugging into existing differential-drive code that + doesn't expect omega. + """ + vx, vy, _omega, mode = compute_action( + dog_xy, dog_heading, sheep_positions, pen_target, + drive_mode="differential", + ) + return vx, vy, mode diff --git a/herding/perception/lidar_perception.py b/herding/perception/lidar_perception.py index 7988b53..59d67f1 100644 --- a/herding/perception/lidar_perception.py +++ b/herding/perception/lidar_perception.py @@ -24,9 +24,14 @@ import math import numpy as np -from herding.world.geometry import FIELD_X, FIELD_Y, GATE_Y, PEN_X, PEN_Y +from herding.world.geometry import ( + FIELD_SHAPE, FIELD_ROUND_R, + FIELD_X, FIELD_Y, GATE_X, GATE_Y, + PEN_X, PEN_Y, +) from herding.perception.lidar_sim import ( - LIDAR_FOV, LIDAR_MAX_RANGE, LIDAR_N_RAYS, SHEEP_RADIUS, ray_angles, + LIDAR_FOV, LIDAR_MAX_RANGE, LIDAR_N_RAYS, SHEEP_RADIUS, POST_RADIUS, + ray_angles, ) @@ -35,16 +40,94 @@ MAX_CLUSTER_SPAN = 1.5 # m — wider clusters are walls / structures RANGE_HIT_EPS = 0.05 # m — hit if range < max_range - eps WALL_REJECT = 0.5 # m — drop detections this close to a known wall line -# Sheep-sized static features (gate posts, corner pillars). A cluster -# centred within STATIC_REJECT of any of these is never a sheep. -_STATIC_FEATURES = ( +# Multi-peak splitting: within a single cluster, if the range profile +# has a local dip (i.e. the range increases then decreases) deeper than +# SPLIT_RANGE_GAP, the cluster is split into two detections. +SPLIT_RANGE_GAP = 0.20 # m — range increase that triggers a split + +# Sheep-sized static features. A cluster centred within STATIC_REJECT of +# any of these is never a sheep. +_STATIC_FEATURES_RECT = ( ( 10.0, -15.0), ( 13.0, -15.0), # gate posts ( 15.0, 15.0), ( 15.0, -15.0), (-15.0, 15.0), (-15.0, -15.0), # field corners ) + +_STATIC_FEATURES_ROUND = ( + (GATE_X[0], GATE_Y), + (GATE_X[1], GATE_Y), +) + STATIC_REJECT = 0.8 +def _get_static_features(): + if FIELD_SHAPE == "field_round": + return _STATIC_FEATURES_ROUND + return _STATIC_FEATURES_RECT + + +_STATIC_FEATURES = _get_static_features() + + +def _in_field_region(cx: float, cy: float) -> bool: + """Check if a detection is inside the field (with small margin).""" + if FIELD_SHAPE == "field_round": + r = math.hypot(cx, cy) + return r < FIELD_ROUND_R + 0.2 + return (FIELD_X[0] - 0.2 < cx < FIELD_X[1] + 0.2 and + FIELD_Y[0] - 0.2 < cy < FIELD_Y[1] + 0.2) + + +def _near_wall(cx: float, cy: float) -> 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 ( + 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], +) -> list[list[tuple[float, float]]]: + """Split a cluster at range-profile local maxima (gaps between sheep). + + When two sheep are close, the LiDAR sees them as one arc, but the + range profile has a local peak between them (the ray passes between + the two discs). This function finds those peaks and splits. + """ + if len(points) < 4: + return [points] + # Find the minimum range in the cluster (closest point to dog). + r_min = min(range_vals) + # 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: + return [points] + # Find the split point: the index with the maximum range. + split_idx = range_vals.index(r_max) + if split_idx <= 1 or split_idx >= len(points) - 2: + return [points] + # Split into two sub-clusters. + left = points[:split_idx] + right = points[split_idx + 1:] + # Recursively split each half. + result = [] + for sub_pts, sub_ranges in [ + (left, range_vals[:split_idx]), + (right, range_vals[split_idx + 1:]), + ]: + if len(sub_pts) >= 1: + result.extend(_split_cluster_by_range(sub_pts, sub_ranges)) + return result if result else [points] + + def detections_from_scan( ranges: np.ndarray, dog_x: float, dog_y: float, dog_heading: float, @@ -64,63 +147,62 @@ def detections_from_scan( # Walk rays in angular order; a large jump between consecutive # world-frame hit points closes the current cluster. - clusters: list[list[tuple[float, float]]] = [] - current: list[tuple[float, float]] = [] - prev: tuple[float, float] | None = None + # Store (x, y, range) per hit ray for multi-peak splitting. + clusters: list[list[tuple[float, float, float]]] = [] + current: list[tuple[float, float, float]] = [] + prev_xy: tuple[float, float] | None = None for i in range(n_rays): if not bool(hit[i]): if current: clusters.append(current) current = [] - prev = None + prev_xy = None continue - pt = (float(px[i]), float(py[i])) - if prev is not None and math.hypot(pt[0] - prev[0], pt[1] - prev[1]) > GAP_THRESHOLD: + 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: clusters.append(current) current = [] current.append(pt) - prev = pt + prev_xy = (pt[0], pt[1]) if current: clusters.append(current) detections: list[tuple[float, float]] = [] for cluster in clusters: - xs = [p[0] for p in cluster] - ys = [p[1] for p in cluster] - 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: - continue - # Rays hit the front edge of the sheep; offset outward by - # SHEEP_RADIUS along the dog→cluster direction to estimate the - # centre. - 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 - # Region filter: in-field clusters, plus a narrow strip south of - # the gate so sheep mid-crossing get latched penned. Detections - # deeper into the pen are dropped — pen posts and rails would - # otherwise generate phantom penned tracks. - in_main = (FIELD_X[0] - 0.2 < cx < FIELD_X[1] + 0.2 and - FIELD_Y[0] - 0.2 < cy < FIELD_Y[1] + 0.2) - 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 - # Known sheep-sized static features. - if any(math.hypot(cx - fx, cy - fy) < STATIC_REJECT - for fx, fy in _STATIC_FEATURES): - continue - # Wall-proximity filter — sheep can't get this close to a wall, - # so detections right at the wall line are structure noise. - near_field_wall = ( - 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])) - ) - if near_field_wall: - continue - detections.append((cx, cy)) + points_xy = [(p[0], p[1]) for p in cluster] + range_vals = [p[2] for p in cluster] + + # Multi-peak splitting. + if len(cluster) >= 4: + sub_clusters = _split_cluster_by_range(points_xy, range_vals) + else: + sub_clusters = [points_xy] + + for sub in sub_clusters: + if len(sub) < 1: + continue + xs = [p[0] for p in sub] + 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: + continue + # Rays hit the front edge of the sheep; offset outward by + # 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 + 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 + for fx, fy in _STATIC_FEATURES): + continue + if _near_wall(cx, cy): + continue + detections.append((cx, cy)) return detections diff --git a/herding/perception/lidar_sim.py b/herding/perception/lidar_sim.py index 957cd2b..3c8b322 100644 --- a/herding/perception/lidar_sim.py +++ b/herding/perception/lidar_sim.py @@ -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) diff --git a/herding/perception/obs.py b/herding/perception/obs.py index 1b209ce..f78be34 100644 --- a/herding/perception/obs.py +++ b/herding/perception/obs.py @@ -26,14 +26,15 @@ import math import numpy as np from herding.world.geometry import ( - FIELD_X, FIELD_Y, PEN_ENTRY, MAX_SHEEP, + PEN_ENTRY, MAX_SHEEP, distance_to_wall, ) OBS_DIM = 32 def build_obs(dog_xy, dog_heading, sheep_xy_list, sheep_penned_list, - n_max: int = MAX_SHEEP) -> np.ndarray: + n_max: int = MAX_SHEEP, + n_expected: int | None = None) -> np.ndarray: """Assemble the dog policy's observation vector. Parameters @@ -43,6 +44,7 @@ def build_obs(dog_xy, dog_heading, sheep_xy_list, sheep_penned_list, sheep_xy_list : iterable of (x, y) for ALL known sheep sheep_penned_list : parallel iterable of bool — True if sheep is penned n_max : maximum supported flock size used for the count normaliser + n_expected : unused, kept for API compatibility. """ dog_x, dog_y = dog_xy obs = np.zeros(OBS_DIM, dtype=np.float32) @@ -89,16 +91,8 @@ def build_obs(dog_xy, dog_heading, sheep_xy_list, sheep_penned_list, obs[15] = float(rel[far_idx, 0]) / 15.0 obs[16] = float(rel[far_idx, 1]) / 15.0 - min_sheep_wall = min( - float(np.min(arr[:, 0] - FIELD_X[0])), - float(np.min(FIELD_X[1] - arr[:, 0])), - float(np.min(arr[:, 1] - FIELD_Y[0])), - float(np.min(FIELD_Y[1] - arr[:, 1])), - ) - min_dog_wall = min( - dog_x - FIELD_X[0], FIELD_X[1] - dog_x, - dog_y - FIELD_Y[0], FIELD_Y[1] - dog_y, - ) + min_sheep_wall = float(min(distance_to_wall(sx, sy) for sx, sy in active)) + min_dog_wall = distance_to_wall(dog_x, dog_y) obs[17] = min_sheep_wall / 15.0 obs[18] = float(min_dog_wall) / 15.0 obs[19] = n / n_max diff --git a/herding/perception/sheep_tracker.py b/herding/perception/sheep_tracker.py index 278a4fc..44ea090 100644 --- a/herding/perception/sheep_tracker.py +++ b/herding/perception/sheep_tracker.py @@ -6,6 +6,14 @@ 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. +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. + 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. @@ -25,16 +33,71 @@ PENNED_GATE_M = 4.0 # m — gate for matching detections to existing penne FORGET_STEPS = 200 # ~3.2 s — delete stale active tracks (penned ones kept forever) MAX_ACTIVE_TRACKS = MAX_SHEEP +# Predictive tracking constants. +PREDICT_STEPS = 120 # ~1.9 s — extrapolate velocity this many frames +VELOCITY_CLAMP = 1.0 # m/s — max predicted speed (sheep max is ~0.78 m/s) + + +class Track: + """Single track with position, velocity, and age.""" + + __slots__ = ("x", "y", "vx", "vy", "last_seen", "penned") + + def __init__(self, x: float, y: float, step: int, penned: bool = False): + self.x = x + self.y = y + self.vx = 0.0 + self.vy = 0.0 + self.last_seen = step + self.penned = penned + + @property + def age(self) -> int: + """Not-a-property in the hot loop — callers pass current step.""" + raise NotImplementedError + + def predicted_position(self, current_step: int) -> tuple[float, float]: + """Extrapolated position using constant velocity, clamped.""" + dt = current_step - self.last_seen + if dt <= 0 or dt > PREDICT_STEPS: + return self.x, self.y + speed = math.hypot(self.vx, self.vy) + if speed < 1e-4: + return self.x, self.y + # Clamp extrapolation distance. + max_d = VELOCITY_CLAMP * dt * 0.016 # steps → seconds + d = min(speed * dt * 0.016, max_d) + return ( + self.x + d * (self.vx / speed), + self.y + d * (self.vy / speed), + ) + + def update(self, x: float, y: float, step: int) -> None: + """Absorb a new detection and re-estimate velocity.""" + dt = step - self.last_seen + if dt > 0: + dt_s = dt * 0.016 # steps → seconds + new_vx = (x - self.x) / dt_s + new_vy = (y - self.y) / dt_s + # Exponential smoothing on velocity. + alpha = 0.6 + self.vx = alpha * new_vx + (1.0 - alpha) * self.vx + self.vy = alpha * new_vy + (1.0 - alpha) * self.vy + self.x = x + self.y = y + self.last_seen = step + class SheepTracker: - """Online tracker with NN association and forgetful memory. + """Online tracker with NN association, prediction, and forgetful memory. - Each track stores ``(x, y, last_seen_step, penned)``. + Each track is a :class:`Track` with position, velocity estimate, + last-seen step, and penned flag. """ def __init__(self, gate: float = GATE_M): self.gate = gate - self._tracks: dict[int, tuple[float, float, int, bool]] = {} + self._tracks: dict[int, Track] = {} self._next_id = 0 self.step = 0 @@ -50,13 +113,14 @@ class SheepTracker: det_used: set[int] = set() updated_tids: set[int] = set() - # Pass 1 — match active tracks within the primary gate. Oldest- - # seen tracks bind first so a re-emerging long-lost sheep keeps - # its old ID instead of being grabbed by a fresh neighbour. - active_tids = [tid for tid, t in self._tracks.items() if not t[3]] - active_tids.sort(key=lambda tid: self._tracks[tid][2]) + # Pass 1 — match active tracks within the primary gate. + # Use predicted positions for matching, oldest-first. + active_tids = [tid for tid, t in self._tracks.items() if not t.penned] + active_tids.sort(key=lambda tid: self._tracks[tid].last_seen) for tid in active_tids: - tx, ty, _, _ = self._tracks[tid] + track = self._tracks[tid] + # Use predicted position for matching. + tx, ty = track.predicted_position(self.step) best_j, best_d = -1, self.gate for j, (dx, dy) in enumerate(detections): if j in det_used: @@ -67,20 +131,18 @@ class SheepTracker: best_j = j if best_j >= 0: dx, dy = detections[best_j] - self._tracks[tid] = (dx, dy, self.step, False) + track.update(dx, dy, self.step) det_used.add(best_j) updated_tids.add(tid) - # Pass 1b — re-acquisition. Sheep flee at ~0.6 m/s, so over a - # 1–2 s occlusion the same sheep may reappear outside the primary - # gate. Allow rebinding within a wider gate for stale-enough - # tracks; otherwise phantom tracks accumulate and corrupt CoM. + # Pass 1b — re-acquisition with wider gate for stale tracks. for tid in active_tids: if tid in updated_tids: continue - tx, ty, last, _ = self._tracks[tid] - if (self.step - last) < REACQUIRE_MIN_AGE: + track = self._tracks[tid] + if (self.step - track.last_seen) < REACQUIRE_MIN_AGE: continue + tx, ty = track.predicted_position(self.step) best_j, best_d = -1, REACQUIRE_GATE_M for j, (dx, dy) in enumerate(detections): if j in det_used: @@ -91,53 +153,52 @@ class SheepTracker: best_j = j if best_j >= 0: dx, dy = detections[best_j] - self._tracks[tid] = (dx, dy, self.step, False) + track.update(dx, dy, self.step) det_used.add(best_j) updated_tids.add(tid) # Pass 2 — match remaining detections to penned tracks. - penned_tids = [tid for tid, t in self._tracks.items() if t[3]] + penned_tids = [tid for tid, t in self._tracks.items() if t.penned] for tid in penned_tids: - tx, ty, _, _ = self._tracks[tid] + track = self._tracks[tid] best_j, best_d = -1, PENNED_GATE_M for j, (dx, dy) in enumerate(detections): if j in det_used: continue - d = math.hypot(dx - tx, dy - ty) + d = math.hypot(dx - track.x, dy - track.y) if d < best_d: best_d = d best_j = j if best_j >= 0: dx, dy = detections[best_j] - self._tracks[tid] = (dx, dy, self.step, True) + track.update(dx, dy, self.step) det_used.add(best_j) - # Spawn new tracks for unmatched detections. Born "penned" if - # the detection already sits inside the pen geometry. + # Spawn new tracks for unmatched detections. for j, (dx, dy) in enumerate(detections): if j in det_used: continue penned = in_pen(dx, dy) or is_penned_position(dx, dy) - self._tracks[self._next_id] = (dx, dy, self.step, penned) + self._tracks[self._next_id] = Track(dx, dy, self.step, penned) self._next_id += 1 # Promote active tracks whose current estimate crosses the gate. - for tid, (tx, ty, last, penned) in list(self._tracks.items()): - if penned: + for track in self._tracks.values(): + if track.penned: continue - if is_penned_position(tx, ty): - self._tracks[tid] = (tx, ty, last, True) + px, py = track.predicted_position(self.step) + if is_penned_position(px, py): + track.penned = True # Forget stale active tracks; penned tracks live forever. - for tid, (tx, ty, last, penned) in list(self._tracks.items()): - if penned: - continue - if (self.step - last) > FORGET_STEPS: - del self._tracks[tid] + stale = [tid for tid, t in self._tracks.items() + if not t.penned and (self.step - t.last_seen) > FORGET_STEPS] + for tid in stale: + del self._tracks[tid] # Hard cap on the active set — drop the oldest-seen overflow. - active = [(tid, last) for tid, (_, _, last, p) in self._tracks.items() - if not p] + active = [(tid, t.last_seen) for tid, t in self._tracks.items() + if not t.penned] if len(active) > MAX_ACTIVE_TRACKS: active.sort(key=lambda kv: kv[1]) for tid, _ in active[: len(active) - MAX_ACTIVE_TRACKS]: @@ -146,16 +207,31 @@ class SheepTracker: return self.get_positions() def get_positions(self) -> dict[str, tuple[float, float]]: - """Active (not-penned) tracks as a ``{name: (x, y)}`` dict.""" - return {f"t{tid}": (x, y) - for tid, (x, y, _, penned) in self._tracks.items() - if not penned} + """Active (not-penned) tracks as a ``{name: (x, y)}`` dict. + + For tracks currently being predicted (occluded but within + PREDICT_STEPS), returns the extrapolated position so the teacher + sees a smooth estimate. + """ + result = {} + for tid, track in self._tracks.items(): + if track.penned: + continue + px, py = track.predicted_position(self.step) + result[f"t{tid}"] = (px, py) + return result def get_penned_set(self) -> set[str]: - return {f"t{tid}" for tid, (_, _, _, penned) in self._tracks.items() if penned} + return {f"t{tid}" for tid, t in self._tracks.items() if t.penned} def n_active(self) -> int: - return sum(1 for _, _, _, penned in self._tracks.values() if not penned) + return sum(1 for t in self._tracks.values() if not t.penned) def n_penned(self) -> int: - return sum(1 for _, _, _, penned in self._tracks.values() if penned) + return sum(1 for t in self._tracks.values() if t.penned) + + def n_predicted(self) -> int: + """Number of active tracks currently being extrapolated (not directly observed).""" + return sum(1 for t in self._tracks.values() + if not t.penned and (self.step - t.last_seen) > 0 + and (self.step - t.last_seen) <= PREDICT_STEPS) diff --git a/herding/world/diffdrive.py b/herding/world/diffdrive.py index 5e9c821..b965746 100644 --- a/herding/world/diffdrive.py +++ b/herding/world/diffdrive.py @@ -1,4 +1,5 @@ -"""Differential-drive kinematics, shared by the env and Webots controllers. +"""Differential-drive and mecanum kinematics, shared by the env and Webots +controllers. First-order rigid-body model — no slip, wheel-accel limits, or contact forces. Webots' ODE physics handles those at inference; the env stays @@ -59,3 +60,131 @@ def heading_speed_to_wheels(heading, speed_motor, h, max_wheel_omega, left = max(-max_wheel_omega, min(max_wheel_omega, fwd - turn)) right = max(-max_wheel_omega, min(max_wheel_omega, fwd + turn)) return left, right + + +# --------------------------------------------------------------------------- +# Mecanum (4-wheel omnidirectional) kinematics +# --------------------------------------------------------------------------- + +def mecanum_kinematics_step(x, y, h, w_fl, w_fr, w_rl, w_rr, + wheel_radius, lx, ly, dt): + """Integrate one step of mecanum forward kinematics. + + Parameters + ---------- + x, y : robot position (m) + h : robot heading (rad), 0 = +x axis + w_fl, w_fr, w_rl, w_rr : wheel angular velocities (rad/s) + wheel_radius : wheel radius (m) + lx : half the front-to-back axle distance (m) + ly : half the left-to-right axle distance (m) + dt : timestep (s) + + Returns (new_x, new_y, new_h). + """ + r = wheel_radius + vx_body = (w_fl + w_fr + w_rl + w_rr) * r / 4.0 + vy_body = (-w_fl + w_fr + w_rl - w_rr) * r / 4.0 + omega = (-w_fl + w_fr - w_rl + w_rr) * r / (4.0 * (lx + ly)) + + cos_h = math.cos(h) + sin_h = math.sin(h) + vx_world = vx_body * cos_h - vy_body * sin_h + vy_world = vx_body * sin_h + vy_body * cos_h + + new_x = x + vx_world * dt + new_y = y + vy_world * dt + new_h = math.atan2(math.sin(h + omega * dt), math.cos(h + omega * dt)) + return new_x, new_y, new_h + + +def mecanum_inverse(vx_body, vy_body, omega, wheel_radius, lx, ly, + max_wheel_omega): + """Mecanum inverse kinematics: body-frame velocities to 4 wheel speeds. + + Parameters + ---------- + vx_body, vy_body : desired body-frame linear velocities (m/s) + omega : desired yaw rate (rad/s) + wheel_radius : wheel radius (m) + lx : half front-to-back axle distance (m) + ly : half left-to-right axle distance (m) + max_wheel_omega : wheel angular velocity clamp (rad/s) + + Returns (w_fl, w_fr, w_rl, w_rr). + """ + r = wheel_radius + k = lx + ly + w_fl = (vx_body - vy_body - k * omega) / r + w_fr = (vx_body + vy_body + k * omega) / r + w_rl = (vx_body + vy_body - k * omega) / r + w_rr = (vx_body - vy_body + k * omega) / r + + scale = max(abs(w_fl), abs(w_fr), abs(w_rl), abs(w_rr), 1e-9) + if scale > max_wheel_omega: + ratio = max_wheel_omega / scale + w_fl *= ratio + w_fr *= ratio + w_rl *= ratio + w_rr *= ratio + + return w_fl, w_fr, w_rl, w_rr + + +def velocity_to_mecanum_wheels(vx, vy, omega, h, max_linear, wheel_radius, + lx, ly, max_wheel_omega, + k_turn=4.0, wheel_base=0.28): + """Convert world-frame (vx, vy, omega) action in [-1, 1]^3 to 4 wheel speeds. + + Truly holonomic interpretation: (vx, vy) is the desired *world-frame* + velocity (magnitude up to ``max_linear`` m/s) and ``omega`` is the + desired yaw rate (independent of motion direction). The dog can + crab-walk and rotate at the same time. + + This matches the universal teacher's signal: drive toward a standoff + point while facing the sheep / pen separately. With the older + non-holonomic version, ``omega`` from the teacher fought against the + forward-only kinematics and dropped success rates instead of helping. + + Parameters + ---------- + vx, vy : desired world-frame velocity intent in [-1, 1] (clamped on + magnitude to ≤ 1) + omega : desired yaw rate intent in [-1, 1] + h : current heading (rad), 0 = +x + max_linear : max linear speed (m/s) + wheel_radius : wheel radius (m) + lx, ly : half axle distances (m) + max_wheel_omega : wheel angular velocity clamp (rad/s) + k_turn : unused (kept for signature compatibility) + wheel_base : unused (kept for signature compatibility) + + Returns (w_fl, w_fr, w_rl, w_rr). + """ + # Clamp the action magnitude in the (vx, vy) unit disk. + norm = math.hypot(vx, vy) + if norm > 1.0: + vx /= norm + vy /= norm + + # World-frame velocity → body-frame velocity (rotate by -h). + vx_world = vx * max_linear + vy_world = vy * max_linear + cos_h = math.cos(h) + sin_h = math.sin(h) + vx_body = cos_h * vx_world + sin_h * vy_world + vy_body = -sin_h * vx_world + cos_h * vy_world + + # Yaw rate: omega ∈ [-1, 1] maps to ± max_linear / (lx + ly) — same + # peak yaw as the old "omega_extra" channel, but used directly + # rather than added to a heading-tracker. + yaw_max = max_linear / max(lx + ly, 1e-6) + omega_rad = omega * yaw_max + + if abs(vx_body) < 1e-3 and abs(vy_body) < 1e-3 and abs(omega_rad) < 1e-3: + return 0.0, 0.0, 0.0, 0.0 + + return mecanum_inverse( + vx_body, vy_body, omega_rad, + wheel_radius, lx, ly, max_wheel_omega, + ) diff --git a/herding/world/flocking_sim.py b/herding/world/flocking_sim.py index 2515be7..1447dd5 100644 --- a/herding/world/flocking_sim.py +++ b/herding/world/flocking_sim.py @@ -27,9 +27,10 @@ import math import random from herding.world.geometry import ( + FIELD_SHAPE, FIELD_ROUND_R, FIELD_X, FIELD_Y, PEN_X, PEN_Y, - GATE_X, + GATE_X, GATE_Y, ) # Speeds are in wheel rad/s (motor units); m/s = speed * SHEEP_WHEEL_RADIUS. @@ -131,33 +132,49 @@ def compute_heading_speed(x, y, penned, dog_xy, peers, wander_angle, rng=None): fx -= (ddx / d) * push * 2.5 fy -= (ddy / d) * push * 2.5 - # Wall soft repulsion (south wall absent inside the gate column). - if x < FIELD_X[0] + WALL_MARGIN: - fx += ((FIELD_X[0] + WALL_MARGIN - x) / WALL_MARGIN) * 6.0 - if x > FIELD_X[1] - WALL_MARGIN: - fx -= ((x - (FIELD_X[1] - WALL_MARGIN)) / WALL_MARGIN) * 6.0 - if y > FIELD_Y[1] - WALL_MARGIN: - fy -= ((y - (FIELD_Y[1] - WALL_MARGIN)) / WALL_MARGIN) * 6.0 - if y < FIELD_Y[0] + WALL_MARGIN and not (GATE_X[0] <= x <= GATE_X[1]): - fy += ((FIELD_Y[0] + WALL_MARGIN - y) / WALL_MARGIN) * 6.0 + # Wall soft repulsion. + if FIELD_SHAPE == "field_round": + r = math.hypot(x, y) + wall_d = FIELD_ROUND_R - r + in_gate_col = (GATE_X[0] <= x <= GATE_X[1] + and y < GATE_Y + WALL_MARGIN) + if wall_d < WALL_MARGIN and r > 1e-6 and not in_gate_col: + gain = ((WALL_MARGIN - wall_d) / WALL_MARGIN) * 6.0 + fx -= (x / r) * gain + fy -= (y / r) * gain + # Hard escape band. + if wall_d < WALL_HARD_MARGIN and not in_gate_col: + hgain = WALL_HARD_GAIN * (1.0 - wall_d / WALL_HARD_MARGIN) + fx -= (x / r) * hgain + fy -= (y / r) * hgain + else: + # Rectangular: south wall absent inside the gate column. + if x < FIELD_X[0] + WALL_MARGIN: + fx += ((FIELD_X[0] + WALL_MARGIN - x) / WALL_MARGIN) * 6.0 + if x > FIELD_X[1] - WALL_MARGIN: + fx -= ((x - (FIELD_X[1] - WALL_MARGIN)) / WALL_MARGIN) * 6.0 + if y > FIELD_Y[1] - WALL_MARGIN: + fy -= ((y - (FIELD_Y[1] - WALL_MARGIN)) / WALL_MARGIN) * 6.0 + if y < FIELD_Y[0] + WALL_MARGIN and not (GATE_X[0] <= x <= GATE_X[1]): + fy += ((FIELD_Y[0] + WALL_MARGIN - y) / WALL_MARGIN) * 6.0 + + # Hard escape band — overrides everything else near a wall. + m, g = WALL_HARD_MARGIN, WALL_HARD_GAIN + if x - FIELD_X[0] < m: + fx = max(fx, g * (1.0 - (x - FIELD_X[0]) / m)) + if FIELD_X[1] - x < m: + fx = min(fx, -g * (1.0 - (FIELD_X[1] - x) / m)) + if FIELD_Y[1] - y < m: + fy = min(fy, -g * (1.0 - (FIELD_Y[1] - y) / m)) + if (y - FIELD_Y[0] < m) and not (GATE_X[0] <= x <= GATE_X[1]): + fy = max(fy, g * (1.0 - (y - FIELD_Y[0]) / m)) if not fleeing: - if random.random() < 0.02: - wander_angle += random.uniform(-0.6, 0.6) + if rnd.random() < 0.02: + wander_angle += rnd.uniform(-0.6, 0.6) fx += math.cos(wander_angle) * 0.5 fy += math.sin(wander_angle) * 0.5 - # Hard escape band — overrides everything else near a wall. - m, g = WALL_HARD_MARGIN, WALL_HARD_GAIN - if x - FIELD_X[0] < m: - fx = max(fx, g * (1.0 - (x - FIELD_X[0]) / m)) - if FIELD_X[1] - x < m: - fx = min(fx, -g * (1.0 - (FIELD_X[1] - x) / m)) - if FIELD_Y[1] - y < m: - fy = min(fy, -g * (1.0 - (FIELD_Y[1] - y) / m)) - if (not penned) and (y - FIELD_Y[0] < m) and not (GATE_X[0] <= x <= GATE_X[1]): - fy = max(fy, g * (1.0 - (y - FIELD_Y[0]) / m)) - heading = math.atan2(fy, fx) mag = math.hypot(fx, fy) speed = max(WANDER_SPEED, min(FLEE_SPEED, mag * 3.0)) diff --git a/herding/world/geometry.py b/herding/world/geometry.py index f6defb4..28d1c03 100644 --- a/herding/world/geometry.py +++ b/herding/world/geometry.py @@ -4,20 +4,35 @@ Coordinates are metres; (0, 0) is the field centre, +x east, +y north. These constants mirror ``worlds/field.wbt`` and the proto files — if the world changes, this file is the single point of update. - field +y north + field (rectangular) +-----------+ | | - | | | ...... | - +---||||----+ y = -15 (south wall, 3 m gate at x ∈ [10, 13]) + +---||||----+ y = -15 (south wall, 3 m gate at x in [10, 13]) |||| - |pen| y ∈ [-22, -15] + |pen| y in [-22, -15] +---+ + + field_round (circular, R = 15 m) + .---. + / ... \\ + | ..... | gate at south, x in [-1.83, 1.83] + \\ ... / + '-+-' pen y in [-22, -15] """ +import os import math -# Field (square, stone-walled) +# --------------------------------------------------------------------------- +# Field shape selection — controlled by HERDING_WORLD env var at runtime. +# Defaults to "field" (rectangular). The launcher writes it into the +# runtime cfg so the controller can pick it up too. +# --------------------------------------------------------------------------- +FIELD_SHAPE = (os.environ.get("HERDING_WORLD", "field")).lower() + + +# ==================== Rectangular field (field.wbt) ==================== FIELD_X = (-15.0, 15.0) FIELD_Y = (-15.0, 15.0) FIELD_INSIDE_MARGIN = 0.5 @@ -32,12 +47,67 @@ PEN_ENTRY = (0.5 * (PEN_X[0] + PEN_X[1]), -15.0) GATE_X = PEN_X GATE_Y = -15.0 + +# ==================== Round field (field_round.wbt) ==================== +FIELD_ROUND_R = 15.0 +FIELD_ROUND_PEN_X = (-1.5, 1.5) +FIELD_ROUND_PEN_Y = (-22.0, -15.0) +FIELD_ROUND_PEN_CENTER = ( + 0.5 * (FIELD_ROUND_PEN_X[0] + FIELD_ROUND_PEN_X[1]), + 0.5 * (FIELD_ROUND_PEN_Y[0] + FIELD_ROUND_PEN_Y[1]), +) +FIELD_ROUND_PEN_ENTRY = (0.0, -15.0) +FIELD_ROUND_GATE_X = FIELD_ROUND_PEN_X +FIELD_ROUND_GATE_Y = -15.0 + + +# ==================== Active geometry (resolved at import) =============== +# Rectangular defaults are already assigned above. Override for round. +if FIELD_SHAPE == "field_round": + PEN_X = FIELD_ROUND_PEN_X + PEN_Y = FIELD_ROUND_PEN_Y + PEN_CENTER = FIELD_ROUND_PEN_CENTER + PEN_ENTRY = FIELD_ROUND_PEN_ENTRY + GATE_X = FIELD_ROUND_GATE_X + GATE_Y = FIELD_ROUND_GATE_Y + + +def configure(shape: str) -> None: + """Switch the active field geometry at runtime. + + Call this **before** importing any other ``herding.*`` module that + depends on the constants below (flocking_sim, lidar_sim, obs, etc.). + The import-time env-var path (``HERDING_WORLD``) still works; this + function is for scripts that need to choose the world via a CLI flag. + """ + global FIELD_SHAPE, PEN_X, PEN_Y, PEN_CENTER, PEN_ENTRY, GATE_X, GATE_Y + shape = shape.lower() + FIELD_SHAPE = shape + if shape == "field_round": + PEN_X = FIELD_ROUND_PEN_X + PEN_Y = FIELD_ROUND_PEN_Y + PEN_CENTER = FIELD_ROUND_PEN_CENTER + PEN_ENTRY = FIELD_ROUND_PEN_ENTRY + GATE_X = FIELD_ROUND_GATE_X + GATE_Y = FIELD_ROUND_GATE_Y + else: + PEN_X = (10.0, 13.0) + PEN_Y = (-22.0, -15.0) + PEN_CENTER = (0.5 * (PEN_X[0] + PEN_X[1]), 0.5 * (PEN_Y[0] + PEN_Y[1])) + PEN_ENTRY = (0.5 * (PEN_X[0] + PEN_X[1]), -15.0) + GATE_X = PEN_X + GATE_Y = -15.0 + # Dog spec — protos/ShepherdDog.proto DOG_WHEEL_RADIUS = 0.038 # m DOG_WHEEL_BASE = 0.28 # m, axle-to-axle DOG_MAX_WHEEL_OMEGA = 70.0 # rad/s DOG_MAX_LINEAR = DOG_WHEEL_RADIUS * DOG_MAX_WHEEL_OMEGA # ≈ 2.66 m/s +# Dog mecanum spec — 4-wheel omnidirectional layout +DOG_WHEEL_BASE_X = 0.28 # m, front-to-back axle distance +DOG_WHEEL_BASE_Y = 0.28 # m, left-to-right axle distance + # Sheep spec — protos/Sheep.proto SHEEP_WHEEL_RADIUS = 0.031 # m SHEEP_WHEEL_BASE = 0.20 # m @@ -58,21 +128,58 @@ def in_pen(x: float, y: float) -> bool: def in_field(x: float, y: float, margin: float = 0.0) -> bool: + if FIELD_SHAPE == "field_round": + r = FIELD_ROUND_R - margin + return x * x + y * y <= r * r return (FIELD_X[0] + margin <= x <= FIELD_X[1] - margin and FIELD_Y[0] + margin <= y <= FIELD_Y[1] - margin) def in_gate_corridor(x: float, y: float, margin: float = 0.0) -> bool: """True if (x, y) lies in the column of the gate (between field and pen).""" - return (PEN_X[0] - margin <= x <= PEN_X[1] + margin + return (GATE_X[0] - margin <= x <= GATE_X[1] + margin and PEN_Y[0] - margin <= y <= GATE_Y + margin) def is_penned_position(x: float, y: float, latch_margin: float = 0.2) -> bool: """True iff (x, y) is in the gate column and south of the gate line.""" - return (PEN_X[0] - latch_margin <= x <= PEN_X[1] + latch_margin + return (GATE_X[0] - latch_margin <= x <= GATE_X[1] + latch_margin and y <= GATE_Y) def distance_to_pen_entry(x: float, y: float) -> float: return math.hypot(x - PEN_ENTRY[0], y - PEN_ENTRY[1]) + + +def distance_to_wall(x: float, y: float) -> float: + """Shortest distance from (x, y) to the nearest field wall. + + For a rectangular field this is the minimum Manhattan distance to the + four bounding walls. For a round field it is ``R - sqrt(x²+y²)``. + Returns a negative value if the point is outside the field. + """ + if FIELD_SHAPE == "field_round": + return FIELD_ROUND_R - math.hypot(x, y) + return min( + x - FIELD_X[0], FIELD_X[1] - x, + y - FIELD_Y[0], FIELD_Y[1] - y, + ) + + +def clip_to_field(x: float, y: float, margin: float = 0.2) -> tuple[float, float]: + """Clip (x, y) inside the field boundary with a small margin. + + For round fields the point is projected radially inward if it exceeds + the circular boundary. + """ + if FIELD_SHAPE == "field_round": + r = math.hypot(x, y) + limit = FIELD_ROUND_R - margin + if r > limit and r > 1e-6: + scale = limit / r + return x * scale, y * scale + return x, y + return ( + max(FIELD_X[0] + margin, min(FIELD_X[1] - margin, x)), + max(FIELD_Y[0] + margin, min(FIELD_Y[1] - margin, y)), + ) diff --git a/protos/ShepherdDogMecanum.proto b/protos/ShepherdDogMecanum.proto new file mode 100644 index 0000000..dd3ead6 --- /dev/null +++ b/protos/ShepherdDogMecanum.proto @@ -0,0 +1,885 @@ +#VRML_SIM R2025a utf8 +# Shepherd Dog Robot - mecanum-wheeled base with dog character on top +# 4-wheel omnidirectional drive (front-left, front-right, rear-left, rear-right). + +PROTO ShepherdDogMecanum [ + field SFVec3f translation 0 0 0 + field SFRotation rotation 0 1 0 0 + field SFString name "ShepherdDog" + field SFString controller "shepherd_dog" + field MFString controllerArgs [] + field SFString customData "" + field SFBool supervisor FALSE + field SFBool synchronization TRUE +] +{ + Robot { + translation IS translation + rotation IS rotation + name IS name + controller IS controller + controllerArgs IS controllerArgs + customData IS customData + supervisor IS supervisor + synchronization IS synchronization + children [ + # ========== CHASSIS / BASE ========== + DEF CHASSIS Transform { + translation 0 0 0.05 + children [ + Shape { + appearance DEF CHASSIS_APP PBRAppearance { + baseColor 0.2 0.2 0.2 + roughness 0.6 + metalness 0.3 + } + geometry Box { + size 0.32 0.16 0.06 + } + } + ] + } + # Front slope + DEF CHASSIS_FRONT Transform { + translation 0.14 0 0.07 + children [ + Shape { + appearance USE CHASSIS_APP + geometry Box { + size 0.06 0.14 0.04 + } + } + ] + } + # Rear slope + DEF CHASSIS_REAR Transform { + translation -0.14 0 0.07 + children [ + Shape { + appearance USE CHASSIS_APP + geometry Box { + size 0.06 0.14 0.04 + } + } + ] + } + + # ========== DOG BODY on top of chassis ========== + DEF BODY Transform { + translation 0 0 0.11 + children [ + Shape { + appearance DEF FUR_BROWN PBRAppearance { + baseColor 0.55 0.35 0.17 + roughness 0.85 + metalness 0.0 + } + geometry Box { + size 0.30 0.16 0.08 + } + } + ] + } + + # ========== CHEST ========== + DEF CHEST Transform { + translation 0.12 0 0.11 + children [ + Shape { + appearance DEF FUR_CREAM PBRAppearance { + baseColor 0.85 0.72 0.55 + roughness 0.85 + metalness 0.0 + } + geometry Box { + size 0.08 0.18 0.08 + } + } + ] + } + + # ========== HEAD ========== + DEF HEAD Transform { + translation 0.20 0 0.17 + children [ + Shape { + appearance USE FUR_BROWN + geometry Box { + size 0.10 0.12 0.09 + } + } + ] + } + + # ========== SNOUT + LIDAR ========== + DEF SNOUT Transform { + translation 0.28 0 0.155 + children [ + Shape { + appearance USE FUR_CREAM + geometry Box { + size 0.08 0.07 0.05 + } + } + # Nose + Transform { + translation 0.04 0 0.01 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.1 0.1 0.1 + roughness 0.4 + } + geometry Sphere { + radius 0.013 + subdivision 2 + } + } + ] + } + # Lidar — front-facing 140° FOV, mounted at snout tip + Lidar { + translation 0.05 0 0.01 + name "lidar" + horizontalResolution 180 + fieldOfView 2.44 + numberOfLayers 1 + minRange 0.10 + maxRange 12.0 + noise 0.005 + } + ] + } + + # ========== LEFT EAR ========== + DEF LEFT_EAR HingeJoint { + jointParameters HingeJointParameters { + axis 0 0 1 + anchor 0.19 0.055 0.21 + } + device [ + RotationalMotor { + name "left ear motor" + maxVelocity 10.0 + minPosition -0.5 + maxPosition 0.5 + } + ] + endPoint Solid { + translation 0.19 0.055 0.21 + rotation 0 0 1 0.2 + name "left ear" + children [ + Shape { + appearance DEF FUR_DARK PBRAppearance { + baseColor 0.35 0.20 0.10 + roughness 0.85 + metalness 0.0 + } + geometry Box { + size 0.035 0.025 0.06 + } + } + ] + boundingObject Box { + size 0.035 0.025 0.06 + } + physics Physics { + density -1 + mass 0.005 + } + } + } + + # ========== RIGHT EAR ========== + DEF RIGHT_EAR HingeJoint { + jointParameters HingeJointParameters { + axis 0 0 1 + anchor 0.19 -0.055 0.21 + } + device [ + RotationalMotor { + name "right ear motor" + maxVelocity 10.0 + minPosition -0.5 + maxPosition 0.5 + } + ] + endPoint Solid { + translation 0.19 -0.055 0.21 + rotation 0 0 -1 0.2 + name "right ear" + children [ + Shape { + appearance USE FUR_DARK + geometry Box { + size 0.035 0.025 0.06 + } + } + ] + boundingObject Box { + size 0.035 0.025 0.06 + } + physics Physics { + density -1 + mass 0.005 + } + } + } + + # ========== EYES ========== + DEF LEFT_EYE Transform { + translation 0.25 0.05 0.19 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.95 0.95 0.95 + roughness 0.3 + } + geometry Sphere { + radius 0.016 + subdivision 2 + } + } + # Pupil + Transform { + translation 0.012 0 0.004 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.1 0.1 0.1 + roughness 0.2 + } + geometry Sphere { + radius 0.009 + subdivision 2 + } + } + ] + } + ] + } + DEF RIGHT_EYE Transform { + translation 0.25 -0.05 0.19 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.95 0.95 0.95 + roughness 0.3 + } + geometry Sphere { + radius 0.016 + subdivision 2 + } + } + # Pupil + Transform { + translation 0.012 0 0.004 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.1 0.1 0.1 + roughness 0.2 + } + geometry Sphere { + radius 0.009 + subdivision 2 + } + } + ] + } + ] + } + + # ========== COLLAR ========== + DEF COLLAR Transform { + translation 0.16 0 0.125 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.8 0.1 0.1 + roughness 0.5 + } + geometry Cylinder { + height 0.02 + radius 0.095 + subdivision 16 + } + } + # ID tag + Transform { + translation 0 0.10 0 + rotation 1 0 0 1.5708 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.75 0.75 0.0 + metalness 0.8 + roughness 0.2 + } + geometry Cylinder { + height 0.003 + radius 0.018 + subdivision 8 + } + } + ] + } + ] + } + + # ========== TAIL (lidar inside tail tip ball) ========== + DEF TAIL HingeJoint { + jointParameters HingeJointParameters { + axis 0 1 0 + anchor -0.15 0 0.11 + } + device [ + RotationalMotor { + name "tail motor" + maxVelocity 5.0 + minPosition -1.0 + maxPosition 1.0 + } + ] + endPoint Solid { + translation -0.17 0 0.13 + name "tail solid" + children [ + Shape { + appearance USE FUR_BROWN + geometry Capsule { + height 0.12 + radius 0.013 + top FALSE + } + } + # Tail tip ball + Transform { + translation 0 0 0.08 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.2 0.2 0.2 + roughness 0.3 + metalness 0.6 + } + geometry Sphere { + radius 0.028 + subdivision 4 + } + } + ] + } + ] + boundingObject Group { + children [ + Capsule { + height 0.12 + radius 0.013 + } + Transform { + translation 0 0 0.08 + children [ + Sphere { + radius 0.028 + } + ] + } + ] + } + physics Physics { + density -1 + mass 0.08 + } + } + } + + # ========== AXLE ARMS (4 corners) ========== + DEF FRONT_RIGHT_AXLE Transform { + translation 0.14 -0.115 0.038 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.5 0.5 0.5 + roughness 0.3 + metalness 0.8 + } + geometry Box { + size 0.02 0.08 0.02 + } + } + ] + } + DEF FRONT_LEFT_AXLE Transform { + translation 0.14 0.115 0.038 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.5 0.5 0.5 + roughness 0.3 + metalness 0.8 + } + geometry Box { + size 0.02 0.08 0.02 + } + } + ] + } + DEF REAR_RIGHT_AXLE Transform { + translation -0.14 -0.115 0.038 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.5 0.5 0.5 + roughness 0.3 + metalness 0.8 + } + geometry Box { + size 0.02 0.08 0.02 + } + } + ] + } + DEF REAR_LEFT_AXLE Transform { + translation -0.14 0.115 0.038 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.5 0.5 0.5 + roughness 0.3 + metalness 0.8 + } + geometry Box { + size 0.02 0.08 0.02 + } + } + ] + } + + # ========== FRONT RIGHT WHEEL ========== + DEF FRONT_RIGHT_WHEEL_JOINT HingeJoint { + jointParameters HingeJointParameters { + axis 0 1 0 + anchor 0.14 -0.14 0.038 + } + device [ + RotationalMotor { + name "front right wheel motor" + maxVelocity 70.0 + maxTorque 20.0 + } + PositionSensor { + name "front right wheel sensor" + resolution 0.00628 + } + ] + endPoint Solid { + translation 0.14 -0.14 0.038 + rotation 0 -1 0 1.570796 + children [ + DEF WHEEL_VIS Pose { + rotation 1 0 0 -1.5708 + children [ + # Hub drum + Shape { + appearance PBRAppearance { + baseColor 0.5 0.5 0.5 + roughness 0.3 + metalness 0.7 + } + geometry Cylinder { + height 0.018 + radius 0.022 + subdivision 16 + } + } + # Axle boss + Shape { + appearance PBRAppearance { + baseColor 0.6 0.6 0.6 + roughness 0.2 + metalness 0.8 + } + geometry Cylinder { + height 0.022 + radius 0.008 + subdivision 8 + } + } + # Mecanum roller 1 (top, +y) + DEF ROLLER_1 Pose { + translation 0 0.031 0 + rotation 0 0 1 0.7854 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.12 0.12 0.12 + roughness 0.7 + metalness 0.1 + } + geometry Capsule { + height 0.020 + radius 0.007 + subdivision 8 + } + } + ] + } + # Mecanum roller 2 (right, +x) + DEF ROLLER_2 Pose { + translation 0.031 0 0 + rotation 0 0 1 0.7854 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.12 0.12 0.12 + roughness 0.7 + metalness 0.1 + } + geometry Capsule { + height 0.020 + radius 0.007 + subdivision 8 + } + } + ] + } + # Mecanum roller 3 (bottom, -y) + DEF ROLLER_3 Pose { + translation 0 -0.031 0 + rotation 0 0 1 0.7854 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.12 0.12 0.12 + roughness 0.7 + metalness 0.1 + } + geometry Capsule { + height 0.020 + radius 0.007 + subdivision 8 + } + } + ] + } + # Mecanum roller 4 (left, -x) + DEF ROLLER_4 Pose { + translation -0.031 0 0 + rotation 0 0 1 0.7854 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.12 0.12 0.12 + roughness 0.7 + metalness 0.1 + } + geometry Capsule { + height 0.020 + radius 0.007 + subdivision 8 + } + } + ] + } + # Mecanum roller 5 (diagonal +x+y) + DEF ROLLER_5 Pose { + translation 0.022 0.022 0 + rotation 0 0 1 0.7854 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.12 0.12 0.12 + roughness 0.7 + metalness 0.1 + } + geometry Capsule { + height 0.020 + radius 0.007 + subdivision 8 + } + } + ] + } + # Mecanum roller 6 (diagonal +x-y) + DEF ROLLER_6 Pose { + translation 0.022 -0.022 0 + rotation 0 0 1 0.7854 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.12 0.12 0.12 + roughness 0.7 + metalness 0.1 + } + geometry Capsule { + height 0.020 + radius 0.007 + subdivision 8 + } + } + ] + } + # Mecanum roller 7 (diagonal -x-y) + DEF ROLLER_7 Pose { + translation -0.022 -0.022 0 + rotation 0 0 1 0.7854 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.12 0.12 0.12 + roughness 0.7 + metalness 0.1 + } + geometry Capsule { + height 0.020 + radius 0.007 + subdivision 8 + } + } + ] + } + # Mecanum roller 8 (diagonal -x+y) + DEF ROLLER_8 Pose { + translation -0.022 0.022 0 + rotation 0 0 1 0.7854 + children [ + Shape { + appearance PBRAppearance { + baseColor 0.12 0.12 0.12 + roughness 0.7 + metalness 0.1 + } + geometry Capsule { + height 0.020 + radius 0.007 + subdivision 8 + } + } + ] + } + ] + } + ] + name "front right wheel" + contactMaterial "MecanumWheel" + boundingObject Pose { + rotation 1 0 0 -1.5708 + children [ + Cylinder { + height 0.022 + radius 0.038 + } + ] + } + physics Physics { + density -1 + mass 0.06 + centerOfMass [ + 0 0 0 + ] + } + } + } + + # ========== FRONT LEFT WHEEL ========== + DEF FRONT_LEFT_WHEEL_JOINT HingeJoint { + jointParameters HingeJointParameters { + axis 0 1 0 + anchor 0.14 0.14 0.038 + } + device [ + RotationalMotor { + name "front left wheel motor" + maxVelocity 70.0 + maxTorque 20.0 + } + PositionSensor { + name "front left wheel sensor" + resolution 0.00628 + } + ] + endPoint Solid { + translation 0.14 0.14 0.038 + rotation 0.707105 0 0.707109 -3.14159 + children [ + USE WHEEL_VIS + ] + name "front left wheel" + contactMaterial "MecanumWheel" + boundingObject Pose { + rotation 1 0 0 -1.5708 + children [ + Cylinder { + height 0.022 + radius 0.038 + } + ] + } + physics Physics { + density -1 + mass 0.06 + centerOfMass [ + 0 0 0 + ] + } + } + } + + # ========== REAR RIGHT WHEEL ========== + DEF REAR_RIGHT_WHEEL_JOINT HingeJoint { + jointParameters HingeJointParameters { + axis 0 1 0 + anchor -0.14 -0.14 0.038 + } + device [ + RotationalMotor { + name "rear right wheel motor" + maxVelocity 70.0 + maxTorque 20.0 + } + PositionSensor { + name "rear right wheel sensor" + resolution 0.00628 + } + ] + endPoint Solid { + translation -0.14 -0.14 0.038 + rotation 0 -1 0 1.570796 + children [ + USE WHEEL_VIS + ] + name "rear right wheel" + contactMaterial "MecanumWheel" + boundingObject Pose { + rotation 1 0 0 -1.5708 + children [ + Cylinder { + height 0.022 + radius 0.038 + } + ] + } + physics Physics { + density -1 + mass 0.06 + centerOfMass [ + 0 0 0 + ] + } + } + } + + # ========== REAR LEFT WHEEL ========== + DEF REAR_LEFT_WHEEL_JOINT HingeJoint { + jointParameters HingeJointParameters { + axis 0 1 0 + anchor -0.14 0.14 0.038 + } + device [ + RotationalMotor { + name "rear left wheel motor" + maxVelocity 70.0 + maxTorque 20.0 + } + PositionSensor { + name "rear left wheel sensor" + resolution 0.00628 + } + ] + endPoint Solid { + translation -0.14 0.14 0.038 + rotation 0.707105 0 0.707109 -3.14159 + children [ + USE WHEEL_VIS + ] + name "rear left wheel" + contactMaterial "MecanumWheel" + boundingObject Pose { + rotation 1 0 0 -1.5708 + children [ + Cylinder { + height 0.022 + radius 0.038 + } + ] + } + physics Physics { + density -1 + mass 0.06 + centerOfMass [ + 0 0 0 + ] + } + } + } + + # ========== IMU SENSORS ========== + Accelerometer { + translation 0 0 0.10 + name "accelerometer" + } + Gyro { + translation 0 0 0.10 + name "gyro" + } + Compass { + translation 0 0 0.10 + name "compass" + } + + # ========== GPS ========== + GPS { + translation 0 0 0.17 + name "gps" + } + + # ========== RECEIVER ========== + Receiver { + name "receiver" + channel 1 + } + + # ========== EMITTER ========== + Emitter { + name "emitter" + channel 1 + range 50.0 + } + ] + + # ========== BOUNDING OBJECT ========== + boundingObject Group { + children [ + # Chassis box + Transform { + translation 0 0 0.05 + children [ + Box { + size 0.32 0.16 0.06 + } + ] + } + # Body box + Transform { + translation 0 0 0.11 + children [ + Box { + size 0.30 0.16 0.08 + } + ] + } + ] + } + + # ========== PHYSICS ========== + physics Physics { + density -1 + mass 5.0 + centerOfMass [ + 0 0 0.03 + ] + } + } +} diff --git a/tests/test_control.py b/tests/test_control.py index 5ed64f8..6052a76 100644 --- a/tests/test_control.py +++ b/tests/test_control.py @@ -14,6 +14,7 @@ from herding.control.sequential import compute_action as sequential_action from herding.control.strombom import ( DELTA_DRIVE, F_FACTOR, compute_action as strombom_action, ) +from herding.control.universal import compute_action as universal_action from herding.world.geometry import PEN_ENTRY @@ -119,8 +120,10 @@ def test_sequential_targets_closest_to_pen(): def test_active_scan_initial_phase_rotates(): teacher = ActiveScanTeacher(strombom_action) # First call → opening rotation regardless of input. - vx, vy, mode = teacher((0.0, 0.0), 0.0, {"s0": (5.0, 0.0)}, PEN_ENTRY) + vx, vy, omega, mode = teacher( + (0.0, 0.0), 0.0, {"s0": (5.0, 0.0)}, PEN_ENTRY) assert mode == "scan_initial" + assert omega == 0.0 assert math.isclose(math.hypot(vx, vy), 1.0, abs_tol=1e-6) @@ -129,7 +132,8 @@ def test_active_scan_hands_off_to_base_after_opener(): # Burn through the opener. for _ in range(2): teacher((0.0, 0.0), 0.0, {"s0": (0.0, 8.0)}, PEN_ENTRY) - _vx, _vy, mode = teacher((0.0, 0.0), 0.0, {"s0": (0.0, 8.0)}, PEN_ENTRY) + _vx, _vy, _omega, mode = teacher( + (0.0, 0.0), 0.0, {"s0": (0.0, 8.0)}, PEN_ENTRY) # Either drive (Strömbom mode label) or collect; not scan_initial. assert "scan" not in mode @@ -141,7 +145,7 @@ def test_active_scan_holds_last_action_on_brief_empty(): teacher((0.0, 0.0), 0.0, {"s0": (0.0, 8.0)}, PEN_ENTRY) last = teacher.last_action # Now a single empty frame → hold. - vx, vy, mode = teacher((0.0, 0.0), 0.0, {}, PEN_ENTRY) + vx, vy, _omega, mode = teacher((0.0, 0.0), 0.0, {}, PEN_ENTRY) assert mode == "hold" assert (vx, vy) == last @@ -150,10 +154,30 @@ def test_active_scan_explores_after_sustained_empty(): teacher = ActiveScanTeacher(strombom_action, initial_scan_steps=1) teacher((0.0, 0.0), 0.0, {}, PEN_ENTRY) # opener for _ in range(EMPTY_DEBOUNCE_STEPS): - last_vx, last_vy, mode = teacher((5.0, 5.0), 0.0, {}, PEN_ENTRY) + last_vx, last_vy, _omega, mode = teacher( + (5.0, 5.0), 0.0, {}, PEN_ENTRY) assert mode in ("explore", "scan_at_centre") +def test_active_scan_preserves_mecanum_omega(): + """Regression: ActiveScanTeacher must propagate omega from a mecanum + base teacher, not silently drop it. Without this, BC mecanum demos + have omega=0 everywhere and the policy never learns to rotate. + """ + teacher = ActiveScanTeacher(universal_action, initial_scan_steps=1) + # Burn the opener so we exit phase 1. + teacher((0.0, 0.0), 0.0, {"s0": (8.0, 8.0)}, PEN_ENTRY, + drive_mode="mecanum") + # Place a sheep off to the side so the dog needs to face it. + # Dog at origin facing +x (heading=0); target at (0, 8) → desired + # heading +π/2, so omega should be positive. + vx, vy, omega, mode = teacher( + (0.0, 0.0), 0.0, {"s0": (0.0, 8.0)}, PEN_ENTRY, + drive_mode="mecanum") + assert mode in ("collect", "drive", "recovery") + assert abs(omega) > 0.05, f"omega should be non-zero on mecanum, got {omega}" + + def test_active_scan_reset_clears_state(): teacher = ActiveScanTeacher(strombom_action, initial_scan_steps=5) for _ in range(3): diff --git a/tests/test_diffdrive.py b/tests/test_diffdrive.py index 26c2792..6649cf1 100644 --- a/tests/test_diffdrive.py +++ b/tests/test_diffdrive.py @@ -1,11 +1,13 @@ -"""Differential-drive kinematics and the (vx, vy) → wheel-speed map.""" +"""Differential-drive and mecanum kinematics tests.""" import math import pytest from herding.world.diffdrive import ( - heading_speed_to_wheels, kinematics_step, velocity_to_wheels, + heading_speed_to_wheels, kinematics_step, + mecanum_inverse, mecanum_kinematics_step, + velocity_to_mecanum_wheels, velocity_to_wheels, ) @@ -82,3 +84,109 @@ def test_heading_speed_to_wheels_reverse_target_forwards_zero(): left, right = heading_speed_to_wheels(math.pi, 10.0, 0.0, MAX_OMEGA) # cos(π) clamped at 0 → no forward; pure rotation. assert left + right == pytest.approx(0.0, abs=1e-6) + + +# --------------------------------------------------------------------------- +# Mecanum kinematics tests +# --------------------------------------------------------------------------- + +LX = 0.14 # half wheel_base_x +LY = 0.14 # half wheel_base_y + + +def test_mecanum_kinematics_zero_is_identity(): + x, y, h = mecanum_kinematics_step( + 1.0, 2.0, 0.5, 0.0, 0.0, 0.0, 0.0, WHEEL_R, LX, LY, DT, + ) + assert (x, y, h) == (1.0, 2.0, 0.5) + + +def test_mecanum_kinematics_pure_forward(): + # All 4 wheels equal → pure forward (vx_body > 0, vy_body = 0). + w = 10.0 + x, y, h = mecanum_kinematics_step( + 0.0, 0.0, 0.0, w, w, w, w, WHEEL_R, LX, LY, DT, + ) + assert h == pytest.approx(0.0, abs=1e-9) + assert y == pytest.approx(0.0, abs=1e-9) + assert math.isclose(x, w * WHEEL_R * DT, rel_tol=1e-6) + + +def test_mecanum_kinematics_pure_strafe(): + # Strafe right (positive vy_body) with zero forward: + # vx_body = (w_fl+w_fr+w_rl+w_rr)*r/4 = 0 → sum of wheels = 0 + # vy_body = (-w_fl+w_fr+w_rl-w_rr)*r/4 > 0 + # Use w_fl=-10, w_fr=10, w_rl=10, w_rr=-10. + w_fl, w_fr, w_rl, w_rr = -10.0, 10.0, 10.0, -10.0 + x, y, h = mecanum_kinematics_step( + 0.0, 0.0, 0.0, w_fl, w_fr, w_rl, w_rr, WHEEL_R, LX, LY, DT, + ) + assert h == pytest.approx(0.0, abs=1e-9) + assert x == pytest.approx(0.0, abs=1e-9) + expected_vy = (-w_fl + w_fr + w_rl - w_rr) * WHEEL_R / 4.0 + assert math.isclose(y, expected_vy * DT, rel_tol=1e-6) + + +def test_mecanum_kinematics_pure_rotation(): + # Pure rotation: vx_body=0, vy_body=0, omega>0. + # w_fl=-10, w_fr=10, w_rl=-10, w_rr=10 → all sums cancel except omega. + w_fl, w_fr, w_rl, w_rr = -10.0, 10.0, -10.0, 10.0 + x, y, h = mecanum_kinematics_step( + 0.0, 0.0, 0.0, w_fl, w_fr, w_rl, w_rr, WHEEL_R, LX, LY, DT, + ) + assert x == pytest.approx(0.0, abs=1e-9) + assert y == pytest.approx(0.0, abs=1e-9) + assert h > 0.0 + + +def test_mecanum_inverse_roundtrip(): + # Inverse → forward: pick desired body velocities, compute wheels, + # then verify forward kinematics recovers the same velocities. + vx_b = 0.5 + vy_b = 0.3 + omega = 0.2 + w_fl, w_fr, w_rl, w_rr = mecanum_inverse( + vx_b, vy_b, omega, WHEEL_R, LX, LY, MAX_OMEGA, + ) + vx_check = (w_fl + w_fr + w_rl + w_rr) * WHEEL_R / 4.0 + vy_check = (-w_fl + w_fr + w_rl - w_rr) * WHEEL_R / 4.0 + omega_check = (-w_fl + w_fr - w_rl + w_rr) * WHEEL_R / (4.0 * (LX + LY)) + assert math.isclose(vx_b, vx_check, rel_tol=1e-6) + assert math.isclose(vy_b, vy_check, rel_tol=1e-6) + assert math.isclose(omega, omega_check, rel_tol=1e-6) + + +def test_mecanum_inverse_clamped(): + # Request an extreme velocity — all wheels should be clamped. + w_fl, w_fr, w_rl, w_rr = mecanum_inverse( + 100.0, 100.0, 50.0, WHEEL_R, LX, LY, MAX_OMEGA, + ) + assert max(abs(w_fl), abs(w_fr), abs(w_rl), abs(w_rr)) <= MAX_OMEGA + + +def test_velocity_to_mecanum_wheels_zero(): + result = velocity_to_mecanum_wheels( + 0.0, 0.0, 0.0, 0.0, MAX_LINEAR, WHEEL_R, LX, LY, MAX_OMEGA, + wheel_base=WHEEL_B, + ) + assert result == (0.0, 0.0, 0.0, 0.0) + + +def test_velocity_to_mecanum_wheels_forward(): + w_fl, w_fr, w_rl, w_rr = velocity_to_mecanum_wheels( + 1.0, 0.0, 0.0, 0.0, MAX_LINEAR, WHEEL_R, LX, LY, MAX_OMEGA, + wheel_base=WHEEL_B, + ) + # All 4 wheels should be positive and roughly equal. + assert all(w > 0.0 for w in (w_fl, w_fr, w_rl, w_rr)) + assert math.isclose(w_fl, w_rr, rel_tol=1e-6) + assert math.isclose(w_fr, w_rl, rel_tol=1e-6) + + +def test_velocity_to_mecanum_wheels_clamped(): + # Extreme input — all wheels within max. + ws = velocity_to_mecanum_wheels( + 1.0, 1.0, 1.0, 0.0, MAX_LINEAR, WHEEL_R, LX, LY, MAX_OMEGA, + wheel_base=WHEEL_B, + ) + assert all(abs(w) <= MAX_OMEGA for w in ws) diff --git a/tests/test_env.py b/tests/test_env.py index 6bd87cb..b490a3b 100644 --- a/tests/test_env.py +++ b/tests/test_env.py @@ -38,6 +38,14 @@ def test_env_reset_determinism_same_seed(): assert np.allclose(obs_a, obs_b) +def test_env_constructor_seed_applies_to_first_reset(): + a = HerdingEnv(n_sheep=3, seed=42, use_lidar=False) + b = HerdingEnv(n_sheep=3, seed=42, use_lidar=False) + obs_a, _ = a.reset() + obs_b, _ = b.reset() + assert np.allclose(obs_a, obs_b) + + def test_env_curriculum_samples_full_range(): env = HerdingEnv(seed=0, use_lidar=False) sizes = set() diff --git a/tests/test_perception.py b/tests/test_perception.py index 83a6676..01ca990 100644 --- a/tests/test_perception.py +++ b/tests/test_perception.py @@ -42,8 +42,11 @@ def test_simulate_scan_sheep_in_front_returns_centre_hit(): def test_simulate_scan_sheep_behind_dog_not_hit(): + # With 360° FOV, a sheep behind the dog IS now hit. ranges = simulate_scan(0.0, 0.0, 0.0, [(-5.0, 0.0)], noise=0.0) - assert (ranges == LIDAR_MAX_RANGE).all() + assert (ranges < LIDAR_MAX_RANGE).any() + # Verify the closest hit is near 5m (sheep at distance 5). + assert float(ranges.min()) < 5.3 def test_simulate_scan_wall_hit(): diff --git a/tools/benchmark_lidar.py b/tools/benchmark_lidar.py new file mode 100644 index 0000000..845241f --- /dev/null +++ b/tools/benchmark_lidar.py @@ -0,0 +1,84 @@ +"""Benchmark LiDAR perception improvements. + +Measures success rate, mean steps, and tracker quality metrics for +demo collection across multiple seeds. Compares configurations. + +Usage:: + + python -m tools.benchmark_lidar --n-sheep 5 --seeds 15 + HERDING_WORLD=field_round python -m tools.benchmark_lidar --n-sheep 5 +""" + +from __future__ import annotations + +import argparse +import time +from collections import Counter + +from training.bc.collect import collect_one +from herding.control.universal import compute_action + + +def run_benchmark(n_sheep: int, n_seeds: int, max_steps: int = 100000, + drive_mode: str = "differential"): + results = [] + t0 = time.time() + for seed in range(n_seeds): + obs, actions, success, steps = collect_one( + n_sheep, seed, max_steps, 5, compute_action, + frame_stack=1, privileged=False, drive_mode=drive_mode, + ) + results.append({ + "seed": seed, + "success": success, + "steps": steps, + "logged": len(obs), + }) + tag = "+" if success else "x" + print(f" [{tag}] seed={seed:>2d} steps={steps:>6d}") + elapsed = time.time() - t0 + + successes = [r for r in results if r["success"]] + failures = [r for r in results if not r["success"]] + n_ok = len(successes) + rate = 100.0 * n_ok / len(results) + + mean_steps_ok = (sum(r["steps"] for r in successes) / n_ok) if n_ok else 0 + mean_steps_all = sum(r["steps"] for r in results) / len(results) + + print(f"\n Results: {n_ok}/{len(results)} success ({rate:.0f}%)") + print(f" Mean steps (success): {mean_steps_ok:>8.0f}") + print(f" Mean steps (all): {mean_steps_all:>8.0f}") + print(f" Elapsed: {elapsed:.0f}s") + return { + "n_sheep": n_sheep, + "n_seeds": n_seeds, + "success_rate": rate, + "n_success": n_ok, + "mean_steps_success": mean_steps_ok, + "mean_steps_all": mean_steps_all, + "elapsed_s": elapsed, + } + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--n-sheep", type=int, default=5) + parser.add_argument("--seeds", type=int, default=15) + parser.add_argument("--max-steps", type=int, default=100000) + parser.add_argument("--drive-mode", default="differential", + choices=["differential", "mecanum"]) + args = parser.parse_args() + + from herding.world.geometry import FIELD_SHAPE + print(f"[bench] world={FIELD_SHAPE} n_sheep={args.n_sheep} " + f"seeds={args.seeds} drive={args.drive_mode}") + print() + result = run_benchmark(args.n_sheep, args.seeds, args.max_steps, + args.drive_mode) + print() + print("[bench] summary:", result) + + +if __name__ == "__main__": + main() diff --git a/tools/run_webots.sh b/tools/run_webots.sh index 8df26f9..4f33d2f 100755 --- a/tools/run_webots.sh +++ b/tools/run_webots.sh @@ -5,38 +5,109 @@ # then execs Webots on it. # # Usage: -# tools/run_webots.sh [N] [MODE] -# N : number of active sheep (1..10), default 10 -# MODE : "bc" | "rl" | "strombom" | "sequential", default "bc" +# tools/run_webots.sh [N] [MODE] [DRIVE] [WORLD] +# N : number of active sheep (1..10), default 10 +# MODE : "bc" | "rl" | "strombom" | "sequential", default "bc" +# DRIVE : "differential" | "mecanum", default "differential" +# WORLD : base world name (without .wbt), default "field" +# Supported: "field" (rectangular), "field_round" (circular) # # Examples: -# tools/run_webots.sh 10 bc # behaviour-cloned MLP, 10 sheep -# tools/run_webots.sh 10 rl # KL-PPO fine-tune of bc, 10 sheep -# tools/run_webots.sh 5 sequential # single-target analytic baseline -# tools/run_webots.sh 3 strombom # canonical Strömbom analytic +# tools/run_webots.sh 10 bc # behaviour-cloned MLP, diff drive +# tools/run_webots.sh 10 rl mecanum # KL-PPO fine-tune, mecanum wheels +# tools/run_webots.sh 5 sequential field_round # analytic baseline, round field +# tools/run_webots.sh 3 strombom mecanum field_round # Strömbom, mecanum, round # # Notes: # * bc loads training/runs/bc/policy.zip, rl loads training/runs/rl. # Override via HERDING_POLICY_DIR=/path/to/run env var. # * Conda env "tir" must be active (provides stable-baselines3 + torch). +# +# Headless-ish (no 3D view, fast sim, no modal dialogs): +# WEBOTS_HEADLESS=1 make webots N=10 MODE=rl DRIVE=mecanum +# WEBOTS_HEADLESS=1 tools/run_webots.sh 10 rl mecanum +# This passes --no-rendering --minimize --mode=fast --batch to webots. +# Webots still needs a display (Qt); on a machine without one use e.g.: +# xvfb-run -a env WEBOTS_HEADLESS=1 tools/run_webots.sh 10 rl mecanum +# Optional extra CLI tokens (space-separated): +# WEBOTS_EXTRA_ARGS="--stdout --stderr" WEBOTS_HEADLESS=1 tools/run_webots.sh 10 rl set -e N=${1:-10} MODE=${2:-bc} +DRIVE=${3:-differential} +WORLD=${4:-field} if (( N < 1 || N > 10 )); then echo "N must be 1..10, got $N" >&2; exit 1 fi case "$MODE" in - bc|rl|strombom|sequential) ;; - *) echo "MODE must be bc|rl|strombom|sequential, got '$MODE'" >&2; exit 1 ;; + bc|rl|strombom|sequential|universal) ;; + *) echo "MODE must be bc|rl|strombom|sequential|universal, got '$MODE'" >&2; exit 1 ;; +esac +case "$DRIVE" in + differential|mecanum) ;; + *) echo "DRIVE must be differential|mecanum, got '$DRIVE'" >&2; exit 1 ;; esac ROOT="$( cd "$( dirname "${BASH_SOURCE[0]}" )/.." && pwd )" -SRC="$ROOT/worlds/field.wbt" -DST="$ROOT/worlds/field_test.wbt" +SRC="$ROOT/worlds/${WORLD}.wbt" +if [[ ! -f "$SRC" ]]; then + echo "World file not found: $SRC" >&2; exit 1 +fi +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. + if [[ "$MODE" == "rl" ]]; then + DRIVED="$ROOT/training/runs/rl_${DRIVE}" + LEGACY="$ROOT/training/runs/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" + fi +fi cp "$SRC" "$DST" + +# Swap robot proto based on drive mode. +# Base worlds reference ShepherdDog (diff-drive). For mecanum we swap in +# ShepherdDogMecanum and inject mecanum contact properties. +if [[ "$DRIVE" == "mecanum" ]]; then + sed -i 's|"../protos/ShepherdDog.proto"|"../protos/ShepherdDogMecanum.proto"|' "$DST" + sed -i 's|^ShepherdDog {|ShepherdDogMecanum {|' "$DST" + # Inject mecanum contact properties after the existing contactProperties block. + python3 -c " +import re, sys +with open(sys.argv[1], 'r') as f: + txt = f.read() +# Find the closing ']' of contactProperties and insert before it. +mec = ''' + ContactProperties { + material1 \"MecanumWheel\" + coulombFriction [ + 2 + ] + bounce 0 + forceDependentSlip [ + 10 + ] + softCFM 0.0001 + }''' +# Insert before the first ']' that closes contactProperties [...] +txt = re.sub(r'(contactProperties\s*\[[^\]]*)(\])', r'\1' + mec + r'\2', txt, count=1) +with open(sys.argv[1], 'w') as f: + f.write(txt) +" "$DST" +fi + # Comment out sheep N+1..10 by prefixing the matching Sheep { ... } line. for i in $(seq $((N+1)) 10); do sed -i "s|^Sheep .* \"sheep${i}\".*|# &|" "$DST" @@ -46,20 +117,24 @@ active=$(grep -c '^Sheep' "$DST") echo "------------------------------------------------------------" echo "World : $DST" echo "Mode : $MODE" +echo "Drive : $DRIVE" echo "Sheep : $active active" -echo "Policy dir : ${HERDING_POLICY_DIR:-$ROOT/training/runs/bc}" +echo "Policy dir : $RESOLVED_POLICY_DIR" echo "------------------------------------------------------------" # Webots strips HERDING_* env vars from controller subprocesses in some # setups, so we also write a runtime config file the controller reads. -RESOLVED_POLICY_DIR="${HERDING_POLICY_DIR:-$ROOT/training/runs/bc}" cat > "$ROOT/herding_runtime.cfg" < 1: print(f"[bc] inferred frame_stack={frame_stack} from demo obs dim {obs_dim}") + + # Infer drive mode from action dimension if not explicitly set. + action_dim = actions.shape[1] + if args.drive_mode is not None: + drive_mode = args.drive_mode + elif action_dim == 3: + drive_mode = "mecanum" + else: + drive_mode = "differential" + print(f"[bc] drive_mode={drive_mode} (action_dim={action_dim})") + model, _env = build_model(net_arch_pi, net_arch_vf, args.log_std_init, - frame_stack=frame_stack) + frame_stack=frame_stack, drive_mode=drive_mode) policy = model.policy.to(args.device) optimizer = optim.Adam(policy.parameters(), lr=args.lr) diff --git a/training/eval.py b/training/eval.py index f39e827..e290449 100644 --- a/training/eval.py +++ b/training/eval.py @@ -12,11 +12,28 @@ Usage:: from __future__ import annotations import argparse +import os from pathlib import Path from statistics import mean import numpy as np +# Early CLI pre-parse for --world so geometry is configured before +# other herding.* modules are imported. +_pre_argv = [a for a in os.sys.argv[1:]] +_pre_world = None +for i, a in enumerate(_pre_argv): + if a == "--world" and i + 1 < len(_pre_argv): + _pre_world = _pre_argv[i + 1] + break + if a.startswith("--world="): + _pre_world = a.split("=", 1)[1] + break +if _pre_world is not None: + from herding.world.geometry import configure as _geo_configure + _geo_configure(_pre_world) + os.environ["HERDING_WORLD"] = _pre_world + from herding.world.geometry import MAX_SHEEP, PEN_ENTRY from herding.control.sequential import compute_action as sequential_action from herding.control.strombom import compute_action as strombom_action @@ -38,18 +55,20 @@ def rollout(env: HerdingEnv, predict_fn, max_steps: int) -> dict: "n_penned": int(env.sheep_penned.sum())} -def make_analytic_predictor(action_fn): +def make_analytic_predictor(action_fn, drive_mode: str = "differential"): """Wrap an analytic teacher so it runs on the env's exposed perception (tracker in LiDAR mode, GT in privileged mode).""" def _predict(env, _obs): positions = env.perceived_positions() vx, vy, _mode = action_fn((env.dog_x, env.dog_y), positions, PEN_ENTRY) + if drive_mode == "mecanum": + return np.array([vx, vy, 0.0], dtype=np.float32) return np.array([vx, vy], dtype=np.float32) return _predict -def make_strombom_predictor(): - return make_analytic_predictor(strombom_action) +def make_strombom_predictor(drive_mode: str = "differential"): + return make_analytic_predictor(strombom_action, drive_mode) def make_policy_predictor(model, vecnorm): @@ -73,13 +92,21 @@ def main(): parser.add_argument("--difficulty", type=float, default=1.0, help="0 = sheep spawn near the gate (easy); " "1 = full field (deployment distribution).") + parser.add_argument("--drive-mode", default="differential", + choices=["differential", "mecanum"], + help="Drive mode for the dog robot.") + parser.add_argument("--world", default=None, + choices=["field", "field_round"], + help="World shape. If not set, uses HERDING_WORLD " + "env var or defaults to 'field'.") args = parser.parse_args() + drive_mode = args.drive_mode frame_stack = 1 if args.policy == "strombom": - predict = make_analytic_predictor(strombom_action) + predict = make_analytic_predictor(strombom_action, drive_mode) elif args.policy == "sequential": - predict = make_analytic_predictor(sequential_action) + predict = make_analytic_predictor(sequential_action, drive_mode) else: from stable_baselines3 import PPO run = Path(args.policy) @@ -114,6 +141,18 @@ def main(): vecnorm.norm_reward = False predict = make_policy_predictor(model, vecnorm) + # Infer drive_mode from policy action dim if using a learned policy. + if args.policy not in ("strombom", "sequential"): + policy_action_dim = int(model.action_space.shape[0]) + if policy_action_dim == 2 and drive_mode == "mecanum": + drive_mode = "differential" + print(f"[eval] policy has 2D actions — overriding drive_mode " + f"to differential") + elif policy_action_dim == 3 and drive_mode == "differential": + drive_mode = "mecanum" + print(f"[eval] policy has 3D actions — overriding drive_mode " + f"to mecanum") + print(f"{'n_sheep':>8} {'success%':>10} {'mean_steps':>12} {'mean_penned':>12}") print("-" * 46) for n in range(1, args.max_flock + 1): @@ -121,7 +160,7 @@ def main(): for seed in range(args.n_seeds): env = HerdingEnv(n_sheep=n, max_steps=args.max_steps, difficulty=args.difficulty, seed=seed, - frame_stack=frame_stack) + frame_stack=frame_stack, drive_mode=drive_mode) r = rollout(env, predict, args.max_steps) successes.append(int(r["success"])) steps.append(r["steps"]) diff --git a/training/herding_env.py b/training/herding_env.py index 7637ef2..3948e2e 100644 --- a/training/herding_env.py +++ b/training/herding_env.py @@ -1,11 +1,12 @@ """Gymnasium environment for the shepherd-dog herding task. Single-agent: the dog is the policy; sheep are env-controlled flocking -agents (``herding.world.flocking_sim``). Differential-drive kinematics -match the proto specs (``herding.world.diffdrive``) so a policy trained -here transfers to Webots without re-tuning. +agents (``herding.world.flocking_sim``). Kinematics match the proto specs +(``herding.world.diffdrive``) so a policy trained here transfers to Webots +without re-tuning. -* **Action**: ``Box(-1, 1, (2,))`` — desired ``(vx, vy)`` intent. +* **Action** (differential): ``Box(-1, 1, (2,))`` — ``(vx, vy)`` intent. +* **Action** (mecanum): ``Box(-1, 1, (3,))`` — ``(vx, vy, omega)`` intent. * **Observation**: ``Box(-inf, inf, (32·K,))`` from ``herding.perception.obs.build_obs`` with optional frame stacking K (concatenated oldest → newest). * **Reset**: ``options["n_sheep"]`` overrides flock size; otherwise @@ -26,17 +27,20 @@ import numpy as np from gymnasium import spaces from herding.world.diffdrive import ( - heading_speed_to_wheels, kinematics_step, velocity_to_wheels, + heading_speed_to_wheels, kinematics_step, + mecanum_kinematics_step, velocity_to_mecanum_wheels, velocity_to_wheels, ) from herding.world.flocking_sim import ( FLEE_SPEED, MAX_SPEED, WANDER_SPEED, compute_heading_speed, ) from herding.world.geometry import ( - DOG_MAX_LINEAR, DOG_MAX_WHEEL_OMEGA, DOG_SOUTH_LIMIT, DOG_WHEEL_BASE, - DOG_WHEEL_RADIUS, FIELD_X, FIELD_Y, GATE_X, MAX_SHEEP, + DOG_MAX_LINEAR, DOG_MAX_WHEEL_OMEGA, + DOG_SOUTH_LIMIT, DOG_WHEEL_BASE, DOG_WHEEL_BASE_X, DOG_WHEEL_BASE_Y, + DOG_WHEEL_RADIUS, FIELD_SHAPE, FIELD_ROUND_R, FIELD_X, FIELD_Y, + GATE_X, GATE_Y, MAX_SHEEP, PEN_ENTRY, PEN_X, PEN_Y, SHEEP_MAX_WHEEL_OMEGA, SHEEP_WHEEL_BASE, SHEEP_WHEEL_RADIUS, - WEBOTS_DT, is_penned_position, + WEBOTS_DT, clip_to_field, is_penned_position, ) from herding.perception.lidar_perception import detections_from_scan from herding.perception.lidar_sim import simulate_scan @@ -82,6 +86,7 @@ class HerdingEnv(gym.Env): seed: Optional[int] = None, use_lidar: bool = True, frame_stack: int = 1, + drive_mode: str = "differential", ): super().__init__() # ``use_lidar=True`` (default): obs and imitation-reward teacher @@ -95,7 +100,14 @@ class HerdingEnv(gym.Env): # giving a memoryless MLP temporal context. K=1 → single frame. self._frame_stack = max(1, int(frame_stack)) self._frame_buffer: list[np.ndarray] = [] - self.action_space = spaces.Box(-1.0, 1.0, shape=(2,), dtype=np.float32) + + # Drive mode: "differential" (2-wheel) or "mecanum" (4-wheel omni). + self._drive_mode = drive_mode.lower() + if self._drive_mode not in ("differential", "mecanum"): + raise ValueError(f"Unknown drive_mode: {drive_mode!r}") + action_dim = 3 if self._drive_mode == "mecanum" else 2 + self.action_space = spaces.Box(-1.0, 1.0, shape=(action_dim,), + dtype=np.float32) self._single_obs_dim = OBS_DIM self.observation_space = spaces.Box( low=-np.inf, high=np.inf, @@ -110,6 +122,11 @@ class HerdingEnv(gym.Env): # 1 = sheep spawn anywhere in the field (deployment distribution). self._difficulty = float(difficulty) self._initial_seed = seed + self._initial_seed_used = False + + # Env-owned RNG for wander jitter, re-seeded from np_random on reset. + self._py_rng = random.Random() + self._action_dim = action_dim # State (initialised in reset) self.dog_x = self.dog_y = self.dog_heading = 0.0 @@ -119,17 +136,14 @@ class HerdingEnv(gym.Env): self.sheep_penned = np.zeros(0, dtype=bool) self.sheep_wander = np.zeros(0, dtype=np.float32) - self.prev_action = np.zeros(2, dtype=np.float32) - self.smoothed_action = np.zeros(2, dtype=np.float32) + self.prev_action = np.zeros(self._action_dim, dtype=np.float32) + self.smoothed_action = np.zeros(self._action_dim, dtype=np.float32) self.steps = 0 self.n_sheep = 0 self.prev_n_penned = 0 self.prev_d_pen = 0.0 self.prev_radius = 0.0 - # Env-owned RNG for wander jitter, re-seeded from np_random on reset. - self._py_rng = random.Random() - # --- Public knobs --- def set_max_n_sheep(self, value: int) -> None: self._max_n_sheep = int(np.clip(value, 1, MAX_SHEEP)) @@ -149,6 +163,10 @@ class HerdingEnv(gym.Env): # --- gym API --- def reset(self, *, seed=None, options=None): + if (seed is None and self._initial_seed is not None + and not self._initial_seed_used): + seed = self._initial_seed + self._initial_seed_used = True super().reset(seed=seed) self._py_rng.seed(int(self.np_random.integers(0, 2**31 - 1))) opts = options or {} @@ -168,16 +186,32 @@ class HerdingEnv(gym.Env): # Sheep spawn region linearly interpolates with difficulty: # 0 → small box near the gate, 1 → full field. d = self._difficulty - sx_lo = 7.0 - d * 20.0 - sx_hi = 14.0 - d * 1.0 - sy_lo = -12.0 + d * 0.0 - sy_hi = -6.0 + d * 19.0 + if FIELD_SHAPE == "field_round": + # Round field: spawn in a sector near the gate (south), + # expanding to the full circle at difficulty=1. + spawn_r_lo = 3.0 + spawn_r_hi = d * FIELD_ROUND_R * 0.8 + (1.0 - d) * 6.0 + # Angle spread around south (±60° at d=0, full circle at d=1). + half_angle = math.radians(60) + d * math.radians(120) + angle_lo = math.pi / 2.0 - half_angle # from south = -π/2 + angle_hi = math.pi / 2.0 + half_angle + else: + sx_lo = 7.0 - d * 20.0 + sx_hi = 14.0 - d * 1.0 + sy_lo = -12.0 + d * 0.0 + sy_hi = -6.0 + d * 19.0 sxs, sys_, shs, sws = [], [], [], [] for _ in range(self.n_sheep): for _try in range(100): - sx = float(self.np_random.uniform(sx_lo, sx_hi)) - sy = float(self.np_random.uniform(sy_lo, sy_hi)) + if FIELD_SHAPE == "field_round": + r_spawn = float(self.np_random.uniform(spawn_r_lo, spawn_r_hi)) + a_spawn = float(self.np_random.uniform(angle_lo, angle_hi)) + sx = r_spawn * math.cos(a_spawn) + sy = -r_spawn * math.sin(a_spawn) + else: + sx = float(self.np_random.uniform(sx_lo, sx_hi)) + sy = float(self.np_random.uniform(sy_lo, sy_hi)) # Reject if too close to the dog or another sheep, or # already in the gate column (would start "penned"). if math.hypot(sx - self.dog_x, sy - self.dog_y) < 3.0: @@ -198,8 +232,8 @@ class HerdingEnv(gym.Env): self.sheep_wander = np.asarray(sws, dtype=np.float32) self.sheep_penned = np.zeros(self.n_sheep, dtype=bool) - self.prev_action = np.zeros(2, dtype=np.float32) - self.smoothed_action = np.zeros(2, dtype=np.float32) + self.prev_action = np.zeros(self._action_dim, dtype=np.float32) + self.smoothed_action = np.zeros(self._action_dim, dtype=np.float32) self.steps = 0 self.prev_n_penned = 0 self.prev_d_pen, self.prev_radius = self._flock_metrics() @@ -225,25 +259,46 @@ class HerdingEnv(gym.Env): ) self.prev_action = self.smoothed_action.copy() vx, vy = float(self.smoothed_action[0]), float(self.smoothed_action[1]) + omega = float(self.smoothed_action[2]) if self._action_dim >= 3 else 0.0 # Safety supervisor — dog stays north of the gate. if self.dog_y < DOG_SOUTH_LIMIT and vy < 0.0: vx, vy = 0.0, 1.0 # Step the dog. - wL, wR = velocity_to_wheels( - vx, vy, self.dog_heading, - max_linear=DOG_MAX_LINEAR, - wheel_radius=DOG_WHEEL_RADIUS, - max_wheel_omega=DOG_MAX_WHEEL_OMEGA, - k_turn=4.0, - ) - self.dog_x, self.dog_y, self.dog_heading = kinematics_step( - self.dog_x, self.dog_y, self.dog_heading, - wL, wR, DOG_WHEEL_RADIUS, DOG_WHEEL_BASE, WEBOTS_DT, - ) - self.dog_x = float(np.clip(self.dog_x, FIELD_X[0] + 0.3, FIELD_X[1] - 0.3)) - self.dog_y = float(np.clip(self.dog_y, DOG_SOUTH_LIMIT, FIELD_Y[1] - 0.3)) + if self._drive_mode == "mecanum": + w_fl, w_fr, w_rl, w_rr = velocity_to_mecanum_wheels( + vx, vy, omega, self.dog_heading, + max_linear=DOG_MAX_LINEAR, + wheel_radius=DOG_WHEEL_RADIUS, + lx=DOG_WHEEL_BASE_X / 2.0, ly=DOG_WHEEL_BASE_Y / 2.0, + max_wheel_omega=DOG_MAX_WHEEL_OMEGA, + k_turn=4.0, + wheel_base=DOG_WHEEL_BASE, + ) + self.dog_x, self.dog_y, self.dog_heading = mecanum_kinematics_step( + self.dog_x, self.dog_y, self.dog_heading, + w_fl, w_fr, w_rl, w_rr, + DOG_WHEEL_RADIUS, + DOG_WHEEL_BASE_X / 2.0, DOG_WHEEL_BASE_Y / 2.0, + WEBOTS_DT, + ) + else: + wL, wR = velocity_to_wheels( + vx, vy, self.dog_heading, + max_linear=DOG_MAX_LINEAR, + wheel_radius=DOG_WHEEL_RADIUS, + max_wheel_omega=DOG_MAX_WHEEL_OMEGA, + k_turn=4.0, + ) + self.dog_x, self.dog_y, self.dog_heading = kinematics_step( + self.dog_x, self.dog_y, self.dog_heading, + wL, wR, DOG_WHEEL_RADIUS, DOG_WHEEL_BASE, WEBOTS_DT, + ) + self.dog_x, self.dog_y = clip_to_field(self.dog_x, self.dog_y, margin=0.3) + # Extra constraint: dog stays north of the gate area. + if self.dog_y < DOG_SOUTH_LIMIT: + self.dog_y = DOG_SOUTH_LIMIT # Step sheep and update penned flags (GT-based). for i in range(self.n_sheep): @@ -304,13 +359,21 @@ class HerdingEnv(gym.Env): SHEEP_WHEEL_RADIUS, SHEEP_WHEEL_BASE, WEBOTS_DT, ) - # Wall clipping (south wall absent inside the gate column). - nx = float(np.clip(nx, FIELD_X[0] + 0.2, FIELD_X[1] - 0.2)) - in_gate_col = PEN_X[0] <= nx <= PEN_X[1] - if in_gate_col: - ny = float(np.clip(ny, PEN_Y[0] + 0.2, FIELD_Y[1] - 0.2)) + # Wall clipping. + if FIELD_SHAPE == "field_round": + in_gate_col = PEN_X[0] <= nx <= PEN_X[1] + if in_gate_col: + # Allow passage through the gate column (south of the wall). + ny = float(np.clip(ny, PEN_Y[0] + 0.2, FIELD_Y[1] - 0.2)) + else: + nx, ny = clip_to_field(nx, ny, margin=0.2) else: - ny = float(np.clip(ny, FIELD_Y[0] + 0.2, FIELD_Y[1] - 0.2)) + nx = float(np.clip(nx, FIELD_X[0] + 0.2, FIELD_X[1] - 0.2)) + in_gate_col = PEN_X[0] <= nx <= PEN_X[1] + if in_gate_col: + ny = float(np.clip(ny, PEN_Y[0] + 0.2, FIELD_Y[1] - 0.2)) + else: + ny = float(np.clip(ny, FIELD_Y[0] + 0.2, FIELD_Y[1] - 0.2)) self.sheep_x[i] = nx self.sheep_y[i] = ny @@ -374,6 +437,7 @@ class HerdingEnv(gym.Env): (self.dog_x, self.dog_y), self.dog_heading, sheep_xy_list, sheep_penned_list, n_max=self._max_n_sheep, + n_expected=self.n_sheep, ) def _build_obs(self) -> np.ndarray: diff --git a/training/rl/train.py b/training/rl/train.py index 5bc2fde..a0a24e9 100644 --- a/training/rl/train.py +++ b/training/rl/train.py @@ -20,8 +20,26 @@ Usage:: from __future__ import annotations import argparse +import os from pathlib import Path +# Early CLI pre-parse for --world so geometry is configured before any +# herding.* / training.* import binds geometry constants. Matches the +# pattern used by training.bc.collect and training.eval. +_pre_argv = [a for a in os.sys.argv[1:]] +_pre_world = None +for i, a in enumerate(_pre_argv): + if a == "--world" and i + 1 < len(_pre_argv): + _pre_world = _pre_argv[i + 1] + break + if a.startswith("--world="): + _pre_world = a.split("=", 1)[1] + break +if _pre_world is not None: + from herding.world.geometry import configure as _geo_configure + _geo_configure(_pre_world) + os.environ["HERDING_WORLD"] = _pre_world + import numpy as np import torch as th import torch.nn.functional as F @@ -38,9 +56,14 @@ from training.herding_env import HerdingEnv # Env factory # -------------------------------------------------------------------- -def _make_env(rank: int, seed: int, frame_stack: int): +def _make_env(rank: int, seed: int, frame_stack: int, + drive_mode: str = "differential", + difficulty: float = 1.0, + max_n_sheep: int = 10): def _thunk(): - env = HerdingEnv(seed=seed + rank, frame_stack=frame_stack) + env = HerdingEnv(seed=seed + rank, frame_stack=frame_stack, + drive_mode=drive_mode, difficulty=difficulty, + max_n_sheep=max_n_sheep) env = Monitor(env, info_keywords=("is_success", "n_sheep", "n_penned")) return env return _thunk @@ -198,13 +221,34 @@ def main() -> None: help="SB3 per-batch KL early-stop guard.") parser.add_argument("--seed", type=int, default=0) parser.add_argument("--device", default="cpu") + parser.add_argument("--drive-mode", default=None, + choices=["differential", "mecanum"], + help="Drive mode. If not set, inferred from " + "BC action dimension (2→differential, 3→mecanum).") parser.add_argument("--imitate-weight", type=float, default=None, help="Override env.W_IMITATE (e.g. 0.0 to drop " "Strömbom imitation during fine-tune).") parser.add_argument("--time-weight", type=float, default=None, help="Override env.W_TIME (e.g. -0.1 for a " "per-step time penalty).") + parser.add_argument("--difficulty", type=float, default=1.0, + help="HerdingEnv difficulty for PPO rollouts. " + "Must match eval (1.0) to avoid train/eval " + "distribution mismatch.") + parser.add_argument("--max-n-sheep", type=int, default=10, + help="Upper bound on flock size sampled each reset.") + parser.add_argument("--world", default=None, + choices=["field", "field_round"], + help="World shape. If not set, uses HERDING_WORLD " + "env var or defaults to 'field'.") args = parser.parse_args() + # --world was already honoured in the early pre-parse above; here we + # just sanity-check that the final argparse view agrees. + if args.world is not None: + from herding.world.geometry import FIELD_SHAPE as _CURRENT_SHAPE + if args.world != _CURRENT_SHAPE: + print(f"[rl] WARNING: --world={args.world} but geometry is " + f"'{_CURRENT_SHAPE}'. File a bug.") bc_zip = Path(args.bc) / "policy.zip" if not bc_zip.exists(): @@ -226,9 +270,26 @@ def main() -> None: frame_stack = obs_dim // OBS_DIM print(f"[rl] BC obs dim {obs_dim} → frame_stack={frame_stack}") - env_fns = [_make_env(i, args.seed, frame_stack) for i in range(args.n_envs)] + # Infer drive mode from BC action dim if not explicitly set. + bc_action_dim = int(ref_only.action_space.shape[0]) + if args.drive_mode is not None: + drive_mode = args.drive_mode + elif bc_action_dim == 3: + drive_mode = "mecanum" + else: + drive_mode = "differential" + print(f"[rl] drive_mode={drive_mode} (BC action_dim={bc_action_dim})") + + env_fns = [_make_env(i, args.seed, frame_stack, drive_mode, + difficulty=args.difficulty, + max_n_sheep=args.max_n_sheep) + for i in range(args.n_envs)] venv = SubprocVecEnv(env_fns) if args.n_envs > 1 else DummyVecEnv(env_fns) - eval_venv = DummyVecEnv([_make_env(99, args.seed + 999, frame_stack)]) + eval_venv = DummyVecEnv([_make_env(99, args.seed + 999, frame_stack, + drive_mode, + difficulty=args.difficulty, + max_n_sheep=args.max_n_sheep)]) + print(f"[rl] difficulty={args.difficulty} max_n_sheep={args.max_n_sheep}") # Reward-shaping overrides (broadcast to every env instance). def _broadcast(method: str, value): diff --git a/training/runs/bc/policy.zip b/training/runs/bc/policy.zip new file mode 100644 index 0000000..7c07d38 Binary files /dev/null and b/training/runs/bc/policy.zip differ diff --git a/training/runs/rl/policy.zip b/training/runs/rl/policy.zip new file mode 100644 index 0000000..516c758 Binary files /dev/null and b/training/runs/rl/policy.zip differ diff --git a/worlds/field_round.wbt b/worlds/field_round.wbt new file mode 100644 index 0000000..0f52a1d --- /dev/null +++ b/worlds/field_round.wbt @@ -0,0 +1,537 @@ +#VRML_SIM R2025a utf8 + +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/backgrounds/protos/TexturedBackground.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/objects/floors/protos/UnevenTerrain.proto" +EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2025a/projects/appearances/protos/Grass.proto" +EXTERNPROTO "../protos/ShepherdDog.proto" +EXTERNPROTO "../protos/Sheep.proto" + +# World +WorldInfo { + info [ + "Autonomous Shepherd Robot (Strömbom)" + "Group G25" + ] + title "Shepherd Herding (Round)" + ERP 0.62 + basicTimeStep 16 + contactProperties [ + ContactProperties { + coulombFriction [ + 12 + ] + softCFM 1e-05 + } + ] +} + +# Viewpoint +DEF VIEWPOINT Viewpoint { + position 4.34 -100.99 41.73 + orientation 0.199 -0.190 -0.961 4.624 + fieldOfView 0.785 +} + +# Background +Background { + skyColor [0.55 0.75 0.95] +} +# Single sun (diagonal from SW) +DirectionalLight { + ambientIntensity 1 + direction -0.3 0.5 -0.85 + color 1 0.98 0.92 + intensity 2.5 + castShadows TRUE +} + +# Grass terrain +UnevenTerrain { + rotation 0 0 1 -1.5708 + size 100 100 0.3 + xDimension 50 + yDimension 50 + appearance Grass { + colorOverride 0.78 0.88 0.68 + textureTransform TextureTransform { + scale 100 100 + } + } + perlinNOctaves 4 +} + +# ==================== APPEARANCES ==================== +Transform { + children [ + Shape { appearance DEF STONE_A PBRAppearance { baseColor 0.48 0.45 0.40 roughness 0.95 metalness 0 } } + Shape { appearance DEF STONE_B PBRAppearance { baseColor 0.36 0.33 0.29 roughness 0.95 metalness 0 } } + Shape { appearance DEF STONE_C PBRAppearance { baseColor 0.58 0.55 0.50 roughness 0.90 metalness 0 } } + Shape { appearance DEF CAP PBRAppearance { baseColor 0.54 0.51 0.46 roughness 0.80 metalness 0 } } + Shape { appearance DEF BARN_RED PBRAppearance { baseColor 0.62 0.18 0.12 roughness 0.80 metalness 0 } } + Shape { appearance DEF BARN_ROOF PBRAppearance { baseColor 0.28 0.20 0.13 roughness 0.72 metalness 0 } } + Shape { appearance DEF WOOD PBRAppearance { baseColor 0.48 0.32 0.16 roughness 0.90 metalness 0 } } + Shape { appearance DEF TRUNK PBRAppearance { baseColor 0.38 0.24 0.11 roughness 0.90 metalness 0 } } + Shape { appearance DEF LEAF_A PBRAppearance { baseColor 0.22 0.52 0.16 roughness 0.85 metalness 0 } } + Shape { appearance DEF LEAF_B PBRAppearance { baseColor 0.16 0.42 0.10 roughness 0.85 metalness 0 } } + Shape { appearance DEF LEAF_C PBRAppearance { baseColor 0.30 0.60 0.20 roughness 0.80 metalness 0 } } + Shape { appearance DEF STRAW PBRAppearance { baseColor 0.85 0.75 0.35 roughness 0.95 metalness 0 } } + Shape { appearance DEF HAT PBRAppearance { baseColor 0.50 0.35 0.18 roughness 0.85 metalness 0 } } + Shape { appearance DEF SHIRT PBRAppearance { baseColor 0.60 0.30 0.30 roughness 0.80 metalness 0 } } + Shape { appearance DEF PANTS PBRAppearance { baseColor 0.25 0.25 0.30 roughness 0.80 metalness 0 } } + Shape { appearance DEF DOOR_MAT PBRAppearance { baseColor 0.55 0.38 0.20 roughness 0.72 metalness 0 } } + Shape { appearance DEF GLASS PBRAppearance { baseColor 0.60 0.80 0.95 roughness 0.20 metalness 0.05 } } + Shape { appearance DEF HAY PBRAppearance { baseColor 0.82 0.72 0.32 roughness 0.95 metalness 0 } } + ] +} +DEF TRIM PBRAppearance { baseColor 0.90 0.88 0.82 roughness 0.70 metalness 0 } + +# ==================== CIRCULAR STONE WALL (R=15 m) ==================== + +Solid { translation 15.00 0.00 0.40 rotation 0 0 1 -1.5708 children [ Shape { appearance USE STONE_A geometry Box { size 5.21 0.16 0.80 } } ] boundingObject Box { size 5.21 0.16 0.80 } } +Solid { translation 15.00 0.00 0.84 rotation 0 0 1 -1.5708 children [ Shape { appearance USE CAP geometry Box { size 5.2 0.26 0.07 } } ] boundingObject Box { size 5.2 0.26 0.07 } } +Solid { translation 14.10 5.13 0.40 rotation 0 0 1 -1.2217 children [ Shape { appearance USE STONE_A geometry Box { size 5.21 0.16 0.80 } } ] boundingObject Box { size 5.21 0.16 0.80 } } +Solid { translation 14.10 5.13 0.84 rotation 0 0 1 -1.2217 children [ Shape { appearance USE CAP geometry Box { size 5.2 0.26 0.07 } } ] boundingObject Box { size 5.2 0.26 0.07 } } +Solid { translation 11.49 9.64 0.40 rotation 0 0 1 -0.8727 children [ Shape { appearance USE STONE_A geometry Box { size 5.21 0.16 0.80 } } ] boundingObject Box { size 5.21 0.16 0.80 } } +Solid { translation 11.49 9.64 0.84 rotation 0 0 1 -0.8727 children [ Shape { appearance USE CAP geometry Box { size 5.2 0.26 0.07 } } ] boundingObject Box { size 5.2 0.26 0.07 } } +Solid { translation 7.50 12.99 0.40 rotation 0 0 1 -0.5236 children [ Shape { appearance USE STONE_A geometry Box { size 5.21 0.16 0.80 } } ] boundingObject Box { size 5.21 0.16 0.80 } } +Solid { translation 7.50 12.99 0.84 rotation 0 0 1 -0.5236 children [ Shape { appearance USE CAP geometry Box { size 5.2 0.26 0.07 } } ] boundingObject Box { size 5.2 0.26 0.07 } } +Solid { translation 2.60 14.77 0.40 rotation 0 0 1 -0.1745 children [ Shape { appearance USE STONE_A geometry Box { size 5.21 0.16 0.80 } } ] boundingObject Box { size 5.21 0.16 0.80 } } +Solid { translation 2.60 14.77 0.84 rotation 0 0 1 -0.1745 children [ Shape { appearance USE CAP geometry Box { size 5.2 0.26 0.07 } } ] boundingObject Box { size 5.2 0.26 0.07 } } +Solid { translation -2.60 14.77 0.40 rotation 0 0 1 0.1745 children [ Shape { appearance USE STONE_A geometry Box { size 5.21 0.16 0.80 } } ] boundingObject Box { size 5.21 0.16 0.80 } } +Solid { translation -2.60 14.77 0.84 rotation 0 0 1 0.1745 children [ Shape { appearance USE CAP geometry Box { size 5.2 0.26 0.07 } } ] boundingObject Box { size 5.2 0.26 0.07 } } +Solid { translation -7.50 12.99 0.40 rotation 0 0 1 0.5236 children [ Shape { appearance USE STONE_A geometry Box { size 5.21 0.16 0.80 } } ] boundingObject Box { size 5.21 0.16 0.80 } } +Solid { translation -7.50 12.99 0.84 rotation 0 0 1 0.5236 children [ Shape { appearance USE CAP geometry Box { size 5.2 0.26 0.07 } } ] boundingObject Box { size 5.2 0.26 0.07 } } +Solid { translation -11.49 9.64 0.40 rotation 0 0 1 0.8727 children [ Shape { appearance USE STONE_A geometry Box { size 5.21 0.16 0.80 } } ] boundingObject Box { size 5.21 0.16 0.80 } } +Solid { translation -11.49 9.64 0.84 rotation 0 0 1 0.8727 children [ Shape { appearance USE CAP geometry Box { size 5.2 0.26 0.07 } } ] boundingObject Box { size 5.2 0.26 0.07 } } +Solid { translation -14.10 5.13 0.40 rotation 0 0 1 1.2217 children [ Shape { appearance USE STONE_A geometry Box { size 5.21 0.16 0.80 } } ] boundingObject Box { size 5.21 0.16 0.80 } } +Solid { translation -14.10 5.13 0.84 rotation 0 0 1 1.2217 children [ Shape { appearance USE CAP geometry Box { size 5.2 0.26 0.07 } } ] boundingObject Box { size 5.2 0.26 0.07 } } +Solid { translation -15.00 0.00 0.40 rotation 0 0 1 1.5708 children [ Shape { appearance USE STONE_A geometry Box { size 5.21 0.16 0.80 } } ] boundingObject Box { size 5.21 0.16 0.80 } } +Solid { translation -15.00 0.00 0.84 rotation 0 0 1 1.5708 children [ Shape { appearance USE CAP geometry Box { size 5.2 0.26 0.07 } } ] boundingObject Box { size 5.2 0.26 0.07 } } +Solid { translation -14.10 -5.13 0.40 rotation 0 0 1 1.9199 children [ Shape { appearance USE STONE_A geometry Box { size 5.21 0.16 0.80 } } ] boundingObject Box { size 5.21 0.16 0.80 } } +Solid { translation -14.10 -5.13 0.84 rotation 0 0 1 1.9199 children [ Shape { appearance USE CAP geometry Box { size 5.2 0.26 0.07 } } ] boundingObject Box { size 5.2 0.26 0.07 } } +Solid { translation -11.49 -9.64 0.40 rotation 0 0 1 2.2689 children [ Shape { appearance USE STONE_A geometry Box { size 5.21 0.16 0.80 } } ] boundingObject Box { size 5.21 0.16 0.80 } } +Solid { translation -11.49 -9.64 0.84 rotation 0 0 1 2.2689 children [ Shape { appearance USE CAP geometry Box { size 5.2 0.26 0.07 } } ] boundingObject Box { size 5.2 0.26 0.07 } } +Solid { translation -7.50 -12.99 0.40 rotation 0 0 1 2.6180 children [ Shape { appearance USE STONE_A geometry Box { size 5.21 0.16 0.80 } } ] boundingObject Box { size 5.21 0.16 0.80 } } +Solid { translation -7.50 -12.99 0.84 rotation 0 0 1 2.6180 children [ Shape { appearance USE CAP geometry Box { size 5.2 0.26 0.07 } } ] boundingObject Box { size 5.2 0.26 0.07 } } +Solid { translation -3.37 -14.62 0.40 rotation 0 0 1 2.9671 children [ Shape { appearance USE STONE_A geometry Box { size 3.65 0.16 0.80 } } ] boundingObject Box { size 3.65 0.16 0.80 } } +Solid { translation -3.37 -14.62 0.84 rotation 0 0 1 2.9671 children [ Shape { appearance USE CAP geometry Box { size 3.7 0.26 0.07 } } ] boundingObject Box { size 3.7 0.26 0.07 } } +Solid { translation 3.37 -14.62 0.40 rotation 0 0 1 3.3161 children [ Shape { appearance USE STONE_A geometry Box { size 3.65 0.16 0.80 } } ] boundingObject Box { size 3.65 0.16 0.80 } } +Solid { translation 3.37 -14.62 0.84 rotation 0 0 1 3.3161 children [ Shape { appearance USE CAP geometry Box { size 3.7 0.26 0.07 } } ] boundingObject Box { size 3.7 0.26 0.07 } } +Solid { translation 7.50 -12.99 0.40 rotation 0 0 1 3.6652 children [ Shape { appearance USE STONE_A geometry Box { size 5.21 0.16 0.80 } } ] boundingObject Box { size 5.21 0.16 0.80 } } +Solid { translation 7.50 -12.99 0.84 rotation 0 0 1 3.6652 children [ Shape { appearance USE CAP geometry Box { size 5.2 0.26 0.07 } } ] boundingObject Box { size 5.2 0.26 0.07 } } +Solid { translation 11.49 -9.64 0.40 rotation 0 0 1 4.0143 children [ Shape { appearance USE STONE_A geometry Box { size 5.21 0.16 0.80 } } ] boundingObject Box { size 5.21 0.16 0.80 } } +Solid { translation 11.49 -9.64 0.84 rotation 0 0 1 4.0143 children [ Shape { appearance USE CAP geometry Box { size 5.2 0.26 0.07 } } ] boundingObject Box { size 5.2 0.26 0.07 } } +Solid { translation 14.10 -5.13 0.40 rotation 0 0 1 4.3633 children [ Shape { appearance USE STONE_A geometry Box { size 5.21 0.16 0.80 } } ] boundingObject Box { size 5.21 0.16 0.80 } } +Solid { translation 14.10 -5.13 0.84 rotation 0 0 1 4.3633 children [ Shape { appearance USE CAP geometry Box { size 5.2 0.26 0.07 } } ] boundingObject Box { size 5.2 0.26 0.07 } } + +# Gate posts +Solid { translation -1.57 -14.92 0.56 children [ Shape { appearance USE STONE_B geometry Box { size 0.44 0.44 1.12 } } Shape { appearance USE CAP geometry Box { size 0.54 0.54 0.08 } } ] } +Solid { translation 1.57 -14.92 0.56 children [ Shape { appearance USE STONE_B geometry Box { size 0.44 0.44 1.12 } } Shape { appearance USE CAP geometry Box { size 0.54 0.54 0.08 } } ] } +# Outer gate — swung-back beside west gate post +Solid { translation -2.97 -14.92 0.55 rotation 0 0 1 0 children [ + Shape { appearance USE WOOD geometry Box { size 2.80 0.05 1.00 } } + Transform { translation 0 0.02 0 rotation 0 1 0 0.34 children [ + Shape { appearance DEF FPOST PBRAppearance { baseColor 0.35 0.22 0.10 roughness 0.90 } geometry Box { size 2.97 0.04 0.06 } } + ] } +] boundingObject Box { size 2.80 0.08 1.00 } } + +# Pillars between wall sections +Solid { translation 14.97 2.64 0.53 rotation 0 0 1 0.9599 children [ Shape { appearance USE STONE_B geometry Box { size 0.34 0.34 1.06 } } Shape { appearance USE CAP geometry Box { size 0.44 0.44 0.07 } } ] } +Solid { translation 13.16 7.60 0.53 rotation 0 0 1 1.3090 children [ Shape { appearance USE STONE_B geometry Box { size 0.34 0.34 1.06 } } Shape { appearance USE CAP geometry Box { size 0.44 0.44 0.07 } } ] } +Solid { translation 9.77 11.64 0.53 rotation 0 0 1 1.6581 children [ Shape { appearance USE STONE_B geometry Box { size 0.34 0.34 1.06 } } Shape { appearance USE CAP geometry Box { size 0.44 0.44 0.07 } } ] } +Solid { translation 5.20 14.28 0.53 rotation 0 0 1 2.0071 children [ Shape { appearance USE STONE_B geometry Box { size 0.34 0.34 1.06 } } Shape { appearance USE CAP geometry Box { size 0.44 0.44 0.07 } } ] } +Solid { translation 0.00 15.20 0.53 rotation 0 0 1 2.3562 children [ Shape { appearance USE STONE_B geometry Box { size 0.34 0.34 1.06 } } Shape { appearance USE CAP geometry Box { size 0.44 0.44 0.07 } } ] } +Solid { translation -5.20 14.28 0.53 rotation 0 0 1 2.7053 children [ Shape { appearance USE STONE_B geometry Box { size 0.34 0.34 1.06 } } Shape { appearance USE CAP geometry Box { size 0.44 0.44 0.07 } } ] } +Solid { translation -9.77 11.64 0.53 rotation 0 0 1 3.0543 children [ Shape { appearance USE STONE_B geometry Box { size 0.34 0.34 1.06 } } Shape { appearance USE CAP geometry Box { size 0.44 0.44 0.07 } } ] } +Solid { translation -13.16 7.60 0.53 rotation 0 0 1 3.4034 children [ Shape { appearance USE STONE_B geometry Box { size 0.34 0.34 1.06 } } Shape { appearance USE CAP geometry Box { size 0.44 0.44 0.07 } } ] } +Solid { translation -14.97 2.64 0.53 rotation 0 0 1 3.7525 children [ Shape { appearance USE STONE_B geometry Box { size 0.34 0.34 1.06 } } Shape { appearance USE CAP geometry Box { size 0.44 0.44 0.07 } } ] } +Solid { translation -14.97 -2.64 0.53 rotation 0 0 1 4.1015 children [ Shape { appearance USE STONE_B geometry Box { size 0.34 0.34 1.06 } } Shape { appearance USE CAP geometry Box { size 0.44 0.44 0.07 } } ] } +Solid { translation -13.16 -7.60 0.53 rotation 0 0 1 4.4506 children [ Shape { appearance USE STONE_B geometry Box { size 0.34 0.34 1.06 } } Shape { appearance USE CAP geometry Box { size 0.44 0.44 0.07 } } ] } +Solid { translation -9.77 -11.64 0.53 rotation 0 0 1 4.7997 children [ Shape { appearance USE STONE_B geometry Box { size 0.34 0.34 1.06 } } Shape { appearance USE CAP geometry Box { size 0.44 0.44 0.07 } } ] } +Solid { translation -5.20 -14.28 0.53 rotation 0 0 1 5.1487 children [ Shape { appearance USE STONE_B geometry Box { size 0.34 0.34 1.06 } } Shape { appearance USE CAP geometry Box { size 0.44 0.44 0.07 } } ] } +Solid { translation 5.20 -14.28 0.53 rotation 0 0 1 5.8469 children [ Shape { appearance USE STONE_B geometry Box { size 0.34 0.34 1.06 } } Shape { appearance USE CAP geometry Box { size 0.44 0.44 0.07 } } ] } +Solid { translation 9.77 -11.64 0.53 rotation 0 0 1 6.1959 children [ Shape { appearance USE STONE_B geometry Box { size 0.34 0.34 1.06 } } Shape { appearance USE CAP geometry Box { size 0.44 0.44 0.07 } } ] } +Solid { translation 13.16 -7.60 0.53 rotation 0 0 1 6.5450 children [ Shape { appearance USE STONE_B geometry Box { size 0.34 0.34 1.06 } } Shape { appearance USE CAP geometry Box { size 0.44 0.44 0.07 } } ] } +Solid { translation 14.97 -2.64 0.53 rotation 0 0 1 6.8941 children [ Shape { appearance USE STONE_B geometry Box { size 0.34 0.34 1.06 } } Shape { appearance USE CAP geometry Box { size 0.44 0.44 0.07 } } ] } + +# ==================== EXTERNAL PEN (south of round field gate) ==================== +# Pen west wall +Solid { translation -1.57 -18.5 0.55 children [ + Transform { translation 0 -3.46 0 children [ Shape { appearance USE FPOST geometry Box { size 0.12 0.12 1.10 } } ] } + Transform { translation 0 -1.73 0 children [ Shape { appearance USE FPOST geometry Box { size 0.12 0.12 1.10 } } ] } + Transform { translation 0 0 0 children [ Shape { appearance USE FPOST geometry Box { size 0.12 0.12 1.10 } } ] } + Transform { translation 0 1.73 0 children [ Shape { appearance USE FPOST geometry Box { size 0.12 0.12 1.10 } } ] } + Transform { translation 0 3.46 0 children [ Shape { appearance USE FPOST geometry Box { size 0.12 0.12 1.10 } } ] } + Transform { translation 0 0 -0.38 children [ Shape { appearance USE WOOD geometry Box { size 0.06 6.92 0.08 } } ] } + Transform { translation 0 0 -0.05 children [ Shape { appearance USE WOOD geometry Box { size 0.06 6.92 0.08 } } ] } + Transform { translation 0 0 0.30 children [ Shape { appearance USE WOOD geometry Box { size 0.06 6.92 0.08 } } ] } + Transform { translation 0 0 0.53 children [ Shape { appearance USE FPOST geometry Box { size 0.14 6.92 0.04 } } ] } +] boundingObject Box { size 0.14 6.92 1.10 } } +# Pen east wall +Solid { translation 1.57 -18.5 0.55 children [ + Transform { translation 0 -3.46 0 children [ Shape { appearance USE FPOST geometry Box { size 0.12 0.12 1.10 } } ] } + Transform { translation 0 -1.73 0 children [ Shape { appearance USE FPOST geometry Box { size 0.12 0.12 1.10 } } ] } + Transform { translation 0 0 0 children [ Shape { appearance USE FPOST geometry Box { size 0.12 0.12 1.10 } } ] } + Transform { translation 0 1.73 0 children [ Shape { appearance USE FPOST geometry Box { size 0.12 0.12 1.10 } } ] } + Transform { translation 0 3.46 0 children [ Shape { appearance USE FPOST geometry Box { size 0.12 0.12 1.10 } } ] } + Transform { translation 0 0 -0.38 children [ Shape { appearance USE WOOD geometry Box { size 0.06 6.92 0.08 } } ] } + Transform { translation 0 0 -0.05 children [ Shape { appearance USE WOOD geometry Box { size 0.06 6.92 0.08 } } ] } + Transform { translation 0 0 0.30 children [ Shape { appearance USE WOOD geometry Box { size 0.06 6.92 0.08 } } ] } + Transform { translation 0 0 0.53 children [ Shape { appearance USE FPOST geometry Box { size 0.14 6.92 0.04 } } ] } +] boundingObject Box { size 0.14 6.92 1.10 } } +# Pen south wall +Solid { translation 0.00 -22 0.55 children [ + Transform { translation -1.52 0 0 children [ Shape { appearance USE FPOST geometry Box { size 0.12 0.12 1.10 } } ] } + Transform { translation 0 0 0 children [ Shape { appearance USE FPOST geometry Box { size 0.12 0.12 1.10 } } ] } + Transform { translation 1.52 0 0 children [ Shape { appearance USE FPOST geometry Box { size 0.12 0.12 1.10 } } ] } + Transform { translation 0 0 -0.38 children [ Shape { appearance USE WOOD geometry Box { size 3.16 0.06 0.08 } } ] } + Transform { translation 0 0 -0.05 children [ Shape { appearance USE WOOD geometry Box { size 3.16 0.06 0.08 } } ] } + Transform { translation 0 0 0.30 children [ Shape { appearance USE WOOD geometry Box { size 3.16 0.06 0.08 } } ] } + Transform { translation 0 0 0.53 children [ Shape { appearance USE FPOST geometry Box { size 3.16 0.14 0.04 } } ] } +] boundingObject Box { size 3.16 0.14 1.10 } } +# Pen north corner posts at the gate opening +Solid { translation -1.57 -15.0 0.55 children [ Shape { appearance USE FPOST geometry Box { size 0.12 0.12 1.10 } } ] } +Solid { translation 1.57 -15.0 0.55 children [ Shape { appearance USE FPOST geometry Box { size 0.12 0.12 1.10 } } ] } + +# Gate width: 3.14 m (pen x: [-1.57, 1.57]) + +# ==================== BARN 1 — Gambrel/Dutch style (NE, outside fence) ==================== +# Body 10×7×4, weathered gray-brown wood, gambrel roof, large double doors +Solid { + translation 18.5 25.49 2 + children [ + Shape { appearance PBRAppearance { baseColor 0.52 0.42 0.30 roughness 0.92 metalness 0 } geometry Box { size 10 7 4 } } + # Gambrel roof + Transform { translation -3.5 0 3.05 rotation 0 1 0 -0.611 children [ Shape { appearance PBRAppearance { baseColor 0.20 0.18 0.16 roughness 0.82 metalness 0.02 } geometry Box { size 3.9 7.2 0.18 } } ] } + Transform { translation 3.5 0 3.05 rotation 0 1 0 0.611 children [ Shape { appearance PBRAppearance { baseColor 0.20 0.18 0.16 roughness 0.82 metalness 0.02 } geometry Box { size 3.9 7.2 0.18 } } ] } + Transform { translation -1.0 0 4.55 rotation 0 1 0 -0.422 children [ Shape { appearance PBRAppearance { baseColor 0.20 0.18 0.16 roughness 0.82 metalness 0.02 } geometry Box { size 2.5 7.2 0.18 } } ] } + Transform { translation 1.0 0 4.55 rotation 0 1 0 0.422 children [ Shape { appearance PBRAppearance { baseColor 0.20 0.18 0.16 roughness 0.82 metalness 0.02 } geometry Box { size 2.5 7.2 0.18 } } ] } + Transform { translation 0 0 5.04 children [ Shape { appearance PBRAppearance { baseColor 0.20 0.18 0.16 roughness 0.82 metalness 0.02 } geometry Box { size 1.6 7.2 0.22 } } ] } + # South gable fill + Transform { translation 0 -3.57 2.40 children [ Shape { appearance PBRAppearance { baseColor 0.52 0.42 0.30 roughness 0.92 metalness 0 } geometry Box { size 8.8 0.16 0.80 } } ] } + Transform { translation 0 -3.57 3.10 children [ Shape { appearance PBRAppearance { baseColor 0.52 0.42 0.30 roughness 0.92 metalness 0 } geometry Box { size 6.8 0.16 0.70 } } ] } + Transform { translation 0 -3.57 3.70 children [ Shape { appearance PBRAppearance { baseColor 0.52 0.42 0.30 roughness 0.92 metalness 0 } geometry Box { size 5.1 0.16 0.60 } } ] } + Transform { translation 0 -3.57 4.10 children [ Shape { appearance PBRAppearance { baseColor 0.52 0.42 0.30 roughness 0.92 metalness 0 } geometry Box { size 4.0 0.16 0.40 } } ] } + Transform { translation 0 -3.57 4.42 children [ Shape { appearance PBRAppearance { baseColor 0.52 0.42 0.30 roughness 0.92 metalness 0 } geometry Box { size 2.7 0.16 0.60 } } ] } + Transform { translation 0 -3.57 4.84 children [ Shape { appearance PBRAppearance { baseColor 0.52 0.42 0.30 roughness 0.92 metalness 0 } geometry Box { size 0.9 0.16 0.36 } } ] } + # North gable fill + Transform { translation 0 3.57 2.40 children [ Shape { appearance PBRAppearance { baseColor 0.52 0.42 0.30 roughness 0.92 metalness 0 } geometry Box { size 8.8 0.16 0.80 } } ] } + Transform { translation 0 3.57 3.10 children [ Shape { appearance PBRAppearance { baseColor 0.52 0.42 0.30 roughness 0.92 metalness 0 } geometry Box { size 6.8 0.16 0.70 } } ] } + Transform { translation 0 3.57 3.70 children [ Shape { appearance PBRAppearance { baseColor 0.52 0.42 0.30 roughness 0.92 metalness 0 } geometry Box { size 5.1 0.16 0.60 } } ] } + Transform { translation 0 3.57 4.10 children [ Shape { appearance PBRAppearance { baseColor 0.52 0.42 0.30 roughness 0.92 metalness 0 } geometry Box { size 4.0 0.16 0.40 } } ] } + Transform { translation 0 3.57 4.42 children [ Shape { appearance PBRAppearance { baseColor 0.52 0.42 0.30 roughness 0.92 metalness 0 } geometry Box { size 2.7 0.16 0.60 } } ] } + Transform { translation 0 3.57 4.84 children [ Shape { appearance PBRAppearance { baseColor 0.52 0.42 0.30 roughness 0.92 metalness 0 } geometry Box { size 0.9 0.16 0.36 } } ] } + # Double barn doors (south face) + Transform { + translation 0 -3.51 -0.50 + children [ + Shape { appearance PBRAppearance { baseColor 0.44 0.30 0.14 roughness 0.88 metalness 0 } geometry Box { size 2.8 0.10 3.0 } } + Transform { rotation 0 0 1 0.83 children [ Shape { appearance PBRAppearance { baseColor 0.34 0.22 0.10 roughness 0.90 metalness 0 } geometry Box { size 0.10 0.12 3.75 } } ] } + Transform { rotation 0 0 1 -0.83 children [ Shape { appearance PBRAppearance { baseColor 0.34 0.22 0.10 roughness 0.90 metalness 0 } geometry Box { size 0.10 0.12 3.75 } } ] } + Transform { translation -1.45 0 0 children [ Shape { appearance PBRAppearance { baseColor 0.34 0.22 0.10 roughness 0.90 metalness 0 } geometry Box { size 0.12 0.14 3.24 } } ] } + Transform { translation 1.45 0 0 children [ Shape { appearance PBRAppearance { baseColor 0.34 0.22 0.10 roughness 0.90 metalness 0 } geometry Box { size 0.12 0.14 3.24 } } ] } + Transform { translation 0 0 1.62 children [ Shape { appearance PBRAppearance { baseColor 0.34 0.22 0.10 roughness 0.90 metalness 0 } geometry Box { size 3.04 0.14 0.14 } } ] } + ] + } + # Windows + Transform { translation -3.6 -3.52 0.55 children [ Shape { appearance PBRAppearance { baseColor 0.60 0.80 0.95 roughness 0.20 metalness 0.05 } geometry Box { size 1.40 0.12 1.10 } } ] } + Transform { translation 3.6 -3.52 0.55 children [ Shape { appearance PBRAppearance { baseColor 0.60 0.80 0.95 roughness 0.20 metalness 0.05 } geometry Box { size 1.40 0.12 1.10 } } ] } + Transform { translation 5.06 2.0 0.55 children [ Shape { appearance PBRAppearance { baseColor 0.60 0.80 0.95 roughness 0.20 metalness 0.05 } geometry Box { size 0.12 1.20 1.0 } } ] } + Transform { translation 5.06 -2.0 0.55 children [ Shape { appearance PBRAppearance { baseColor 0.60 0.80 0.95 roughness 0.20 metalness 0.05 } geometry Box { size 0.12 1.20 1.0 } } ] } + Transform { translation 0 -3.52 3.90 children [ Shape { appearance PBRAppearance { baseColor 0.44 0.30 0.14 roughness 0.88 metalness 0 } geometry Box { size 1.30 0.12 1.00 } } ] } + ] + boundingObject Box { size 10 7 7 } +} + +# ==================== BARN 3 — Red barn (NE, outside fence, gate facing fence) ==================== +# Body 7×9×3.5, red walls, steep dark roof +Solid { + translation 29.76 9.52 1.75 + rotation 0 0 1 -1.5708 + children [ + Shape { appearance USE BARN_RED geometry Box { size 7 9 3.5 } } + # Roof + Transform { translation -2.0 0 3.0 rotation 0 1 0 -0.70 children [ Shape { appearance USE BARN_ROOF geometry Box { size 4.2 9.2 0.20 } } ] } + Transform { translation 2.0 0 3.0 rotation 0 1 0 0.70 children [ Shape { appearance USE BARN_ROOF geometry Box { size 4.2 9.2 0.20 } } ] } + Transform { translation 0 0 4.28 children [ Shape { appearance USE BARN_ROOF geometry Box { size 2.0 9.2 0.24 } } ] } + # South gable fill + Transform { translation 0 -4.52 2.05 children [ Shape { appearance USE BARN_RED geometry Box { size 6.2 0.16 0.60 } } ] } + Transform { translation 0 -4.52 2.65 children [ Shape { appearance USE BARN_RED geometry Box { size 4.5 0.16 0.60 } } ] } + Transform { translation 0 -4.52 3.25 children [ Shape { appearance USE BARN_RED geometry Box { size 2.9 0.16 0.60 } } ] } + Transform { translation 0 -4.52 3.85 children [ Shape { appearance USE BARN_RED geometry Box { size 1.2 0.16 0.60 } } ] } + # North gable fill + Transform { translation 0 4.52 2.05 children [ Shape { appearance USE BARN_RED geometry Box { size 6.2 0.16 0.60 } } ] } + Transform { translation 0 4.52 2.65 children [ Shape { appearance USE BARN_RED geometry Box { size 4.5 0.16 0.60 } } ] } + Transform { translation 0 4.52 3.25 children [ Shape { appearance USE BARN_RED geometry Box { size 2.9 0.16 0.60 } } ] } + Transform { translation 0 4.52 3.85 children [ Shape { appearance USE BARN_RED geometry Box { size 1.2 0.16 0.60 } } ] } + # Door + Transform { + translation 0 -4.52 -0.62 + children [ + Shape { appearance USE DOOR_MAT geometry Box { size 1.70 0.14 2.26 } } + Transform { translation 0 0 1.22 children [ Shape { appearance USE WOOD geometry Box { size 2.10 0.18 0.26 } } ] } + Transform { translation -0.90 0 0 children [ Shape { appearance USE WOOD geometry Box { size 0.24 0.18 2.52 } } ] } + Transform { translation 0.90 0 0 children [ Shape { appearance USE WOOD geometry Box { size 0.24 0.18 2.52 } } ] } + Transform { translation 0 0 -0.68 children [ Shape { appearance USE WOOD geometry Box { size 1.60 0.12 0.12 } } ] } + Transform { translation 0 0 0.30 children [ Shape { appearance USE WOOD geometry Box { size 1.60 0.12 0.12 } } ] } + ] + } + # Windows — south face + Transform { translation -2.2 -4.53 0.30 children [ Shape { appearance USE GLASS geometry Box { size 0.80 0.14 0.70 } } ] } + Transform { translation 2.2 -4.53 0.30 children [ Shape { appearance USE GLASS geometry Box { size 0.80 0.14 0.70 } } ] } + # East-face windows + Transform { translation 3.52 3.0 0.30 children [ Shape { appearance USE GLASS geometry Box { size 0.14 0.80 0.70 } } ] } + Transform { translation 3.52 0.0 0.30 children [ Shape { appearance USE GLASS geometry Box { size 0.14 0.80 0.70 } } ] } + Transform { translation 3.52 -3.0 0.30 children [ Shape { appearance USE GLASS geometry Box { size 0.14 0.80 0.70 } } ] } + ] + boundingObject Box { size 7 9 6 } +} + +# ==================== TREES (outside fence) ==================== + +# Tree A — large oak, SE +Solid { + translation 20 -18 0 + children [ + Transform { translation 0 0 2.0 children [ Shape { appearance USE TRUNK geometry Cylinder { height 4.0 radius 0.30 subdivision 10 } } ] } + Transform { translation 0.0 0.0 4.6 children [ Shape { appearance USE LEAF_A geometry Sphere { radius 2.6 subdivision 4 } } ] } + Transform { translation 1.2 0.6 5.6 children [ Shape { appearance USE LEAF_B geometry Sphere { radius 1.9 subdivision 4 } } ] } + Transform { translation -1.0 0.9 5.3 children [ Shape { appearance USE LEAF_C geometry Sphere { radius 1.7 subdivision 4 } } ] } + Transform { translation 0.4 -1.1 5.1 children [ Shape { appearance USE LEAF_A geometry Sphere { radius 1.5 subdivision 4 } } ] } + Transform { translation -0.5 -0.4 6.2 children [ Shape { appearance USE LEAF_B geometry Sphere { radius 1.0 subdivision 4 } } ] } + ] +} + +# Tree B — medium, NE near barn +Solid { + translation -8 26 0 + children [ + Transform { translation 0 0 1.7 children [ Shape { appearance USE TRUNK geometry Cylinder { height 3.4 radius 0.25 subdivision 10 } } ] } + Transform { translation 0.0 0.0 3.8 children [ Shape { appearance USE LEAF_C geometry Sphere { radius 2.2 subdivision 4 } } ] } + Transform { translation 0.9 -0.7 4.7 children [ Shape { appearance USE LEAF_A geometry Sphere { radius 1.6 subdivision 4 } } ] } + Transform { translation -0.6 0.8 4.4 children [ Shape { appearance USE LEAF_B geometry Sphere { radius 1.4 subdivision 4 } } ] } + ] +} + +# Tree C — large, NW +Solid { + translation -23 20 0 + children [ + Transform { translation 0 0 2.3 children [ Shape { appearance USE TRUNK geometry Cylinder { height 4.6 radius 0.36 subdivision 10 } } ] } + Transform { translation 0.0 0.0 5.2 children [ Shape { appearance USE LEAF_B geometry Sphere { radius 2.9 subdivision 4 } } ] } + Transform { translation 1.3 0.9 6.3 children [ Shape { appearance USE LEAF_A geometry Sphere { radius 2.1 subdivision 4 } } ] } + Transform { translation -1.1 1.1 6.0 children [ Shape { appearance USE LEAF_C geometry Sphere { radius 1.9 subdivision 4 } } ] } + Transform { translation 0.6 -1.3 5.8 children [ Shape { appearance USE LEAF_A geometry Sphere { radius 1.6 subdivision 4 } } ] } + ] +} + +# Tree D — small, SW +Solid { + translation -20 -23 0 + children [ + Transform { translation 0 0 1.4 children [ Shape { appearance USE TRUNK geometry Cylinder { height 2.8 radius 0.20 subdivision 10 } } ] } + Transform { translation 0.0 0.0 3.2 children [ Shape { appearance USE LEAF_C geometry Sphere { radius 1.9 subdivision 4 } } ] } + Transform { translation -0.7 0.6 4.0 children [ Shape { appearance USE LEAF_A geometry Sphere { radius 1.4 subdivision 4 } } ] } + Transform { translation 0.6 -0.5 3.8 children [ Shape { appearance USE LEAF_B geometry Sphere { radius 1.2 subdivision 4 } } ] } + ] +} + +# Tree E — north cluster +Solid { + translation 7 23 0 + children [ + Transform { translation 0 0 1.9 children [ Shape { appearance USE TRUNK geometry Cylinder { height 3.8 radius 0.27 subdivision 10 } } ] } + Transform { translation 0.0 0.0 4.1 children [ Shape { appearance USE LEAF_A geometry Sphere { radius 2.3 subdivision 4 } } ] } + Transform { translation 1.0 0.5 5.0 children [ Shape { appearance USE LEAF_C geometry Sphere { radius 1.7 subdivision 4 } } ] } + Transform { translation -0.6 -0.9 4.8 children [ Shape { appearance USE LEAF_B geometry Sphere { radius 1.4 subdivision 4 } } ] } + ] +} + +# Tree F — SW +Solid { + translation -2.98 -22.8 0 + children [ + Transform { translation 0 0 1.3 children [ Shape { appearance USE TRUNK geometry Cylinder { height 2.6 radius 0.19 subdivision 10 } } ] } + Transform { translation 0.0 0.0 2.9 children [ Shape { appearance USE LEAF_B geometry Sphere { radius 1.7 subdivision 4 } } ] } + Transform { translation 0.6 0.4 3.7 children [ Shape { appearance USE LEAF_A geometry Sphere { radius 1.2 subdivision 4 } } ] } + ] +} + +# Tree G — west side +Solid { + translation -23 -5 0 + children [ + Transform { translation 0 0 2.0 children [ Shape { appearance USE TRUNK geometry Cylinder { height 4.0 radius 0.29 subdivision 10 } } ] } + Transform { translation 0.0 0.0 4.4 children [ Shape { appearance USE LEAF_C geometry Sphere { radius 2.4 subdivision 4 } } ] } + Transform { translation -1.0 0.8 5.3 children [ Shape { appearance USE LEAF_A geometry Sphere { radius 1.8 subdivision 4 } } ] } + Transform { translation 0.9 -0.7 5.0 children [ Shape { appearance USE LEAF_B geometry Sphere { radius 1.6 subdivision 4 } } ] } + ] +} + +# Tree H — east side +Solid { + translation 21.35 -1.05 0 + children [ + Transform { translation 0 0 1.5 children [ Shape { appearance USE TRUNK geometry Cylinder { height 3.0 radius 0.22 subdivision 10 } } ] } + Transform { translation 0.0 0.0 3.4 children [ Shape { appearance USE LEAF_A geometry Sphere { radius 2.0 subdivision 4 } } ] } + Transform { translation 0.7 0.6 4.2 children [ Shape { appearance USE LEAF_C geometry Sphere { radius 1.4 subdivision 4 } } ] } + Transform { translation -0.5 -0.8 4.0 children [ Shape { appearance USE LEAF_B geometry Sphere { radius 1.2 subdivision 4 } } ] } + ] +} + +# ==================== SCARECROW (east side, outside fence) ==================== +Solid { + translation 20 -10 0 + rotation 0 0 1 2.61799 + children [ + Transform { translation 0 0 1.22 children [ Shape { appearance USE TRUNK geometry Cylinder { height 2.44 radius 0.045 subdivision 8 } } ] } + Transform { translation 0 0 2.02 rotation 1 0 0 1.5708 children [ Shape { appearance USE TRUNK geometry Cylinder { height 1.60 radius 0.032 subdivision 8 } } ] } + Transform { + translation 0 0 2.44 + children [ + Shape { appearance USE STRAW geometry Sphere { radius 0.17 subdivision 3 } } + Transform { translation 0.13 0.05 0.06 children [ Shape { appearance PBRAppearance { baseColor 0.06 0.06 0.06 } geometry Sphere { radius 0.028 subdivision 2 } } ] } + Transform { translation 0.13 -0.05 0.06 children [ Shape { appearance PBRAppearance { baseColor 0.06 0.06 0.06 } geometry Sphere { radius 0.028 subdivision 2 } } ] } + Transform { translation 0.16 0 -0.02 rotation 0 1 0 1.5708 children [ Shape { appearance PBRAppearance { baseColor 0.75 0.50 0.30 } geometry Cone { height 0.07 bottomRadius 0.032 subdivision 6 } } ] } + Transform { translation 0.14 0.04 -0.06 children [ Shape { appearance PBRAppearance { baseColor 0.18 0.08 0.08 } geometry Box { size 0.01 0.04 0.01 } } ] } + Transform { translation 0.14 -0.04 -0.06 children [ Shape { appearance PBRAppearance { baseColor 0.18 0.08 0.08 } geometry Box { size 0.01 0.04 0.01 } } ] } + ] + } + Transform { translation 0 0 2.62 children [ Shape { appearance USE HAT geometry Cylinder { height 0.04 radius 0.28 subdivision 12 } } ] } + Transform { translation 0 0 2.80 children [ Shape { appearance USE HAT geometry Cylinder { height 0.30 radius 0.17 subdivision 10 } } ] } + Transform { translation 0 0 1.60 children [ Shape { appearance USE SHIRT geometry Box { size 0.20 0.40 0.46 } } ] } + Transform { translation 0 0 1.14 children [ Shape { appearance USE PANTS geometry Box { size 0.17 0.32 0.34 } } ] } + Transform { translation 0 0.68 2.03 rotation 0 0 1 0.25 children [ Shape { appearance USE STRAW geometry Box { size 0.03 0.24 0.03 } } ] } + Transform { translation 0 -0.68 2.03 rotation 0 0 -1 0.25 children [ Shape { appearance USE STRAW geometry Box { size 0.03 0.24 0.03 } } ] } + Transform { translation 0.10 0.08 1.82 children [ Shape { appearance USE STRAW geometry Box { size 0.03 0.03 0.14 } } ] } + Transform { translation 0.10 -0.08 1.82 children [ Shape { appearance USE STRAW geometry Box { size 0.03 0.03 0.14 } } ] } + ] +} + +# ==================== HAY BALES (near barn) ==================== +Solid { translation 25.75 13.76 0.62 children [ Transform { rotation 1 0 0 1.5708 children [ Shape { appearance USE HAY geometry Cylinder { height 1.30 radius 0.62 subdivision 14 } } ] } ] boundingObject Box { size 1.30 1.24 1.24 } } +Solid { translation 24.34 12.32 0.62 rotation -1 0 0 1.5708 children [ Transform { rotation 1 0 0 1.5708 children [ Shape { appearance USE HAY geometry Cylinder { height 1.30 radius 0.62 subdivision 14 } } ] } ] boundingObject Box { size 1.30 1.24 1.24 } } +Solid { translation 24.28 13.79 0.62 children [ Transform { rotation 1 0 0 1.5708 children [ Shape { appearance USE HAY geometry Cylinder { height 1.30 radius 0.62 subdivision 14 } } ] } ] boundingObject Box { size 1.30 1.24 1.24 } } + +# ==================== TRACTOR (near barn) ==================== +Solid { + translation 17 19 0.18 + rotation 0 0 1 1.9 + children [ + # Chassis + Transform { translation 0 0 0.35 children [ Shape { appearance PBRAppearance { baseColor 0.20 0.20 0.20 roughness 0.6 metalness 0.3 } geometry Box { size 2.0 0.90 0.12 } } ] } + # Engine hood + Transform { translation 0.60 0 0.60 children [ Shape { appearance PBRAppearance { baseColor 0.15 0.50 0.12 roughness 0.7 metalness 0.1 } geometry Box { size 0.65 0.80 0.45 } } ] } + # Main body + Transform { translation -0.15 0 0.60 children [ Shape { appearance PBRAppearance { baseColor 0.15 0.50 0.12 roughness 0.7 metalness 0.1 } geometry Box { size 0.80 0.85 0.45 } } ] } + # Cabin + Transform { translation -0.20 0 0.95 children [ Shape { appearance PBRAppearance { baseColor 0.15 0.50 0.12 roughness 0.7 metalness 0.1 } geometry Box { size 0.75 0.80 0.45 } } ] } + # Cabin roof + Transform { translation -0.20 0 1.22 children [ Shape { appearance PBRAppearance { baseColor 0.12 0.40 0.10 roughness 0.75 metalness 0.1 } geometry Box { size 0.85 0.90 0.06 } } ] } + # Windshield + Transform { translation 0.12 0 0.95 children [ Shape { appearance USE GLASS geometry Box { size 0.02 0.55 0.35 } } ] } + # Rear window + Transform { translation -0.58 0 0.95 children [ Shape { appearance USE GLASS geometry Box { size 0.02 0.55 0.35 } } ] } + # Side windows + Transform { translation -0.20 0.40 0.95 children [ Shape { appearance USE GLASS geometry Box { size 0.55 0.02 0.30 } } ] } + Transform { translation -0.20 -0.40 0.95 children [ Shape { appearance USE GLASS geometry Box { size 0.55 0.02 0.30 } } ] } + # Seat + Transform { translation -0.25 0 0.55 children [ Shape { appearance PBRAppearance { baseColor 0.12 0.12 0.12 roughness 0.9 } geometry Box { size 0.30 0.35 0.06 } } ] } + # Exhaust stack + Transform { translation 0.50 0.30 0.60 children [ + Shape { appearance PBRAppearance { baseColor 0.25 0.25 0.25 roughness 0.4 metalness 0.6 } geometry Cylinder { height 0.90 radius 0.03 subdivision 6 } } + Transform { translation 0 0 0.50 children [ Shape { appearance PBRAppearance { baseColor 0.20 0.20 0.20 roughness 0.4 metalness 0.6 } geometry Cylinder { height 0.04 radius 0.045 subdivision 6 } } ] } + ] } + # Rear axle + Transform { translation -0.45 0 0.40 children [ Shape { appearance PBRAppearance { baseColor 0.25 0.25 0.25 roughness 0.5 metalness 0.5 } geometry Box { size 0.08 1.15 0.08 } } ] } + # Front axle + Transform { translation 0.60 0 0.25 children [ Shape { appearance PBRAppearance { baseColor 0.25 0.25 0.25 roughness 0.5 metalness 0.5 } geometry Box { size 0.08 0.90 0.08 } } ] } + # Rear left wheel + Transform { translation -0.45 0.60 0.40 rotation 1 0 0 1.5708 children [ + Shape { appearance PBRAppearance { baseColor 0.08 0.08 0.08 roughness 0.95 } geometry Cylinder { height 0.22 radius 0.40 subdivision 20 } } + Shape { appearance PBRAppearance { baseColor 0.35 0.35 0.35 metalness 0.5 } geometry Cylinder { height 0.24 radius 0.14 subdivision 10 } } + ] } + # Rear right wheel + Transform { translation -0.45 -0.60 0.40 rotation 1 0 0 1.5708 children [ + Shape { appearance PBRAppearance { baseColor 0.08 0.08 0.08 roughness 0.95 } geometry Cylinder { height 0.22 radius 0.40 subdivision 20 } } + Shape { appearance PBRAppearance { baseColor 0.35 0.35 0.35 metalness 0.5 } geometry Cylinder { height 0.24 radius 0.14 subdivision 10 } } + ] } + # Front left wheel + Transform { translation 0.60 0.45 0.25 rotation 1 0 0 1.5708 children [ + Shape { appearance PBRAppearance { baseColor 0.08 0.08 0.08 roughness 0.95 } geometry Cylinder { height 0.16 radius 0.25 subdivision 16 } } + Shape { appearance PBRAppearance { baseColor 0.35 0.35 0.35 metalness 0.5 } geometry Cylinder { height 0.18 radius 0.09 subdivision 8 } } + ] } + # Front right wheel + Transform { translation 0.60 -0.45 0.25 rotation 1 0 0 1.5708 children [ + Shape { appearance PBRAppearance { baseColor 0.08 0.08 0.08 roughness 0.95 } geometry Cylinder { height 0.16 radius 0.25 subdivision 16 } } + Shape { appearance PBRAppearance { baseColor 0.35 0.35 0.35 metalness 0.5 } geometry Cylinder { height 0.18 radius 0.09 subdivision 8 } } + ] } + # Rear fenders + Transform { translation -0.45 0.50 0.72 children [ Shape { appearance PBRAppearance { baseColor 0.12 0.40 0.10 roughness 0.75 metalness 0.1 } geometry Box { size 0.50 0.12 0.20 } } ] } + Transform { translation -0.45 -0.50 0.72 children [ Shape { appearance PBRAppearance { baseColor 0.12 0.40 0.10 roughness 0.75 metalness 0.1 } geometry Box { size 0.50 0.12 0.20 } } ] } + # Front bumper + Transform { translation 0.95 0 0.35 children [ Shape { appearance PBRAppearance { baseColor 0.35 0.35 0.35 roughness 0.7 metalness 0.3 } geometry Box { size 0.12 0.75 0.30 } } ] } + # Headlights + Transform { translation 0.97 0.25 0.45 children [ Shape { appearance PBRAppearance { baseColor 0.95 0.92 0.70 roughness 0.3 } geometry Sphere { radius 0.05 subdivision 3 } } ] } + Transform { translation 0.97 -0.25 0.45 children [ Shape { appearance PBRAppearance { baseColor 0.95 0.92 0.70 roughness 0.3 } geometry Sphere { radius 0.05 subdivision 3 } } ] } + # Taillights + Transform { translation -0.58 0.25 0.45 children [ Shape { appearance PBRAppearance { baseColor 0.80 0.10 0.10 roughness 0.4 } geometry Box { size 0.04 0.08 0.06 } } ] } + Transform { translation -0.58 -0.25 0.45 children [ Shape { appearance PBRAppearance { baseColor 0.80 0.10 0.10 roughness 0.4 } geometry Box { size 0.04 0.08 0.06 } } ] } + # Drawbar hitch + Transform { translation -0.95 0 0.20 children [ Shape { appearance PBRAppearance { baseColor 0.25 0.25 0.25 roughness 0.5 metalness 0.5 } geometry Box { size 0.12 0.06 0.06 } } ] } + ] + boundingObject Box { size 2.2 1.4 1.3 } +} + +# ==================== GRASS PATCHES (inside field, decorative) ==================== +Solid { translation -8 6 0.15 children [ + Transform { translation 0.10 0.00 0 children [ Shape { appearance USE LEAF_B geometry Box { size 0.04 0.02 0.30 } } ] } + Transform { translation -0.05 0.12 0 rotation 0 0 1 0.4 children [ Shape { appearance USE LEAF_A geometry Box { size 0.04 0.02 0.26 } } ] } + Transform { translation 0.08 -0.10 0 rotation 0 0 1 -0.3 children [ Shape { appearance USE LEAF_C geometry Box { size 0.04 0.02 0.28 } } ] } + Transform { translation -0.12 0.04 0 rotation 0 0 1 0.2 children [ Shape { appearance USE LEAF_B geometry Box { size 0.04 0.02 0.24 } } ] } +] } +Solid { translation 6 -9 0.15 children [ + Transform { translation 0.08 0.06 0 children [ Shape { appearance USE LEAF_A geometry Box { size 0.04 0.02 0.28 } } ] } + Transform { translation -0.10 0.00 0 rotation 0 0 1 -0.3 children [ Shape { appearance USE LEAF_C geometry Box { size 0.04 0.02 0.32 } } ] } + Transform { translation 0.02 -0.12 0 rotation 0 0 1 0.35 children [ Shape { appearance USE LEAF_B geometry Box { size 0.04 0.02 0.26 } } ] } + Transform { translation -0.06 0.10 0 children [ Shape { appearance USE LEAF_A geometry Box { size 0.04 0.02 0.22 } } ] } +] } +Solid { translation -3 11 0.15 children [ + Transform { translation 0.06 -0.06 0 children [ Shape { appearance USE LEAF_C geometry Box { size 0.04 0.02 0.26 } } ] } + Transform { translation -0.08 0.08 0 rotation 0 0 1 0.3 children [ Shape { appearance USE LEAF_A geometry Box { size 0.04 0.02 0.30 } } ] } + Transform { translation 0.12 0.02 0 rotation 0 0 1 -0.25 children [ Shape { appearance USE LEAF_B geometry Box { size 0.04 0.02 0.28 } } ] } +] } +Solid { translation 10 8 0.15 children [ + Transform { translation -0.07 0.05 0 children [ Shape { appearance USE LEAF_B geometry Box { size 0.04 0.02 0.24 } } ] } + Transform { translation 0.09 -0.07 0 rotation 0 0 1 0.4 children [ Shape { appearance USE LEAF_C geometry Box { size 0.04 0.02 0.28 } } ] } + Transform { translation 0.00 0.11 0 rotation 0 0 1 -0.2 children [ Shape { appearance USE LEAF_A geometry Box { size 0.04 0.02 0.26 } } ] } +] } +Solid { translation -11 -7 0.15 children [ + Transform { translation 0.05 0.08 0 children [ Shape { appearance USE LEAF_A geometry Box { size 0.04 0.02 0.30 } } ] } + Transform { translation -0.09 -0.04 0 rotation 0 0 1 0.35 children [ Shape { appearance USE LEAF_B geometry Box { size 0.04 0.02 0.28 } } ] } + Transform { translation 0.10 -0.09 0 rotation 0 0 1 -0.3 children [ Shape { appearance USE LEAF_C geometry Box { size 0.04 0.02 0.24 } } ] } + Transform { translation -0.03 0.12 0 children [ Shape { appearance USE LEAF_A geometry Box { size 0.04 0.02 0.26 } } ] } +] } + +# ==================== SHEPHERD DOG ==================== +ShepherdDog { + translation 0 0 0.5 + rotation 0 0 1 0 + controller "shepherd_dog" +} + +# ==================== SHEEP ==================== +# Up to 10 sheep, scattered through the field's central/north zone. Comment +# out trailing slots to test smaller flock sizes; the dog policy is trained +# to handle 1..10 sheep so any prefix works. +Sheep { translation 3.0 2.0 0.5 name "sheep1" controller "sheep" } +Sheep { translation 3.0 -2.0 0.5 name "sheep2" controller "sheep" } +Sheep { translation 4.0 0.0 0.5 name "sheep3" controller "sheep" } +Sheep { translation -3.0 4.0 0.5 name "sheep4" controller "sheep" } +Sheep { translation -5.0 -2.0 0.5 name "sheep5" controller "sheep" } +Sheep { translation 6.0 5.0 0.5 name "sheep6" controller "sheep" } +Sheep { translation -6.0 6.0 0.5 name "sheep7" controller "sheep" } +Sheep { translation 0.0 8.0 0.5 name "sheep8" controller "sheep" } +Sheep { translation -8.0 0.0 0.5 name "sheep9" controller "sheep" } +Sheep { translation 7.0 -4.0 0.5 name "sheep10" controller "sheep" }