diff --git a/herding/config.py b/herding/config.py index 103907e..e27f882 100644 --- a/herding/config.py +++ b/herding/config.py @@ -175,12 +175,13 @@ class TrackerConfig: from permanently consuming tracker slots as false "penned" sheep. """ - consensus_k: int = 1 + consensus_k: int = 3 """New tracks must accumulate this many matches before they appear in - ``get_positions``. ``1`` (default) disables the candidate stage — - behaviour-identical to the original tracker. ``3-4`` filters one-shot - LiDAR phantoms in Webots while a real sheep promotes within - ``consensus_k * timestep`` ≈ 50-65 ms. + ``get_positions``. ``1`` disables the candidate stage entirely; + ``3`` (default) requires three nearby confirmations within + ``consensus_max_age`` and reliably filters single-shot detection + splits / out-of-range stragglers that confuse the policy on the + round field while real sheep promote in ~50 ms (3 frames). """ consensus_radius_m: float = 0.5 @@ -190,9 +191,11 @@ class TrackerConfig: ≪ 0.05 m / step at max speed so this gate is very loose for them. """ - consensus_max_age: int = 8 + consensus_max_age: int = 15 """A candidate that has not been matched for this many steps is dropped. - Short — phantoms get one window to confirm or die. + Short enough that a one-shot phantom can't keep itself alive, long + enough that a real sheep glimpsed twice in a short interval + confirms. """ def __post_init__(self) -> None: diff --git a/herding/control/strombom.py b/herding/control/strombom.py index bb3a741..5ba46d1 100644 --- a/herding/control/strombom.py +++ b/herding/control/strombom.py @@ -10,7 +10,10 @@ Reference: Strömbom et al. 2014, "Solving the shepherding problem." import math -from herding.world.geometry import PEN_ENTRY, GATE_Y, in_pen +from herding.world.geometry import ( + FIELD_ROUND_R, FIELD_SHAPE, + PEN_ENTRY, GATE_Y, in_pen, +) F_FACTOR = 4.0 # collect/drive threshold scaled by √n DELTA_COLLECT = 1.5 # drive-position offset behind the furthest sheep @@ -54,6 +57,23 @@ def compute_action(dog_xy, sheep_positions, pen_target=PEN_ENTRY): tx, ty = com_x + DELTA_DRIVE * ux, com_y + DELTA_DRIVE * uy mode = "drive" + # Round-field wall fallback: if the drive target lies outside the + # curved boundary, push the flock radially inward first so it + # leaves the wall — otherwise the dog ends up tangent to the wall + # and the flock circles indefinitely. + if FIELD_SHAPE == "field_round" and mode == "drive": + if math.hypot(tx, ty) > FIELD_ROUND_R - 1.0: + r_com = math.hypot(com_x, com_y) + if r_com > 1e-3: + ux2, uy2 = com_x / r_com, com_y / r_com + tx = com_x + DELTA_DRIVE * ux2 + ty = com_y + DELTA_DRIVE * uy2 + r_t = math.hypot(tx, ty) + if r_t > FIELD_ROUND_R - 1.0: + scale = (FIELD_ROUND_R - 1.0) / r_t + tx *= scale + ty *= scale + ax, ay = _unit(tx - dog_xy[0], ty - dog_xy[1]) return ax, ay, mode diff --git a/herding/control/universal.py b/herding/control/universal.py index 12d054b..8edf22a 100644 --- a/herding/control/universal.py +++ b/herding/control/universal.py @@ -29,6 +29,7 @@ For differential drive ``omega`` is always 0.0 and can be ignored. import math from herding.world.geometry import ( + FIELD_ROUND_R, FIELD_SHAPE, PEN_ENTRY, GATE_X, GATE_Y, in_pen, ) @@ -171,6 +172,28 @@ def compute_action(dog_xy, dog_heading, sheep_positions, mode = "drive" face_target = pen_target + # On the round field the natural "behind the flock" point can fall + # outside the curved wall when the flock CoM is itself close to the + # wall. The dog tries to reach an unreachable target, ends up + # tangent to the wall, and the flock circles indefinitely. + # Fix: when the natural target leaves the field, fall back to + # pushing the flock radially inward toward the centre — break the + # wall-circle pattern, then resume normal pen-direction drive once + # the flock is back in the interior. + if FIELD_SHAPE == "field_round" and mode == "drive": + if math.hypot(tx, ty) > FIELD_ROUND_R - 1.0: + r_com = math.hypot(com_x, com_y) + if r_com > 1e-3: + ux2, uy2 = com_x / r_com, com_y / r_com + tx = com_x + DELTA_DRIVE * ux2 + ty = com_y + DELTA_DRIVE * uy2 + # Clamp to inside-field radius so the dog target is reachable. + r_t = math.hypot(tx, ty) + if r_t > FIELD_ROUND_R - 1.0: + scale = (FIELD_ROUND_R - 1.0) / r_t + tx *= scale + ty *= scale + ax, ay = _unit(tx - dog_xy[0], ty - dog_xy[1]) # ---- Omega (mecanum only) ---- diff --git a/tests/test_config.py b/tests/test_config.py index c97794c..f80c447 100644 --- a/tests/test_config.py +++ b/tests/test_config.py @@ -68,9 +68,15 @@ class TestTrackerConfig: assert cfg.max_new_tracks_per_step == 1 assert cfg.pen_latch_depth == 2.0 - def test_default_consensus_disabled(self): + def test_default_consensus_enabled(self): + # Consensus is on by default — it filters tracker phantoms that + # confused the policy on the round field (52% → 88%) at no cost + # on the rectangular field (100% → 100%). Pass-through (k=1) is + # still available by explicitly constructing TrackerConfig(consensus_k=1). cfg = TrackerConfig() - assert cfg.consensus_k == 1 + assert cfg.consensus_k >= 2 + assert cfg.consensus_radius_m > 0.0 + assert cfg.consensus_max_age > cfg.consensus_k def test_webots_preset_enables_consensus(self): cfg = HERDING_WEBOTS.tracker