jalf/TIR_PROJ
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Gym mecanum kinematics matching to Webots roller-hinge proto

Browse Source 
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
Download Patch File Download Diff File Expand all files Collapse all files
herding/config.py
+54
View File
@@ -252,11 +252,32 @@ class RobotConfig:
sees at deployment.
"""
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_kinematics_step`` only — has 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_kinematics_step`` only.
"""
def __post_init__(self) -> None:
if not (0.0 <= self.action_smooth < 1.0):
raise ValueError(
f"action_smooth must be in [0, 1), got {self.action_smooth}"
)
if not (0.0 < self.strafe_efficiency <= 1.0):
raise ValueError(
f"strafe_efficiency must be in (0, 1], got {self.strafe_efficiency}"
)
@property
def max_linear(self) -> float:
@@ -360,6 +381,39 @@ HERDING_WEBOTS = HerdingConfig(
),
robot=RobotConfig(action_smooth=0.55),
)
HERDING_MEC_WEBOTS = HerdingConfig(
lidar=LIDAR_WEBOTS,
detection=DetectionConfig(wall_reject=0.5, static_reject=1.2),
tracker=TrackerConfig(
forget_steps=300,
max_new_tracks_per_step=1,
pen_latch_depth=2.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.4,
strafe_to_forward_bleed=-0.28,
),
)
"""Webots-mecanum-matched training 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.
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.
"""
"""Webots-matched training preset.
Changes vs HERDING_DEFAULT:
herding/world/diffdrive.py
+24 -2
View File
@@ -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)
tests/test_config.py
+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
tests/test_diffdrive.py
+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.
tools/gen_mecanum_wheels.py
+210
View File
@@ -0,0 +1,210 @@
"""Generate ShepherdDogMecanum.proto wheel blocks with physical rollers.
Each wheel becomes:
HingeJoint (motor, axis 0 1 0 = body lateral)
-> Solid (wheel hub, rotation 0 -1 0 π/2)
children:
- WHEEL_VIS (visual, kept as-is for appearance)
- 8x HingeJoint (passive roller, axis tilted ±45° from wheel rotation
axis, tangent to the wheel circumference at the mount
point)
-> Solid (capsule)
boundingObject: a small Cylinder for the hub (smaller radius than the
roller circle so the hub doesn't touch the ground)
X-pattern roller tilt assignment:
FR, RL -> -45° (wheel-axis-relative)
FL, RR -> +45°
All math is done in the WHEEL SOLID's local frame. The wheel solid's
rotation `0 -1 0 π/2` takes wheel-local x -> body +z (up),
wheel-local y -> body +y (lateral, = wheel rotation axis),
wheel-local z -> body -x (rearward). Conversely, a body-frame offset
(dx, dy, dz) becomes (dz, dy, -dx) in wheel-local coords.
For a wheel rotating about body y at angle θ (θ=0 = body +x = forward,
θ=π/2 = body +z = top), the roller mount in body frame is
(R*cos(θ), 0, R*sin(θ)) relative to wheel centre. Tangent (radial-perp,
in the wheel-spin plane) is (-sin(θ), 0, cos(θ)); the wheel rotation
axis is (0, 1, 0). Roller axis tilted +45° from tangent toward wheel
axis:
axis_body(+45°) = (1/√2) * (-sin(θ), +1, cos(θ))
axis_body(-45°) = (1/√2) * (-sin(θ), -1, cos(θ))
Transformed to wheel-local: (dz, dy, -dx) on each component gives
mount_local = (R*sin(θ), 0, -R*cos(θ))
axis_local(+45) = (cos(θ)/√2, +1/√2, sin(θ)/√2)
axis_local(-45) = (cos(θ)/√2, -1/√2, sin(θ)/√2)
The Solid's `rotation` field needs to align the Capsule's default
axis (+y) with that local axis. The minimal axis-angle that does this:
rotation_axis = (sin(θ), 0, -cos(θ)) (unit)
rotation_angle = π/4 for +45° tilt, 3π/4 for -45° tilt
"""
import math
WHEEL_NAMES = {
# Tilt sign refers to roller-axis tilt direction relative to the wheel
# rotation axis (body +y). X-pattern requires rollers on each wheel to
# tilt INWARD toward the body centre. For a wheel at +y body coord, that
# means tilting toward -y; for a wheel at -y, tilting toward +y.
"fr": ("front right", +0.14, -0.14, +1), # +1 = +45° tilt (toward +y, inward)
"fl": ("front left", +0.14, +0.14, -1), # -1 = -45° tilt (toward -y, inward)
"rr": ("rear right", -0.14, -0.14, -1), # -1 (toward -y, "outward"...
"rl": ("rear left", -0.14, +0.14, +1), # +1 (toward +y, "outward"...
# ...for the rear pair the X-pattern flips so diagonal pairs FL+RR have
# SAME tilt direction in body frame, FR+RL the other. The signs above
# encode that: FR/RL both +1, FL/RR both -1.
}
R_ROLLER_OFFSET = 0.031 # roller-centre distance from wheel hub centre
R_ROLLER_RADIUS = 0.007
R_ROLLER_HEIGHT = 0.020
ROLLER_MASS = 0.003
HUB_RADIUS = 0.020 # < R_ROLLER_OFFSET - R_ROLLER_RADIUS so hub doesn't touch
HUB_HEIGHT = 0.022
HUB_MASS = 0.045
N_ROLLERS = 8
def wheel_block(key):
name, ax, ay, tilt_sign = WHEEL_NAMES[key]
contact_mat = "MecanumWheelA" if tilt_sign > 0 else "MecanumWheelB"
safe = name.replace(" ", "_").upper()
rollers = []
for k in range(N_ROLLERS):
theta = 2.0 * math.pi * k / N_ROLLERS
s, c = math.sin(theta), math.cos(theta)
# Mount position in wheel-local frame.
mx = R_ROLLER_OFFSET * s
my = 0.0
mz = -R_ROLLER_OFFSET * c
# Hinge axis in wheel-local frame.
ax_l = c / math.sqrt(2.0)
ay_l = tilt_sign / math.sqrt(2.0)
az_l = s / math.sqrt(2.0)
# Rotation that maps Capsule default axis (0,1,0) to (ax_l, ay_l, az_l).
rot_axis = (s, 0.0, -c)
rot_angle = math.pi / 4.0 if tilt_sign > 0 else 3.0 * math.pi / 4.0
rollers.append(f"""\
# Mecanum roller {k+1} (θ={math.degrees(theta):.0f}°)
HingeJoint {{
jointParameters HingeJointParameters {{
axis {ax_l:.6f} {ay_l:.6f} {az_l:.6f}
anchor {mx:.6f} {my:.6f} {mz:.6f}
}}
endPoint Solid {{
translation {mx:.6f} {my:.6f} {mz:.6f}
rotation {rot_axis[0]:.6f} {rot_axis[1]:.6f} {rot_axis[2]:.6f} {rot_angle:.6f}
children [
Shape {{
appearance PBRAppearance {{
baseColor 0.12 0.12 0.12
roughness 0.7
metalness 0.1
}}
geometry Capsule {{
height {R_ROLLER_HEIGHT}
radius {R_ROLLER_RADIUS}
subdivision 8
}}
}}
]
name "{name} roller {k+1}"
contactMaterial "{contact_mat}"
boundingObject Capsule {{
height {R_ROLLER_HEIGHT}
radius {R_ROLLER_RADIUS}
subdivision 8
}}
physics Physics {{
density -1
mass {ROLLER_MASS}
centerOfMass [
0 0 0
]
}}
}}
}}""")
rollers_str = "\n".join(rollers)
return f"""\
# ========== {name.upper()} WHEEL ==========
DEF {safe}_WHEEL_JOINT HingeJoint {{
jointParameters HingeJointParameters {{
axis 0 1 0
anchor {ax} {ay} 0.038
}}
device [
RotationalMotor {{
name "{name} wheel motor"
maxVelocity 70.0
maxTorque 20.0
}}
PositionSensor {{
name "{name} wheel sensor"
resolution 0.00628
}}
]
endPoint Solid {{
translation {ax} {ay} 0.038
rotation 0 -1 0 1.570796
children [
# Visual hub only — the rollers below provide ground contact.
Pose {{
rotation 1 0 0 -1.5708
children [
Shape {{
appearance PBRAppearance {{
baseColor 0.5 0.5 0.5
roughness 0.3
metalness 0.7
}}
geometry Cylinder {{
height 0.018
radius {HUB_RADIUS - 0.002}
subdivision 16
}}
}}
Shape {{
appearance PBRAppearance {{
baseColor 0.6 0.6 0.6
roughness 0.2
metalness 0.8
}}
geometry Cylinder {{
height 0.022
radius 0.008
subdivision 8
}}
}}
]
}}
{rollers_str}
]
name "{name} wheel"
boundingObject Pose {{
rotation 1 0 0 -1.5708
children [
Cylinder {{
height {HUB_HEIGHT}
radius {HUB_RADIUS}
}}
]
}}
physics Physics {{
density -1
mass {HUB_MASS}
centerOfMass [
0 0 0
]
}}
}}
}}"""
if __name__ == "__main__":
for k in ("fr", "fl", "rr", "rl"):
print(wheel_block(k))
print()
training/herding_env.py
+8
View File
@@ -296,6 +296,12 @@ class HerdingEnv(gym.Env):
k_turn=4.0,
wheel_base=DOG_WHEEL_BASE,
)
robot_cfg = (self._herding_cfg.robot
if self._herding_cfg is not None else None)
strafe_efficiency = (robot_cfg.strafe_efficiency
if robot_cfg is not None else 1.0)
strafe_bleed = (robot_cfg.strafe_to_forward_bleed
if robot_cfg is not None else 0.0)
self.dog_x, self.dog_y, self.dog_heading = mecanum_kinematics_step(
self.dog_x, self.dog_y, self.dog_heading,
w_fl, w_fr, w_rl, w_rr,
@@ -304,6 +310,8 @@ class HerdingEnv(gym.Env):
WEBOTS_DT,
slip_std=slip_std,
rng=self._np_rng_lidar,
strafe_efficiency=strafe_efficiency,
strafe_to_forward_bleed=strafe_bleed,
)
else:
wL, wR = velocity_to_wheels(