fix(hangar_sim): use the hull rolling radius for the mecanum wheels

[bkanator]

What Changed

• src/hangar_sim/config/control/picknik_ur.ros2_control.yaml: kinematics.wheels_radius 0.0666 → 0.0756 in both mecanum controller blocks (platform_velocity_controller and platform_velocity_controller_nav2), each with a comment naming which of the wheel's three radii this is — the hull rolling radius, perimeter/2*pi = 75.630 mm — so it is not "corrected" to the 75.9002 mm sphere peak or the 75.0889 mm static ride height. The same constant feeds the drive side (1.0/wheels_radius), so the base was also driving fast; vx_max: 0.5 now means 0.5 m/s.
• src/hangar_sim/description/hangar_scene.xml: rewrote the one stale clause in the wheel-geometry comment that said the controller value "still reads 0.0666 … and is corrected on feat/19667-fuse-odometry" (that branch is closed) to state that the controllers now use the hull rolling radius. The derivation itself is unchanged.
• src/hangar_sim/test/test_base_geometry.py: added _rolling_radius() (hull perimeter = N external tangents 2*a*sin(pi/N) plus one full turn of radius r) and two tests — each mecanum controller's configured wheels_radius matches the computed rolling radius to 0.1 mm, and the two controllers agree with each other so switching between them cannot change base speed.

Measurements

Driven in an isolated hangar_sim instance via direct cmd_vel_unstamped, 14.1 s steady-state window, sim reset to the default keyframe before each run, with 0.0666 re-run as a control:

wheels_radius	true speed (commanded 0.5)	wheel odom vs ground truth
0.0666 (before)	0.5401 m/s (+8.0%)	−8.7%
0.0756 (after)	0.4737 m/s (−5.3%)	+4.1%

The +4.1% in that table is the live-stack figure, measured on the running ROS 2 stack through the controller's own ~/odom; the +0.4% in the standalone-MuJoCo transcript under Testing below is the same experiment reading MuJoCo's wheel velocity directly instead of the picknik_mujoco_ros velocity state interface, which is exactly why the two differ (see the first bullet under "Two things to know").

Direct geometric measurement (ground-truth distance ÷ wheel rotation from /joint_states positions) gives an effective rolling radius of 0.07506 m @ 7.20 rad/s, 0.07513 m @ 6.30 rad/s, 0.07525 m @ 2.51 rad/s — within 0.7% of the 75.630 mm hull value, 12.7% away from 0.0666, and stable across the operating range. That independently confirms the hangar_scene.xml derivation and the standalone-MuJoCo measurement from #790 (0.07564 m at 1 rad/s). A nav2 NavigateToPose goal 6 m out, with the controllers switched the way navigate_to_clicked_point.xml switches them, SUCCEEDED in 27.4 s, ending 0.39 m from goal in ground truth.

Two things to know

• The residual odometry error is +4.1%, not the ~1% Add fuse odometry localization for the hangar_sim mobile base #790's evidence predicts — that is a separate defect, not the radius. The controller integrates the wheel velocity state interface, and that interface reads about 3% above the true rate of rotation derived from the wheel positions over the same window (6.526 rad/s vs 6.305 rad/s). The odometry therefore inherits a ~3–4% inflation regardless of the configured radius, which is why the error does not simply scale with it (0.0756/0.0751 − 1 would be +0.7%). This PR moves the error from −8.7% to +4.1% and puts the constant on its physically correct value; the residual belongs with the odometry work. Do not tune 0.0756 to absorb it — 0.0756 is the measured hull rolling radius, and burying a separate defect inside a geometry constant is the mistake this PR exists to fix.
• The green integration suite is not evidence that the speed change is safe. hangar_sim's objective suite passes on this branch (18 passed, 55 skipped, 0 failed, 6 min 24 s), but no objective it actually executes drives the base: Navigate to Clicked Point and Navigate to Clicked Point with Replanning are skipped headless (they need GetPoseFromUser), and the one objective that activates platform_velocity_controller (move_boxes_to_loading_zone_from_waypoint.xml) is runnable=false, its runnable parent ML Move Boxes to Loading Zone is in the CI skip set, and that subtree plans on the manipulator group anyway. The direct measurements and the manual nav2 goal above are the evidence.

