diff --git a/src/hangar_sim/config/config.yaml b/src/hangar_sim/config/config.yaml
index feddea7ab..20911b77f 100644
--- a/src/hangar_sim/config/config.yaml
+++ b/src/hangar_sim/config/config.yaml
@@ -92,12 +92,12 @@ ros2_control:
     - "force_torque_sensor_broadcaster"
     - "imu_sensor_broadcaster"
     - "joint_state_broadcaster"
-    - "platform_velocity_controller"
+    - "platform_velocity_controller_nav2"
     - "vacuum_gripper"
   # Load but do not start these controllers so they can be activated later if needed.
   controllers_inactive_at_startup:
     - "joint_trajectory_controller"
-    - "platform_velocity_controller_nav2"
+    - "platform_velocity_controller"
     - "velocity_force_controller"
     - "arm_only_velocity_force_controller"
     - "joint_velocity_controller"
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 995815a5f..ed8a4a0b3 100644
--- a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml
+++ b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml
@@ -37,26 +37,25 @@ platform_velocity_controller:
     interface_name: velocity
     command_joint_names: ["front_left_wheel", "rear_left_wheel", "rear_right_wheel", "front_right_wheel"]
     reference_joint_names: ["linear_x_joint", "linear_y_joint", "rotational_yaw_joint"]
-    body_frame_control: false
-    body_frame_yaw_joint: "rotational_yaw_joint"
+    body_frame_control: true
     front_left_wheel_command_joint_name: "front_left_wheel"
     front_right_wheel_command_joint_name: "front_right_wheel"
     rear_right_wheel_command_joint_name: "rear_right_wheel"
     rear_left_wheel_command_joint_name: "rear_left_wheel"
 
-    kinematics.wheels_radius: 0.05
-    kinematics.sum_of_robot_center_projection_on_X_Y_axis: 0.4704
+    kinematics.wheels_radius: 0.0666
+    kinematics.sum_of_robot_center_projection_on_X_Y_axis: 0.59
     wheel_separation_multiplier: 1.0
     wheel_radius_multiplier: 1.0
 
     publish_rate: 50.0
-    reference_timeout: 0.1
+    reference_timeout: 0.5
     use_stamped_vel: False
 
-    pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03]
-    twist_covariance_diagonal: [0.001, 0.001, 0.001, 1000000.0, 1000000.0, 0.03]
+    pose_covariance_diagonal: [0.1, 0.1, 1000000.0, 1000000.0, 1000000.0, 0.5]
+    twist_covariance_diagonal: [0.1, 0.1, 0.1, 1000000.0, 1000000.0, 0.5]
 
-    cmd_vel_timeout: 0.5
+    cmd_vel_timeout: 5.0
 
     base_frame_id: "ridgeback_base_link"
     odom_frame_id: "odom"
@@ -105,19 +104,19 @@ 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.05
-    kinematics.sum_of_robot_center_projection_on_X_Y_axis: 0.4704
+    kinematics.wheels_radius: 0.0666
+    kinematics.sum_of_robot_center_projection_on_X_Y_axis: 0.59
     wheel_separation_multiplier: 1.0
     wheel_radius_multiplier: 1.0
 
     publish_rate: 50.0
-    reference_timeout: 0.1
+    reference_timeout: 0.5
     use_stamped_vel: False
 
     pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03]
     twist_covariance_diagonal: [0.001, 0.001, 0.001, 1000000.0, 1000000.0, 0.03]
 
-    cmd_vel_timeout: 0.5
+    cmd_vel_timeout: 5.0
 
     base_frame_id: "ridgeback_base_link"
     odom_frame_id: "odom"
@@ -239,7 +238,7 @@ joint_trajectory_controller:
         ff_velocity_scale: 1.0
     constraints:
       stopped_velocity_tolerance: 0.0
-      goal_time: 0.0
+      goal_time: 5.0
       shoulder_pan_joint:
         goal: 0.05
       shoulder_lift_joint:
@@ -253,11 +252,11 @@ joint_trajectory_controller:
       wrist_3_joint:
         goal: 0.05
       linear_x_joint:
-        goal: 0.05
+        goal: 0.1
       linear_y_joint:
-        goal: 0.05
+        goal: 0.1
       rotational_yaw_joint:
-        goal: 0.05
+        goal: 0.1
 
     acceleration_limits:
       shoulder_pan_joint: 10.0
@@ -282,7 +281,14 @@ force_torque_sensor_broadcaster:
 imu_sensor_broadcaster:
   ros__parameters:
     sensor_name: imu_site
-    frame_id: imu_link
+    frame_id: ridgeback_base_link
+    # Static covariance values (row-major 3x3 matrices)
+    # Orientation covariance (rad^2)
+    static_covariance_orientation: [0.001, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.001]
+    # Angular velocity covariance (rad/s)^2
+    static_covariance_angular_velocity: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
+    # Linear acceleration covariance (m/s^2)^2
+    static_covariance_linear_acceleration: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
 
 
 velocity_force_controller:
diff --git a/src/hangar_sim/config/fuse/fuse.yaml b/src/hangar_sim/config/fuse/fuse.yaml
index db8a0d8a1..3a9ad2b43 100644
--- a/src/hangar_sim/config/fuse/fuse.yaml
+++ b/src/hangar_sim/config/fuse/fuse.yaml
@@ -1,5 +1,5 @@
 # Mobile base localization configuration using fuse
-# Using 3D odometry model for wheel odometry-based pose estimation
+# Ground vehicle state estimation: wheel odometry + IMU
 state_estimator:
   ros__parameters:
     # Fixed-lag smoother configuration
@@ -15,7 +15,10 @@ state_estimator:
     mobile_base_motion:
       # Process noise for state variables
       # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az
-      process_noise_diagonal: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0, 1.0, 1.0]
+      # z, roll, pitch and their derivatives are near-zero for a ground robot.
+      #process_noise_diagonal: [0.25, 0.25, 0.00001, 0.00001, 0.00001, 0.25
+      #   -, 0.1, 0.1, 0.00001, 0.00001, 0.0001, 0.1, 0.0001, 0.0001, 0.00001]
+      process_noise_diagonal: [1.0, 1.0, 0.0001, 0.0001, 0.0001, 0.5, 0.5, 0.5, 0.0001, 0.0001, 0.0001, 0.25, 0.00001, 0.00001, 0.00001]
 
     sensor_models:
       initial_localization_sensor:
@@ -25,32 +28,48 @@ state_estimator:
       wheel_odom_sensor:
         type: fuse_models::Odometry3D
         motion_models: [mobile_base_motion]
+      imu_sensor:
+        type: fuse_models::Imu3D
+        motion_models: [mobile_base_motion]
 
     initial_localization_sensor:
       publish_on_startup: true
       # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az
       initial_state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
-      initial_sigma: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
+      # Order: x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az
+      # z and its derivatives start with very tight sigma — ground robot stays on the plane.
+      initial_sigma: [0.1, 0.1, 0.001, 0.01, 0.01, 0.1, 0.1, 0.1, 0.001, 0.01, 0.01, 0.1, 0.001, 0.001, 0.001]
 
+    # Wheel odometry — provides translation (x, y) and yaw
+    # Using differential mode because platform_velocity_controller/odom is
+    # wheel-integrated odometry that drifts over time. Differential mode fuses
+    # only the relative change between consecutive messages, preventing
+    # accumulated drift from corrupting the estimate.
     wheel_odom_sensor:
