"""Sheep flocking dynamics — Strömbom 2014 / Reynolds 1987 hybrid. This is the per-sheep behavioural step used both by the Webots sheep controller (scalar, one sheep at a time) and by the training environment (loop over sheep). Model ----- The force stack each step (summed → heading + speed): flee — quadratic ramp away from dog within FLEE_DIST (Strömbom 2014 §2.1, term ρa) cohesion — drift toward local centre of mass of peers within COHESION_DIST (Strömbom 2014 §2.1, term c). Weight is **higher when fleeing** — modelling the "safety in numbers" / predator-confusion effect Strömbom 2014 describes as fear-induced cohesion. separation — short-range inverse-distance repulsion from peers (Strömbom 2014 §2.1, term α; Reynolds 1987) wander — small persistent drift for natural idle motion (Strömbom 2014 §2.1, noise term ε) References ---------- - Strömbom et al. (2014). "Solving the shepherding problem: heuristics for herding autonomous, interacting agents." J R Soc Interface 11. - Reynolds (1987). "Flocks, herds and schools: A distributed behavioural model." SIGGRAPH '87. Environment-specific adaptations -------------------------------- The original Strömbom model assumes an open field. Our scenario adds: * Field walls — soft repulsion within ``WALL_MARGIN`` plus a hard escape band when inside ``WALL_HARD_MARGIN``. Necessary because the Webots field is fenced (30 m square enclosure). * Gate column — the south wall has a 3 m gap at x ∈ [10, 13]; sheep pass through it freely (no wall force inside the column). * Penned containment — once a sheep crosses the gate plane south (``geometry.is_penned_position``), the caller flags ``penned=True`` and we switch to in-pen wall-bounce + jitter. Sheep do not exit the pen on their own. This is a hard sim constraint, not a behavioural claim about real sheep. Parameter tuning (cohesion weight 3× while fleeing) was chosen so the flock survives passage through the 3 m gate without fragmenting — this is a defensible engineering adaptation of Strömbom's qualitative "fear-induced cohesion" to our gate width. """ import math import random from herding.world.geometry import ( FIELD_X, FIELD_Y, PEN_X, PEN_Y, GATE_X, ) # --- Speed and force constants --- # All speeds here are in wheel rad/s (motor units), matching the existing # sheep controller. Conversion to m/s = speed * SHEEP_WHEEL_RADIUS. MAX_SPEED = 22.0 FLEE_SPEED = 20.0 WANDER_SPEED = 3.0 WALL_MARGIN = 5.0 WALL_HARD_MARGIN = 1.0 WALL_HARD_GAIN = 50.0 FLEE_DIST = 7.0 SEPARATION_DIST = 2.5 COHESION_DIST = 12.0 # was 8.0 — wider engagement so far-flung sheep are pulled in PEN_MARGIN = 0.8 def _peers_iter(peers): """Accept either a {name: (x, y)} dict or an iterable of (x, y) tuples.""" if isinstance(peers, dict): return list(peers.values()) return list(peers) def compute_heading_speed(x, y, penned, dog_xy, peers, wander_angle, rng=None): """Return ``(heading, speed, new_wander_angle)`` for one sheep step. ``speed`` is in wheel rad/s (motor units), bounded by ``[WANDER_SPEED, FLEE_SPEED]``. ``heading`` is the world-frame target heading the sheep should aim for (atan2 convention). ``rng`` is an optional ``random.Random``-compatible object used for the wander-jitter. If ``None``, falls back to Python's global module (matches Webots controller usage). Pass an env-owned RNG to make rollouts deterministic given a seed. """ fx, fy = 0.0, 0.0 peer_list = _peers_iter(peers) rnd = rng if rng is not None else random if penned: # --- Pen containment: bounce off the four pen walls --- pm = PEN_MARGIN if x < PEN_X[0] + pm: fx += ((PEN_X[0] + pm - x) / pm) * 15.0 if x > PEN_X[1] - pm: fx -= ((x - (PEN_X[1] - pm)) / pm) * 15.0 if y < PEN_Y[0] + pm: fy += ((PEN_Y[0] + pm - y) / pm) * 15.0 if y > PEN_Y[1] - pm: fy -= ((y - (PEN_Y[1] - pm)) / pm) * 15.0 # Mild peer separation — penned sheep crowd the corner otherwise. for px, py in peer_list: dx, dy = px - x, py - y d = math.hypot(dx, dy) if 0.05 < d < SEPARATION_DIST: push = (SEPARATION_DIST - d) / d fx -= (dx / d) * push * 2.5 fy -= (dy / d) * push * 2.5 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 else: # --- Free-roaming sheep in the field --- fleeing = False if dog_xy is not None: ddx = dog_xy[0] - x ddy = dog_xy[1] - y dist = math.hypot(ddx, ddy) if 0.01 < dist < FLEE_DIST: fleeing = True t = 1.0 - dist / FLEE_DIST s = t * t * 20.0 fx -= (ddx / dist) * s fy -= (ddy / dist) * s # Cohesion — drift toward flock CoM (peers within COHESION_DIST). # Cohesion is *stronger* under flee than at rest (the # predator-confusion / safety-in-numbers effect — sheep huddle when # threatened). This is what makes shepherding work: the flock stays # as one unit through the narrow gate instead of fragmenting. cx, cy, cn = 0.0, 0.0, 0 for px, py in peer_list: d = math.hypot(px - x, py - y) if 0.3 < d < COHESION_DIST: cx += px cy += py cn += 1 if cn > 0: # Cohesion needs to dominate flee at close range so the flock # stays glued together when squeezing through the narrow gate. # Flee at 2 m has magnitude ~10; cohesion of w=3.0 with the # peer-CoM 4 m away contributes ~12, so the flock prefers # bunching to dispersing under pressure. This is what makes # canonical Strömbom drive work in our 3 m gate. w = 3.0 if fleeing else 1.0 fx += (cx / cn - x) * w fy += (cy / cn - y) * w # Separation — inverse-distance push from peers. for px, py in peer_list: ddx, ddy = px - x, py - y d = math.hypot(ddx, ddy) if 0.05 < d < SEPARATION_DIST: push = (SEPARATION_DIST - d) / d fx -= (ddx / d) * push * 2.5 fy -= (ddy / d) * push * 2.5 # Wall soft repulsion. The south wall is absent inside the gate # column so sheep can be driven through it by the dog. if x < FIELD_X[0] + WALL_MARGIN: fx += ((FIELD_X[0] + WALL_MARGIN - x) / WALL_MARGIN) * 6.0 if x > FIELD_X[1] - WALL_MARGIN: fx -= ((x - (FIELD_X[1] - WALL_MARGIN)) / WALL_MARGIN) * 6.0 if y > FIELD_Y[1] - WALL_MARGIN: fy -= ((y - (FIELD_Y[1] - WALL_MARGIN)) / WALL_MARGIN) * 6.0 if y < FIELD_Y[0] + WALL_MARGIN and not (GATE_X[0] <= x <= GATE_X[1]): fy += ((FIELD_Y[0] + WALL_MARGIN - y) / WALL_MARGIN) * 6.0 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 when very close to a wall --- m, g = WALL_HARD_MARGIN, WALL_HARD_GAIN if x - FIELD_X[0] < m: fx = max(fx, g * (1.0 - (x - FIELD_X[0]) / m)) if FIELD_X[1] - x < m: fx = min(fx, -g * (1.0 - (FIELD_X[1] - x) / m)) if FIELD_Y[1] - y < m: fy = min(fy, -g * (1.0 - (FIELD_Y[1] - y) / m)) # South wall hard escape only when not in the gate column and not penned. if (not penned) and (y - FIELD_Y[0] < m) and not (GATE_X[0] <= x <= GATE_X[1]): fy = max(fy, g * (1.0 - (y - FIELD_Y[0]) / m)) heading = math.atan2(fy, fx) mag = math.hypot(fx, fy) speed = max(WANDER_SPEED, min(FLEE_SPEED, mag * 3.0)) return heading, speed, wander_angle