Fix: Derive the hangar_sim base height from the wheel geometry - #882
Fix: Derive the hangar_sim base height from the wheel geometry#882L4co77 wants to merge 4 commits into
Conversation
|
Caution Review failedAn error occurred during the review process. Please try again later. No actionable comments were generated in the recent review. 🎉 ℹ️ Recent review info⚙️ Run configurationConfiguration used: Organization UI Review profile: CHILL Plan: Pro Plus Run ID: 📒 Files selected for processing (3)
🚧 Files skipped from review as they are similar to previous changes (1)
Included review availability: 3 reviews are currently available. Your included PR review attempts over the past 7 days set your current allowance at 5 reviews per hour. 📝 WalkthroughSummary by CodeRabbit
WalkthroughChangesThe Ridgeback URDF and MJCF now use roller-based vertical geometry. The test module parses roller dimensions, computes static ride height, compares wheel and arm-mount positions, and validates floor contact. CMake registers the test with a 30-second timeout. Ridgeback base geometry
Suggested reviewers: 🚥 Pre-merge checks | ✅ 4✅ Passed checks (4 passed)
Full details: Human Review CheckExplanation The PR is low-risk under the stated criteria. The verified diff contains only six files in Comment |
ef562e6 to
1b13a5f
Compare
![coderabbitai[bot]](https://avatars.githubusercontent.com/in/347564?s=60&v=4){kind=small}
|
|
There was a problem hiding this comment.
[from firstmate]
I re-derived this from the meshes and both body trees — the geometry argument is correct, including the front/rear asymmetry. Four should-fix items; the first is the one I'd want resolved before merge.
1. The anchor uses the wheel's peak collision radius. Inline on ur5e_ridgeback.xacro. Short version: the MJCF wheel is 20 spheres, not a cylinder, so a+r = 0.0759002 is the top of an envelope that dips to 0.0750889 — a 0.811 mm ripple per 18° sector. With no z DOF on the base, three of four wheels start the keyframe clear of the floor. One-line fix in the comment.
2. test_wheels_rest_on_the_floor doesn't check the physics. Inline on the test. Both terms come from the URDF, so the exact change #875 made three commits ago would pass it silently.
3. ur5e_ridgeback.xml:1519 — ham_assem is 5.163 mm out from the URDF (0.278813 vs 0.27365). Rotations agree to 2 µm, so it all lands in z: the whole UR5e sits 5.2 mm higher in physics than in TF. HAM_Assem.stl starts at +0.00635 local, and 0.27365 + 0.00635 = 0.280000 = mid_mount exactly, so the URDF is the correct one; 0.278813 looks copied from the ur_mount_point offset two lines down. Pre-existing rather than yours, but it's the same bug class one link up, it survives this PR, and the new test should grow a case for it — 5 mm at the gripper matters more than 5 mm at the wheel.
4. hangar_scene.xml:15-19 — the roller comment is wrong twice, and this PR's anchor leans on it. The spheres aren't tangent: a·sin(π/20) = 0.010309064 against r = 0.010, so adjacent centres are 0.020618128 apart versus 0.020 of summed radii — a 0.618 mm gap. And tangency wouldn't fix the rolling-radius variation anyway; the ripple is a·(1−cos(π/N)), 0.811 mm as built and 0.808 mm if re-sized to exact tangency. It's inherent to approximating a circle with N spheres. Worth correcting while you're in here, because the next person to derive an anchor from "a + r = 75.9 mm" will make the same assumption this one did.
| axle_offset (0.05) above chassis_link and the wheel radius is 0.0759, so the | ||
| chassis must ride 0.0259 up for the wheels to touch z = 0. Keep in step with | ||
| the MJCF base_platform_rotation anchor. --> | ||
| <origin xyz="0 0 0.0259" rpy="0 0 0" /> |
There was a problem hiding this comment.
The derivation is right, but it uses the wrong radius. This wheel has no collision cylinder — it's 20 spheres of r=0.010 on a circle of a=0.0659002081, so a+r = 0.0759002 is reached only 20× per revolution. Along the mid-pitch direction the envelope is a·cos(π/20)+r = 0.0750889.
Pinning the chassis so the wheel sits at exactly 0.0759 gives 0.2 µm of penetration at bottom-dead-centre and up to 0.811 mm of clearance everywhere else — and base_platform_rotation has no z DOF (free joint commented out at line 204), so nothing takes that up.
At the keyframe the config actually loads (mujoco_keyframe: default, which the integration suite resets to between objectives): front_left 0.865 mm clear, rear_left 0.522 mm clear, rear_right 0.535 mm clear, only front_right touching at 0.276 mm. On main all four penetrate 3.0–4.2 mm. The robot now starts on one wheel corner.
That preload is exactly why #875's residual ripple never showed — it went 8 spheres (4.522 mm ripple) → 20 (0.811 mm), and the −0.048 anchor was pressing every wheel 3.9 mm in. Remove the preload and the ripple is exposed again, 5.6× smaller. I'd guess that's also the real source of the rocker-jitter increase, rather than the longer lever in the description — base_platform_rotation has no z/roll/pitch DOF, so the chassis and arm are welded in z and never load the rocker.
One line, still derived rather than tuned:
anchor = a·cos(π/N) + r − axle_offset = 0.0750889 − 0.05 = 0.0251
Contact at every wheel angle (0 → 0.811 mm penetration), costs 0.8 mm of the 29.6 mm clearance, TF and physics unchanged. Raising N doesn't help — you'd need N≈57 to get the ripple under 0.1 mm.
There was a problem hiding this comment.
[written by AI]
You're right — reproduced every number from front_left_wheel_link.xml (N=20, a=0.0659002080, r=0.010, ripple 0.811 mm), so the anchor is now 0.0250889 in both models, along with your ham_assem and roller-comment points.
Three runs a side on jitter: corrected 0.204 / 0.216 / 0.191 against 0.292 / 0.348 / 0.314 on main — no overlap, so the increase was the wheels hanging clear, not the longer lever, and the PR description no longer claims otherwise.
|
|
||
| def test_wheels_rest_on_the_floor() -> None: | ||
| """Agreeing on a wrong height is still wrong: the axle sits one radius up.""" | ||
| radius = _xacro_property(RIDGEBACK_XACRO, "wheel_radius") |
There was a problem hiding this comment.
Both terms here come from the URDF — wheel_radius from ridgeback.urdf.xacro, and the chain height from the anchor — so this asserts URDF-internal consistency rather than floor contact in the physics. MuJoCo's contact radius is a + r from the roller bodies in {front,rear}_{left,right}_wheel_link.xml, which this file never reads.
#875 changed exactly those numbers three commits ago (8 spheres a=0.0594/r=0.0165 → 20 spheres a=0.0659002081/r=0.010); the identical edit today passes all six tests silently.
Cheap fix: parse one roller <body pos> and its <geom size> from a wheel include, assert a + r == wheel_radius (0.0759002081 vs 0.0759 today, so 1e-5 is plenty), and take the contact height from the MJCF side. That also gives you the hook for the ripple check above.
There was a problem hiding this comment.
[written by AI]
Correct — the contact height now comes from a·cos(π/N) + r on the MJCF side, read from all four wheel includes, instead of the URDF's wheel_radius.
#875 case (r 0.010 → 0.0165, nothing else) now fails 2 of 10; it also exposed that my TOLERANCE_M was 1e-3, larger than the 0.811 mm ripple it was meant to catch, so it is 1e-5 now.
kinematics.wheels_radius was 0.0666 in both the whole-body and the body-frame mecanum controllers, ~11.9% below the wheels' true rolling radius. The tread is a ring of 20 spheres of r=0.010 centred at a=0.0659002081. Rolling on that ring traces the convex hull of the spheres — 20 tangent segments of 2*a*sin(pi/20) plus arcs summing to one circle of radius r — giving a rolling radius of (20*2*a*sin(pi/20) + 2*pi*r) / (2*pi) = 0.07563. This is deliberately not the 0.0759 outer radius a+r, which is the static ride height PR #882 uses. The controller divides commanded body velocity by this constant, so the base drove ~13.6% faster than commanded; wheel odometry multiplies by it, so it under-reported distance by ~11.9% — about 0.72 m over a 6 m leg, the dominant source of the localization drift this stack was built to absorb. Valid only against main's rebuilt 20-roller wheels; the older 8-roller geometry had a different answer. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_0124EJ6ebbmvMJMjyMYPnWHD
2 similar comments
Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
d3c608f to
14e49ea
Compare
|
|
2 participants
Screencast.from.2026-08-25.23-20-59.webm
Motivation
hangar_simhung the robot off the world frame at z = −0.048, which put the wheels 74 mm below the floor and the chassis 44 mm into it. Viewed from under the ground plane the whole base is visibly through it. Addresses PickNikRobotics/moveit_pro#21933.The anchor was a compensation, and says so:
It was compensating for the MJCF placing the wheel bodies 70 mm higher than the URDF chain does.
Brief description
The meshes decide which model is wrong, so this does not need a judgement call:
wheel.stlrocker.stlbody.stlWith the URDF as authored (
axle_offset0.05, no wheel z) the wheel contacts 0.0259 below the chassis origin and the body bottom is 0.0037 above it — a 29.6 mm ground clearance, internally consistent. Add the MJCF's extra 0.07 and the contact point lands above the chassis bottom, which cannot happen. The URDF chain is sound; the MJCF wheel placement is not.So the wheel geometry needed no change in either the vendored
ridgeback_description(untouched here) or the URDF. What was wrong is the anchor, and it follows from the geometry rather than from tuning:a·cos(π/N) + rrather than thea + router radius, because the wheel is a ring of N=20 spheres and not a cylinder: the peak is reached only 20× per revolution, and the base carries no z DOF to take up the 0.811 mm ripple between spheres. Anchoring to the peak leaves wheels hanging clear at every other angle. Caught in review — the first version of this PR made exactly that mistake.Three files change: the anchor in
ur5e_ridgeback.xacro; the MJCF brought into agreement (front wheels 0.07 → 0, rear 0.12 → 0.05, anchor likewise, andham_assem0.278813 → 0.27365); and the roller comment inhangar_scene.xml, which claimed a tangency the spheres do not have.The
ham_assemcorrection is pre-existing rather than introduced here, and it is the same bug one link up — 5.163 mm between the models, which lands at the gripper rather than at a wheel.HAM_Assem.stlruns from y=+0.006350 and the mount rotation maps local y to world z, so the URDF's 0.27365 + 0.00635 = 0.280000 =deck_height: the arm sits on the deck, and the URDF is the correct side again.How it was tested
Measured against live TF on a running
hangar_sim, before and after:chassis_linkzham_assem_linkzNavigate to Clicked Pointsucceeds and the wheels hold 0.076 for the whole drive. Launch log carries only the four pre-existingrotational_yaw_jointstartup errors.colcon test --packages-select hangar_sim: the newbase_geometry_testis 6/6. One unrelated pre-existing failure inforward_stereo_publisher_testis untouched by this change.The new test, and why it is trustworthy
test/test_base_geometry.pywalks the URDF chain and the MJCF body nesting and asserts they agree, then that the wheels rest on z = 0. It reads the description sources directly rather than expanding the xacro, which would drag in the launch's argument set (visual_parameters_fileand friends) for assertions that do not care about it. No simulator, no ROS, 0.03 s.Every assertion was mutation-checked rather than merely observed to pass:
contact z = -0.0739 mwheel_radiusfrom the URDF on both sides and would have passed that silentlyham_assemto 0.278813 fails 1 of 8TOLERANCE_Mwas 1e-3, larger than the 0.811 mm error it was meant to catch. It is 1e-5 nowEffect on base jitter
#21933 raised the possibility that this mismatch was contributing to the
hangar_simbase jitter. It was, and correcting it makes the base quieter and far more repeatable. Three nav runs per geometry, same goal, fresh sim each run, one variable changed:main)No overlap with either other condition — the worst run here (0.216°) sits below the best original one (0.292°) — and the spread collapses from 0.899 to 0.025.
The middle row is the instructive one. It is this PR's earlier revision, which anchored to the wheels' peak radius
a + r; with no z DOF on the base that left three of four wheels hanging clear of the floor at the keyframe, so the robot started on one corner and the excursions went erratic. Anchoring to the envelope between spheres puts all four in contact and the jitter drops below where it started.An earlier version of this description read that the correction increases rocker excursion, on the strength of that middle row. That was wrong: the increase belonged to the bad anchor, not to the corrected geometry. Thanks to @bkanator for catching the anchor error, which is what made the difference visible.
This still does not establish how much the real Ridgeback rocks — that needs hardware, or someone who knows the platform.
Release notes
Bug Fix:Fixed thehangar_simRidgeback base being modelled below the floor, which placed the wheels 74 mm underground and left TF, the navigation footprint and collision checking describing a robot the simulator was not running.Bug Fix:Fixed thehangar_simUR5e mount sitting 5.2 mm higher in the simulation than in TF, which offset every arm frame between the two. Grasp poses tuned against the old simulation may need re-checking.