-      # Using /odom_reliable from QoS relay (bridges BEST_EFFORT to RELIABLE)
-      topic: /odom_reliable
+      topic: /platform_velocity_controller_nav2/odom
       queue_size: 10
-      # 2D position from wheels (no z)
-      position_dimensions: ['x', 'y']
-      # Heading only
+      pose_loss:
+        type: fuse_loss::HuberLoss
+        a: 1.0
+      linear_velocity_loss:
+        type: fuse_loss::HuberLoss
+        a: 1.0
+      position_dimensions: ['x', 'y', 'z']
       orientation_dimensions: ['yaw']
-      # Forward velocity
-      linear_velocity_dimensions: ['x']
-      # Turn rate
+      linear_velocity_dimensions: ['x', 'y']
       angular_velocity_dimensions: ['yaw']
-      # Use relative constraints (pose changes rather than absolute poses)
       differential: true
-      # Covariances - use large values for unused dimensions
-      # Order: x, y, z, roll, pitch, yaw
-      pose_covariance_diagonal: [0.05, 0.05, 1e9, 1e9, 1e9, 0.05]
-      # Order: vx, vy, vz, vroll, vpitch, vyaw
-      twist_covariance_diagonal: [0.05, 1e9, 1e9, 1e9, 1e9, 0.05]
+      independent: false
+      use_twist_covariance: true
+
+    # IMU sensor — provides orientation and yaw rate
+    # - Roll/pitch/yaw orientation (absolute, covariance 0.001 vs wheel odom 0.5)
+    # - Yaw angular velocity from gyroscope
+    imu_sensor:
+      topic: /imu_sensor_broadcaster/imu_reliable
+      orientation_dimensions: ['roll', 'pitch', 'yaw']
+      angular_velocity_dimensions: ['yaw']
+      queue_size: 10
 
     # Publish filtered odometry
     publishers:
@@ -59,8 +78,8 @@ state_estimator:
 
     filtered_publisher:
       topic: 'odom_filtered'
-      base_link_frame_id: 'base_footprint'
-      base_link_output_frame_id: 'base_footprint'
+      base_link_frame_id: 'ridgeback_base_link'
+      base_link_output_frame_id: 'ridgeback_base_link'
       odom_frame_id: 'odom'
       map_frame_id: 'map'
       world_frame_id: 'odom'
diff --git a/src/hangar_sim/description/front_left_wheel_link.xml b/src/hangar_sim/description/front_left_wheel_link.xml
index f06fcf854..9d16b089f 100644
--- a/src/hangar_sim/description/front_left_wheel_link.xml
+++ b/src/hangar_sim/description/front_left_wheel_link.xml
@@ -2,356 +2,238 @@
   <inertial
     pos="0 0 0"
     quat="0.707107 0 0 0.707107"
-    mass="1.0"
-    diaginertia="0.0524193 0.0303095 0.0303095"
+    mass="2.5"
+    diaginertia="0.0072010 0.0041213 0.0041213"
+  />
+  <joint
+    name="front_left_wheel"
+    pos="0 0 0"
+    axis="0 1 0"
+    damping="0.5"
+    armature="1.0"
   />
-  <joint name="front_left_wheel" pos="0 0 0" axis="0 1 0" />
 
-  <body
-    name="front_left_roller_0_link"
-    pos="0.0438515969889343 0.0 -0.01175000000000001"
-  >
+  <body name="front_left_roller_0_link" pos="0.0594000000 0.0 0.0000000000">
     <joint
       name="front_left_slipping_0_joint"
       type="hinge"
       pos="0 0 0"
-      axis="-0.006296806022131396 0.0435 -0.02350000000000002"
+      axis="-0.0000000000 -0.7071067812 0.7071067812"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="front_left_roller_1_link"
-    pos="0.04385159698893431 0.0 0.011749999999999998"
-  >
+  <body name="front_left_roller_1_link" pos="0.0420021428 0.0 0.0420021428">
     <joint
       name="front_left_slipping_1_joint"
       type="hinge"
       pos="0 0 0"
-      axis="0.006296806022131382 0.0435 -0.023499999999999997"
+      axis="-0.5000000000 -0.7071067812 0.5000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="front_left_roller_2_link"
-    pos="0.03210159698893431 0.0 0.032101596988934306"
-  >
+  <body name="front_left_roller_2_link" pos="0.0000000000 0.0 0.0594000000">
     <joint
       name="front_left_slipping_2_joint"
       type="hinge"
       pos="0 0 0"
-      axis="0.01720319397786861 0.0435 -0.017203193977868615"
+      axis="-0.7071067812 -0.7071067812 0.0000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="front_left_roller_3_link"
-    pos="0.011750000000000005 0.0 0.0438515969889343"
-  >
+  <body name="front_left_roller_3_link" pos="-0.0420021428 0.0 0.0420021428">
     <joint
       name="front_left_slipping_3_joint"
       type="hinge"
       pos="0 0 0"
-      axis="0.023500000000000004 0.0435 -0.006296806022131389"
+      axis="-0.5000000000 -0.7071067812 -0.5000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="front_left_roller_4_link"
-    pos="-0.011749999999999993 0.0 0.04385159698893431"
-  >
+  <body name="front_left_roller_4_link" pos="-0.0594000000 0.0 0.0000000000">
     <joint
       name="front_left_slipping_4_joint"
       type="hinge"
       pos="0 0 0"
-      axis="0.023499999999999993 0.0435 0.006296806022131382"
+      axis="-0.0000000000 -0.7071067812 -0.7071067812"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="front_left_roller_5_link"
-    pos="-0.0321015969889343 0.0 0.03210159698893432"
-  >
+  <body name="front_left_roller_5_link" pos="-0.0420021428 0.0 -0.0420021428">
     <joint
       name="front_left_slipping_5_joint"
       type="hinge"
       pos="0 0 0"
-      axis="0.01720319397786862 0.0435 0.0172031939778686"
+      axis="0.5000000000 -0.7071067812 -0.5000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="front_left_roller_6_link"
-    pos="-0.0438515969889343 0.0 0.01175000000000001"
-  >
+  <body name="front_left_roller_6_link" pos="-0.0000000000 0.0 -0.0594000000">
     <joint
       name="front_left_slipping_6_joint"
       type="hinge"
       pos="0 0 0"
-      axis="0.006296806022131389 0.0435 0.02350000000000001"
+      axis="0.7071067812 -0.7071067812 -0.0000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="front_left_roller_7_link"
-    pos="-0.043851596988934316 0.0 -0.01174999999999999"
-  >
+  <body name="front_left_roller_7_link" pos="0.0420021428 0.0 -0.0420021428">
     <joint
       name="front_left_slipping_7_joint"
       type="hinge"
       pos="0 0 0"
