Gym mecanum kinematics matching to Webots roller-hinge proto

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>
This commit is contained in:
Johnny Fernandes
2026-05-17 01:09:47 +00:00
parent b3cf9909a8
commit ee77c8606c
6 changed files with 353 additions and 2 deletions
+18
View File
@@ -141,6 +141,24 @@ class TestRobotConfig:
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
+39
View File
@@ -127,6 +127,45 @@ def test_mecanum_kinematics_pure_strafe():
assert math.isclose(y, expected_vy * DT, rel_tol=1e-6)
def test_mecanum_kinematics_strafe_efficiency_scales_y():
# With strafe_efficiency=0.4, realised strafe should be 40% of ideal.
w_fl, w_fr, w_rl, w_rr = -10.0, 10.0, 10.0, -10.0
x, y, _ = mecanum_kinematics_step(
0.0, 0.0, 0.0, w_fl, w_fr, w_rl, w_rr, WHEEL_R, LX, LY, DT,
strafe_efficiency=0.4,
)
ideal_vy = (-w_fl + w_fr + w_rl - w_rr) * WHEEL_R / 4.0
assert math.isclose(y, 0.4 * ideal_vy * DT, rel_tol=1e-6)
assert x == pytest.approx(0.0, abs=1e-9)
def test_mecanum_kinematics_strafe_bleed_pushes_backward():
# Negative bleed means strafe commands also push the body backward.
w_fl, w_fr, w_rl, w_rr = -10.0, 10.0, 10.0, -10.0
x, y, _ = mecanum_kinematics_step(
0.0, 0.0, 0.0, w_fl, w_fr, w_rl, w_rr, WHEEL_R, LX, LY, DT,
strafe_efficiency=1.0,
strafe_to_forward_bleed=-0.28,
)
ideal_vy = (-w_fl + w_fr + w_rl - w_rr) * WHEEL_R / 4.0
assert math.isclose(y, ideal_vy * DT, rel_tol=1e-6)
expected_x = -0.28 * abs(ideal_vy) * DT
assert math.isclose(x, expected_x, rel_tol=1e-6)
def test_mecanum_kinematics_forward_unaffected_by_strafe_params():
# Forward command should be untouched by strafe_efficiency / bleed.
w_fl = w_fr = w_rl = w_rr = 10.0
x, y, _ = mecanum_kinematics_step(
0.0, 0.0, 0.0, w_fl, w_fr, w_rl, w_rr, WHEEL_R, LX, LY, DT,
strafe_efficiency=0.4,
strafe_to_forward_bleed=-0.28,
)
expected_vx = (w_fl + w_fr + w_rl + w_rr) * WHEEL_R / 4.0
assert math.isclose(x, expected_vx * DT, rel_tol=1e-6)
assert y == pytest.approx(0.0, abs=1e-9)
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.