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:
@@ -83,7 +83,9 @@ def heading_speed_to_wheels(heading, speed_motor, h, max_wheel_omega,
|
||||
def mecanum_kinematics_step(x, y, h, w_fl, w_fr, w_rl, w_rr,
|
||||
wheel_radius, lx, ly, dt,
|
||||
slip_std: float = 0.0,
|
||||
rng: Optional[np.random.Generator] = None):
|
||||
rng: Optional[np.random.Generator] = None,
|
||||
strafe_efficiency: float = 1.0,
|
||||
strafe_to_forward_bleed: float = 0.0):
|
||||
"""Integrate one step of mecanum forward kinematics.
|
||||
|
||||
Parameters
|
||||
@@ -97,6 +99,19 @@ def mecanum_kinematics_step(x, y, h, w_fl, w_fr, w_rl, w_rr,
|
||||
dt : timestep (s)
|
||||
slip_std : optional Gaussian std (rad/s) added to each wheel speed
|
||||
rng : numpy Generator for slip noise; required when slip_std > 0
|
||||
strafe_efficiency : scales the realised lateral (vy_body) velocity.
|
||||
``1.0`` (default) = perfect mecanum (textbook X-pattern). Set to
|
||||
the value that matches deployed-platform calibration to train
|
||||
a policy that compensates for under-actuated strafing — Webots
|
||||
with the roller-hinge mecanum proto currently calibrates to
|
||||
~0.4 of textbook on strafe.
|
||||
strafe_to_forward_bleed : fraction of |vy_body_ideal| added to
|
||||
vx_body to simulate the consistent body-x bleed-through that
|
||||
accompanies strafing in Webots' physical-roller mecanum. Use a
|
||||
*negative* value (Webots calibrates to ≈ -0.28) to model the
|
||||
backward bleed seen on strafe; positive would model forward
|
||||
bleed. The bleed magnitude is symmetric in strafe sign — both
|
||||
+y and -y commands produce the same x-direction error.
|
||||
|
||||
Returns (new_x, new_y, new_h).
|
||||
"""
|
||||
@@ -106,7 +121,14 @@ def mecanum_kinematics_step(x, y, h, w_fl, w_fr, w_rl, w_rr,
|
||||
w_rl, w_rr = w_rl + noise[2], w_rr + noise[3]
|
||||
r = wheel_radius
|
||||
vx_body = (w_fl + w_fr + w_rl + w_rr) * r / 4.0
|
||||
vy_body = (-w_fl + w_fr + w_rl - w_rr) * r / 4.0
|
||||
vy_body_ideal = (-w_fl + w_fr + w_rl - w_rr) * r / 4.0
|
||||
vy_body = vy_body_ideal * strafe_efficiency
|
||||
if strafe_to_forward_bleed != 0.0:
|
||||
# Bleed-through is asymmetric — forward in body frame, matching
|
||||
# Webots behaviour where strafe commands push the dog forward
|
||||
# regardless of strafe sign (the rollers slip the same way
|
||||
# symmetrically across the body's longitudinal axis).
|
||||
vx_body += strafe_to_forward_bleed * abs(vy_body_ideal)
|
||||
omega = (-w_fl + w_fr - w_rl + w_rr) * r / (4.0 * (lx + ly))
|
||||
|
||||
cos_h = math.cos(h)
|
||||
|
||||
Reference in New Issue
Block a user