-      axis="-0.006296806022131375 0.0435 0.023499999999999993"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
-  </body>
-
-  <body
-    name="front_left_roller_8_link"
-    pos="-0.03210159698893432 0.0 -0.03210159698893429"
-  >
-    <joint
-      name="front_left_slipping_8_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.017203193977868604 0.0435 0.017203193977868618"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
-  </body>
-
-  <body
-    name="front_left_roller_9_link"
-    pos="-0.011750000000000016 0.0 -0.0438515969889343"
-  >
-    <joint
-      name="front_left_slipping_9_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.023500000000000014 0.0435 0.006296806022131396"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
-  </body>
-
-  <body
-    name="front_left_roller_10_link"
-    pos="0.011749999999999981 0.0 -0.043851596988934316"
-  >
-    <joint
-      name="front_left_slipping_10_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.023499999999999976 0.0435 -0.006296806022131368"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
-  </body>
-
-  <body
-    name="front_left_roller_11_link"
-    pos="0.032101596988934285 0.0 -0.032101596988934326"
-  >
-    <joint
-      name="front_left_slipping_11_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.017203193977868635 0.0435 -0.01720319397786861"
+      axis="0.5000000000 -0.7071067812 0.5000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 </body>
diff --git a/src/hangar_sim/description/front_right_wheel_link.xml b/src/hangar_sim/description/front_right_wheel_link.xml
index 38c6acce2..a99d647c2 100644
--- a/src/hangar_sim/description/front_right_wheel_link.xml
+++ b/src/hangar_sim/description/front_right_wheel_link.xml
@@ -2,356 +2,238 @@
   <inertial
     pos="0 0 0"
     quat="0.707107 0 0 0.707107"
-    mass="1.0"
-    diaginertia="0.0524193 0.0303095 0.0303095"
+    mass="2.5"
+    diaginertia="0.0072010 0.0041213 0.0041213"
+  />
+  <joint
+    name="front_right_wheel"
+    pos="0 0 0"
+    axis="0 1 0"
+    damping="0.5"
+    armature="1.0"
   />
-  <joint name="front_right_wheel" pos="0 0 0" axis="0 1 0" />
 
-  <body
-    name="front_right_roller_0_link"
-    pos="0.04385159698893431 0.0 0.011749999999999998"
-  >
+  <body name="front_right_roller_0_link" pos="0.0594000000 0.0 0.0000000000">
     <joint
       name="front_right_slipping_0_joint"
       type="hinge"
       pos="0 0 0"
-      axis="-0.006296806022131382 0.0435 0.023499999999999997"
+      axis="-0.0000000000 0.7071067812 0.7071067812"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="front_right_roller_1_link"
-    pos="0.03210159698893431 0.0 0.032101596988934306"
-  >
+  <body name="front_right_roller_1_link" pos="0.0420021428 0.0 0.0420021428">
     <joint
       name="front_right_slipping_1_joint"
       type="hinge"
       pos="0 0 0"
-      axis="-0.01720319397786861 0.0435 0.017203193977868615"
+      axis="-0.5000000000 0.7071067812 0.5000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="front_right_roller_2_link"
-    pos="0.011750000000000005 0.0 0.0438515969889343"
-  >
+  <body name="front_right_roller_2_link" pos="0.0000000000 0.0 0.0594000000">
     <joint
       name="front_right_slipping_2_joint"
       type="hinge"
       pos="0 0 0"
-      axis="-0.023500000000000004 0.0435 0.006296806022131389"
+      axis="-0.7071067812 0.7071067812 0.0000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="front_right_roller_3_link"
-    pos="-0.011749999999999993 0.0 0.04385159698893431"
-  >
+  <body name="front_right_roller_3_link" pos="-0.0420021428 0.0 0.0420021428">
     <joint
       name="front_right_slipping_3_joint"
       type="hinge"
       pos="0 0 0"
-      axis="-0.023499999999999993 0.0435 -0.006296806022131382"
+      axis="-0.5000000000 0.7071067812 -0.5000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="front_right_roller_4_link"
-    pos="-0.0321015969889343 0.0 0.03210159698893432"
-  >
+  <body name="front_right_roller_4_link" pos="-0.0594000000 0.0 0.0000000000">
     <joint
       name="front_right_slipping_4_joint"
       type="hinge"
       pos="0 0 0"
-      axis="-0.01720319397786862 0.0435 -0.0172031939778686"
+      axis="-0.0000000000 0.7071067812 -0.7071067812"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="front_right_roller_5_link"
-    pos="-0.0438515969889343 0.0 0.011750000000000012"
-  >
+  <body name="front_right_roller_5_link" pos="-0.0420021428 0.0 -0.0420021428">
     <joint
       name="front_right_slipping_5_joint"
       type="hinge"
       pos="0 0 0"
-      axis="-0.006296806022131389 0.0435 -0.02350000000000001"
+      axis="0.5000000000 0.7071067812 -0.5000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="front_right_roller_6_link"
-    pos="-0.043851596988934316 0.0 -0.011749999999999991"
-  >
+  <body name="front_right_roller_6_link" pos="-0.0000000000 0.0 -0.0594000000">
     <joint
       name="front_right_slipping_6_joint"
       type="hinge"
       pos="0 0 0"
-      axis="0.006296806022131375 0.0435 -0.023499999999999993"
+      axis="0.7071067812 0.7071067812 -0.0000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="front_right_roller_7_link"
-    pos="-0.03210159698893432 0.0 -0.03210159698893429"
-  >
+  <body name="front_right_roller_7_link" pos="0.0420021428 0.0 -0.0420021428">
     <joint
       name="front_right_slipping_7_joint"
       type="hinge"
       pos="0 0 0"
-      axis="0.017203193977868604 0.0435 -0.017203193977868618"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
-  </body>
-
-  <body
-    name="front_right_roller_8_link"
-    pos="-0.011750000000000014 0.0 -0.0438515969889343"
-  >
-    <joint
-      name="front_right_slipping_8_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.023500000000000014 0.0435 -0.006296806022131396"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
-  </body>
-
-  <body
-    name="front_right_roller_9_link"
-    pos="0.01174999999999998 0.0 -0.043851596988934316"
-  >
-    <joint
-      name="front_right_slipping_9_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.023499999999999976 0.0435 0.006296806022131368"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
-  </body>
-
-  <body
-    name="front_right_roller_10_link"
-    pos="0.032101596988934285 0.0 -0.032101596988934326"
-  >
-    <joint
-      name="front_right_slipping_10_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.017203193977868635 0.0435 0.01720319397786861"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
-  </body>
-
-  <body
-    name="front_right_roller_11_link"
-    pos="0.0438515969889343 0.0 -0.01175000000000001"
-  >
-    <joint
-      name="front_right_slipping_11_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.006296806022131396 0.0435 0.02350000000000002"
+      axis="0.5000000000 0.7071067812 0.5000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 </body>
diff --git a/src/hangar_sim/description/hangar_scene.xml b/src/hangar_sim/description/hangar_scene.xml
index a5c4d2e66..6629ca677 100644
--- a/src/hangar_sim/description/hangar_scene.xml
+++ b/src/hangar_sim/description/hangar_scene.xml
@@ -5,7 +5,13 @@
   <compiler angle="radian" meshdir="assets" autolimits="true" />
   <include file="ur5e_ridgeback.xml" />
   <include file="hangar.xml" />
-  <option integrator="implicit" timestep="0.01" />
+  <option
+    integrator="implicitfast"
+    timestep="0.025"
+    noslip_iterations="5"
+    impratio="20"
+    cone="elliptic"
+  />
   <visual>
     <headlight diffuse="1.0 1.0 1.0" ambient="0.4 0.4 0.4" specular="0 0 0" />
     <rgba haze="0.15 0.25 0.35 1" />
