From 1a57245e7ec08e44d2b0da8e196e181abf7e3103 Mon Sep 17 00:00:00 2001 From: Breelyn Styler Date: Tue, 8 Sep 2026 21:56:24 -0400 Subject: [PATCH 1/2] fix(hangar_sim): use the hull rolling radius for the mecanum wheels `kinematics.wheels_radius` was 0.0666 m in both mecanum controller blocks, against a measured effective rolling radius of ~0.0751 m for the rounded wheels of #875. The constant feeds both the odometry forward kinematics and the drive-side `1.0/wheels_radius`, so wheel odometry under-reported distance and the base drove correspondingly fast. Set both blocks to 0.0756 m -- the hull rolling radius (perimeter/2*pi) that `hangar_scene.xml` already derives -- and name it in a comment beside each value so it is not "fixed" to one of the other two radii the same geometry produces (the 75.9002 mm sphere peak or the 75.0889 mm static ride height). Also rewrite the clause in `hangar_scene.xml` that said the controller value "still reads 0.0666 ... and is corrected on feat/19667-fuse-odometry"; that branch is being closed and the correction is here. Measured in the running hangar_sim stack (MuJoCo ground truth `/odom` vs the controller's `~/odom`, 14 s steady-state window, commanded vx = 0.5 m/s): 0.0666: true speed 0.5401 m/s (+8.0% vs command), odometry -8.7% vs truth 0.0756: true speed 0.4737 m/s (-5.3% vs command), odometry +4.1% vs truth Ground-truth distance over wheel rotation gives an effective rolling radius of 0.07506-0.07537 m across 2.5-7.2 rad/s -- within 0.7% of the hull value and 12.7% away from 0.0666. Co-Authored-By: Claude Opus 5 (1M context) Claude-Session: https://claude.ai/code/session_01SuGutcPeG3RsF4UqHt1XPe --- .../config/control/picknik_ur.ros2_control.yaml | 10 ++++++++-- src/hangar_sim/description/hangar_scene.xml | 7 +++---- 2 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml index 4eee1a986..f563c8dab 100644 --- a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml +++ b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml @@ -46,7 +46,10 @@ platform_velocity_controller: rear_right_wheel_command_joint_name: "rear_right_wheel" rear_left_wheel_command_joint_name: "rear_left_wheel" - kinematics.wheels_radius: 0.0666 + # Hull rolling radius (perimeter/2*pi = 75.630 mm), NOT the sphere-peak + # radius (75.9002 mm) or the static ride height (75.0889 mm). See the + # wheel-geometry comment in hangar_sim/description/hangar_scene.xml. + kinematics.wheels_radius: 0.0756 kinematics.sum_of_robot_center_projection_on_X_Y_axis: 0.59 wheel_separation_multiplier: 1.0 wheel_radius_multiplier: 1.0 @@ -98,7 +101,10 @@ platform_velocity_controller_nav2: rear_right_wheel_command_joint_name: "rear_right_wheel" rear_left_wheel_command_joint_name: "rear_left_wheel" - kinematics.wheels_radius: 0.0666 + # Hull rolling radius (perimeter/2*pi = 75.630 mm), NOT the sphere-peak + # radius (75.9002 mm) or the static ride height (75.0889 mm). See the + # wheel-geometry comment in hangar_sim/description/hangar_scene.xml. + kinematics.wheels_radius: 0.0756 kinematics.sum_of_robot_center_projection_on_X_Y_axis: 0.59 wheel_separation_multiplier: 1.0 wheel_radius_multiplier: 1.0 diff --git a/src/hangar_sim/description/hangar_scene.xml b/src/hangar_sim/description/hangar_scene.xml index e8950f00f..42e1c837c 100644 --- a/src/hangar_sim/description/hangar_scene.xml +++ b/src/hangar_sim/description/hangar_scene.xml @@ -17,10 +17,9 @@ a + r = 75.9002 mm peak, reached only at each sphere a*cos(pi/N) + r = 75.0889 mm between spheres; the static ride height, which ur5e_ridgeback.xacro anchors to - hull perimeter/2pi = 75.630 mm rolling radius, for the controllers' - kinematics.wheels_radius — which still reads - 0.0666 in picknik_ur.ros2_control.yaml and is - corrected on feat/19667-fuse-odometry + hull perimeter/2pi = 75.630 mm rolling radius, which is what the + controllers use for kinematics.wheels_radius + (0.0756 in picknik_ur.ros2_control.yaml) The 0.811 mm ripple between the first two is inherent to approximating a circle with N spheres — a*(1-cos(pi/N)) — and sizing them tangent would not remove it. They are not tangent as built: tangency wants r = 10.309 mm, so From c2d7da2cd851312619a56b1fed800b8ebf465a9b Mon Sep 17 00:00:00 2001 From: Breelyn Styler Date: Tue, 8 Sep 2026 22:07:59 -0400 Subject: [PATCH 2/2] no-mistakes(document): verify radius docs are current; apply black formatting --- src/hangar_sim/test/test_base_geometry.py | 66 +++++++++++++++++++++++ 1 file changed, 66 insertions(+) diff --git a/src/hangar_sim/test/test_base_geometry.py b/src/hangar_sim/test/test_base_geometry.py index e98de2292..59eed7b0a 100644 --- a/src/hangar_sim/test/test_base_geometry.py +++ b/src/hangar_sim/test/test_base_geometry.py @@ -49,11 +49,24 @@ from pathlib import Path import pytest +import yaml from ament_index_python.packages import get_package_share_directory DESCRIPTION = Path(__file__).resolve().parent.parent / "description" MJCF = DESCRIPTION / "ur5e_ridgeback.xml" XACRO = DESCRIPTION / "ur5e_ridgeback.xacro" +CONTROL_YAML = ( + Path(__file__).resolve().parent.parent + / "config" + / "control" + / "picknik_ur.ros2_control.yaml" +) + +# Both mecanum controller instances drive the same four wheels. +MECANUM_CONTROLLERS = ( + "platform_velocity_controller", + "platform_velocity_controller_nav2", +) RIDGEBACK_XACRO = ( Path(get_package_share_directory("ridgeback_description")) / "urdf" @@ -172,6 +185,59 @@ def _static_ride_height() -> float: return ring * math.cos(math.pi / count) + radius +def _rolling_radius() -> float: + """Radius of a circle with the roller hull's perimeter — the rolling radius. + + One turn of the wheel lays down the hull's perimeter on the floor, so wheel + odometry and the drive-side `1/wheels_radius` both need `perimeter / 2pi`, + which is neither of the other two radii the ring produces. The hull of N equal + circles of radius `r` centred on a ring of radius `a` is N common external + tangents — each as long as a centre-polygon edge, `2*a*sin(pi/N)` — joined by + arcs that together sweep one full turn of radius `r`. + """ + ring, radius, count = _roller_ring() + perimeter = count * 2 * ring * math.sin(math.pi / count) + 2 * math.pi * radius + return perimeter / (2 * math.pi) + + +def _configured_wheels_radius(controller: str) -> float: + parameters = yaml.safe_load(CONTROL_YAML.read_text())[controller]["ros__parameters"] + radius = parameters.get("kinematics.wheels_radius") + assert ( + radius is not None + ), f"{controller} has no kinematics.wheels_radius in {CONTROL_YAML.name}" + return float(radius) + + +@pytest.mark.parametrize("controller", MECANUM_CONTROLLERS) +def test_controller_wheels_radius_is_the_rolling_radius(controller: str) -> None: + """The one radius of the three that wheel odometry and the drive IK may use. + + Held to 0.1 mm: tight enough to tell the rolling radius apart from the peak + (0.27 mm above it) and from the static ride height (0.54 mm below it), loose + enough for the value to stay written to four decimals. + """ + ring, radius, count = _roller_ring() + configured = _configured_wheels_radius(controller) + assert configured == pytest.approx(_rolling_radius(), abs=1e-4), ( + f"{controller} uses kinematics.wheels_radius = {configured:.4f} m, but the " + f"roller hull rolls at {_rolling_radius():.6f} m. The other two radii the " + f"ring produces — the {ring + radius:.6f} m peak and the " + f"{ring * math.cos(math.pi / count) + radius:.6f} m static ride height — are " + f"not interchangeable with it: odometry and the drive-side 1/wheels_radius " + f"both scale directly with this number." + ) + + +def test_both_mecanum_controllers_agree_on_the_wheels() -> None: + """They swap in for each other at runtime, so the base must not change speed.""" + radii = {c: _configured_wheels_radius(c) for c in MECANUM_CONTROLLERS} + assert len(set(radii.values())) == 1, ( + f"the mecanum controllers disagree on the wheel radius: {radii}. Switching " + f"between them would change how fast the base drives." + ) + + def _mjcf_wheel_heights() -> dict[str, float]: """World z of each wheel body, accumulated down the MJCF body nesting.""" heights: dict[str, float] = {}