Johnny Fernandes
236 lines
9.4 KiB
Python
236 lines
9.4 KiB
Python
"""
|
|
Sheep flocking controller (Webots, Reynolds boids variant).
|
|
|
|
Each sheep broadcasts its GPS position every 3 steps on channel 1 and
|
|
listens for the dog and peer sheep positions. Peers are keyed by robot
|
|
name so each neighbour has exactly one current entry in the dict.
|
|
|
|
Force stack each step (summed then converted to a heading + speed):
|
|
flee — away from dog, quadratic ramp, dominant when close
|
|
cohesion — toward flock centre, halved while fleeing
|
|
separation — inverse-distance push, prevents physical overlap
|
|
walls — linear repulsion from field boundary
|
|
wander — small persistent drift for natural idle motion
|
|
|
|
Pen behaviour: on first entry into the quarantine pen the sheep latches
|
|
permanently — it turns pink (via the exposed woolColor PROTO field) and
|
|
the normal force stack is replaced by pen-confinement forces only.
|
|
"""
|
|
|
|
import random
|
|
import math
|
|
from controller import Supervisor
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# Tuning constants
|
|
# ---------------------------------------------------------------------------
|
|
|
|
MAX_SPEED = 22.0 # rad/s hard clamp on both motors
|
|
FLEE_SPEED = 20.0 # rad/s upper bound while panicking
|
|
WANDER_SPEED = 3.0 # rad/s lower bound during calm wandering
|
|
|
|
X_MIN, X_MAX = -14.5, 14.5 # stone wall inner edges (metres)
|
|
Y_MIN, Y_MAX = -14.5, 14.5
|
|
WALL_MARGIN = 3.5 # avoidance starts this far from the wall
|
|
|
|
FLEE_DIST = 7.0 # dog within this radius triggers flee (metres)
|
|
SEPARATION_DIST = 2.5 # inverse-distance push active inside this radius
|
|
COHESION_DIST = 8.0 # pull toward flock centre active inside this radius
|
|
|
|
PEN_X_MIN, PEN_X_MAX = 10.0, 13.0 # quarantine pen extents (metres)
|
|
PEN_Y_MIN, PEN_Y_MAX = -15.0, -8.0 # open entrance at y=-8, gate at y=-15
|
|
PEN_MARGIN = 0.8 # confinement force starts this far from pen wall
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# Device setup
|
|
# ---------------------------------------------------------------------------
|
|
|
|
robot = Supervisor()
|
|
timestep = int(robot.getBasicTimeStep())
|
|
name = robot.getName()
|
|
self_node = robot.getSelf()
|
|
|
|
left_motor = robot.getDevice("left wheel motor")
|
|
right_motor = robot.getDevice("right wheel motor")
|
|
left_motor.setPosition(float("inf"))
|
|
right_motor.setPosition(float("inf"))
|
|
left_motor.setVelocity(0.0)
|
|
right_motor.setVelocity(0.0)
|
|
|
|
gps = robot.getDevice("gps"); gps.enable(timestep)
|
|
compass = robot.getDevice("compass"); compass.enable(timestep)
|
|
receiver = robot.getDevice("receiver"); receiver.enable(timestep)
|
|
emitter = robot.getDevice("emitter")
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# Helpers
|
|
# ---------------------------------------------------------------------------
|
|
|
|
def norm_angle(a):
|
|
return math.atan2(math.sin(a), math.cos(a))
|
|
|
|
|
|
def bearing():
|
|
# Compass returns north direction in sensor frame; for this Z-up world
|
|
# with north = +Y, atan2(n[0], n[1]) gives the standard math angle
|
|
# (0 = east, π/2 = north) matching atan2(fy, fx) used for heading.
|
|
n = compass.getValues()
|
|
return math.atan2(n[0], n[1])
|
|
|
|
|
|
def drive(heading, speed):
|
|
err = norm_angle(heading - bearing())
|
|
# Scale forward component by cos(err): at 90° error fwd→0 so the robot
|
|
# spins in place to realign rather than driving sideways at full speed.
|
|
fwd = speed * max(0.0, math.cos(err))
|
|
k = 4.0
|
|
left_motor.setVelocity( max(-MAX_SPEED, min(MAX_SPEED, fwd - k * err)))
|
|
right_motor.setVelocity(max(-MAX_SPEED, min(MAX_SPEED, fwd + k * err)))
|
|
|
|
|
|
def paint_pink():
|
|
# woolColor is declared as a PROTO field with IS binding to the DEF WOOL
|
|
# PBRAppearance baseColor. Changing it here propagates to every USE WOOL
|
|
# shape on the body. Direct field access avoids PROTO-internal opacity.
|
|
self_node.getField("woolColor").setSFColor([1.0, 0.55, 0.72])
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# State
|
|
# ---------------------------------------------------------------------------
|
|
|
|
wander_angle = random.uniform(-math.pi, math.pi)
|
|
step = 0
|
|
dog_x = None
|
|
dog_y = None
|
|
peers = {} # name → (x, y), one entry per neighbour, cleared every 30 steps
|
|
penned = False
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# Main loop
|
|
# ---------------------------------------------------------------------------
|
|
|
|
while robot.step(timestep) != -1:
|
|
step += 1
|
|
pos = gps.getValues()
|
|
x, y = pos[0], pos[1]
|
|
|
|
# Pen entry: one-way latch, never unset
|
|
if not penned and PEN_X_MIN < x < PEN_X_MAX and PEN_Y_MIN < y < PEN_Y_MAX:
|
|
penned = True
|
|
paint_pink()
|
|
|
|
# Refresh peer table (clear before receiving so fresh data is never lost)
|
|
if step % 30 == 0:
|
|
peers.clear()
|
|
while receiver.getQueueLength() > 0:
|
|
msg = receiver.getString()
|
|
receiver.nextPacket()
|
|
p = msg.split(":")
|
|
if p[0] == "dog" and len(p) >= 3:
|
|
dog_x, dog_y = float(p[1]), float(p[2])
|
|
elif p[0] == "sheep" and len(p) >= 4 and p[1] != name:
|
|
peers[p[1]] = (float(p[2]), float(p[3]))
|
|
|
|
fx, fy = 0.0, 0.0
|
|
|
|
# Outside the pen: repel from the exterior of the side and back walls so
|
|
# sheep don't get pinned against them when fleeing from the dog.
|
|
# The pen entrance is open on the north (y > PEN_Y_MAX), so only push away
|
|
# from the west (x≈PEN_X_MIN), east (x≈PEN_X_MAX), and south (y≈PEN_Y_MIN) exteriors.
|
|
PEN_EXT_MARGIN = 1.2
|
|
if not penned:
|
|
if PEN_Y_MIN - PEN_EXT_MARGIN < y < PEN_Y_MAX and x < PEN_X_MIN + PEN_EXT_MARGIN:
|
|
fx -= ((PEN_X_MIN + PEN_EXT_MARGIN - x) / PEN_EXT_MARGIN) * 8.0
|
|
if PEN_Y_MIN - PEN_EXT_MARGIN < y < PEN_Y_MAX and x > PEN_X_MAX - PEN_EXT_MARGIN:
|
|
fx += ((x - (PEN_X_MAX - PEN_EXT_MARGIN)) / PEN_EXT_MARGIN) * 8.0
|
|
if y < PEN_Y_MIN + PEN_EXT_MARGIN and PEN_X_MIN < x < PEN_X_MAX:
|
|
fy += ((PEN_Y_MIN + PEN_EXT_MARGIN - y) / PEN_EXT_MARGIN) * 8.0
|
|
|
|
if penned:
|
|
# Inside pen: wander freely, strong boundary forces prevent exit,
|
|
# separation still active to avoid collisions with other penned sheep.
|
|
|
|
pm = PEN_MARGIN
|
|
if x < PEN_X_MIN + pm: fx += ((PEN_X_MIN + pm - x) / pm) * 15.0
|
|
if x > PEN_X_MAX - pm: fx -= ((x - (PEN_X_MAX - pm)) / pm) * 15.0
|
|
if y < PEN_Y_MIN + pm: fy += ((PEN_Y_MIN + pm - y) / pm) * 15.0
|
|
if y > PEN_Y_MAX - pm: fy -= ((y - (PEN_Y_MAX - pm)) / pm) * 15.0
|
|
|
|
for px, py in peers.values():
|
|
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 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
|
|
|
|
else:
|
|
fleeing = False
|
|
|
|
# Flee — quadratic ramp so force grows rapidly as the dog closes in
|
|
if dog_x is not None:
|
|
dx = dog_x - x
|
|
dy = dog_y - y
|
|
dist = math.hypot(dx, dy)
|
|
if 0.01 < dist < FLEE_DIST:
|
|
fleeing = True
|
|
t = 1.0 - dist / FLEE_DIST
|
|
s = t * t * 20.0
|
|
fx -= (dx / dist) * s
|
|
fy -= (dy / dist) * s
|
|
|
|
# Cohesion — halved while fleeing to reduce mid-panic collisions
|
|
cx, cy, cn = 0.0, 0.0, 0
|
|
for px, py in peers.values():
|
|
d = math.hypot(px - x, py - y)
|
|
if 0.3 < d < COHESION_DIST:
|
|
cx += px; cy += py; cn += 1
|
|
if cn > 0:
|
|
w = 0.08 if fleeing else 0.15
|
|
fx += (cx / cn - x) * w
|
|
fy += (cy / cn - y) * w
|
|
|
|
# Separation — inverse-distance: huge when nearly overlapping, fades quickly
|
|
for px, py in peers.values():
|
|
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
|
|
|
|
# Walls
|
|
if x < X_MIN + WALL_MARGIN: fx += ((X_MIN + WALL_MARGIN - x) / WALL_MARGIN) * 6.0
|
|
if x > X_MAX - WALL_MARGIN: fx -= ((x - (X_MAX - WALL_MARGIN)) / WALL_MARGIN) * 6.0
|
|
if y < Y_MIN + WALL_MARGIN: fy += ((Y_MIN + WALL_MARGIN - y) / WALL_MARGIN) * 6.0
|
|
if y > Y_MAX - WALL_MARGIN: fy -= ((y - (Y_MAX - WALL_MARGIN)) / WALL_MARGIN) * 6.0
|
|
|
|
# Wander — suppressed while fleeing so drift cannot deflect the flee heading
|
|
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-stop clamp: within 0.5 m of a wall, zero any force component that
|
|
# would push further into it. Prevents the flee force from pinning a sheep
|
|
# against the boundary when the dog approaches from outside.
|
|
HS = 0.5
|
|
if x < X_MIN + HS and fx < 0: fx = 0.0
|
|
if x > X_MAX - HS and fx > 0: fx = 0.0
|
|
if y < Y_MIN + HS and fy < 0: fy = 0.0
|
|
if y > Y_MAX - HS and fy > 0: fy = 0.0
|
|
|
|
heading = math.atan2(fy, fx)
|
|
mag = math.hypot(fx, fy)
|
|
speed = max(WANDER_SPEED, min(FLEE_SPEED, mag * 3.0))
|
|
drive(heading, speed)
|
|
|
|
if step % 3 == 0:
|
|
emitter.send(f"sheep:{name}:{x:.4f}:{y:.4f}")
|