@@ -14,6 +20,18 @@
   </visual>
 
   <worldbody>
+    <!-- Flat ground plane for stable wheel contact (mesh floor is unreliable with small rollers) -->
+    <geom
+      name="ground_plane"
+      type="plane"
+      size="50 50 0.1"
+      pos="0 0 0"
+      contype="1"
+      conaffinity="1"
+      condim="4"
+      friction="5 0.005 0.0001"
+      rgba="0 0 0 0"
+    />
     <!-- Scene Camera -->
     <site
       name="scene_camera_optical_frame"
@@ -33,7 +51,7 @@
   <keyframe>
     <key
       name="default"
-      qpos="-0.0576239 -0.0172046 -0.0185516 0.00183643 -0.104441 -5.86852e-08 -5.78748e-08 -5.23725e-08 -4.36527e-08 -3.40517e-08 -2.61422e-08 -2.20435e-08 -2.28539e-08 -2.83562e-08 -0.574246 -0.581266 -5.45865e-08 -0.230591 3.23249e-08 2.59432e-08 1.70253e-08 7.96051e-09 1.17788e-09 -1.50522e-09 6.30151e-10 7.01182e-09 -0.230843 -0.260034 3.17772e-08 3.44603e-08 -0.188217 1.65809e-08 1.02713e-08 1.21264e-09 -8.16771e-09 -1.53564e-08 -1.84271e-08 -1.65571e-08 -1.02474e-08 -0.24846 -0.2671 1.53802e-08 1.84509e-08 -0.186578 -1.85093e-08 -1.66726e-08 -1.0385e-08 -1.33104e-09 8.06315e-09 1.52804e-08 1.83869e-08 1.65503e-08 1.02626e-08 -0.503391 -0.520458 -1.54028e-08 -0.00374438 -1.18339 1.00254 0.806741 1.5707 3.15041 -9.76535 3.7638 0.124902 0.97941 1.32151e-06 -9.14725e-06 0.201881 -9.42551 5.1824 0.124903 0.803499 9.50258e-06 -3.70911e-08 0.595306 -11.438 1.94748 0.78904 -0.383066 0.383808 0.594164 -0.594071 -12.0004 5.15872 0.124902 0.666052 -7.33131e-06 -6.66383e-06 0.745905 -12.0488 1.18718 0.125027 0.34658 -0.34669 -0.616068 0.616562 -11.1756 5.30231 0.124903 0.98111 -1.25581e-05 -2.16444e-06 -0.193453 -8.84404 3.16202 0.124904 0.964915 1.25055e-05 2.1434e-06 0.262563 -10.7473
+      qpos="-0.0576239 -0.0172046 -0.0185516 0.00183643 -0.104441 0 0 0 0 0 0 0 0 -0.230591 0 0 0 0 0 0 0 0 -0.188217 0 0 0 0 0 0 0 0 -0.186578 0 0 0 0 0 0 0 0 -0.00374438 -1.18339 1.00254 0.806741 1.5707 3.15041 -9.76535 3.7638 0.124902 0.97941 1.32151e-06 -9.14725e-06 0.201881 -9.42551 5.1824 0.124903 0.803499 9.50258e-06 -3.70911e-08 0.595306 -11.438 1.94748 0.78904 -0.383066 0.383808 0.594164 -0.594071 -12.0004 5.15872 0.124902 0.666052 -7.33131e-06 -6.66383e-06 0.745905 -12.0488 1.18718 0.125027 0.34658 -0.34669 -0.616068 0.616562 -11.1756 5.30231 0.124903 0.98111 -1.25581e-05 -2.16444e-06 -0.193453 -8.84404 3.16202 0.124904 0.964915 1.25055e-05 2.1434e-06 0.262563 -10.7473
        0 -1.57 0 -0.707107 -0.707107 0"
     />
   </keyframe>
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 85c866dd2..dd0b93e96 100644
--- a/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro
+++ b/src/hangar_sim/description/picknik_ur_mujoco_ros2_control.xacro
@@ -16,6 +16,18 @@
           <param name="mujoco_viewer">${mujoco_viewer}</param>
           <param name="mujoco_keyframe">default</param>
       </hardware>
+      <sensor name="imu_site">
+        <state_interface name="orientation.x"/>
+        <state_interface name="orientation.y"/>
+        <state_interface name="orientation.z"/>
+        <state_interface name="orientation.w"/>
+        <state_interface name="angular_velocity.x"/>
+        <state_interface name="angular_velocity.y"/>
+        <state_interface name="angular_velocity.z"/>
+        <state_interface name="linear_acceleration.x"/>
+        <state_interface name="linear_acceleration.y"/>
+        <state_interface name="linear_acceleration.z"/>
+      </sensor>
     </ros2_control>
   </xacro:macro>
 </robot>
diff --git a/src/hangar_sim/description/rear_left_wheel_link.xml b/src/hangar_sim/description/rear_left_wheel_link.xml
index 2484daab0..5b914a1fb 100644
--- a/src/hangar_sim/description/rear_left_wheel_link.xml
+++ b/src/hangar_sim/description/rear_left_wheel_link.xml
@@ -2,356 +2,238 @@
   <inertial
     pos="0 0 0"
     quat="0.707107 0 0 0.707107"
-    mass="1.0"
-    diaginertia="0.0524193 0.0303095 0.0303095"
+    mass="2.5"
+    diaginertia="0.0072010 0.0041213 0.0041213"
+  />
+  <joint
+    name="rear_left_wheel"
+    pos="0 0 0"
+    axis="0 1 0"
+    damping="0.5"
+    armature="1.0"
   />
-  <joint name="rear_left_wheel" pos="0 0 0" axis="0 1 0" />
 
-  <body
-    name="rear_left_roller_0_link"
-    pos="0.04385159698893431 0.0 0.011749999999999998"
-  >
+  <body name="rear_left_roller_0_link" pos="0.0594000000 0.0 0.0000000000">
     <joint
       name="rear_left_slipping_0_joint"
       type="hinge"
       pos="0 0 0"
-      axis="-0.006296806022131382 0.0435 0.023499999999999997"
+      axis="-0.0000000000 0.7071067812 0.7071067812"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="rear_left_roller_1_link"
-    pos="0.03210159698893431 0.0 0.032101596988934306"
-  >
+  <body name="rear_left_roller_1_link" pos="0.0420021428 0.0 0.0420021428">
     <joint
       name="rear_left_slipping_1_joint"
       type="hinge"
       pos="0 0 0"
-      axis="-0.01720319397786861 0.0435 0.017203193977868615"
+      axis="-0.5000000000 0.7071067812 0.5000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="rear_left_roller_2_link"
-    pos="0.011750000000000005 0.0 0.0438515969889343"
-  >
+  <body name="rear_left_roller_2_link" pos="0.0000000000 0.0 0.0594000000">
     <joint
       name="rear_left_slipping_2_joint"
       type="hinge"
       pos="0 0 0"
-      axis="-0.023500000000000004 0.0435 0.006296806022131389"
+      axis="-0.7071067812 0.7071067812 0.0000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="rear_left_roller_3_link"
-    pos="-0.011749999999999993 0.0 0.04385159698893431"
-  >
+  <body name="rear_left_roller_3_link" pos="-0.0420021428 0.0 0.0420021428">
     <joint
       name="rear_left_slipping_3_joint"
       type="hinge"
       pos="0 0 0"