Deliberately out of scope

Each has its own PR coming: the dead beluga beam-skip parameters in nav2_params.yaml (untouched here), merging the two mecanum controller instances, the odom → world drift publisher, the beluga retune, and the docs. Every drift-related localization constant downstream is fitted to the wrong radius, so this had to land before any of them are retuned.

Risk Assessment

✅ Low: The change is two identical config constants plus clarifying comments, the new value is independently re-derivable from the documented roller geometry (r + N·a·sin(π/N)/π = 75.62 mm ≈ 0.0756 m), no other copies of the stale 0.0666 exist in the repo, and the diff matches the stated intent exactly with no out-of-scope edits.

Testing

Verified the change end-to-end by driving the real hangar_sim MuJoCo scene headless with the vendored mecanum controller's own IK/FK: at a commanded 0.5 m/s the base now travels 0.4914 m/s (-1.7%) instead of 0.5570 m/s (+11.4%), and wheel odometry error drops from -11.5% to +0.4%, while the physics-measured effective rolling radius (0.0753 m) independently confirms the 75.630 mm hull derivation and is 13% away from the old 0.0666. I added a focused regression test to the existing base-geometry suite that ties each controller block's kinematics.wheels_radius to the rolling radius computed from the MJCF roller spheres at 0.1 mm (tight enough to reject both the peak and the ride-height radius); the suite is 13 passed with the fix and fails on both controller blocks with 0.0666 temporarily restored. Artifacts are a before/after speed and odometry chart, a drive GIF, and the measurement and regression transcripts. One nuance supporting PR-body point (a): my in-sim odometry error is +0.4% rather than the +4.1% seen on the live stack because I read MuJoCo's wheel velocity directly rather than the picknik_mujoco_ros velocity state interface, which corroborates that the residual inflation belongs to that interface and not to the radius; the ROS/MoveIt Pro stack and the objectives integration suite could not be run locally (controller_interface and the sim plugin are not installed in this environment), and per the author's own analysis that suite exercises no base motion anyway.

• Evidence: Before/after: ground-truth travel at commanded 0.5 m/s and wheel-odometry error (local file: /home/breelynk/.no-mistakes/evidence/01M21Y7RHBREEV6QXMDHAEWPZG/rolling_radius_before_after.png)
• Evidence: Base driving in the hangar scene, left = 0.0666 (before), right = 0.0756 (after), same 12 s and fixed camera (local file: /home/breelynk/.no-mistakes/evidence/01M21Y7RHBREEV6QXMDHAEWPZG/base_drive_before_after.gif)

Evidence: MuJoCo measurement transcript

roller ring: N=20 spheres, r=10.000 mm, a=65.9002081 mm peak a+r = 75.9002 mm ride hgt a*cos(pi/N)+r= 75.0889 mm ROLLING perim/2pi = 75.6295 mm config wheels_radius: {'platform_velocity_controller': 0.0756, 'platform_velocity_controller_nav2': 0.0756} === before (0.0666) === commanded vx : 0.500 m/s wheel command : 7.5075 rad/s TRUE ground speed : 0.5570 m/s (+11.4% vs commanded) wheel odometry distance: 7.3901 m vs ground truth 8.3510 m (-11.5%) measured rolling radius: 0.07526 m (window 15.0 s) === after (0.0756) === commanded vx : 0.500 m/s wheel command : 6.6138 rad/s TRUE ground speed : 0.4914 m/s (-1.7% vs commanded) wheel odometry distance: 7.3959 m vs ground truth 7.3673 m (+0.4%) measured rolling radius: 0.07531 m (window 15.0 s)

roller ring: N=20 spheres, r=10.000 mm, a=65.9002081 mm
  peak      a+r          = 75.9002 mm
  ride hgt  a*cos(pi/N)+r= 75.0889 mm
  ROLLING   perim/2pi    = 75.6295 mm
