15586 imu fuse plugin

[suchkristenwow]

This PR adds an IMU (fuse_models::Imu3D) to the hangar_sim state estimation pipeline, fusing IMU data with the wheel odom (from Mujoco).

In ur5e_ridgeback.xml

• imu_link is added to the Ridgeback chassis to match the actual IMU location from the ridgeback.urdf.xacro
• 3 sensors: framequat (orientation), gyro (angular vel), and accelerometer (linear acceleration)

In picknik_ur_mujoco_ros2_contro.xacro

• added imu_site sensor to bridge Mujoco sensor data into the rso2_control hardware interface

Controller config: picknik_ur.ros2_control.yaml & config.yaml

• Register imu_sensor_broadcaster/IMUSensorBroadcaster in the controller manager
• imu_sensor_broadcaster is added to controllers_active_at_startup

Fuse Configuration

• Wheel Odometry:

  • differential is true: only fuses relative changes between consecutive messages
  • uses twist covariance instead of the pose covariance which has terrible covariance
  • independent: false - consecutive odom messages come from the same integrator

• IMU:

  • differential: false
  • No linear acceleration dimensions: removed because otherwise the estimate flies up into space, I'm guessing because of gravity compensation but not sure
  • subscribes to /imu_sensor_broadcaster/imu_reliable (which is just /imu_sensor_broadcaster/imu with QOS set to reliable)

Screencast from 2026-02-23 12-56-23.webm
^In this video, the blue is odom_reliable (the republished gt odometry) and red is odom_filtered (the product of fuse)

Screencast from 2026-02-23 13-49-46.webm
^In this video, I change odom_reliable > /platform_velocity_controller/odom which for some reason has a completely messed up covariance

[suchkristenwow]

Documenting discussion with Griz:

• Griz was asking if I accidentally was double-publishing the TF between odom and base_footprint with this PR
• I'm not but the fuse state estimator might have been getting double-launched, once in agent_bridge.launch.xml (via fuse.launch.py) and again with robot_drivers_to_persist_sim.launch.py
• I thought it made more sense to keep it in robot_drivers_to_persist_simlaunch.py since

1. that's where the robot infrastructure lives (agent_bridge is for the moveit_studio agent bridge, as the name would suggest) and
2. I can get rid of fuse.launch.py

Now the fuse state estimator will now only launch from robot_drivers_to_persist_sim.launch.py

[suchkristenwow]

Screencast from 2026-02-23 14-50-57.webm
^ the commit d926b93 uses the mecanum wheel odom topic and the estimate is just terrible but better than the config I was using when using the ground truth odometry message from Mujoco.

[suchkristenwow]

Screencast from 2026-02-24 13-53-24.webm
This video is the result of this commit: d7fc501

At 0:15 I toggle on the covariance for the mecanum wheel odometry message.

Blue is the gt odometry from Mujoco, red is the filtered odom, green is the mecanum wheel odom

This makes sense though since the mecanum wheel odom message shouldn't have info about z, roll, or pitch so effectively it's saying, don't listen to me about these.

[suchkristenwow]

After talking to Nathan D (from JSC) want to visualize in Mujoco to sanity check contact forces

[bkanator]

From AI buddy: " So the "quick fixes" have already been attempted and didn't solve it. This points more toward the fundamental architecture problem. our virtual rail approach may just not produce good odom, regardless of how well the parameters are tuned. The controller is back-calculating wheel velocities from commanded rail positions, not reading actual wheel-ground interaction."
"why the parameter tuning didn't help much. With virtual rails, the odom pipeline is:

1. Command X/Y/yaw position to rails
2. Controller inverse-kinematics that into "what wheel velocities would produce this"
3. Controller forward-kinematics those wheel velocities back into odom
4. You get back a noisy version of what you commanded — not what the robot actually did"

Next step is to use claude to: So the real path forward is switching to wheel velocity actuators like NASA did. That's the only way to get odom that reflects actual simulated physics.
It would involve:

1. Replacing the rail position actuators with wheel velocity actuators (like NASA's kv=50)
2. Enabling roller ground contact (conaffinity=1)
3. Adding NASA's contact params (friction, solimp, solref, noslip_iterations, etc.)
4. Possibly using NASA's wheel_code_gen.py output or adapting it
5. Keeping Kristen's IMU + fuse work on top

[bkanator]

Step 1: Regenerate Wheel XMLs with 8 Rollers

The real Ridgeback has 8 rollers per wheel. Our model has 12 sphere rollers; NASA's has 8 with CAD mesh collision. We'll adapt
NASA's wheel_code_gen.py to generate 8-roller wheels with sphere collision (keeping our sphere approach for now, but with
correct count and geometry).

Action: Create a local copy of NASA's wheel_code_gen.py, adapt it to output our 4 wheel XML files with:

• 8 rollers per wheel (matching real hardware)
• Sphere collision geometry (size matching NASA's roller_r = 0.0165, or tuned to contact the floor)
• contype="1" conaffinity="1" friction="1.0 0.005 0.0001" condim="4"
• Correct wheel radius (0.0759), roller positions, and wheel types (left/right diagonal)
• Wheel mass 2.5 kg (matching NASA), joint damping 5.0 on main wheel joint

Files replaced (4):

• src/hangar_sim/description/front_left_wheel_link.xml
• src/hangar_sim/description/front_right_wheel_link.xml
• src/hangar_sim/description/rear_left_wheel_link.xml
• src/hangar_sim/description/rear_right_wheel_link.xml

condim=4 enables 3D friction with torsional component. Low torsional/rolling friction allows the rollers to slide laterally
(mecanum behavior). NASA uses condim=4 on their rubber class.

Note: Going from 12 to 8 rollers changes the joint count, which will require updating the keyframe qpos vector in Step 5.

Step 2: Add Physics Solver Settings

File: src/hangar_sim/description/hangar_scene.xml

Change line 8:

to:

• implicitfast: matches ur5e_ridgeback.xml and NASA's setting
• timestep=0.005: halved for more stable contact resolution
• noslip_iterations=5: post-processing to reduce contact slip (NASA uses this)
• impratio=20: higher frictional-to-normal impedance ratio (NASA uses this)
• cone=elliptic: more accurate multi-directional friction model (NASA uses this)

Step 3: Fix Controller Kinematic Parameters

File: src/hangar_sim/config/control/picknik_ur.ros2_control.yaml

For BOTH platform_velocity_controller and platform_velocity_controller_nav2:
kinematics.wheels_radius: 0.0759 # was 0.05 (34% error!)
kinematics.sum_of_robot_center_projection_on_X_Y_axis: 0.59 # was 0.4704

These match NASA's values and the actual ridgeback geometry:

• wheel_base/2 + track_width/2 = 0.319 + 0.2755 = 0.5945 ≈ 0.59

Step 4: Verify and Adjust Ground Contact (Empirical)

Load the sim and check MuJoCo viewer for:

1. Contact points visible between rollers and floor
2. Robot standing stably (no vibration, no sinking, no floating)
3. Wheels spinning when velocity is commanded

If rollers don't reach the floor, adjust base_platform_rotation Z position in ur5e_ridgeback.xml (line 197) to lower the
robot.

If contact is unstable (vibration/bouncing), tune:

• Roller friction values
• solimp and solref on roller geoms (try solimp="0.9 0.95 0.001" solref="0.002 1" per NASA)
• Rail joint damping (add small damping like 0.1 to rail joints if oscillating)

Step 5: Update Keyframe

File: src/hangar_sim/description/hangar_scene.xml

Switching from 12 to 8 rollers per wheel changes the joint count (removing 4×4=16 slipping joints). The keyframe qpos vector
must be regenerated. Load the model without a keyframe, pose the robot correctly, then export the qpos.

[bkanator]

Update: currently testing step 4. I verified contact points and am ensuring that mecanum drive is driving properly in mujoco viewer.

[bkanator]

what was done:

Wheel model — Replaced 12-roller virtual wheels with 8-roller wheel-ground contact (matching real
Ridgeback hardware). Enabled roller ground contact (conaffinity=1) and tuned MuJoCo physics
(implicitfast, noslip_iterations=5, impratio=20, cone=elliptic, roller friction=0.5, condim=4) for
stable mecanum behavior.

Odometry calibration — Tuned wheels_radius (0.0759 → 0.0666) and
sum_of_robot_center_projection_on_X_Y_axis (0.4704 → 0.59) against ground truth from mj_data.xpos.
Results: 0.08% linear error, 1.1% angular error.

Controller startup — Switched active-at-startup controller from joint_trajectory_controller to
platform_velocity_controller_nav2 so Nav2 work without requiring whole-body planning to
be active.

cmd_vel relay — Added relay in odom_qos_relay.py forwarding /cmd_vel →
/platform_velocity_controller_nav2/cmd_vel_unstamped so standard ROS convention works out of the box.

Fuse topic — Fixed fuse.yaml subscribing to /platform_velocity_controller/odom (inactive
controller, no publisher) → /platform_velocity_controller_nav2/odom.

JTC goal tolerances — Wheel-driven locomotion can't hit the original 5cm/5° rail joint tolerance
precisely on controller activation. Set goal_time: 5.0 (was 0.0 — hung forever) and loosened rail
joint goal tolerances to 0.1 to account for wheel-driven positioning imprecision.

Controller switch-back — All 17 runnable whole-body objectives now reactivate
platform_velocity_controller_nav2 on completion. When joint_trajectory_controller activates, it
displaces platform_velocity_controller_nav2; without an explicit switch-back, /cmd_vel stops working
after any whole-body objective.

---

green is the wheel odometery /platform_velocity_controller_nav2/odom, red is the ground truth, and yellow is the filtered fuse.
so here is a video for current odom showing: straight, spin, straight, strafe, straight.....

odom-2026-04-02_21.33.00.mp4

[bkanator]

i'm unsure if after using teleop it will switch back to the nav2 controller since it uses whole body (and platform velocity controller), so this is still an open question. Update: I added another switch controller at the end of hangar_sim's teleop objective

Generally, I am looking for feedback on how switching (or not) between the whole body and nav2 pipelines is done

I verified the whole body objectives work, but only if I add the tolerance and time since it wasn't accounting for the controller activation time if I run the objective before sending any cmd_vel commands. Then after those objectives run i need to switch back to the nav2 controller for it to be able to receive any future cmd_vel messages.