-      axis="-0.023499999999999993 0.0435 -0.006296806022131382"
+      axis="-0.5000000000 0.7071067812 -0.5000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="rear_left_roller_4_link"
-    pos="-0.0321015969889343 0.0 0.03210159698893432"
-  >
+  <body name="rear_left_roller_4_link" pos="-0.0594000000 0.0 0.0000000000">
     <joint
       name="rear_left_slipping_4_joint"
       type="hinge"
       pos="0 0 0"
-      axis="-0.01720319397786862 0.0435 -0.0172031939778686"
+      axis="-0.0000000000 0.7071067812 -0.7071067812"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="rear_left_roller_5_link"
-    pos="-0.0438515969889343 0.0 0.011750000000000012"
-  >
+  <body name="rear_left_roller_5_link" pos="-0.0420021428 0.0 -0.0420021428">
     <joint
       name="rear_left_slipping_5_joint"
       type="hinge"
       pos="0 0 0"
-      axis="-0.006296806022131389 0.0435 -0.02350000000000001"
+      axis="0.5000000000 0.7071067812 -0.5000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="rear_left_roller_6_link"
-    pos="-0.043851596988934316 0.0 -0.011749999999999991"
-  >
+  <body name="rear_left_roller_6_link" pos="-0.0000000000 0.0 -0.0594000000">
     <joint
       name="rear_left_slipping_6_joint"
       type="hinge"
       pos="0 0 0"
-      axis="0.006296806022131375 0.0435 -0.023499999999999993"
+      axis="0.7071067812 0.7071067812 -0.0000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="rear_left_roller_7_link"
-    pos="-0.03210159698893432 0.0 -0.03210159698893429"
-  >
+  <body name="rear_left_roller_7_link" pos="0.0420021428 0.0 -0.0420021428">
     <joint
       name="rear_left_slipping_7_joint"
       type="hinge"
       pos="0 0 0"
-      axis="0.017203193977868604 0.0435 -0.017203193977868618"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
-  </body>
-
-  <body
-    name="rear_left_roller_8_link"
-    pos="-0.011750000000000014 0.0 -0.0438515969889343"
-  >
-    <joint
-      name="rear_left_slipping_8_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.023500000000000014 0.0435 -0.006296806022131396"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
-  </body>
-
-  <body
-    name="rear_left_roller_9_link"
-    pos="0.01174999999999998 0.0 -0.043851596988934316"
-  >
-    <joint
-      name="rear_left_slipping_9_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.023499999999999976 0.0435 0.006296806022131368"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
-  </body>
-
-  <body
-    name="rear_left_roller_10_link"
-    pos="0.032101596988934285 0.0 -0.032101596988934326"
-  >
-    <joint
-      name="rear_left_slipping_10_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.017203193977868635 0.0435 0.01720319397786861"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
-  </body>
-
-  <body
-    name="rear_left_roller_11_link"
-    pos="0.0438515969889343 0.0 -0.01175000000000001"
-  >
-    <joint
-      name="rear_left_slipping_11_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="0.006296806022131396 0.0435 0.02350000000000002"
+      axis="0.5000000000 0.7071067812 0.5000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 </body>
diff --git a/src/hangar_sim/description/rear_right_wheel_link.xml b/src/hangar_sim/description/rear_right_wheel_link.xml
index c044b53c6..553eaf417 100644
--- a/src/hangar_sim/description/rear_right_wheel_link.xml
+++ b/src/hangar_sim/description/rear_right_wheel_link.xml
@@ -2,356 +2,238 @@
   <inertial
     pos="0 0 0"
     quat="0.707107 0 0 0.707107"
-    mass="1.0"
-    diaginertia="0.0524193 0.0303095 0.0303095"
+    mass="2.5"
+    diaginertia="0.0072010 0.0041213 0.0041213"
+  />
+  <joint
+    name="rear_right_wheel"
+    pos="0 0 0"
+    axis="0 1 0"
+    damping="0.5"
+    armature="1.0"
   />
-  <joint name="rear_right_wheel" pos="0 0 0" axis="0 1 0" />
 
-  <body
-    name="rear_right_roller_0_link"
-    pos="0.0438515969889343 0.0 -0.01175000000000001"
-  >
+  <body name="rear_right_roller_0_link" pos="0.0594000000 0.0 0.0000000000">
     <joint
       name="rear_right_slipping_0_joint"
       type="hinge"
       pos="0 0 0"
-      axis="-0.006296806022131396 0.0435 -0.02350000000000002"
+      axis="-0.0000000000 -0.7071067812 0.7071067812"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="rear_right_roller_1_link"
-    pos="0.04385159698893431 0.0 0.011749999999999998"
-  >
+  <body name="rear_right_roller_1_link" pos="0.0420021428 0.0 0.0420021428">
     <joint
       name="rear_right_slipping_1_joint"
       type="hinge"
       pos="0 0 0"
-      axis="0.006296806022131382 0.0435 -0.023499999999999997"
+      axis="-0.5000000000 -0.7071067812 0.5000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="rear_right_roller_2_link"
-    pos="0.03210159698893431 0.0 0.032101596988934306"
-  >
+  <body name="rear_right_roller_2_link" pos="0.0000000000 0.0 0.0594000000">
     <joint
       name="rear_right_slipping_2_joint"
       type="hinge"
       pos="0 0 0"
-      axis="0.01720319397786861 0.0435 -0.017203193977868615"
+      axis="-0.7071067812 -0.7071067812 0.0000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="rear_right_roller_3_link"
-    pos="0.011750000000000005 0.0 0.0438515969889343"
-  >
+  <body name="rear_right_roller_3_link" pos="-0.0420021428 0.0 0.0420021428">
     <joint
       name="rear_right_slipping_3_joint"
       type="hinge"
       pos="0 0 0"
-      axis="0.023500000000000004 0.0435 -0.006296806022131389"
+      axis="-0.5000000000 -0.7071067812 -0.5000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="rear_right_roller_4_link"
-    pos="-0.011749999999999993 0.0 0.04385159698893431"
-  >
+  <body name="rear_right_roller_4_link" pos="-0.0594000000 0.0 0.0000000000">
     <joint
       name="rear_right_slipping_4_joint"
       type="hinge"
       pos="0 0 0"
-      axis="0.023499999999999993 0.0435 0.006296806022131382"
+      axis="-0.0000000000 -0.7071067812 -0.7071067812"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="rear_right_roller_5_link"
-    pos="-0.0321015969889343 0.0 0.03210159698893432"
-  >
+  <body name="rear_right_roller_5_link" pos="-0.0420021428 0.0 -0.0420021428">
     <joint
       name="rear_right_slipping_5_joint"
       type="hinge"
       pos="0 0 0"
-      axis="0.01720319397786862 0.0435 0.0172031939778686"
+      axis="0.5000000000 -0.7071067812 -0.5000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="rear_right_roller_6_link"
-    pos="-0.0438515969889343 0.0 0.01175000000000001"
-  >
+  <body name="rear_right_roller_6_link" pos="-0.0000000000 0.0 -0.0594000000">
     <joint
       name="rear_right_slipping_6_joint"
       type="hinge"
       pos="0 0 0"