config wheels_radius: {'platform_velocity_controller': 0.0756, 'platform_velocity_controller_nav2': 0.0756}

=== before (0.0666) ===
  commanded vx           : 0.500 m/s
  wheel command          : 7.5075 rad/s
  TRUE ground speed      : 0.5570 m/s (+11.4% vs commanded)
  wheel odometry distance: 7.3901 m vs ground truth 8.3510 m (-11.5%)
  wheel vel state        : 7.4015 rad/s, from positions 7.4015 rad/s (+0.0%)
  measured rolling radius: 0.07526 m (window 15.0 s)

=== after  (0.0756) ===
  commanded vx           : 0.500 m/s
  wheel command          : 6.6138 rad/s
  TRUE ground speed      : 0.4914 m/s (-1.7% vs commanded)
  wheel odometry distance: 7.3959 m vs ground truth 7.3673 m (+0.4%)
  wheel vel state        : 6.5254 rad/s, from positions 6.5254 rad/s (+0.0%)
  measured rolling radius: 0.07531 m (window 15.0 s)

Evidence: New regression test: fails at 0.0666, passes at 0.0756

### with the fix (kinematics.wheels_radius: 0.0756) 13 passed in 0.09s ### with the pre-fix value restored (kinematics.wheels_radius: 0.0666) E AssertionError: platform_velocity_controller uses kinematics.wheels_radius = 0.0666 m, but the roller hull rolls at 0.075630 m. The other two radii the ring produces — the 0.075900 m peak and the 0.075089 m static ride height — are not interchangeable with it: odometry and the drive-side 1/wheels_radius both scale directly with this number. 2 failed, 11 deselected in 0.07s

\### with the fix (kinematics.wheels_radius: 0.0756)
                                                            [100%]
13 passed in 0.09s

