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:
@@ -75,7 +75,7 @@ def paint_pink():
|
||||
# --- State ---
|
||||
wander_angle = random.uniform(-math.pi, math.pi)
|
||||
step_count = 0
|
||||
dog_x, dog_y = None, None
|
||||
dogs = {} # name → (x, y); supports the dual-dog setup
|
||||
peers = {} # name → (x, y); periodically pruned
|
||||
penned = False
|
||||
|
||||
@@ -97,19 +97,36 @@ while robot.step(timestep) != -1:
|
||||
paint_pink()
|
||||
|
||||
# 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:
|
||||
peers.clear()
|
||||
dogs.clear()
|
||||
while receiver.getQueueLength() > 0:
|
||||
msg = receiver.getString()
|
||||
receiver.nextPacket()
|
||||
parts = msg.split(":")
|
||||
if parts[0] == "dog" and len(parts) >= 3:
|
||||
dog_x, dog_y = float(parts[1]), float(parts[2])
|
||||
# Legacy single-dog message: "dog:x:y".
|
||||
# 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:
|
||||
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(
|
||||
x=x, y=y, penned=penned, dog_xy=dog_xy, peers=peers,
|
||||
wander_angle=wander_angle,
|
||||
|
||||
@@ -267,6 +267,22 @@ def drive_mecanum(vx: float, vy: float, omega: float,
|
||||
robot = Robot()
|
||||
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":
|
||||
fl_motor = robot.getDevice("front left 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).
|
||||
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.
|
||||
if DRIVE_MODE == "mecanum":
|
||||
vx = ACTION_SMOOTH * prev_action[0] + (1.0 - ACTION_SMOOTH) * vx
|
||||
@@ -485,7 +508,7 @@ while robot.step(timestep) != -1:
|
||||
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}")
|
||||
emitter.send(f"dog:{DOG_NAME}:{dog_xy[0]:.4f}:{dog_xy[1]:.4f}")
|
||||
|
||||
# Cosmetic ear wiggle.
|
||||
ear_phase += 0.12
|
||||
|
||||
Reference in New Issue
Block a user