-      axis="0.006296806022131389 0.0435 0.02350000000000001"
+      axis="0.7071067812 -0.7071067812 -0.0000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 
-  <body
-    name="rear_right_roller_7_link"
-    pos="-0.043851596988934316 0.0 -0.01174999999999999"
-  >
+  <body name="rear_right_roller_7_link" pos="0.0420021428 0.0 -0.0420021428">
     <joint
       name="rear_right_slipping_7_joint"
       type="hinge"
       pos="0 0 0"
-      axis="-0.006296806022131375 0.0435 0.023499999999999993"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
-  </body>
-
-  <body
-    name="rear_right_roller_8_link"
-    pos="-0.03210159698893432 0.0 -0.03210159698893429"
-  >
-    <joint
-      name="rear_right_slipping_8_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.017203193977868604 0.0435 0.017203193977868618"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
-  </body>
-
-  <body
-    name="rear_right_roller_9_link"
-    pos="-0.011750000000000016 0.0 -0.0438515969889343"
-  >
-    <joint
-      name="rear_right_slipping_9_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.023500000000000014 0.0435 0.006296806022131396"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
-  </body>
-
-  <body
-    name="rear_right_roller_10_link"
-    pos="0.011749999999999981 0.0 -0.043851596988934316"
-  >
-    <joint
-      name="rear_right_slipping_10_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.023499999999999976 0.0435 -0.006296806022131368"
-      damping="0.1"
-      limited="false"
-      actuatorfrclimited="false"
-    />
-    <inertial
-      pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
-    />
-    <geom
-      size="0.08"
-      quat="1 0 0 0"
-      type="sphere"
-      rgba="0.2 0.2 0.2 1"
-      contype="1"
-      conaffinity="0"
-    />
-  </body>
-
-  <body
-    name="rear_right_roller_11_link"
-    pos="0.032101596988934285 0.0 -0.032101596988934326"
-  >
-    <joint
-      name="rear_right_slipping_11_joint"
-      type="hinge"
-      pos="0 0 0"
-      axis="-0.017203193977868635 0.0435 -0.01720319397786861"
+      axis="0.5000000000 -0.7071067812 0.5000000000"
       damping="0.1"
       limited="false"
       actuatorfrclimited="false"
     />
     <inertial
       pos="0 0 0"
-      quat="0.711549 0.711549 0 0 "
-      mass="0.001"
-      diaginertia="0.00001 0.00001 0.00001"
+      mass="0.01"
+      diaginertia="0.0000010890 0.0000010890 0.0000010890"
     />
     <geom
-      size="0.08"
-      quat="1 0 0 0"
+      size="0.0165"
       type="sphere"
       rgba="0.2 0.2 0.2 1"
       contype="1"
-      conaffinity="0"
+      conaffinity="1"
+      condim="4"
+      friction="0.5 0.005 0.0001"
+      solimp="0.9 0.95 0.001"
+      solref="0.002 1"
     />
   </body>
 </body>
diff --git a/src/hangar_sim/description/ur5e_ridgeback.xml b/src/hangar_sim/description/ur5e_ridgeback.xml
index 4216f0992..e5203cd66 100644
--- a/src/hangar_sim/description/ur5e_ridgeback.xml
+++ b/src/hangar_sim/description/ur5e_ridgeback.xml
@@ -196,7 +196,7 @@
   </asset>
 
   <worldbody>
-    <body name="base_platform_rotation" pos="0 0 0" euler="0 0 0">
+    <body name="base_platform_rotation" pos="0 0 -0.048" euler="0 0 0">
       <inertial mass="0.01" pos="0 0 0" diaginertia="0.001 0.001 0.001" />
       <!--      <joint-->
       <!--        type="free"-->
@@ -205,9 +205,14 @@
       <!--        frictionloss="0"-->
       <!--        armature="0"-->
       <!--      />-->
-      <joint name="linear_x_joint" type="slide" axis="1 0 0" />
-      <joint name="linear_y_joint" type="slide" axis="0 1 0" />
-      <joint name="rotational_yaw_joint" type="hinge" axis="0 0 1" />
+      <joint name="linear_x_joint" type="slide" axis="1 0 0" damping="0.5" />
+      <joint name="linear_y_joint" type="slide" axis="0 1 0" damping="0.5" />
+      <joint
+        name="rotational_yaw_joint"
+        type="hinge"
+        axis="0 0 1"
+        damping="1.0"
+      />
       <body name="base_platform" pos="0 0 0" euler="0 0 0">
         <body name="ridgeback_base_link">
           <!-- Front SICK TIM571: mounted at x=+0.45m (front bumper), z=0.15m.
@@ -290,6 +295,8 @@
             quat="0.92388 0 0 0.382683"
             type="box"
             rgba="0.4 0.4 0.4 1"
+            contype="0"
+            conaffinity="0"
           />
           <geom
             name="top_link"
@@ -311,7 +318,9 @@
               name="front_rocker"
               pos="0 0 0"
               axis="1 0 0"
-              range="-0.08726 0.08726"
+              range="-0.001 0.001"
+              stiffness="10000"
+              damping="500"
             />
             <body name="front_left_wheel_link" pos="0 0.2755 0.07">
               <include file="front_left_wheel_link.xml" />
diff --git a/src/hangar_sim/launch/agent_bridge.launch.xml b/src/hangar_sim/launch/agent_bridge.launch.xml
index 3cc960c03..b2aa75434 100644
--- a/src/hangar_sim/launch/agent_bridge.launch.xml
+++ b/src/hangar_sim/launch/agent_bridge.launch.xml
@@ -3,5 +3,4 @@
   <include
     file="$(find-pkg-share moveit_studio_agent)/launch/studio_agent_bridge.launch.xml"
   />
-  <include file="$(find-pkg-share hangar_sim)/launch/fuse.launch.py" />
 </launch>
diff --git a/src/hangar_sim/launch/fuse.launch.py b/src/hangar_sim/launch/fuse.launch.py
deleted file mode 100644
index 089f93f53..000000000
--- a/src/hangar_sim/launch/fuse.launch.py
+++ /dev/null
@@ -1,38 +0,0 @@
-#! /usr/bin/env python3
-
-# Copyright 2024 PickNik Robotics
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-from launch import LaunchDescription
-from launch.substitutions import PathJoinSubstitution
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-
-def generate_launch_description():
-    pkg_dir = FindPackageShare("hangar_sim")
-
-    return LaunchDescription(
-        [
-            # Run the fuse fixed-lag smoother for mobile base state estimation
-            Node(
-                package="fuse_optimizers",
-                executable="fixed_lag_smoother_node",
-                name="state_estimator",
-                parameters=[
-                    PathJoinSubstitution([pkg_dir, "config", "fuse", "fuse.yaml"])
-                ],
-            ),
-        ]
-    )
diff --git a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
index 848672db8..964b33d09 100644
--- a/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
+++ b/src/hangar_sim/launch/sim/robot_drivers_to_persist_sim.launch.py
@@ -268,19 +268,11 @@ def generate_launch_description():
         arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "map", "odom"],
     )
 