\### with the pre-fix value restored (kinematics.wheels_radius: 0.0666)
>       assert configured == pytest.approx(_rolling_radius(), abs=1e-4), (
E       AssertionError: platform_velocity_controller uses kinematics.wheels_radius = 0.0666 m, but the roller hull rolls at 0.075630 m. The other two radii the ring produces — the 0.075900 m peak and the 0.075089 m static ride height — are not interchangeable with it: odometry and the drive-side 1/wheels_radius both scale directly with this number.
E       assert 0.0666 == 0.07562953839823965 ± 1.0e-04
E         comparison failed
E         Obtained: 0.0666
E         Expected: 0.07562953839823965 ± 1.0e-04
>       assert configured == pytest.approx(_rolling_radius(), abs=1e-4), (
E       AssertionError: platform_velocity_controller_nav2 uses kinematics.wheels_radius = 0.0666 m, but the roller hull rolls at 0.075630 m. The other two radii the ring produces — the 0.075900 m peak and the 0.075089 m static ride height — are not interchangeable with it: odometry and the drive-side 1/wheels_radius both scale directly with this number.
E       assert 0.0666 == 0.07562953839823965 ± 1.0e-04
E         comparison failed
E         Obtained: 0.0666
E         Expected: 0.07562953839823965 ± 1.0e-04
2 failed, 11 deselected in 0.07s

Evidence: Measurement script (MuJoCo drive using the vendored controller IK/FK)

"""Drive the real hangar_sim MuJoCo base at a commanded 0.5 m/s and measure it.

Wheel commands come from the vendored clearpath_mecanum_drive_controller IK
(w = 1/wheels_radius * (vx -/+ vy -/+ L*wz)), with wheels_radius read out of the
real picknik_ur.ros2_control.yaml. Odometry comes from that controller's FK
(0.25 * r * sum(wheel velocities)), fed with the wheel VELOCITY state, exactly
as the controller does. Ground truth is the base slide joint in MuJoCo.
"""
import math, re, sys, json
from pathlib import Path
import numpy as np
import mujoco

WS = Path("/home/breelynk/.no-mistakes/worktrees/a3f4f2625ba0/01M21Y7RHBREEV6QXMDHAEWPZG")
DESC = WS / "src/hangar_sim/description"
YAML = WS / "src/hangar_sim/config/control/picknik_ur.ros2_control.yaml"
WHEELS = ["front_left_wheel", "rear_left_wheel", "rear_right_wheel", "front_right_wheel"]

def yaml_radii():
    """wheels_radius per controller block, straight out of the deployed config."""
    out, block = {}, None
    for line in YAML.read_text().splitlines():
        m = re.match(r"^(\S+):\s*$", line)
        if m:
            block = m.group(1)
        m = re.search(r"kinematics\.wheels_radius:\s*([0-9.]+)", line)
        if m:
            out[block] = float(m.group(1))
    return out

def hull_rolling_radius():
    """perimeter/2*pi of the roller-sphere hull, computed from the MJCF spheres."""
    text = (DESC / "front_left_wheel_link.xml").read_text()
    r = {float(v) for v in re.findall(r'<geom\s+size="([-0-9.eE]+)"', text)}
    assert len(r) == 1
    r = r.pop()
    centres = [(float(x), float(z)) for x, _, z in
               re.findall(r'pos="([-0-9.eE]+)\s+([-0-9.eE]+)\s+([-0-9.eE]+)"', text)]
    centres = [c for c in centres if math.hypot(*c) > 1e-9]
    a = max(math.hypot(*c) for c in centres)
    n = len(centres)
    # Convex hull of N equal circles of radius r on a ring of radius a: N common
    # external tangent segments (each parallel to a centre-polygon edge, so the same
    # length) plus arcs that together sweep a full turn.
    perimeter = n * 2 * a * math.sin(math.pi / n) + 2 * math.pi * r
    return perimeter / (2 * math.pi), a, r, n

def run(radius, vx=0.5, duration=20.0, settle=5.0):
    m = mujoco.MjModel.from_xml_path(str(DESC / "hangar_scene.xml"))
    d = mujoco.MjData(m)
    mujoco.mj_resetDataKeyframe(m, d, 0)          # the 'default' keyframe
    mujoco.mj_forward(m, d)
    aid = {w: mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_ACTUATOR, w) for w in WHEELS}
    jid = {w: mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_JOINT, w) for w in WHEELS}
    qadr = {w: m.jnt_qposadr[jid[w]] for w in WHEELS}
    vadr = {w: m.jnt_dofadr[jid[w]] for w in WHEELS}
    bx = m.jnt_qposadr[mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_JOINT, "linear_x_joint")]
    by = m.jnt_qposadr[mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_JOINT, "linear_y_joint")]

    w_cmd = vx / radius                            # controller IK, pure +x
    for w in WHEELS:
        d.ctrl[aid[w]] = w_cmd

    dt = m.opt.timestep
    n = int(duration / dt)
    log = {"t": [], "x": [], "y": [], "wpos": [], "wvel": [], "odom": []}
    odom = 0.0
    for i in range(n):
        mujoco.mj_step(m, d)
        wv = np.array([d.qvel[vadr[w]] for w in WHEELS])
        # controller FK on the wheel velocity state interface
        odom += 0.25 * radius * wv.sum() * dt
        log["t"].append(d.time); log["x"].append(d.qpos[bx]); log["y"].append(d.qpos[by])
        log["wpos"].append([d.qpos[qadr[w]] for w in WHEELS])
        log["wvel"].append(wv.tolist()); log["odom"].append(odom)

    t = np.array(log["t"]); x = np.array(log["x"]); y = np.array(log["y"])
    wpos = np.array(log["wpos"]); wvel = np.array(log["wvel"]); od = np.array(log["odom"])
    i0 = int(settle / dt); i1 = n - 1
    win = t[i1] - t[i0]
    dist = math.hypot(x[i1] - x[i0], y[i1] - y[i0])
    rot = (wpos[i1] - wpos[i0]).mean()
    return {
        "radius": radius, "cmd_vx": vx, "wheel_cmd_rad_s": w_cmd,
        "window_s": win,
        "true_speed": dist / win,
        "true_dist": dist,
        "odom_dist": od[i1] - od[i0],
        "mean_wheel_vel_state": wvel[i0:i1].mean(),
        "mean_wheel_vel_from_pos": rot / win,
        "effective_rolling_radius": dist / rot,
        "log": {"t": t.tolist(), "x": x.tolist(), "odom": od.tolist()},
    }

