Checkpoint 8

tests/test_control.py

@@ -14,6 +14,7 @@ from herding.control.sequential import compute_action as sequential_action
 from herding.control.strombom import (
     DELTA_DRIVE, F_FACTOR, compute_action as strombom_action,
 )
+from herding.control.universal import compute_action as universal_action
 from herding.world.geometry import PEN_ENTRY
 
 
@@ -119,8 +120,10 @@ def test_sequential_targets_closest_to_pen():
 def test_active_scan_initial_phase_rotates():
     teacher = ActiveScanTeacher(strombom_action)
     # First call → opening rotation regardless of input.
-    vx, vy, mode = teacher((0.0, 0.0), 0.0, {"s0": (5.0, 0.0)}, PEN_ENTRY)
+    vx, vy, omega, mode = teacher(
+        (0.0, 0.0), 0.0, {"s0": (5.0, 0.0)}, PEN_ENTRY)
     assert mode == "scan_initial"
+    assert omega == 0.0
     assert math.isclose(math.hypot(vx, vy), 1.0, abs_tol=1e-6)
 
 
@@ -129,7 +132,8 @@ def test_active_scan_hands_off_to_base_after_opener():
     # Burn through the opener.
     for _ in range(2):
         teacher((0.0, 0.0), 0.0, {"s0": (0.0, 8.0)}, PEN_ENTRY)
-    _vx, _vy, mode = teacher((0.0, 0.0), 0.0, {"s0": (0.0, 8.0)}, PEN_ENTRY)
+    _vx, _vy, _omega, mode = teacher(
+        (0.0, 0.0), 0.0, {"s0": (0.0, 8.0)}, PEN_ENTRY)
     # Either drive (Strömbom mode label) or collect; not scan_initial.
     assert "scan" not in mode
 
@@ -141,7 +145,7 @@ def test_active_scan_holds_last_action_on_brief_empty():
     teacher((0.0, 0.0), 0.0, {"s0": (0.0, 8.0)}, PEN_ENTRY)
     last = teacher.last_action
     # Now a single empty frame → hold.
-    vx, vy, mode = teacher((0.0, 0.0), 0.0, {}, PEN_ENTRY)
+    vx, vy, _omega, mode = teacher((0.0, 0.0), 0.0, {}, PEN_ENTRY)
     assert mode == "hold"
     assert (vx, vy) == last
 
@@ -150,10 +154,30 @@ def test_active_scan_explores_after_sustained_empty():
     teacher = ActiveScanTeacher(strombom_action, initial_scan_steps=1)
     teacher((0.0, 0.0), 0.0, {}, PEN_ENTRY)  # opener
     for _ in range(EMPTY_DEBOUNCE_STEPS):
-        last_vx, last_vy, mode = teacher((5.0, 5.0), 0.0, {}, PEN_ENTRY)
+        last_vx, last_vy, _omega, mode = teacher(
+            (5.0, 5.0), 0.0, {}, PEN_ENTRY)
     assert mode in ("explore", "scan_at_centre")
 
 
+def test_active_scan_preserves_mecanum_omega():
+    """Regression: ActiveScanTeacher must propagate omega from a mecanum
+    base teacher, not silently drop it. Without this, BC mecanum demos
+    have omega=0 everywhere and the policy never learns to rotate.
+    """
+    teacher = ActiveScanTeacher(universal_action, initial_scan_steps=1)
+    # Burn the opener so we exit phase 1.
+    teacher((0.0, 0.0), 0.0, {"s0": (8.0, 8.0)}, PEN_ENTRY,
+            drive_mode="mecanum")
+    # Place a sheep off to the side so the dog needs to face it.
+    # Dog at origin facing +x (heading=0); target at (0, 8) → desired
+    # heading +π/2, so omega should be positive.
+    vx, vy, omega, mode = teacher(
+        (0.0, 0.0), 0.0, {"s0": (0.0, 8.0)}, PEN_ENTRY,
+        drive_mode="mecanum")
+    assert mode in ("collect", "drive", "recovery")
+    assert abs(omega) > 0.05, f"omega should be non-zero on mecanum, got {omega}"
+
+
 def test_active_scan_reset_clears_state():
     teacher = ActiveScanTeacher(strombom_action, initial_scan_steps=5)
     for _ in range(3):

