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:
+49
-20
@@ -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.
|
||||
|
||||
|
||||
Reference in New Issue
Block a user