Johnny Fernandes
75 lines
2.3 KiB
Python
75 lines
2.3 KiB
Python
"""Sequential "pin-and-push" shepherd-dog controller.
|
|
|
|
Single-target alternative to Strömbom: each step, target the sheep
|
|
closest to the pen, park behind it, drive it through; once it latches
|
|
penned the next-closest sheep becomes the target. Naturally queues
|
|
the flock through a narrow gate.
|
|
"""
|
|
|
|
import math
|
|
|
|
from herding.world.geometry import GATE_Y, PEN_ENTRY, in_pen
|
|
|
|
|
|
DELTA_DRIVE = 1.5 # standoff behind the target sheep
|
|
APPROACH_GAIN = 1.0 # action magnitude scale (1 = full speed)
|
|
|
|
|
|
def _unit(x, y):
|
|
d = math.hypot(x, y)
|
|
if d < 1e-6:
|
|
return 0.0, 0.0
|
|
return x / d, y / d
|
|
|
|
|
|
def _is_active(x, y) -> bool:
|
|
return (not in_pen(x, y)) and y > GATE_Y
|
|
|
|
|
|
def compute_action(dog_xy, sheep_positions, pen_target=PEN_ENTRY):
|
|
"""Return ``(vx, vy, mode)`` — same call signature as Strömbom."""
|
|
active = [(name, x, y) for name, (x, y) in sheep_positions.items()
|
|
if _is_active(x, y)]
|
|
if not active:
|
|
return 0.0, 0.0, "idle"
|
|
|
|
name, sx, sy = min(
|
|
active,
|
|
key=lambda s: math.hypot(s[1] - pen_target[0], s[2] - pen_target[1]),
|
|
)
|
|
|
|
ux, uy = _unit(sx - pen_target[0], sy - pen_target[1])
|
|
tx = sx + DELTA_DRIVE * ux
|
|
ty = sy + DELTA_DRIVE * uy
|
|
|
|
ax, ay = _unit(tx - dog_xy[0], ty - dog_xy[1])
|
|
return APPROACH_GAIN * ax, APPROACH_GAIN * ay, f"drive:{name}"
|
|
|
|
|
|
def compute_action_debug(dog_xy, sheep_positions, pen_target=PEN_ENTRY):
|
|
"""``compute_action`` plus a debug dict (target, drive point)."""
|
|
active = [(name, x, y) for name, (x, y) in sheep_positions.items()
|
|
if _is_active(x, y)]
|
|
if not active:
|
|
return 0.0, 0.0, "idle", {
|
|
"n_active": 0, "target_name": "",
|
|
"target_x": 0.0, "target_y": 0.0,
|
|
"drive_x": dog_xy[0], "drive_y": dog_xy[1],
|
|
}
|
|
|
|
name, sx, sy = min(
|
|
active,
|
|
key=lambda s: math.hypot(s[1] - pen_target[0], s[2] - pen_target[1]),
|
|
)
|
|
|
|
ux, uy = _unit(sx - pen_target[0], sy - pen_target[1])
|
|
tx = sx + DELTA_DRIVE * ux
|
|
ty = sy + DELTA_DRIVE * uy
|
|
ax, ay = _unit(tx - dog_xy[0], ty - dog_xy[1])
|
|
|
|
return APPROACH_GAIN * ax, APPROACH_GAIN * ay, f"drive:{name}", {
|
|
"n_active": len(active), "target_name": name,
|
|
"target_x": sx, "target_y": sy,
|
|
"drive_x": tx, "drive_y": ty,
|
|
}
|