Johnny Fernandes
ee77c8606c
Mecanum proto rewrite in b3cf990 made the wheels truly omnidirectional
in Webots, but with asymmetric slip: forward command produces ~89% of
textbook speed while strafe produces only ~38% plus a consistent
~28% backward bleed-through. v1 BC/RL trained on perfect mecanum
gym kinematics could not herd the new dynamics. To unblock that:
* `mecanum_kinematics_step` gains two parameters that scale the
realised motion to match a deployed-platform calibration:
- strafe_efficiency ∈ (0, 1] default 1.0
- strafe_to_forward_bleed default 0.0
Forward motion is untouched (textbook X-pattern continues to apply
to vx_body); only the lateral channel is scaled and bleed is added.
* `RobotConfig` exposes both as drive-config fields with the same
pass-through defaults so existing diff-drive code and existing
mecanum training pipelines see no behaviour change.
* `HERDING_MEC_WEBOTS` preset bakes in the values measured against the
current Webots mecanum proto (strafe_efficiency=0.4,
strafe_to_forward_bleed=-0.28). Training mecanum BC/RL with this
preset produces policies that compensate for the imperfect
physical mecanum at deploy.
* `HerdingEnv` plumbs `RobotConfig.strafe_*` through to
`mecanum_kinematics_step` so the preset takes effect.
* tools/gen_mecanum_wheels.py is added so the proto's 32 roller
hinges can be regenerated by editing a single set of constants
rather than hand-editing 1500+ lines of VRML.
Tests:
* 4 new mecanum_kinematics_step tests (default pass-through, strafe
scaling, backward bleed, forward unaffected by strafe params).
* 3 new RobotConfig tests (defaults, validation, preset shape).
* Sanity check: gym strafe with HERDING_MEC_WEBOTS over 100 steps
reproduces the Webots calibration to 2 decimal places.
126 unit tests pass (was 120).
Co-Authored-By: Claude Opus 4.7 <noreply@anthropic.com>
286 lines
10 KiB
Python
286 lines
10 KiB
Python
"""Tests for herding/config.py — dataclass construction, defaults, overrides."""
|
|
|
|
import math
|
|
import pytest
|
|
|
|
from herding.config import (
|
|
DetectionConfig,
|
|
DomainRandomConfig,
|
|
HerdingConfig,
|
|
HERDING_DEFAULT,
|
|
HERDING_WEBOTS,
|
|
LidarConfig,
|
|
LIDAR_FULL,
|
|
LIDAR_WEBOTS,
|
|
RobotConfig,
|
|
TrackerConfig,
|
|
)
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# LidarConfig
|
|
# ---------------------------------------------------------------------------
|
|
|
|
class TestLidarConfig:
|
|
def test_defaults_match_full_circle_preset(self):
|
|
assert LidarConfig() == LIDAR_FULL
|
|
|
|
def test_webots_preset(self):
|
|
assert LIDAR_WEBOTS.n_rays == 180
|
|
assert abs(LIDAR_WEBOTS.fov_rad - math.radians(140.0)) < 1e-9
|
|
|
|
def test_frozen(self):
|
|
cfg = LidarConfig()
|
|
with pytest.raises((AttributeError, TypeError)):
|
|
cfg.n_rays = 42 # type: ignore[misc]
|
|
|
|
def test_invalid_n_rays(self):
|
|
with pytest.raises(ValueError):
|
|
LidarConfig(n_rays=0)
|
|
|
|
def test_invalid_fov(self):
|
|
with pytest.raises(ValueError):
|
|
LidarConfig(fov_rad=0.0)
|
|
with pytest.raises(ValueError):
|
|
LidarConfig(fov_rad=math.pi * 3)
|
|
|
|
def test_invalid_max_range(self):
|
|
with pytest.raises(ValueError):
|
|
LidarConfig(max_range=-1.0)
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# TrackerConfig
|
|
# ---------------------------------------------------------------------------
|
|
|
|
class TestTrackerConfig:
|
|
def test_defaults(self):
|
|
cfg = TrackerConfig()
|
|
assert cfg.forget_steps == 200
|
|
assert cfg.max_new_tracks_per_step == 10
|
|
|
|
def test_webots_preset_tighter(self):
|
|
cfg = HERDING_WEBOTS.tracker
|
|
# forget_steps was extended so confirmed sheep tracks survive
|
|
# sparse 140° FOV re-sightings; consensus blocks phantoms from
|
|
# reaching this lifetime.
|
|
assert cfg.forget_steps >= 200
|
|
assert cfg.max_new_tracks_per_step == 1
|
|
assert cfg.pen_latch_depth == 2.0
|
|
|
|
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 >= 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
|
|
assert cfg.consensus_k > 1
|
|
assert cfg.consensus_radius_m > 0.0
|
|
assert cfg.consensus_max_age >= cfg.consensus_k
|
|
|
|
def test_invalid_forget_steps(self):
|
|
with pytest.raises(ValueError):
|
|
TrackerConfig(forget_steps=0)
|
|
|
|
def test_invalid_max_new_tracks(self):
|
|
with pytest.raises(ValueError):
|
|
TrackerConfig(max_new_tracks_per_step=0)
|
|
|
|
def test_invalid_consensus_params(self):
|
|
with pytest.raises(ValueError):
|
|
TrackerConfig(consensus_k=0)
|
|
with pytest.raises(ValueError):
|
|
TrackerConfig(consensus_radius_m=0.0)
|
|
with pytest.raises(ValueError):
|
|
TrackerConfig(consensus_max_age=0)
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# DetectionConfig
|
|
# ---------------------------------------------------------------------------
|
|
|
|
class TestDetectionConfig:
|
|
def test_defaults(self):
|
|
cfg = DetectionConfig()
|
|
assert cfg.wall_reject == 0.5
|
|
|
|
def test_webots_preset_wall_reject(self):
|
|
# wall_reject stays at 0.5 m — 1.0 m was too aggressive near the south gate
|
|
cfg = HERDING_WEBOTS.detection
|
|
assert cfg.wall_reject == 0.5
|
|
|
|
def test_invalid_wall_reject(self):
|
|
with pytest.raises(ValueError):
|
|
DetectionConfig(wall_reject=-0.1)
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# RobotConfig
|
|
# ---------------------------------------------------------------------------
|
|
|
|
class TestRobotConfig:
|
|
def test_max_linear_derived(self):
|
|
cfg = RobotConfig()
|
|
assert abs(cfg.max_linear - cfg.wheel_radius * cfg.max_wheel_omega) < 1e-9
|
|
|
|
def test_default_action_smooth_zero(self):
|
|
assert RobotConfig().action_smooth == 0.0
|
|
|
|
def test_webots_action_smooth(self):
|
|
assert HERDING_WEBOTS.robot.action_smooth == 0.55
|
|
|
|
def test_invalid_action_smooth(self):
|
|
with pytest.raises(ValueError):
|
|
RobotConfig(action_smooth=1.0)
|
|
with pytest.raises(ValueError):
|
|
RobotConfig(action_smooth=-0.1)
|
|
|
|
def test_default_strafe_passthrough(self):
|
|
cfg = RobotConfig()
|
|
assert cfg.strafe_efficiency == 1.0
|
|
assert cfg.strafe_to_forward_bleed == 0.0
|
|
|
|
def test_invalid_strafe_efficiency(self):
|
|
with pytest.raises(ValueError):
|
|
RobotConfig(strafe_efficiency=0.0)
|
|
with pytest.raises(ValueError):
|
|
RobotConfig(strafe_efficiency=1.5)
|
|
with pytest.raises(ValueError):
|
|
RobotConfig(strafe_efficiency=-0.1)
|
|
|
|
def test_mec_webots_preset(self):
|
|
from herding.config import HERDING_MEC_WEBOTS
|
|
assert 0.0 < HERDING_MEC_WEBOTS.robot.strafe_efficiency < 1.0
|
|
assert HERDING_MEC_WEBOTS.robot.strafe_to_forward_bleed < 0.0
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# DomainRandomConfig
|
|
# ---------------------------------------------------------------------------
|
|
|
|
class TestDomainRandomConfig:
|
|
def test_all_zeros_by_default(self):
|
|
cfg = DomainRandomConfig()
|
|
assert cfg.fp_rate == 0.0
|
|
assert cfg.wheel_slip_std == 0.0
|
|
assert cfg.compass_noise_std == 0.0
|
|
|
|
def test_invalid_fp_rate(self):
|
|
with pytest.raises(ValueError):
|
|
DomainRandomConfig(fp_rate=-1.0)
|
|
|
|
def test_invalid_slip_std(self):
|
|
with pytest.raises(ValueError):
|
|
DomainRandomConfig(wheel_slip_std=-0.01)
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# HerdingConfig
|
|
# ---------------------------------------------------------------------------
|
|
|
|
class TestHerdingConfig:
|
|
def test_default_is_herding_default(self):
|
|
assert HerdingConfig() == HERDING_DEFAULT
|
|
|
|
def test_replace_sub_config(self):
|
|
new_cfg = HERDING_WEBOTS.replace(
|
|
domain_random=DomainRandomConfig(fp_rate=2.0)
|
|
)
|
|
assert new_cfg.domain_random.fp_rate == 2.0
|
|
# Other sub-configs unchanged
|
|
assert new_cfg.tracker == HERDING_WEBOTS.tracker
|
|
assert new_cfg.lidar == HERDING_WEBOTS.lidar
|
|
|
|
def test_herding_default_matches_original_module_constants(self):
|
|
"""Verify the default config reproduces the original hardcoded values."""
|
|
from herding.perception.lidar_sim import (
|
|
LIDAR_N_RAYS, LIDAR_FOV, LIDAR_MAX_RANGE, LIDAR_NOISE,
|
|
SHEEP_RADIUS, POST_RADIUS,
|
|
)
|
|
from herding.perception.lidar_perception import (
|
|
GAP_THRESHOLD, MAX_CLUSTER_SPAN, RANGE_HIT_EPS,
|
|
SPLIT_RANGE_GAP, WALL_REJECT, STATIC_REJECT,
|
|
)
|
|
from herding.perception.sheep_tracker import (
|
|
GATE_M, REACQUIRE_GATE_M, REACQUIRE_MIN_AGE, PENNED_GATE_M,
|
|
FORGET_STEPS, PREDICT_STEPS, VELOCITY_CLAMP,
|
|
)
|
|
cfg = HERDING_DEFAULT
|
|
assert cfg.lidar.n_rays == LIDAR_N_RAYS
|
|
assert cfg.lidar.fov_rad == LIDAR_FOV
|
|
assert cfg.lidar.max_range == LIDAR_MAX_RANGE
|
|
assert cfg.lidar.noise_std == LIDAR_NOISE
|
|
assert cfg.lidar.sheep_radius == SHEEP_RADIUS
|
|
assert cfg.lidar.post_radius == POST_RADIUS
|
|
assert cfg.detection.gap_threshold == GAP_THRESHOLD
|
|
assert cfg.detection.max_cluster_span == MAX_CLUSTER_SPAN
|
|
assert cfg.detection.range_hit_eps == RANGE_HIT_EPS
|
|
assert cfg.detection.split_range_gap == SPLIT_RANGE_GAP
|
|
assert cfg.detection.wall_reject == WALL_REJECT
|
|
assert cfg.detection.static_reject == STATIC_REJECT
|
|
assert cfg.tracker.gate_m == GATE_M
|
|
assert cfg.tracker.reacquire_gate_m == REACQUIRE_GATE_M
|
|
assert cfg.tracker.reacquire_min_age == REACQUIRE_MIN_AGE
|
|
assert cfg.tracker.penned_gate_m == PENNED_GATE_M
|
|
assert cfg.tracker.forget_steps == FORGET_STEPS
|
|
assert cfg.tracker.predict_steps == PREDICT_STEPS
|
|
assert cfg.tracker.velocity_clamp == VELOCITY_CLAMP
|
|
|
|
|
|
# ---------------------------------------------------------------------------
|
|
# Integration: HerdingEnv honours the config
|
|
# ---------------------------------------------------------------------------
|
|
|
|
class TestHerdingEnvConfig:
|
|
def test_default_env_unchanged(self):
|
|
"""HerdingEnv() still works with no config — zero behaviour change."""
|
|
from training.herding_env import HerdingEnv
|
|
env = HerdingEnv(n_sheep=1, max_steps=5, difficulty=1.0, seed=0)
|
|
obs, info = env.reset()
|
|
assert obs.shape == (32,)
|
|
obs2, *_ = env.step(env.action_space.sample())
|
|
assert obs2.shape == (32,)
|
|
|
|
def test_webots_config_propagates_action_smooth(self):
|
|
from training.herding_env import HerdingEnv
|
|
env = HerdingEnv(herding_cfg=HERDING_WEBOTS)
|
|
assert env.ACTION_SMOOTH == 0.55
|
|
|
|
def test_webots_config_runs(self):
|
|
from training.herding_env import HerdingEnv
|
|
env = HerdingEnv(
|
|
n_sheep=2, max_steps=10, difficulty=1.0, seed=42,
|
|
herding_cfg=HERDING_WEBOTS,
|
|
)
|
|
obs, _ = env.reset()
|
|
for _ in range(5):
|
|
obs, _, terminated, truncated, _ = env.step(env.action_space.sample())
|
|
assert obs.shape == (32,)
|
|
|
|
def test_domain_random_fp_runs(self):
|
|
from training.herding_env import HerdingEnv
|
|
cfg = HERDING_WEBOTS.replace(
|
|
domain_random=DomainRandomConfig(fp_rate=3.0, fp_std_pos=0.2)
|
|
)
|
|
env = HerdingEnv(n_sheep=2, max_steps=10, difficulty=1.0, seed=7, herding_cfg=cfg)
|
|
env.reset()
|
|
for _ in range(5):
|
|
env.step(env.action_space.sample())
|
|
|
|
def test_domain_random_slip_runs(self):
|
|
from training.herding_env import HerdingEnv
|
|
cfg = HERDING_WEBOTS.replace(
|
|
domain_random=DomainRandomConfig(wheel_slip_std=0.05, compass_noise_std=0.02)
|
|
)
|
|
env = HerdingEnv(n_sheep=1, max_steps=10, difficulty=1.0, seed=3,
|
|
drive_mode="mecanum", herding_cfg=cfg)
|
|
env.reset()
|
|
for _ in range(5):
|
|
env.step(env.action_space.sample())
|