tests/test_diffdrive.py

@@ -1,11 +1,13 @@
-"""Differential-drive kinematics and the (vx, vy) → wheel-speed map."""
+"""Differential-drive and mecanum kinematics tests."""
 
 import math
 
 import pytest
 
 from herding.world.diffdrive import (
-    heading_speed_to_wheels, kinematics_step, velocity_to_wheels,
+    heading_speed_to_wheels, kinematics_step,
+    mecanum_inverse, mecanum_kinematics_step,
+    velocity_to_mecanum_wheels, velocity_to_wheels,
 )
 
 
@@ -82,3 +84,109 @@ def test_heading_speed_to_wheels_reverse_target_forwards_zero():
     left, right = heading_speed_to_wheels(math.pi, 10.0, 0.0, MAX_OMEGA)
     # cos(π) clamped at 0 → no forward; pure rotation.
     assert left + right == pytest.approx(0.0, abs=1e-6)
+
+
+# ---------------------------------------------------------------------------
+# Mecanum kinematics tests
+# ---------------------------------------------------------------------------
+
+LX = 0.14   # half wheel_base_x
+LY = 0.14   # half wheel_base_y
+
+
+def test_mecanum_kinematics_zero_is_identity():
+    x, y, h = mecanum_kinematics_step(
+        1.0, 2.0, 0.5, 0.0, 0.0, 0.0, 0.0, WHEEL_R, LX, LY, DT,
+    )
+    assert (x, y, h) == (1.0, 2.0, 0.5)
+
+
+def test_mecanum_kinematics_pure_forward():
+    # All 4 wheels equal → pure forward (vx_body > 0, vy_body = 0).
+    w = 10.0
+    x, y, h = mecanum_kinematics_step(
+        0.0, 0.0, 0.0, w, w, w, w, WHEEL_R, LX, LY, DT,
+    )
+    assert h == pytest.approx(0.0, abs=1e-9)
+    assert y == pytest.approx(0.0, abs=1e-9)
+    assert math.isclose(x, w * WHEEL_R * DT, rel_tol=1e-6)
+
+
+def test_mecanum_kinematics_pure_strafe():
+    # Strafe right (positive vy_body) with zero forward:
+    # vx_body = (w_fl+w_fr+w_rl+w_rr)*r/4 = 0 → sum of wheels = 0
+    # vy_body = (-w_fl+w_fr+w_rl-w_rr)*r/4 > 0
+    # Use w_fl=-10, w_fr=10, w_rl=10, w_rr=-10.
+    w_fl, w_fr, w_rl, w_rr = -10.0, 10.0, 10.0, -10.0
+    x, y, h = mecanum_kinematics_step(
+        0.0, 0.0, 0.0, w_fl, w_fr, w_rl, w_rr, WHEEL_R, LX, LY, DT,
+    )
+    assert h == pytest.approx(0.0, abs=1e-9)
+    assert x == pytest.approx(0.0, abs=1e-9)
+    expected_vy = (-w_fl + w_fr + w_rl - w_rr) * WHEEL_R / 4.0
+    assert math.isclose(y, expected_vy * DT, rel_tol=1e-6)
+
+
+def test_mecanum_kinematics_pure_rotation():
+    # Pure rotation: vx_body=0, vy_body=0, omega>0.
+    # w_fl=-10, w_fr=10, w_rl=-10, w_rr=10 → all sums cancel except omega.
+    w_fl, w_fr, w_rl, w_rr = -10.0, 10.0, -10.0, 10.0
+    x, y, h = mecanum_kinematics_step(
+        0.0, 0.0, 0.0, w_fl, w_fr, w_rl, w_rr, WHEEL_R, LX, LY, DT,
+    )
+    assert x == pytest.approx(0.0, abs=1e-9)
+    assert y == pytest.approx(0.0, abs=1e-9)
+    assert h > 0.0
+
+
+def test_mecanum_inverse_roundtrip():
+    # Inverse → forward: pick desired body velocities, compute wheels,
+    # then verify forward kinematics recovers the same velocities.
+    vx_b = 0.5
+    vy_b = 0.3
+    omega = 0.2
+    w_fl, w_fr, w_rl, w_rr = mecanum_inverse(
+        vx_b, vy_b, omega, WHEEL_R, LX, LY, MAX_OMEGA,
+    )
+    vx_check = (w_fl + w_fr + w_rl + w_rr) * WHEEL_R / 4.0
+    vy_check = (-w_fl + w_fr + w_rl - w_rr) * WHEEL_R / 4.0
+    omega_check = (-w_fl + w_fr - w_rl + w_rr) * WHEEL_R / (4.0 * (LX + LY))
+    assert math.isclose(vx_b, vx_check, rel_tol=1e-6)
+    assert math.isclose(vy_b, vy_check, rel_tol=1e-6)
+    assert math.isclose(omega, omega_check, rel_tol=1e-6)
+
+
+def test_mecanum_inverse_clamped():
+    # Request an extreme velocity — all wheels should be clamped.
+    w_fl, w_fr, w_rl, w_rr = mecanum_inverse(
+        100.0, 100.0, 50.0, WHEEL_R, LX, LY, MAX_OMEGA,
+    )
+    assert max(abs(w_fl), abs(w_fr), abs(w_rl), abs(w_rr)) <= MAX_OMEGA
+
+
+def test_velocity_to_mecanum_wheels_zero():
+    result = velocity_to_mecanum_wheels(
+        0.0, 0.0, 0.0, 0.0, MAX_LINEAR, WHEEL_R, LX, LY, MAX_OMEGA,
+        wheel_base=WHEEL_B,
+    )
+    assert result == (0.0, 0.0, 0.0, 0.0)
+
+
+def test_velocity_to_mecanum_wheels_forward():
+    w_fl, w_fr, w_rl, w_rr = velocity_to_mecanum_wheels(
+        1.0, 0.0, 0.0, 0.0, MAX_LINEAR, WHEEL_R, LX, LY, MAX_OMEGA,
+        wheel_base=WHEEL_B,
+    )
+    # All 4 wheels should be positive and roughly equal.
+    assert all(w > 0.0 for w in (w_fl, w_fr, w_rl, w_rr))
+    assert math.isclose(w_fl, w_rr, rel_tol=1e-6)
+    assert math.isclose(w_fr, w_rl, rel_tol=1e-6)
+
+
+def test_velocity_to_mecanum_wheels_clamped():
+    # Extreme input — all wheels within max.
+    ws = velocity_to_mecanum_wheels(
+        1.0, 1.0, 1.0, 0.0, MAX_LINEAR, WHEEL_R, LX, LY, MAX_OMEGA,
+        wheel_base=WHEEL_B,
+    )
+    assert all(abs(w) <= MAX_OMEGA for w in ws)

