Johnny Fernandes
206 lines
8.1 KiB
Python
206 lines
8.1 KiB
Python
"""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
|