-    # Publish odometry as joint state messages
-    odom_to_joint_state_repub = Node(
-        package="hangar_sim",
-        executable="odometry_joint_state_publisher.py",
-        name="odometry_joint_state_publisher",
-        output="log",
-    )
-
-    # QoS relay to bridge BEST_EFFORT odom to RELIABLE for fuse
-    odom_qos_relay = Node(
+    # QoS relay to bridge BEST_EFFORT odom and IMU to RELIABLE for fuse
+    sensor_qos_relay = Node(
         package="hangar_sim",
         executable="odom_qos_relay.py",
-        name="odom_qos_relay",
+        name="sensor_qos_relay",
         output="log",
     )
 
@@ -341,8 +333,7 @@ def generate_launch_description():
 
     ld.add_action(static_tf_world_to_map)
     ld.add_action(static_tf_map_to_odom)
-    ld.add_action(odom_to_joint_state_repub)
-    ld.add_action(odom_qos_relay)
+    ld.add_action(sensor_qos_relay)
     ld.add_action(laser_filter_node)
     ld.add_action(fuse_state_estimator)
 
diff --git a/src/hangar_sim/objectives/cartesian_draw_geometry_from_file.xml b/src/hangar_sim/objectives/cartesian_draw_geometry_from_file.xml
index df61eec23..ac5833ee2 100644
--- a/src/hangar_sim/objectives/cartesian_draw_geometry_from_file.xml
+++ b/src/hangar_sim/objectives/cartesian_draw_geometry_from_file.xml
@@ -66,6 +66,10 @@
           />
         </Control>
       </Control>
+      <Action
+        ID="SwitchController"
+        activate_controllers="platform_velocity_controller_nav2"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/hangar_sim/objectives/cartesian_path_with_collision_checking.xml b/src/hangar_sim/objectives/cartesian_path_with_collision_checking.xml
index cf7e4dcc2..22a3ea683 100644
--- a/src/hangar_sim/objectives/cartesian_path_with_collision_checking.xml
+++ b/src/hangar_sim/objectives/cartesian_path_with_collision_checking.xml
@@ -76,6 +76,10 @@
           solution="{debug_solution}"
         />
       </Control>
+      <Action
+        ID="SwitchController"
+        activate_controllers="platform_velocity_controller_nav2"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/hangar_sim/objectives/cartesian_plan_simple_square.xml b/src/hangar_sim/objectives/cartesian_plan_simple_square.xml
index e681f3450..9f833f091 100644
--- a/src/hangar_sim/objectives/cartesian_plan_simple_square.xml
+++ b/src/hangar_sim/objectives/cartesian_plan_simple_square.xml
@@ -115,6 +115,10 @@
         execution_pipeline="jtc"
         controller_action_name="/joint_trajectory_controller/follow_joint_trajectory"
       />
+      <Action
+        ID="SwitchController"
+        activate_controllers="platform_velocity_controller_nav2"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/hangar_sim/objectives/compute_ik_plan_and_move.xml b/src/hangar_sim/objectives/compute_ik_plan_and_move.xml
index 2b39b6214..8b442c70e 100644
--- a/src/hangar_sim/objectives/compute_ik_plan_and_move.xml
+++ b/src/hangar_sim/objectives/compute_ik_plan_and_move.xml
@@ -62,6 +62,10 @@
         execution_pipeline="jtc"
         controller_action_name="/joint_trajectory_controller/follow_joint_trajectory"
       />
+      <Action
+        ID="SwitchController"
+        activate_controllers="platform_velocity_controller_nav2"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/hangar_sim/objectives/find_and_spray_plane.xml b/src/hangar_sim/objectives/find_and_spray_plane.xml
index 79e975a43..482bf3a27 100644
--- a/src/hangar_sim/objectives/find_and_spray_plane.xml
+++ b/src/hangar_sim/objectives/find_and_spray_plane.xml
@@ -75,6 +75,10 @@
         execution_pipeline="jtc"
         controller_action_name="/joint_trajectory_controller/follow_joint_trajectory"
       />
+      <Action
+        ID="SwitchController"
+        activate_controllers="platform_velocity_controller_nav2"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/hangar_sim/objectives/generate_grid_pattern_on_airplane.xml b/src/hangar_sim/objectives/generate_grid_pattern_on_airplane.xml
index 66b6cf31f..1a9146443 100644
--- a/src/hangar_sim/objectives/generate_grid_pattern_on_airplane.xml
+++ b/src/hangar_sim/objectives/generate_grid_pattern_on_airplane.xml
@@ -68,6 +68,10 @@
         execution_pipeline="jtc"
         controller_action_name="/joint_trajectory_controller/follow_joint_trajectory"
       />
+      <Action
+        ID="SwitchController"
+        activate_controllers="platform_velocity_controller_nav2"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/hangar_sim/objectives/move_to_arm_upright.xml b/src/hangar_sim/objectives/move_to_arm_upright.xml
index 956be2f43..7693eab55 100644
--- a/src/hangar_sim/objectives/move_to_arm_upright.xml
+++ b/src/hangar_sim/objectives/move_to_arm_upright.xml
@@ -21,6 +21,10 @@
         velocity_scale_factor="1.0"
         waypoint_name="Arm upright"
       />
+      <Action
+        ID="SwitchController"
+        activate_controllers="platform_velocity_controller_nav2"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/hangar_sim/objectives/move_to_look_at_plane.xml b/src/hangar_sim/objectives/move_to_look_at_plane.xml
index db343c510..8c669f955 100644
--- a/src/hangar_sim/objectives/move_to_look_at_plane.xml
+++ b/src/hangar_sim/objectives/move_to_look_at_plane.xml
@@ -21,6 +21,10 @@
         velocity_scale_factor="1.0"
         waypoint_name="Look at Plane"
       />
+      <Action
+        ID="SwitchController"
+        activate_controllers="platform_velocity_controller_nav2"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/hangar_sim/objectives/plan_path_along_surface.xml b/src/hangar_sim/objectives/plan_path_along_surface.xml
index 2f6c17609..125fbd80d 100644
--- a/src/hangar_sim/objectives/plan_path_along_surface.xml
+++ b/src/hangar_sim/objectives/plan_path_along_surface.xml
@@ -106,6 +106,10 @@
         execution_pipeline="jtc"
         controller_action_name="/joint_trajectory_controller/follow_joint_trajectory"
       />
+      <Action
+        ID="SwitchController"
+        activate_controllers="platform_velocity_controller_nav2"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/hangar_sim/objectives/plan_path_along_surface_-_loop.xml b/src/hangar_sim/objectives/plan_path_along_surface_-_loop.xml
index 792087ef0..0b461306d 100644
--- a/src/hangar_sim/objectives/plan_path_along_surface_-_loop.xml
+++ b/src/hangar_sim/objectives/plan_path_along_surface_-_loop.xml
@@ -120,6 +120,10 @@
           acceleration_scale_factor="1.0"
           _skipIf="true"
         />
+        <Action
+          ID="SwitchController"
+          activate_controllers="platform_velocity_controller_nav2"
+        />
       </Control>
     </Decorator>
   </BehaviorTree>
diff --git a/src/hangar_sim/objectives/point-to-point_trajectory.xml b/src/hangar_sim/objectives/point-to-point_trajectory.xml
index fabafdd47..44569f9b0 100644
--- a/src/hangar_sim/objectives/point-to-point_trajectory.xml
+++ b/src/hangar_sim/objectives/point-to-point_trajectory.xml
@@ -22,6 +22,10 @@
         controller_action_server="/joint_trajectory_controller/follow_joint_trajectory"
         acceleration_scale_factor="1.0"
       />
+      <Action
+        ID="SwitchController"
+        activate_controllers="platform_velocity_controller_nav2"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/hangar_sim/objectives/request_teleoperation.xml b/src/hangar_sim/objectives/request_teleoperation.xml
index 465167901..f42eb0c4e 100644
--- a/src/hangar_sim/objectives/request_teleoperation.xml
+++ b/src/hangar_sim/objectives/request_teleoperation.xml
@@ -229,6 +229,10 @@
           </Control>
         </Decorator>
       </Control>
+      <Action
+        ID="SwitchController"
+        activate_controllers="platform_velocity_controller_nav2"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/hangar_sim/objectives/solution_-_find_and_spray_plane.xml b/src/hangar_sim/objectives/solution_-_find_and_spray_plane.xml
index 452bd5a90..8651d9e93 100644
--- a/src/hangar_sim/objectives/solution_-_find_and_spray_plane.xml
+++ b/src/hangar_sim/objectives/solution_-_find_and_spray_plane.xml
@@ -75,6 +75,10 @@
         execution_pipeline="jtc"
         controller_action_name="/joint_trajectory_controller/follow_joint_trajectory"
       />
+      <Action
+        ID="SwitchController"
+        activate_controllers="platform_velocity_controller_nav2"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/hangar_sim/objectives/solution_draw_picknik.xml b/src/hangar_sim/objectives/solution_draw_picknik.xml
index fc8b165a1..acc2ca9f6 100644
--- a/src/hangar_sim/objectives/solution_draw_picknik.xml
+++ b/src/hangar_sim/objectives/solution_draw_picknik.xml
@@ -62,6 +62,10 @@
           />
         </Control>
       </Control>
+      <Action
+        ID="SwitchController"
+        activate_controllers="platform_velocity_controller_nav2"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/hangar_sim/objectives/solution_draw_square.xml b/src/hangar_sim/objectives/solution_draw_square.xml
index 0829f11ee..5c20d6162 100644
--- a/src/hangar_sim/objectives/solution_draw_square.xml
+++ b/src/hangar_sim/objectives/solution_draw_square.xml
@@ -115,6 +115,10 @@
         execution_pipeline="jtc"
         controller_action_name="/joint_trajectory_controller/follow_joint_trajectory"
       />
+      <Action
+        ID="SwitchController"
+        activate_controllers="platform_velocity_controller_nav2"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/hangar_sim/objectives/solution_generate_coverage_path.xml b/src/hangar_sim/objectives/solution_generate_coverage_path.xml
index 14d20a015..f66cdef2b 100644
--- a/src/hangar_sim/objectives/solution_generate_coverage_path.xml
+++ b/src/hangar_sim/objectives/solution_generate_coverage_path.xml
@@ -68,6 +68,10 @@
         execution_pipeline="jtc"
         controller_action_name="/joint_trajectory_controller/follow_joint_trajectory"
       />
+      <Action
+        ID="SwitchController"
+        activate_controllers="platform_velocity_controller_nav2"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/hangar_sim/objectives/solution_move_forward_2m.xml b/src/hangar_sim/objectives/solution_move_forward_2m.xml
index 8d356cd67..937058b20 100644
--- a/src/hangar_sim/objectives/solution_move_forward_2m.xml
+++ b/src/hangar_sim/objectives/solution_move_forward_2m.xml
@@ -27,6 +27,10 @@
         controller_names="joint_trajectory_controller"
         target_pose="{stamped_pose}"
       />
+      <Action
+        ID="SwitchController"
+        activate_controllers="platform_velocity_controller_nav2"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/hangar_sim/objectives/solution_spray_plane.xml b/src/hangar_sim/objectives/solution_spray_plane.xml
index 8597daeb0..dc35e8036 100644
--- a/src/hangar_sim/objectives/solution_spray_plane.xml
+++ b/src/hangar_sim/objectives/solution_spray_plane.xml
@@ -65,6 +65,10 @@
         execution_pipeline="jtc"
         controller_action_name="/joint_trajectory_controller/follow_joint_trajectory"
       />
+      <Action
+        ID="SwitchController"
+        activate_controllers="platform_velocity_controller_nav2"
+      />
     </Control>
   </BehaviorTree>
   <TreeNodesModel>
diff --git a/src/hangar_sim/script/odom_qos_relay.py b/src/hangar_sim/script/odom_qos_relay.py
index 11c588922..b82fa5704 100755
--- a/src/hangar_sim/script/odom_qos_relay.py
+++ b/src/hangar_sim/script/odom_qos_relay.py
@@ -28,12 +28,14 @@
 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
-"""Relay node to bridge BEST_EFFORT odometry to RELIABLE QoS for fuse."""
+"""Relay node to bridge BEST_EFFORT sensor topics to RELIABLE QoS for fuse."""
 
 import rclpy
 from rclpy.node import Node
 
+from geometry_msgs.msg import Twist
 from nav_msgs.msg import Odometry
+from sensor_msgs.msg import Imu
 
 from rclpy.qos import (
     QoSProfile,
@@ -43,11 +45,11 @@
 )
 
 
-class OdomQoSRelay(Node):
-    """Relay node that subscribes to odometry with BEST_EFFORT and republishes with RELIABLE QoS."""
+class SensorQoSRelay(Node):
+    """Relay node that subscribes to sensor topics with BEST_EFFORT and republishes with RELIABLE QoS."""
 
     def __init__(self):
-        super().__init__("odom_qos_relay")
+        super().__init__("sensor_qos_relay")
 
         # Subscribe with BEST_EFFORT to match mujoco_system publisher
         qos_sub = QoSProfile(
@@ -65,18 +67,41 @@ def __init__(self):
             depth=10,
         )
 
-        self.sub = self.create_subscription(
+        # Odometry relay
+        self.odom_sub = self.create_subscription(
             Odometry, "/odom", self.odom_callback, qos_sub
         )
-        self.pub = self.create_publisher(Odometry, "/odom_reliable", qos_pub)
+        self.odom_pub = self.create_publisher(Odometry, "/odom_reliable", qos_pub)
+
+        # IMU relay
+        self.imu_sub = self.create_subscription(
+            Imu, "/imu_sensor_broadcaster/imu", self.imu_callback, qos_sub
+        )
+        self.imu_pub = self.create_publisher(
+            Imu, "/imu_sensor_broadcaster/imu_reliable", qos_pub
+        )
+
+        # cmd_vel relay
+        self.cmd_vel_sub = self.create_subscription(
+            Twist, "/cmd_vel", self.cmd_vel_callback, 10
+        )
+        self.cmd_vel_pub = self.create_publisher(
+            Twist, "/platform_velocity_controller_nav2/cmd_vel_unstamped", 10
+        )
 
     def odom_callback(self, msg):
-        self.pub.publish(msg)
+        self.odom_pub.publish(msg)
+
+    def imu_callback(self, msg):
+        self.imu_pub.publish(msg)
+
+    def cmd_vel_callback(self, msg):
+        self.cmd_vel_pub.publish(msg)
 
 
 def main(args=None):
     rclpy.init(args=args)
-    node = OdomQoSRelay()
+    node = SensorQoSRelay()
     rclpy.spin(node)
     node.destroy_node()
     rclpy.shutdown()