tests/test_env.py

@@ -38,6 +38,14 @@ def test_env_reset_determinism_same_seed():
     assert np.allclose(obs_a, obs_b)
 
 
+def test_env_constructor_seed_applies_to_first_reset():
+    a = HerdingEnv(n_sheep=3, seed=42, use_lidar=False)
+    b = HerdingEnv(n_sheep=3, seed=42, use_lidar=False)
+    obs_a, _ = a.reset()
+    obs_b, _ = b.reset()
+    assert np.allclose(obs_a, obs_b)
+
+
 def test_env_curriculum_samples_full_range():
     env = HerdingEnv(seed=0, use_lidar=False)
     sizes = set()

tests/test_perception.py

@@ -42,8 +42,11 @@ def test_simulate_scan_sheep_in_front_returns_centre_hit():
 
 
 def test_simulate_scan_sheep_behind_dog_not_hit():
+    # With 360° FOV, a sheep behind the dog IS now hit.
     ranges = simulate_scan(0.0, 0.0, 0.0, [(-5.0, 0.0)], noise=0.0)
-    assert (ranges == LIDAR_MAX_RANGE).all()
+    assert (ranges < LIDAR_MAX_RANGE).any()
+    # Verify the closest hit is near 5m (sheep at distance 5).
+    assert float(ranges.min()) < 5.3
 
 
 def test_simulate_scan_wall_hit():