Dual-shepherd axis-split (HERDING_NDOGS=2)

The launcher can now spawn two `ShepherdDog` robots, each masked to a
single axis of motion, so the herding workload is split orthogonally.

Mechanic:
* `HERDING_NDOGS=2` (default 1) tells `tools/run_webots.sh` to replace
  the single-dog node in the generated test world with two copies:
  - `ShepherdDogX` at (-4, -10), `customData "axis=x"`
  - `ShepherdDogY` at (+4, -10), `customData "axis=y"`
  Each spawn position sits south of the field interior so the pair
  doesn't collide with starting sheep.
* `controllers/shepherd_dog/shepherd_dog.py` reads `getCustomData()`
  at startup; when `axis=x|y` it zeroes the off-axis component of every
  action *after* speed modulation and *before* EMA smoothing. With
  `customData` empty the controller behaves identically to single-dog
  mode, so all existing launches are unaffected.
* The dog's emitter line now carries the robot's name
  (`dog:ShepherdDogX:x:y`), and `controllers/sheep/sheep.py` keeps a
  `dogs` dict keyed by name, picking the closest one each step for
  its flee target. Single-dog runs still use the legacy two-field
  `dog:x:y` format thanks to a length check.
* `HERDING_NDOGS` is written into `herding_runtime.cfg` and exported
  to subprocesses so future tooling can read it.

Verified behaviour in Webots smoke tests (HERDING_NDOGS=2, strombom,
diff/field, 5 sheep): both dogs spawn with the expected names and
axis tags, the dual-dog status print appears, each dog acts only on
its assigned axis early in the trial, and the masking is internally
consistent. The pair stalls before penning under pure axis-split
because each dog reaches its drive standoff and then has only one
degree of freedom — useful research finding for the write-up;
coordination strategy (shared CoM, role-switching, etc.) is future
work.