if __name__ == "__main__":
    cfg = yaml_radii()
    roll, a, r, n = hull_rolling_radius()
    print(f"roller ring: N={n} spheres, r={r*1000:.3f} mm, a={a*1000:.7f} mm")
    print(f"  peak      a+r          = {(a+r)*1000:.4f} mm")
    print(f"  ride hgt  a*cos(pi/N)+r= {(a*math.cos(math.pi/n)+r)*1000:.4f} mm")
    print(f"  ROLLING   perim/2pi    = {roll*1000:.4f} mm")
    print(f"config wheels_radius: {cfg}")
    print()
    results = {}
    for label, radius in (("before (0.0666)", 0.0666), ("after  (0.0756)", 0.0756)):
        res = run(radius)
        results[label] = res
        print(f"=== {label} ===")
        print(f"  commanded vx           : {res['cmd_vx']:.3f} m/s")
        print(f"  wheel command          : {res['wheel_cmd_rad_s']:.4f} rad/s")
        print(f"  TRUE ground speed      : {res['true_speed']:.4f} m/s "
              f"({100*(res['true_speed']/res['cmd_vx']-1):+.1f}% vs commanded)")
        print(f"  wheel odometry distance: {res['odom_dist']:.4f} m vs ground truth "
              f"{res['true_dist']:.4f} m ({100*(res['odom_dist']/res['true_dist']-1):+.1f}%)")
        print(f"  wheel vel state        : {res['mean_wheel_vel_state']:.4f} rad/s, "
              f"from positions {res['mean_wheel_vel_from_pos']:.4f} rad/s "
              f"({100*(res['mean_wheel_vel_state']/res['mean_wheel_vel_from_pos']-1):+.1f}%)")
        print(f"  measured rolling radius: {res['effective_rolling_radius']:.5f} m "
              f"(window {res['window_s']:.1f} s)")
        print()
    json.dump({k: {kk: vv for kk, vv in v.items()} for k, v in results.items()},
              open(sys.argv[1] if len(sys.argv) > 1 else "/dev/null", "w"))

Pipeline

Updates from git push no-mistakes

✅ **intent** - passed

✅ No issues found.

✅ **Rebase** - passed

✅ No issues found.

✅ **Review** - passed

✅ No issues found.

✅ **Test** - passed

✅ No issues found.

• python3 -m pytest src/hangar_sim/test/test_base_geometry.py — 13 passed (10 pre-existing + 3 new)
• pytest -k wheels_radius_is_the_rolling with kinematics.wheels_radius temporarily reverted to 0.0666 — 2 failed, confirming the new test is a real regression guard (yaml restored afterwards, git status clean apart from the test file)
• Headless MuJoCo drive of src/hangar_sim/description/hangar_scene.xml at commanded vx = 0.5 m/s using the vendored clearpath_mecanum_drive_controller IK/FK, with the radius read from the real picknik_ur.ros2_control.yaml, for both 0.0666 and 0.0756 (drive_base_experiment.py)
• Geometric recomputation of the three roller radii from the actual sphere positions in front_left_wheel_link.xml (peak 75.9002 mm / ride height 75.0889 mm / rolling 75.6295 mm)
• Rendered a fixed-camera before/after GIF of the base driving in the hangar scene (render2.py)
• git diff cd5c8ff 1a57245 — verified scope: only the two intended files, nav2_params.yaml untouched

⚠️ **Document** - 1 info

