From b7eb40c9530a4659ef7056e598dda3fc83670169 Mon Sep 17 00:00:00 2001
From: Josh Whitley
Date: Fri, 14 Aug 2026 14:00:35 -0600
Subject: [PATCH 01/17] Fix Batch Merge Release Branch
---
.github/workflows/batch-merge-release-branch.yaml | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/.github/workflows/batch-merge-release-branch.yaml b/.github/workflows/batch-merge-release-branch.yaml
index 8854536c2..4882746c9 100644
--- a/.github/workflows/batch-merge-release-branch.yaml
+++ b/.github/workflows/batch-merge-release-branch.yaml
@@ -67,4 +67,4 @@ jobs:
PR_BODY="⚠️Merge failed with conflicts! Please pull this branch and manually resolve the conflict by resetting this branch and re-merging."
fi
git push origin "$TEMP_BRANCH"
- gh pr create --title "Merge $RELEASE_BRANCH into main" --body "$PR_BODY" --base main --head "$TEMP_BRANCH" --draft --reviewer infrastructure-devs
+ gh pr create --title "Merge $RELEASE_BRANCH into main" --body "$PR_BODY" --base main --head "$TEMP_BRANCH" --draft --reviewer JWhitleyWork
From 420c3e31332665ead6a554195400bcd35935dcc3 Mon Sep 17 00:00:00 2001
From: Ladislav Skvarka
Date: Mon, 24 Aug 2026 02:16:47 +0200
Subject: [PATCH 02/17] fix(hangar_sim): round out the mecanum wheels so the
base stops juddering
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
Each wheel approximated its rollers with 8 spheres on a 59.4 mm ring, so
the effective rolling radius swung 4.52 mm (6%) eight times per
revolution. That polygon drove the whole base: front_rocker swung 1.9
deg peak-to-peak against its own 0.11 deg range, and the camera — rigid
on a chassis with no roll or pitch DOF — was shaken in-plane instead.
Going to 16 rollers quarters the polygon error. Total roller mass per
wheel is unchanged. Contact solref goes 0.002 -> 0.016 s, which is the
2*timestep MuJoCo wants at the 0.008 s step this scene runs.
front_rocker's range/stiffness/damping clamp is reverted: it was added
in 98a1f5f8 to suppress this rocking and is unnecessary once the wheels
are round.
Co-Authored-By: Claude Opus 5
---
.../description/front_left_wheel_link.xml | 299 +++++++++++++++---
.../description/front_right_wheel_link.xml | 299 +++++++++++++++---
src/hangar_sim/description/hangar_scene.xml | 6 +-
.../description/rear_left_wheel_link.xml | 299 +++++++++++++++---
.../description/rear_right_wheel_link.xml | 299 +++++++++++++++---
src/hangar_sim/description/ur5e_ridgeback.xml | 4 +-
6 files changed, 1053 insertions(+), 153 deletions(-)
diff --git a/src/hangar_sim/description/front_left_wheel_link.xml b/src/hangar_sim/description/front_left_wheel_link.xml
index 9d16b089f..b8f319495 100644
--- a/src/hangar_sim/description/front_left_wheel_link.xml
+++ b/src/hangar_sim/description/front_left_wheel_link.xml
@@ -25,8 +25,8 @@
/>
+
-
+
-
+
-
+
-
+
-
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/hangar_sim/description/front_right_wheel_link.xml b/src/hangar_sim/description/front_right_wheel_link.xml
index a99d647c2..eafb862d0 100644
--- a/src/hangar_sim/description/front_right_wheel_link.xml
+++ b/src/hangar_sim/description/front_right_wheel_link.xml
@@ -25,8 +25,8 @@
/>
-
+
-
+
-
+
-
+
-
+
-
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/hangar_sim/description/hangar_scene.xml b/src/hangar_sim/description/hangar_scene.xml
index 405962ca5..20c581cbd 100644
--- a/src/hangar_sim/description/hangar_scene.xml
+++ b/src/hangar_sim/description/hangar_scene.xml
@@ -12,6 +12,9 @@
objectives. 0.008 keeps the runner at-or-under realtime while staying
finer than the original 0.025. Wheel velocity actuators still hold:
armature/kv = 1.0/500 = 0.002 s < 0.008 s (see CLAUDE.md). -->
+
diff --git a/src/hangar_sim/description/rear_left_wheel_link.xml b/src/hangar_sim/description/rear_left_wheel_link.xml
index 5b914a1fb..caf71e123 100644
--- a/src/hangar_sim/description/rear_left_wheel_link.xml
+++ b/src/hangar_sim/description/rear_left_wheel_link.xml
@@ -25,8 +25,8 @@
/>
-
+
-
+
-
+
-
+
-
+
-
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/hangar_sim/description/rear_right_wheel_link.xml b/src/hangar_sim/description/rear_right_wheel_link.xml
index 553eaf417..05caed2e4 100644
--- a/src/hangar_sim/description/rear_right_wheel_link.xml
+++ b/src/hangar_sim/description/rear_right_wheel_link.xml
@@ -25,8 +25,8 @@
/>
-
+
-
+
-
+
-
+
-
+
-
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/hangar_sim/description/ur5e_ridgeback.xml b/src/hangar_sim/description/ur5e_ridgeback.xml
index 2c18a2d13..c524a745d 100644
--- a/src/hangar_sim/description/ur5e_ridgeback.xml
+++ b/src/hangar_sim/description/ur5e_ridgeback.xml
@@ -1492,9 +1492,7 @@
name="front_rocker"
pos="0 0 0"
axis="1 0 0"
- range="-0.001 0.001"
- stiffness="10000"
- damping="500"
+ range="-0.08726 0.08726"
/>
From 3149bfcf9861dcbc013e949f382068708eebc47d Mon Sep 17 00:00:00 2001
From: Ladislav Skvarka
Date: Mon, 24 Aug 2026 03:27:47 +0200
Subject: [PATCH 03/17] fix(hangar_sim): 12 rollers per mecanum wheel instead
of 16
16 rollers removed the judder but doubled the ground-contact count
(ncon 64 -> 135), and the extra contact-solve time starves controller
mode-switching: 6 surface-following objectives died on "Current goal
cancelled during deactivate transition". 12 rollers with condim=3 costs
less (ncon 115) and keeps most of the benefit, but still fails 5 of
those objectives against a clean 98/98 baseline.
Recorded so the trade is visible; this is not yet shippable.
Co-Authored-By: Claude Opus 5
---
.../description/front_left_wheel_link.xml | 229 +++++-------------
.../description/front_right_wheel_link.xml | 229 +++++-------------
src/hangar_sim/description/hangar_scene.xml | 7 +-
.../description/rear_left_wheel_link.xml | 229 +++++-------------
.../description/rear_right_wheel_link.xml | 229 +++++-------------
5 files changed, 236 insertions(+), 687 deletions(-)
diff --git a/src/hangar_sim/description/front_left_wheel_link.xml b/src/hangar_sim/description/front_left_wheel_link.xml
index b8f319495..38e4bb887 100644
--- a/src/hangar_sim/description/front_left_wheel_link.xml
+++ b/src/hangar_sim/description/front_left_wheel_link.xml
@@ -25,8 +25,8 @@
/>
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/hangar_sim/description/front_right_wheel_link.xml b/src/hangar_sim/description/front_right_wheel_link.xml
index eafb862d0..95b908164 100644
--- a/src/hangar_sim/description/front_right_wheel_link.xml
+++ b/src/hangar_sim/description/front_right_wheel_link.xml
@@ -25,8 +25,8 @@
/>
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/hangar_sim/description/hangar_scene.xml b/src/hangar_sim/description/hangar_scene.xml
index 20c581cbd..69db04150 100644
--- a/src/hangar_sim/description/hangar_scene.xml
+++ b/src/hangar_sim/description/hangar_scene.xml
@@ -12,9 +12,10 @@
objectives. 0.008 keeps the runner at-or-under realtime while staying
finer than the original 0.025. Wheel velocity actuators still hold:
armature/kv = 1.0/500 = 0.002 s < 0.008 s (see CLAUDE.md). -->
-
+ 16 is smoother still but its contact count starves controller
+ mode-switching and fails the surface-following objectives. -->
diff --git a/src/hangar_sim/description/rear_left_wheel_link.xml b/src/hangar_sim/description/rear_left_wheel_link.xml
index caf71e123..b4e4f47cc 100644
--- a/src/hangar_sim/description/rear_left_wheel_link.xml
+++ b/src/hangar_sim/description/rear_left_wheel_link.xml
@@ -25,8 +25,8 @@
/>
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/src/hangar_sim/description/rear_right_wheel_link.xml b/src/hangar_sim/description/rear_right_wheel_link.xml
index 05caed2e4..a17bb3271 100644
--- a/src/hangar_sim/description/rear_right_wheel_link.xml
+++ b/src/hangar_sim/description/rear_right_wheel_link.xml
@@ -25,8 +25,8 @@
/>
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
From ca24d951c8e9371b8d3b3e89a192ff1e825daa4b Mon Sep 17 00:00:00 2001
From: Ladislav Skvarka
Date: Mon, 24 Aug 2026 09:58:41 +0200
Subject: [PATCH 04/17] fix(hangar_sim): size the mecanum rollers tangent, 20
per wheel
The earlier 12- and 16-roller attempts kept the original 16.5 mm sphere
on the original 59.4 mm ring, so the rollers overlapped and every one
collided with its neighbours: 48 spurious roller-roller contacts per
robot, contact count 64 -> 115, and the extra solve time starved
controller mode-switching (5-6 objectives failed).
Sizing the sphere to sit tangent to its neighbours instead
(r = a*sin(pi/N), outer radius held at 75.9 mm) removes that entirely.
20 rollers now costs 9 extra contacts, not 51:
contacts rocker p2p base yaw std camera jitter
8 (main) 64 1.729 deg 1.490 deg/s 17.60 mm
20 tangent 73 0.042 deg 0.084 deg/s 5.46 mm
Co-Authored-By: Claude Opus 5
---
.../description/front_left_wheel_link.xml | 366 ++++++++++++++----
.../description/front_right_wheel_link.xml | 366 ++++++++++++++----
src/hangar_sim/description/hangar_scene.xml | 10 +-
.../description/rear_left_wheel_link.xml | 366 ++++++++++++++----
.../description/rear_right_wheel_link.xml | 366 ++++++++++++++----
5 files changed, 1185 insertions(+), 289 deletions(-)
diff --git a/src/hangar_sim/description/front_left_wheel_link.xml b/src/hangar_sim/description/front_left_wheel_link.xml
index 38e4bb887..f7ca18589 100644
--- a/src/hangar_sim/description/front_left_wheel_link.xml
+++ b/src/hangar_sim/description/front_left_wheel_link.xml
@@ -13,7 +13,7 @@
armature="1.0"
/>
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
diff --git a/src/hangar_sim/description/rear_left_wheel_link.xml b/src/hangar_sim/description/rear_left_wheel_link.xml
index b4e4f47cc..c87a98cc3 100644
--- a/src/hangar_sim/description/rear_left_wheel_link.xml
+++ b/src/hangar_sim/description/rear_left_wheel_link.xml
@@ -13,7 +13,7 @@
armature="1.0"
/>
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Date: Mon, 24 Aug 2026 11:10:23 +0200
Subject: [PATCH 05/17] fix(hangar_sim): keep the wheel outer radius at exactly
75.9 mm
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
The 3% non-overlap clearance was applied to the sphere radius after the
ring radius had been solved, so a + r came out at 75.5918 mm — 0.31 mm
(0.41%) under the original wheel, an unintended change to ride height and
rolling radius. Folding the clearance into the tangency condition instead
(a = OUT / (1 + 0.97*sin(pi/N))) holds a + r at 75.9000 mm exactly and
keeps the same 3% margin.
Behaviour is unchanged: yaw std 0.071 deg/s and camera jitter 6.02 mm,
against 1.490 deg/s and 17.60 mm on main.
Co-Authored-By: Claude Opus 5
---
.../description/front_left_wheel_link.xml | 120 +++++++++---------
.../description/front_right_wheel_link.xml | 120 +++++++++---------
.../description/rear_left_wheel_link.xml | 120 +++++++++---------
.../description/rear_right_wheel_link.xml | 120 +++++++++---------
4 files changed, 240 insertions(+), 240 deletions(-)
diff --git a/src/hangar_sim/description/front_left_wheel_link.xml b/src/hangar_sim/description/front_left_wheel_link.xml
index f7ca18589..e4ecf5389 100644
--- a/src/hangar_sim/description/front_left_wheel_link.xml
+++ b/src/hangar_sim/description/front_left_wheel_link.xml
@@ -13,7 +13,7 @@
armature="1.0"
/>
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
-
+
Date: Mon, 24 Aug 2026 11:53:52 +0200
Subject: [PATCH 06/17] docs(hangar_sim): state both failure modes the tangent
sizing avoids
The comment said only that overlapping rollers self-collide, which read as
if the shipped 20-roller layout overlapped. Tangency is the boundary
between two failure modes, so name both: bigger spheres self-collide,
smaller ones leave gaps that vary the rolling radius.
Co-Authored-By: Claude Opus 5
---
src/hangar_sim/description/hangar_scene.xml | 7 ++++---
1 file changed, 4 insertions(+), 3 deletions(-)
diff --git a/src/hangar_sim/description/hangar_scene.xml b/src/hangar_sim/description/hangar_scene.xml
index 783ad2cb7..86ab2289c 100644
--- a/src/hangar_sim/description/hangar_scene.xml
+++ b/src/hangar_sim/description/hangar_scene.xml
@@ -12,9 +12,10 @@
objectives. 0.008 keeps the runner at-or-under realtime while staying
finer than the original 0.025. Wheel velocity actuators still hold:
armature/kv = 1.0/500 = 0.002 s < 0.008 s (see CLAUDE.md). -->
-
Date: Mon, 24 Aug 2026 17:19:09 +0000
Subject: [PATCH 07/17] Update phoebe workspace licensing
---
src/external_dependencies/phoebe_ws | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/external_dependencies/phoebe_ws b/src/external_dependencies/phoebe_ws
index f5014310f..22a1c0048 160000
--- a/src/external_dependencies/phoebe_ws
+++ b/src/external_dependencies/phoebe_ws
@@ -1 +1 @@
-Subproject commit f5014310f7f7975b5f1949a95d18e4be836f9c03
+Subproject commit 22a1c004895e1ca35bbcc287ddeccdc41d486056
From 8c767d4f7cc7e3528bde0606538307feaa23b174 Mon Sep 17 00:00:00 2001
From: Devanshu Sharma
Date: Fri, 21 Aug 2026 18:08:20 +0530
Subject: [PATCH 08/17] fix(hangar_sim): replace deprecated behavior and ports
in move boxes subtree
Running "ML Move Boxes to Loading Zone" emitted two deprecation warnings on
every cycle, both from the subtree it calls.
Swap AddPoseStampedToVector for PushBackVector, remapping `input` to `element`
and splitting `vector` into `input_vector`/`output_vector` on the same
blackboard key so the ForEach still accumulates. ResetPoseStampedVector already
clears the vector ahead of the loop.
On PlanCartesianPath, fold the deprecated velocity_scale_factor,
acceleration_scale_factor, and trajectory_sampling_rate ports into the
trajectory_timing port. All three values carry over unchanged, so the planned
lift is timed exactly as before.
Fixes PickNikRobotics/BCR_platform_mirror#27
Co-Authored-By: Claude Opus 5
---
.../move_boxes_to_loading_zone_from_waypoint.xml | 11 +++++------
1 file changed, 5 insertions(+), 6 deletions(-)
diff --git a/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml b/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml
index 57f3355e6..436e4860a 100644
--- a/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml
+++ b/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml
@@ -285,9 +285,10 @@
output_pose="{leveled_grasp_pose}"
/>
@@ -428,7 +429,6 @@
/>
@@ -44,13 +46,15 @@
ID="Interpolate to Joint State"
_collapsed="false"
target_joint_state="{target_joint_state}"
- controller_names="joint_trajectory_controller"
- controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
- execution_pipeline="jtc"
+ controller_names="{joint_trajectory_controller_name}"
+ controller_action_server="{controller_action_server}"
+ planning_group_name="{default_planning_group}"
+ velocity_scale_factor="{velocity_scale_factor}"
+ execution_pipeline="{execution_pipeline}"
/>
@@ -58,7 +62,7 @@
@@ -94,17 +98,23 @@
@@ -112,7 +122,7 @@
@@ -136,17 +146,16 @@
_collapsed="false"
target_joint_state="{target_joint_state}"
acceleration_scale_factor="1.0"
- controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
+ velocity_scale_factor="{velocity_scale_factor}"
+ controller_action_server="{controller_action_server}"
controller_names="joint_trajectory_controller;platform_velocity_controller"
- joint_group_name="manipulator"
- link_padding="0.0"
- velocity_scale_factor="1.0"
- seed="0"
- execution_pipeline="jtc"
+ joint_group_name="{default_planning_group}"
+ link_padding="{link_padding}"
+ execution_pipeline="{execution_pipeline}"
/>
@@ -154,7 +163,7 @@
@@ -163,45 +172,55 @@
-
+
-
+
+
-
+
-
-
+
-
+
-
@@ -230,13 +248,59 @@
-
-
-
+
+
+ Enable interactive user prompts during teleoperation mode selection
+
+
+ Maximum number of IK solutions PlanToPose will try to produce for a
+ given pose
+
+
+ Maximum number of MTC solutions to find before returning early (0 to
+ find all solutions)
+
+
+
+
+
+
+
+ Fraction of each joint's maximum velocity that the motion branches plan
+ with. DoTeleoperateAction overwrites it with the speed the operator
+ selects; this default only covers the window before its first feedback,
+ and matches the speed the UI itself starts at.
+
+
+
+
+ Whether trajectory previews from Interactive Marker teleop must be
+ approved before execution. Set to false to skip the approval prompt.
+
From 3bede45adcdd4410bb3015f0053b22b007247805 Mon Sep 17 00:00:00 2001
From: Dave Coleman
Date: Mon, 24 Aug 2026 13:32:37 -0600
Subject: [PATCH 10/17] chore: remove the moveit_pro_clipseg submodule
Drops ~520 MB from a `--recurse-submodules` clone. Nothing in this
workspace consumed the package: no `exec_depend`, no
`model_package="moveit_pro_clipseg"` port. Text-prompt segmentation moved
to SAM3 (`GetMasks2DFromExemplar`), and the `GetMasks2DFromTextQuery`
Behavior that used the CLIPSeg models no longer exists in MoveIt Pro.
Also corrects two stale CLIPSeg references left behind by the SAM3 move.
Co-Authored-By: Claude Opus 5 (1M context)
---
.gitmodules | 3 ---
src/dual_arm_sim/objectives/sort_blocks.xml | 2 +-
.../move_boxes_to_loading_zone_from_waypoint.xml | 11 +++++------
src/moveit_pro_clipseg | 1 -
4 files changed, 6 insertions(+), 11 deletions(-)
delete mode 160000 src/moveit_pro_clipseg
diff --git a/.gitmodules b/.gitmodules
index 6c03417d0..308646393 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -17,9 +17,6 @@
path = src/external_dependencies/franka_config/franka_description
url = https://github.com/frankarobotics/franka_description.git
branch = main
-[submodule "src/moveit_pro_clipseg"]
- path = src/moveit_pro_clipseg
- url = https://github.com/PickNikRobotics/moveit_pro_clipseg.git
[submodule "src/external_dependencies/phoebe_ws"]
path = src/external_dependencies/phoebe_ws
url = https://github.com/PickNikRobotics/phoebe_ws.git
diff --git a/src/dual_arm_sim/objectives/sort_blocks.xml b/src/dual_arm_sim/objectives/sort_blocks.xml
index 646199116..5d10ffbdf 100644
--- a/src/dual_arm_sim/objectives/sort_blocks.xml
+++ b/src/dual_arm_sim/objectives/sort_blocks.xml
@@ -2,7 +2,7 @@
diff --git a/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml b/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml
index 436e4860a..27a09e6ea 100644
--- a/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml
+++ b/src/hangar_sim/objectives/move_boxes_to_loading_zone_from_waypoint.xml
@@ -95,12 +95,11 @@
/>
Date: Tue, 28 Jul 2026 19:06:11 -0400
Subject: [PATCH 11/17] feat(hangar_sim): fuse odometry localization for the
mobile base
Give hangar_sim's mobile base a realistic localization stack so beluga_amcl is
exercised against fuse-fused odometry (wheel + IMU) with real drift, including
where the scene is degenerate for scan matching (the smooth fuselage, the
unmapped picking boxes).
- Fuse on by default (use_fuse=true) -> /odom_filtered; odom_world_drift injects
a live odom->world from the fuse estimate so AMCL sees drift to correct while
world->base stays ground truth for whole-body planning.
- slip_aware_odom (new C++ node): grows wheel-yaw covariance during sustained
in-place spin (mecanum roller slip) so fuse defers to the IMU while spinning
and trusts the wheels driving straight; also re-anchors odom across the
whole-body<->nav controller handoff.
- AMCL tuning: OmniMotionModel; alpha1 0.1->0.4; update_min_a 0.1 with
resample_interval 3; likelihood relaxed (sigma_hit 0.1->0.25, z_hit 0.9->0.65,
z_rand 0.1->0.3) so unmapped boxes read as outliers instead of yanking the pose.
- fuse lag_duration 0.5->0.3: shorter smoother window cuts output latency
(moving-yaw 0.70->0.465deg) while still smoothing transient spikes.
- amcl_odom_gate (new C++ node): sole map->odom publisher (AMCL tf_broadcast
false). Holds the last good map->odom and coasts on fuse odom where AMCL is
degenerate, blending back when trustworthy. A large correction is accepted
only if it persists over a sliding window (gated on position AND yaw) with
particle-spread hysteresis; spread_accept_max additionally rejects a
confident-but-WRONG lock (which persists yet stays spread) so a scan-slide
divergence is coasted through while a real recovery (which converges) is still
adopted. latency_compensation_sec composes the correction with odom->base from
one estimator-lag ago (referenced to the cloud stamp), forward-projecting it
with real buffered odometry -> moving-pose error 13.8->8.2cm, moving-yaw
1.2->0.9deg, no overshoot on turns; 0 disables, hangar_sim uses 0.30. The pure
decision logic (detail::updateGate, interpolateOdom, appendOdomSample) has no
ROS/TF deps and is fully unit-tested.
- Removed the per-objective SetInitialPose reseed from the clicked-point
Objectives (superseded by slip_aware_odom re-anchoring; the unconditional
reseed could cement a drifting estimate).
- wz_max 0.6 (below the velocity_smoother cap so it binds) keeps spins within
AMCL's correction bandwidth; odom_rate 50Hz / tf_publish_rate 30Hz and the
broadcaster rates decoupled from the control loop give the stack CPU headroom.
Closes #19667.
Co-Authored-By: Claude Opus 4.8 (1M context)
---
src/hangar_sim/CMakeLists.txt | 36 ++
.../control/picknik_ur.ros2_control.yaml | 2 +
src/hangar_sim/config/fuse/fuse.yaml | 6 +-
.../picknik_ur_mujoco_ros2_control.xacro | 4 +-
src/hangar_sim/description/ur5e_ridgeback.xml | 4 +
.../hangar_sim/amcl_odom_gate_logic.hpp | 186 ++++++
.../hangar_sim/odom_world_drift_logic.hpp | 76 +++
src/hangar_sim/include/hangar_sim/se2.hpp | 61 ++
.../robot_drivers_to_persist_sim.launch.py | 132 +++-
.../objectives/navigate_to_clicked_point.xml | 1 -
...igate_to_clicked_point_with_replanning.xml | 1 -
src/hangar_sim/package.xml | 9 +-
src/hangar_sim/params/nav2_params.yaml | 46 +-
src/hangar_sim/src/amcl_odom_gate.cpp | 348 +++++++++++
src/hangar_sim/src/amcl_odom_gate_logic.cpp | 113 ++++
src/hangar_sim/src/odom_world_drift.cpp | 174 ++++++
src/hangar_sim/src/slip_aware_odom.cpp | 206 +++++++
src/hangar_sim/test/test_amcl_odom_gate.cpp | 583 ++++++++++++++++++
src/hangar_sim/test/test_odom_world_drift.cpp | 104 ++++
19 files changed, 2058 insertions(+), 34 deletions(-)
create mode 100644 src/hangar_sim/include/hangar_sim/amcl_odom_gate_logic.hpp
create mode 100644 src/hangar_sim/include/hangar_sim/odom_world_drift_logic.hpp
create mode 100644 src/hangar_sim/include/hangar_sim/se2.hpp
create mode 100644 src/hangar_sim/src/amcl_odom_gate.cpp
create mode 100644 src/hangar_sim/src/amcl_odom_gate_logic.cpp
create mode 100644 src/hangar_sim/src/odom_world_drift.cpp
create mode 100644 src/hangar_sim/src/slip_aware_odom.cpp
create mode 100644 src/hangar_sim/test/test_amcl_odom_gate.cpp
create mode 100644 src/hangar_sim/test/test_odom_world_drift.cpp
diff --git a/src/hangar_sim/CMakeLists.txt b/src/hangar_sim/CMakeLists.txt
index 0d49770e5..c055f0155 100644
--- a/src/hangar_sim/CMakeLists.txt
+++ b/src/hangar_sim/CMakeLists.txt
@@ -3,6 +3,31 @@ project(hangar_sim)
find_package(ament_cmake REQUIRED)
find_package(picknik_accessories REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+add_executable(slip_aware_odom src/slip_aware_odom.cpp)
+target_include_directories(slip_aware_odom PRIVATE include)
+target_compile_features(slip_aware_odom PRIVATE cxx_std_20)
+ament_target_dependencies(slip_aware_odom rclcpp nav_msgs tf2 tf2_geometry_msgs)
+
+add_executable(amcl_odom_gate src/amcl_odom_gate.cpp src/amcl_odom_gate_logic.cpp)
+target_include_directories(amcl_odom_gate PRIVATE include)
+target_compile_features(amcl_odom_gate PRIVATE cxx_std_20)
+ament_target_dependencies(amcl_odom_gate rclcpp geometry_msgs tf2 tf2_geometry_msgs tf2_ros)
+
+add_executable(odom_world_drift src/odom_world_drift.cpp)
+target_include_directories(odom_world_drift PRIVATE include)
+target_compile_features(odom_world_drift PRIVATE cxx_std_20)
+ament_target_dependencies(odom_world_drift rclcpp nav_msgs sensor_msgs geometry_msgs tf2 tf2_geometry_msgs
+ tf2_ros)
+
+install(TARGETS slip_aware_odom amcl_odom_gate odom_world_drift DESTINATION lib/${PROJECT_NAME})
install(
DIRECTORY
@@ -47,6 +72,17 @@ if(BUILD_TESTING)
ENV MOVEIT_CONFIG_PACKAGE=hangar_sim
MOVEIT_HOST_USER_WORKSPACE=${CMAKE_SOURCE_DIR}
ROS_LOG_DIR=${CMAKE_CURRENT_BINARY_DIR}/test_results/${PROJECT_NAME}/ros_logs)
+
+ # Unit tests for the pure amcl_odom_gate decision logic (no ROS/TF).
+ find_package(ament_cmake_gmock REQUIRED)
+ ament_add_gmock(test_amcl_odom_gate test/test_amcl_odom_gate.cpp src/amcl_odom_gate_logic.cpp)
+ target_include_directories(test_amcl_odom_gate PRIVATE include)
+ target_compile_features(test_amcl_odom_gate PRIVATE cxx_std_20)
+
+ # Unit tests for the pure odom_world_drift rail-joint index resolver (no ROS).
+ ament_add_gmock(test_odom_world_drift test/test_odom_world_drift.cpp)
+ target_include_directories(test_odom_world_drift PRIVATE include)
+ target_compile_features(test_odom_world_drift PRIVATE cxx_std_20)
endif()
ament_package()
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 ab6e0afb3..a6446f8a1 100644
--- a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml
+++ b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml
@@ -160,6 +160,7 @@ platform_velocity_controller_nav2:
joint_state_broadcaster:
ros__parameters:
use_local_topics: false
+ update_rate: 50 # publish /joint_states at 50 Hz, not the 600 Hz control loop
joints:
- shoulder_pan_joint
- shoulder_lift_joint
@@ -297,6 +298,7 @@ force_torque_sensor_broadcaster:
imu_sensor_broadcaster:
ros__parameters:
+ update_rate: 100 # 100 Hz IMU is plenty for fuse (optimizes at 10 Hz); was inheriting the 600 Hz control loop
sensor_name: imu_site
frame_id: ridgeback_base_link
# Static covariance values (row-major 3x3 matrices)
diff --git a/src/hangar_sim/config/fuse/fuse.yaml b/src/hangar_sim/config/fuse/fuse.yaml
index 4ad1039f5..22f5135b6 100644
--- a/src/hangar_sim/config/fuse/fuse.yaml
+++ b/src/hangar_sim/config/fuse/fuse.yaml
@@ -3,9 +3,9 @@
state_estimator:
ros__parameters:
# Fixed-lag smoother configuration
- optimization_frequency: 20.0
+ optimization_frequency: 10.0 # match publish_frequency (10 Hz); optimizing 2x faster than we publish was wasted work
transaction_timeout: 0.01
- lag_duration: 0.5
+ lag_duration: 0.3 # chosen: median 0.465deg (<0.5 goal), clean transient tail vs 0.25
# Motion model for mobile base (3D omnidirectional)
motion_models:
@@ -51,7 +51,7 @@ state_estimator:
# only the relative change between consecutive messages, preventing
# accumulated drift from corrupting the estimate.
wheel_odom_sensor:
- topic: /platform_velocity_controller_nav2/odom
+ topic: /odom_slip_aware # slip_aware_odom republishes with spin-aware yaw covariance
queue_size: 10
pose_loss:
type: fuse_loss::HuberLoss
diff --git a/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro b/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro
index 42241108e..05ba84c3f 100644
--- a/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro
+++ b/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro
@@ -31,11 +31,11 @@
${mujoco_model}
hangar_sim
20
- 60
+ 30
10
${publish_odom}
ridgeback_base_link
- 150
+ 50
diff --git a/src/hangar_sim/description/ur5e_ridgeback.xml b/src/hangar_sim/description/ur5e_ridgeback.xml
index c524a745d..6b184c5c1 100644
--- a/src/hangar_sim/description/ur5e_ridgeback.xml
+++ b/src/hangar_sim/description/ur5e_ridgeback.xml
@@ -313,6 +313,10 @@
conaffinity="0"
group="2"
/>
+
+
-
+