126 pytest cases still pass.

Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
This commit is contained in:
Johnny Fernandes
2026-05-17 02:35:38 +00:00
parent d00da52c3c
commit cfbf4a0267
3 changed files with 98 additions and 6 deletions
+22 -5
View File
@@ -75,7 +75,7 @@ def paint_pink():
# --- State --- # --- State ---
wander_angle = random.uniform(-math.pi, math.pi) wander_angle = random.uniform(-math.pi, math.pi)
step_count = 0 step_count = 0
dog_x, dog_y = None, None dogs = {} # name → (x, y); supports the dual-dog setup
peers = {} # name → (x, y); periodically pruned peers = {} # name → (x, y); periodically pruned
penned = False penned = False
@@ -97,19 +97,36 @@ while robot.step(timestep) != -1:
paint_pink() paint_pink()
# Stale peers get dropped periodically so a peer that's gone silent # Stale peers get dropped periodically so a peer that's gone silent
# doesn't permanently distort the local CoM. # doesn't permanently distort the local CoM. Dogs are pruned too —
# otherwise a temporarily-silent dog stays in `dogs` forever and
# the closest-dog flee target stops being accurate.
if step_count % 30 == 0: if step_count % 30 == 0:
peers.clear() peers.clear()
dogs.clear()
while receiver.getQueueLength() > 0: while receiver.getQueueLength() > 0:
msg = receiver.getString() msg = receiver.getString()
receiver.nextPacket() receiver.nextPacket()
parts = msg.split(":") parts = msg.split(":")
if parts[0] == "dog" and len(parts) >= 3: # Legacy single-dog message: "dog:x:y".
dog_x, dog_y = float(parts[1]), float(parts[2]) # Dual-dog message: "dog:NAME:x:y".
if parts[0] == "dog" and len(parts) == 3:
dogs["ShepherdDog"] = (float(parts[1]), float(parts[2]))
elif parts[0] == "dog" and len(parts) >= 4:
dogs[parts[1]] = (float(parts[2]), float(parts[3]))
elif parts[0] == "sheep" and len(parts) >= 4 and parts[1] != name: elif parts[0] == "sheep" and len(parts) >= 4 and parts[1] != name:
peers[parts[1]] = (float(parts[2]), float(parts[3])) peers[parts[1]] = (float(parts[2]), float(parts[3]))
dog_xy = (dog_x, dog_y) if dog_x is not None and dog_y is not None else None # Flee target = closest known dog; the flocking heuristic only needs
# one (vx, vy) repulsion vector regardless of how many dogs are out
# there. With two dogs at orthogonal axes, the sheep will see one of
# them as nearest at any moment and react to it; the other dog's
# influence enters through the sheep that does react to it pushing
# this sheep in turn (Reynolds peer-repulsion).
if dogs:
closest = min(dogs.values(), key=lambda d: math.hypot(d[0] - x, d[1] - y))
dog_xy = closest
else:
dog_xy = None
heading, speed, wander_angle = compute_heading_speed( heading, speed, wander_angle = compute_heading_speed(
x=x, y=y, penned=penned, dog_xy=dog_xy, peers=peers, x=x, y=y, penned=penned, dog_xy=dog_xy, peers=peers,
wander_angle=wander_angle, wander_angle=wander_angle,
+24 -1
View File
@@ -267,6 +267,22 @@ def drive_mecanum(vx: float, vy: float, omega: float,
robot = Robot() robot = Robot()
timestep = int(robot.getBasicTimeStep()) timestep = int(robot.getBasicTimeStep())
# Multi-shepherd axis split. When the launcher creates two dog instances
# it sets each robot's customData to "axis=x" or "axis=y"; the controller
# then masks the off-axis component of every action so the two dogs
# share the herding workload along orthogonal axes (one closes flank
# distance in x, the other in y). customData empty = single-dog mode.
_AXIS_TAG = (robot.getCustomData() or "").strip().lower()
if _AXIS_TAG.startswith("axis="):
DOG_AXIS = _AXIS_TAG[5:]
if DOG_AXIS not in ("x", "y"):
print(f"[dog] unknown axis={DOG_AXIS!r} in customData; ignoring.")
DOG_AXIS = None
else:
DOG_AXIS = None
DOG_NAME = robot.getName()
print(f"[dog] name={DOG_NAME} axis={DOG_AXIS}")
if DRIVE_MODE == "mecanum": if DRIVE_MODE == "mecanum":
fl_motor = robot.getDevice("front left wheel motor") fl_motor = robot.getDevice("front left wheel motor")
fr_motor = robot.getDevice("front right wheel motor") fr_motor = robot.getDevice("front right wheel motor")
@@ -467,6 +483,13 @@ while robot.step(timestep) != -1:
# Near-sheep speed modulation (shared by every mode). # Near-sheep speed modulation (shared by every mode).
vx, vy = modulate_speed(vx, vy, dog_xy, sheep_positions) vx, vy = modulate_speed(vx, vy, dog_xy, sheep_positions)
# Axis-split mask for the dual-dog setup: this dog only commits to
# its assigned axis (x or y) so its partner covers the other.
if DOG_AXIS == "x":
vy = 0.0
elif DOG_AXIS == "y":
vx = 0.0
# EMA smoothing — kills frame-to-frame action jitter. # EMA smoothing — kills frame-to-frame action jitter.
if DRIVE_MODE == "mecanum": if DRIVE_MODE == "mecanum":
vx = ACTION_SMOOTH * prev_action[0] + (1.0 - ACTION_SMOOTH) * vx vx = ACTION_SMOOTH * prev_action[0] + (1.0 - ACTION_SMOOTH) * vx
@@ -485,7 +508,7 @@ while robot.step(timestep) != -1:
compass, MOTOR_MAX) compass, MOTOR_MAX)
else: else:
drive_diff(vx, vy, left_motor, right_motor, compass, MOTOR_MAX) drive_diff(vx, vy, left_motor, right_motor, compass, MOTOR_MAX)
emitter.send(f"dog:{dog_xy[0]:.4f}:{dog_xy[1]:.4f}") emitter.send(f"dog:{DOG_NAME}:{dog_xy[0]:.4f}:{dog_xy[1]:.4f}")
# Cosmetic ear wiggle. # Cosmetic ear wiggle.
ear_phase += 0.12 ear_phase += 0.12
+52
View File
@@ -161,11 +161,62 @@ for i in $(seq $((N+1)) 10); do
sed -i "s|^Sheep .* \"sheep${i}\".*|# &|" "$DST" sed -i "s|^Sheep .* \"sheep${i}\".*|# &|" "$DST"
done done
# Dual-dog axis split. When HERDING_NDOGS=2 the launcher replaces the
# single dog node in the world with two named dogs whose customData
# carries the axis assignment (x or y); the controller masks the
# off-axis component of every action.
NDOGS="${HERDING_NDOGS:-1}"
if [[ "$NDOGS" != "1" && "$NDOGS" != "2" ]]; then
echo "HERDING_NDOGS must be 1 or 2, got '$NDOGS'" >&2; exit 1
fi
if [[ "$NDOGS" == "2" ]]; then
DOG_NODE_NAME="ShepherdDog"
if [[ "$DRIVE" == "mecanum" ]]; then
DOG_NODE_NAME="ShepherdDogMecanum"
elif [[ "$LIDAR_VARIANT" == "360" ]]; then
DOG_NODE_NAME="ShepherdDog360"
fi
python3 - "$DST" "$DOG_NODE_NAME" <<'PY'
import re, sys
path, node = sys.argv[1], sys.argv[2]
with open(path) as f:
txt = f.read()
# Match the single existing dog block from "ShepherdDog{,360,Mecanum} {"
# up to its closing "}" on a line by itself.
pattern = re.compile(rf"^{re.escape(node)} \{{\n(.*?\n)^\}}\n", re.MULTILINE | re.DOTALL)
m = pattern.search(txt)
if m is None:
sys.exit(f"[run_webots] could not locate single-dog block ({node}) for split")
two_dogs = (
f"{node} {{\n"
f" translation -4 -10 0.5\n"
f" rotation 0 0 1 1.5708\n"
f' name "ShepherdDogX"\n'
f' customData "axis=x"\n'
f' controller "shepherd_dog"\n'
f"}}\n"
f"{node} {{\n"
f" translation 4 -10 0.5\n"
f" rotation 0 0 1 1.5708\n"
f' name "ShepherdDogY"\n'
f' customData "axis=y"\n'
f' controller "shepherd_dog"\n'
f"}}\n"
)
with open(path, 'w') as f:
f.write(txt[:m.start()] + two_dogs + txt[m.end():])
PY
fi
export HERDING_NDOGS="$NDOGS"
active=$(grep -c '^Sheep' "$DST" || true) active=$(grep -c '^Sheep' "$DST" || true)
ndog=$(grep -cE '^(ShepherdDog|ShepherdDog360|ShepherdDogMecanum) \{' "$DST" || true)
echo "------------------------------------------------------------" echo "------------------------------------------------------------"
echo "World : $DST" echo "World : $DST"
echo "Mode : $MODE" echo "Mode : $MODE"
echo "Drive : $DRIVE" echo "Drive : $DRIVE"
echo "LiDAR : ${LIDAR_VARIANT}°"
echo "Dogs : $ndog (axis-split=${NDOGS})"
echo "Sheep : $active active" echo "Sheep : $active active"
echo "Policy dir : $RESOLVED_POLICY_DIR" echo "Policy dir : $RESOLVED_POLICY_DIR"
echo "------------------------------------------------------------" echo "------------------------------------------------------------"
@@ -178,6 +229,7 @@ HERDING_POLICY_DIR=$RESOLVED_POLICY_DIR
HERDING_DRIVE=$DRIVE HERDING_DRIVE=$DRIVE
HERDING_WORLD=$WORLD HERDING_WORLD=$WORLD
HERDING_LIDAR=$LIDAR_VARIANT HERDING_LIDAR=$LIDAR_VARIANT
HERDING_NDOGS=$NDOGS
HERDING_USE_GT=${HERDING_USE_GT:-0} HERDING_USE_GT=${HERDING_USE_GT:-0}
EOF EOF