Mecanum Webots via Supervisor kinematic injection

Replace the failing ODE-rolled mecanum chassis dynamics with a
Supervisor.setVelocity call that uses the gym mecanum forward
kinematics formula directly. Wheel motors still spin (visual);
chassis motion comes from the gym model so training and deployment
match by construction.

Results (seed=42, n=10 sheep): BC + RL mecanum pen 10/10 in both
field and field_round. n=5 mecanum cells still 0/5 due to tracker
phantoms anchored to wall corners under the 360° LiDAR — documented
in docs/status.md as the remaining gap.

Cleanup: drop deploy-time hacks (HERDING_HEADING_*, HERDING_OMEGA_CLAMP,
HERDING_TRACKER_*) that were workarounds for the old ODE chaos;
revert the proto inertiaMatrix, roller dampingConstant, and reduced
motor torque since they no longer carry load; refresh comments
around the mecanum config presets.
This commit is contained in:
Johnny Fernandes
2026-05-18 22:46:37 +00:00
parent 1df84ae4b5
commit 27c0f65722
25 changed files with 2635 additions and 76 deletions
+49 -20
View File
@@ -266,18 +266,21 @@ class RobotConfig:
strafe_efficiency: float = 1.0
"""Mecanum strafe magnitude as a fraction of textbook X-pattern.
``1.0`` (default) = perfect mecanum kinematics. ``0.4`` matches the
Webots roller-hinge mecanum proto calibration (62% slip on strafe,
11% on forward). Used by ``mecanum_step`` only — has no
effect on differential drive.
``1.0`` (default) is the ideal kinematic mecanum. Values below 1
model strafe slip; the Webots controller reads the same value and
applies it in the Supervisor velocity injection, so gym training
and Webots deployment see identical body motion. No effect on
differential drive.
"""
strafe_to_forward_bleed: float = 0.0
"""Fraction of ideal strafe magnitude that bleeds into body-frame x.
``0.0`` (default) = no bleed. ``-0.28`` matches the Webots proto's
consistent backward push under strafe commands. Used by
``mecanum_step`` only.
``0.0`` (default) = no bleed. Non-zero values add
``strafe_to_forward_bleed * |vy_body_ideal|`` to ``vx_body`` to
model the consistent forward (or backward) drift that some
mecanum chassis exhibit during pure-strafe commands. No effect on
differential drive.
"""
def __post_init__(self) -> None:
@@ -407,23 +410,49 @@ HERDING_MEC_WEBOTS = HerdingConfig(
),
robot=RobotConfig(
action_smooth=0.55,
strafe_efficiency=0.4,
strafe_to_forward_bleed=-0.28,
strafe_efficiency=0.26,
strafe_to_forward_bleed=-0.40,
),
)
"""Webots-mecanum-matched training preset.
"""Mecanum + 140° LiDAR preset.
Same as HERDING_WEBOTS but with the gym mecanum kinematics scaled to
match the Webots roller-hinge mecanum proto:
* ``strafe_efficiency=0.4`` — strafing produces ~40% of textbook
X-pattern lateral velocity in Webots; this matches the bias.
* ``strafe_to_forward_bleed=-0.28`` — strafe commands bleed ~28% of
their magnitude into backward body motion in Webots.
Mirrors HERDING_WEBOTS but with mecanum-specific kinematic scaling
(``strafe_efficiency`` and ``strafe_to_forward_bleed``) applied to
the gym forward-kinematics formula. The Webots controller reads
these same values via ``RobotConfig`` and feeds them through the
Supervisor velocity injection, so gym and Webots produce identical
body motion. Diff-drive ignores both fields.
"""
Use this preset when training BC/RL for the mecanum drive so the
policy learns to compensate for the imperfect physical mecanum.
Differential drive ignores both parameters and behaves identically
to HERDING_WEBOTS.
HERDING_MEC_WEBOTS_360 = HerdingConfig(
lidar=LIDAR_WEBOTS_360,
# Looser detection thresholds for the wider FOV — the 360° scan
# catches far walls, gate posts and pen rails the 140° front cone
# never sees, so the cluster/feature filters need slightly more
# margin to keep promotion rates similar.
detection=DetectionConfig(wall_reject=0.6, static_reject=1.2),
tracker=TrackerConfig(
forget_steps=300,
max_new_tracks_per_step=2, # 360° gives more candidates per step
pen_latch_depth=3.0,
predict_steps=180,
consensus_k=3,
consensus_radius_m=0.3,
consensus_max_age=20,
),
robot=RobotConfig(
action_smooth=0.55,
strafe_efficiency=0.26,
strafe_to_forward_bleed=-0.40,
),
)
"""Mecanum + 360° LiDAR preset (the deployable mecanum target).
The 360° FOV gives the policy perception coverage in every direction,
which matches the omnidirectional motion the mecanum chassis can
produce. Used for both gym training and Webots deployment so the
trained policy sees the same observation geometry it will face at
deploy time.
"""
"""Webots-matched training preset.