diff --git a/controllers/sheep/sheep.py b/controllers/sheep/sheep.py index a319836..843ae42 100644 --- a/controllers/sheep/sheep.py +++ b/controllers/sheep/sheep.py @@ -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, diff --git a/controllers/shepherd_dog/shepherd_dog.py b/controllers/shepherd_dog/shepherd_dog.py index efc18e6..2332478 100644 --- a/controllers/shepherd_dog/shepherd_dog.py +++ b/controllers/shepherd_dog/shepherd_dog.py @@ -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 diff --git a/tools/run_webots.sh b/tools/run_webots.sh index 33335f2..fc473ed 100755 --- a/tools/run_webots.sh +++ b/tools/run_webots.sh @@ -161,11 +161,62 @@ for i in $(seq $((N+1)) 10); do sed -i "s|^Sheep .* \"sheep${i}\".*|# &|" "$DST" 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) +ndog=$(grep -cE '^(ShepherdDog|ShepherdDog360|ShepherdDogMecanum) \{' "$DST" || true) echo "------------------------------------------------------------" echo "World : $DST" echo "Mode : $MODE" echo "Drive : $DRIVE" +echo "LiDAR : ${LIDAR_VARIANT}°" +echo "Dogs : $ndog (axis-split=${NDOGS})" echo "Sheep : $active active" echo "Policy dir : $RESOLVED_POLICY_DIR" echo "------------------------------------------------------------" @@ -178,6 +229,7 @@ HERDING_POLICY_DIR=$RESOLVED_POLICY_DIR HERDING_DRIVE=$DRIVE HERDING_WORLD=$WORLD HERDING_LIDAR=$LIDAR_VARIANT +HERDING_NDOGS=$NDOGS HERDING_USE_GT=${HERDING_USE_GT:-0} EOF