• ℹ️ src/hangar_sim/config/control/picknik_ur.ros2_control.yaml:49 - Follow-up (deliberately not done here, per the intent's scope constraint): the new pointer comments beside each kinematics.wheels_radius restate two derived numbers owned by hangar_scene.xml (the 75.9002 mm sphere-peak radius and the 75.0889 mm static ride height), giving four copies across the two controller blocks. They correctly point at the owner comment and are load-bearing for their stated purpose (stopping the next reader from swapping in one of the other two radii), so they are worth keeping as written for this change. But if the roller geometry ever changes (N spheres or r), those copies go stale silently while the owner comment and test_base_geometry.py update. A follow-up could reduce each yaml comment to the label plus the pointer, dropping the two duplicated figures and leaving only the rolling radius named at its use site.

✅ **Lint** - passed

✅ No issues found.

✅ **Push** - passed

✅ No issues found.

[coderabbitai]

No actionable comments were generated in the recent review. 🎉

ℹ️ Recent review info

⚙️ Run configuration

Configuration used: Organization UI

Review profile: CHILL

Plan: Team

Run ID: 03128482-4f5f-44e6-94fc-88810d688887

📥 Commits

Reviewing files that changed from the base of the PR and between f43b28a and c2d7da2.

📒 Files selected for processing (3)

• src/hangar_sim/config/control/picknik_ur.ros2_control.yaml
• src/hangar_sim/description/hangar_scene.xml
• src/hangar_sim/test/test_base_geometry.py

Included review availability: 7 reviews are currently available. Your included PR review attempts over the past 7 days set your current allowance at 10 reviews per hour.

---

📝 Summary

Summary by CodeRabbit

• Bug Fixes

  • Corrected the platform’s wheel rolling-radius configuration to 0.0756, improving controller kinematic accuracy and motion simulation.
  • Synchronized both mecanum velocity controllers to use the same rolling radius.

• Documentation

  • Updated hangar scene documentation to reflect the controller’s hull-based rolling-radius calculation and configured value.

• Tests

  • Added validation to ensure controller settings match the calculated wheel rolling radius and remain consistent across both controllers.

Walkthrough

Changes

The pull request changes both mecanum controller wheel-radius values from 0.0666 to 0.0756. It documents the hull-perimeter basis and adds tests for calculated radius matching and controller consistency.

Mecanum rolling-radius alignment

Layer / File(s)	Summary
Configure the hull rolling radius src/hangar_sim/config/control/picknik_ur.ros2_control.yaml, src/hangar_sim/description/hangar_scene.xml	Both controller configurations use 0.0756. The scene documentation records the 75.630 mm hull-perimeter rolling radius.
Validate controller radius src/hangar_sim/test/test_base_geometry.py	Tests calculate the hull rolling radius, read both controller values from YAML, require a match within 0.1 mm, and require identical controller values.

Suggested reviewers: l4co77

Priority: ⬇️ Low

Merge Risk: ⚪ Minimal · up to c2d7d

Both mecanum controllers now use the hull rolling radius, with geometry checks keeping their configuration aligned. No current merge-blocking risk is identified.

🚥 Pre-merge checks | ✅ 4

✅ Passed checks (4 passed)

Check name	Status	Explanation
Linked Issues check	✅ Passed	Check skipped because no linked issues were found for this pull request.
Out of Scope Changes check	✅ Passed	Check skipped because no linked issues were found for this pull request.
Human Review Check	✅ Passed	PASS: The PR changes only two mecanum simulation configuration values/comments and one geometry regression test under src/hangar_sim. The verified diff has no auth, permissions, secrets, encryption,…
Description check	✅ Passed	The pull request description directly explains the wheel-radius configuration changes, scene documentation update, regression tests, measurements, and scope. It matches the provided changeset.

---

_{Comment @coderabbitai help to get the list of available commands.}

[github-actions]

MoveIt Pro Example WS - Objectives Integration Test Report

• lab_sim
  • ✅ jazzy: view HTML report — 32 passed
• hangar_sim
  • ✅ jazzy: view HTML report — 18 passed