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 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] = {}