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>

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()