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

Checkpoint 8

Browse Source
This commit is contained in:
Johnny Fernandes
2026-05-12 22:41:03 +01:00
parent a01a5c9cef
commit 5c2ee4bba5
31 changed files with 3189 additions and 380 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+8 -23
View File
@@ -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
Makefile
+102 -18
View File
@@ -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)"
README.md
+10 -1
View File
@@ -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
controllers/shepherd_dog/shepherd_dog.py
+127 -47
View File
@@ -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,6 +240,16 @@ def drive(vx: float, vy: float, left_motor, right_motor, compass, motor_max: flo
robot = Robot()
timestep = int(robot.getBasicTimeStep())
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"))
@@ -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.
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()} "
docs/project.md
+4
View File
@@ -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 <up202502576@up.pt>
- Johnny Fernandes <up202402612@up.pt>
herding/control/active_scan.py
+27 -7
View File
@@ -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
herding/control/universal.py
+187
View File
@@ -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
herding/perception/lidar_perception.py
+113 -31
View File
@@ -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]
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 to estimate the
# centre.
# 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
# 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_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
# 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:
if _near_wall(cx, cy):
continue
detections.append((cx, cy))
return detections
herding/perception/lidar_sim.py
+101 -30
View File
@@ -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)
herding/perception/obs.py
+6 -12
View File
@@ -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
herding/perception/sheep_tracker.py
+117 -41
View File
@@ -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:
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)
herding/world/diffdrive.py
+130 -1
View File
@@ -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,
)
herding/world/flocking_sim.py
+26 -9
View File
@@ -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,7 +132,23 @@ 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).
# 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:
@@ -141,12 +158,6 @@ def compute_heading_speed(x, y, penned, dog_xy, peers, wander_angle, rng=None):
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
if not fleeing:
if random.random() < 0.02:
wander_angle += random.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:
@@ -155,9 +166,15 @@ def compute_heading_speed(x, y, penned, dog_xy, peers, wander_angle, rng=None):
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]):
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 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
heading = math.atan2(fy, fx)
mag = math.hypot(fx, fy)
speed = max(WANDER_SPEED, min(FLEE_SPEED, mag * 3.0))
herding/world/geometry.py
+114 -7
View File
@@ -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)),
)
protos/ShepherdDogMecanum.proto
+885
View File
@@ -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
]
}
}
}
tests/test_control.py
+28 -4
View File
@@ -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):
tests/test_diffdrive.py
+110 -2
View File
@@ -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)
tests/test_env.py
+8
View File
@@ -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()
tests/test_perception.py
+4 -1
View File
@@ -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():
tools/benchmark_lidar.py
+84
View File
@@ -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()
tools/run_webots.sh
+94 -12
View File
@@ -5,38 +5,109 @@
# then execs Webots on it.
#
# Usage:
# tools/run_webots.sh [N] [MODE]
# 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" <<EOF
HERDING_MODE=$MODE
HERDING_POLICY_DIR=$RESOLVED_POLICY_DIR
HERDING_DRIVE=$DRIVE
HERDING_WORLD=$WORLD
EOF
export HERDING_MODE="$MODE"
export HERDING_POLICY_DIR="$RESOLVED_POLICY_DIR"
export HERDING_DRIVE="$DRIVE"
export HERDING_WORLD="$WORLD"
# The controller writes this sentinel when all GT sheep are penned. We
# poll for it and kill Webots so the run finishes cleanly instead of
@@ -68,7 +143,14 @@ DONE_FILE="$ROOT/training/.run_done"
mkdir -p "$(dirname "$DONE_FILE")"
rm -f "$DONE_FILE"
webots "$DST" &
if [[ "${WEBOTS_HEADLESS:-}" == "1" ]]; then
echo "[run_webots] headless flags: --no-rendering --minimize --mode=fast --batch"
# shellcheck disable=SC2086
webots --no-rendering --minimize --mode=fast --batch ${WEBOTS_EXTRA_ARGS:-} "$DST" &
else
# shellcheck disable=SC2086
webots ${WEBOTS_EXTRA_ARGS:-} "$DST" &
fi
WEBOTS_PID=$!
cleanup() {
training/README.md
+6 -20
View File
@@ -1,4 +1,9 @@
# Training pipeline
# Training and Evaluation Details
This file is the command-level companion to the root README. It focuses
on data collection, BC, PPO fine-tuning, evaluation flags, and generated
artifacts; use the root README for the high-level architecture and
Webots demo quick start.
Two stages, strictly sequential:
@@ -26,16 +31,6 @@ runs/ — checkpoints (whitelisted entries in top-level .gitignore)
run with ``python -m pytest tests/``.)
```
## Setup
```
pip install -r requirements.txt
```
CPU is the default and recommended device — SB3 PPO with an MLP policy
of this size runs faster on CPU than GPU because the bottleneck is
rollout collection, not gradient compute.
## End-to-end pipeline
The simplest way to run everything is the Makefile at the project
@@ -93,12 +88,3 @@ python -m training.eval --policy strombom --max-flock 10 --max-steps 15000 --
python -m training.eval --policy sequential --max-flock 10 --max-steps 15000 --n-seeds 10
```
## Webots inference
```
tools/run_webots.sh 10 bc # or rl, strombom, sequential
```
The dog controller loads `runs/bc` for `bc` mode and `runs/rl` for
`rl` mode. Override with `HERDING_POLICY_DIR=…` for a specific
checkpoint.
training/bc/collect.py
+81 -14
View File
@@ -15,50 +15,101 @@ Usage::
from __future__ import annotations
import argparse
import os
import time
from pathlib import Path
import numpy as np
# Early CLI parse so we can configure geometry before heavy imports.
# (argparse is used again below for the full parse; this is a lightweight
# pre-pass that only reads --world.)
_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.control.active_scan import ActiveScanTeacher
from herding.world.geometry import PEN_ENTRY
from herding.world.geometry import PEN_ENTRY, FIELD_SHAPE
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 training.herding_env import HerdingEnv
TEACHERS = {
"sequential": sequential_action,
"strombom": strombom_action,
"universal": universal_action,
}
def _call_teacher(fn, dog_xy, dog_heading, sheep_positions, pen_target,
drive_mode="differential"):
"""Call any teacher function and return (vx, vy, omega, mode).
Normalizes across 3-tuple teachers (vx, vy, mode) and 4-tuple
universal teacher (vx, vy, omega, mode). ActiveScanTeacher (when
invoked with drive_mode="mecanum") propagates the base teacher's
omega — see test_active_scan_preserves_mecanum_omega.
"""
# The universal teacher and ActiveScanTeacher accept the extended
# (dog_xy, heading, sheep, pen, drive_mode) signature. Older
# teachers accept (dog_xy, sheep, pen). Detect by trying the
# extended call first.
try:
result = fn(dog_xy, dog_heading, sheep_positions, pen_target,
drive_mode)
except TypeError:
try:
result = fn(dog_xy, dog_heading, sheep_positions, pen_target)
except TypeError:
result = fn(dog_xy, sheep_positions, pen_target)
if len(result) == 4:
return result # (vx, vy, omega, mode)
vx, vy, mode = result
return vx, vy, 0.0, mode
def collect_one(n_sheep: int, seed: int, max_steps: int, subsample: int,
teacher_fn, frame_stack: int = 1, privileged: bool = False):
teacher_fn, frame_stack: int = 1, privileged: bool = False,
drive_mode: str = "differential"):
env = HerdingEnv(n_sheep=n_sheep, max_steps=max_steps,
difficulty=1.0, seed=seed, frame_stack=frame_stack)
difficulty=1.0, seed=seed, frame_stack=frame_stack,
drive_mode=drive_mode)
obs, _ = env.reset(seed=seed)
obs_list, action_list = [], []
# Wrap the base teacher so it opens with a rotation and walks to
# centre when the tracker briefly empties — matches the student.
scan_teacher = ActiveScanTeacher(teacher_fn)
for step in range(max_steps):
if privileged:
# Asymmetric variant: teacher reads ground truth while the
# student keeps the LiDAR obs. Default off.
positions = {f"s{i}": (float(env.sheep_x[i]), float(env.sheep_y[i]))
for i in range(env.n_sheep) if not env.sheep_penned[i]}
if not positions:
break
vx, vy, _mode = teacher_fn(
(env.dog_x, env.dog_y), positions, PEN_ENTRY,
vx, vy, omega, _mode = _call_teacher(
teacher_fn, (env.dog_x, env.dog_y), env.dog_heading,
positions, PEN_ENTRY, drive_mode,
)
else:
positions = env.perceived_positions()
vx, vy, _mode = scan_teacher(
(env.dog_x, env.dog_y), env.dog_heading,
positions, PEN_ENTRY,
result = _call_teacher(
scan_teacher, (env.dog_x, env.dog_y), env.dog_heading,
positions, PEN_ENTRY, drive_mode,
)
vx, vy, omega, _mode = result
if drive_mode == "mecanum":
action = np.array([vx, vy, omega], dtype=np.float32)
else:
action = np.array([vx, vy], dtype=np.float32)
if step % subsample == 0:
obs_list.append(obs.copy())
@@ -85,7 +136,7 @@ def main():
help="Keep every Nth (obs, action) pair.")
parser.add_argument("--keep-failures", action="store_true",
help="Include partial-success trajectories. Default off.")
parser.add_argument("--teacher", default="sequential",
parser.add_argument("--teacher", default="universal",
choices=list(TEACHERS.keys()),
help="Which analytic teacher to demonstrate.")
parser.add_argument("--frame-stack", type=int, default=1,
@@ -94,9 +145,24 @@ def main():
parser.add_argument("--privileged", action="store_true",
help="Teacher reads ground truth instead of "
"tracker output (asymmetric BC).")
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'. Must be set "
"before geometry is imported.")
args = parser.parse_args()
# Validate --world matches geometry (already configured by the
# early pre-parse above, or by HERDING_WORLD env var).
if args.world is not None and args.world != FIELD_SHAPE:
print(f"[demos] WARNING: --world={args.world} but geometry is "
f"'{FIELD_SHAPE}'. This should not happen — file a bug.")
teacher_fn = TEACHERS[args.teacher]
print(f"[demos] teacher: {args.teacher}")
print(f"[demos] teacher: {args.teacher} world: {FIELD_SHAPE}")
n_sheep_list = [int(x) for x in args.n_sheep_list.split(",")]
print(f"[demos] grid: n_sheep={n_sheep_list}, seeds={args.seeds_per_n}, "
@@ -111,6 +177,7 @@ def main():
obs, actions, success, total_steps = collect_one(
n, seed, args.max_steps, args.subsample, teacher_fn,
frame_stack=args.frame_stack, privileged=args.privileged,
drive_mode=args.drive_mode,
)
n_total += 1
if success:
training/bc/demos.npz
BIN
View File
Binary file not shown.
training/bc/pretrain.py
+19 -3
View File
@@ -35,14 +35,15 @@ from training.herding_env import HerdingEnv
def build_model(net_arch_pi, net_arch_vf, log_std_init: float,
frame_stack: int = 1):
frame_stack: int = 1, drive_mode: str = "differential"):
"""Build a fresh SB3 PPO solely as a vehicle for the policy weights.
PPO's training-loop plumbing isn't used during BC. ``frame_stack``
must match the demo file so the env's obs space agrees with the
recorded obs shape.
"""
env = DummyVecEnv([lambda: HerdingEnv(frame_stack=frame_stack)])
env = DummyVecEnv([lambda: HerdingEnv(frame_stack=frame_stack,
drive_mode=drive_mode)])
model = PPO(
"MlpPolicy", env,
policy_kwargs=dict(
@@ -83,6 +84,10 @@ def main():
"term; balances against MSE.")
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 "
"demo action dimension (2→differential, 3→mecanum).")
args = parser.parse_args()
torch.manual_seed(args.seed)
@@ -130,8 +135,19 @@ def main():
frame_stack = obs_dim // _SINGLE
if frame_stack > 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)
training/eval.py
+45 -6
View File
@@ -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"])
training/herding_env.py
+83 -19
View File
@@ -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,6 +186,16 @@ class HerdingEnv(gym.Env):
# Sheep spawn region linearly interpolates with difficulty:
# 0 → small box near the gate, 1 → full field.
d = self._difficulty
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
@@ -176,6 +204,12 @@ class HerdingEnv(gym.Env):
sxs, sys_, shs, sws = [], [], [], []
for _ in range(self.n_sheep):
for _try in range(100):
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
@@ -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,12 +259,31 @@ 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.
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,
@@ -242,8 +295,10 @@ class HerdingEnv(gym.Env):
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))
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,7 +359,15 @@ class HerdingEnv(gym.Env):
SHEEP_WHEEL_RADIUS, SHEEP_WHEEL_BASE, WEBOTS_DT,
)
# Wall clipping (south wall absent inside the gate column).
# 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:
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:
@@ -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:
training/rl/train.py
+65 -4
View File
@@ -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):
training/runs/bc/policy.zip
BIN
View File
Binary file not shown.
training/runs/rl/policy.zip
BIN
View File
Binary file not shown.
worlds/field_round.wbt
+537
View File
@@ -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" }