From ee77c8606c18288841627da7eb5d14b9d476a215 Mon Sep 17 00:00:00 2001 From: Johnny Fernandes Date: Sun, 17 May 2026 01:09:47 +0000 Subject: [PATCH] Gym mecanum kinematics matching to Webots roller-hinge proto MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 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 --- herding/config.py | 54 ++++++++++ herding/world/diffdrive.py | 26 ++++- tests/test_config.py | 18 ++++ tests/test_diffdrive.py | 39 +++++++ tools/gen_mecanum_wheels.py | 210 ++++++++++++++++++++++++++++++++++++ training/herding_env.py | 8 ++ 6 files changed, 353 insertions(+), 2 deletions(-) create mode 100644 tools/gen_mecanum_wheels.py diff --git a/herding/config.py b/herding/config.py index e27f882..bdf2feb 100644 --- a/herding/config.py +++ b/herding/config.py @@ -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: diff --git a/herding/world/diffdrive.py b/herding/world/diffdrive.py index a454d11..6f4e8d8 100644 --- a/herding/world/diffdrive.py +++ b/herding/world/diffdrive.py @@ -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) diff --git a/tests/test_config.py b/tests/test_config.py index f80c447..59e717a 100644 --- a/tests/test_config.py +++ b/tests/test_config.py @@ -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 diff --git a/tests/test_diffdrive.py b/tests/test_diffdrive.py index 6649cf1..27816ac 100644 --- a/tests/test_diffdrive.py +++ b/tests/test_diffdrive.py @@ -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. diff --git a/tools/gen_mecanum_wheels.py b/tools/gen_mecanum_wheels.py new file mode 100644 index 0000000..f236bd0 --- /dev/null +++ b/tools/gen_mecanum_wheels.py @@ -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() diff --git a/training/herding_env.py b/training/herding_env.py index 9dec733..7719e5c 100644 --- a/training/herding_env.py +++ b/training/herding_env.py @@ -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(