added IMU to the ridgeback

suchkristenwow · suchkristenwow · commit ad2204d9ebf6 · 2026-02-06T10:28:02.000-07:00
diff --git a/src/hangar_sim/config/config.yaml b/src/hangar_sim/config/config.yaml
@@ -86,6 +86,7 @@ ros2_control:
   # [Optional, default=[]]
   controllers_active_at_startup:
     - "force_torque_sensor_broadcaster"
+    - "imu_sensor_broadcaster"
     - "joint_state_broadcaster"
     - "platform_velocity_controller"
     - "vacuum_gripper"
diff --git a/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml b/src/hangar_sim/config/control/picknik_ur.ros2_control.yaml
@@ -11,6 +11,8 @@ controller_manager:
       type: clearpath_mecanum_drive_controller/MecanumDriveController
     platform_velocity_controller_nav2:
       type: clearpath_mecanum_drive_controller/MecanumDriveController
+    imu_sensor_broadcaster:
+      type: imu_sensor_broadcaster/IMUSensorBroadcaster
     vacuum_gripper:
       type: position_controllers/GripperActionController
     velocity_force_controller:
@@ -277,6 +279,11 @@ force_torque_sensor_broadcaster:
       - torque.z
     frame_id: tool0
 
+imu_sensor_broadcaster:
+  ros__parameters:
+    sensor_name: imu_site
+    frame_id: imu_link
+
 
 velocity_force_controller:
   ros__parameters:
diff --git a/src/hangar_sim/description/ur5e_ridgeback.xml b/src/hangar_sim/description/ur5e_ridgeback.xml
@@ -260,6 +260,15 @@
           <body name="rear_right_wheel_link" pos="-0.319 -0.2755 0.12">
             <include file="rear_right_wheel_link.xml" />
           </body>
+          <!-- IMU: position from ridgeback.urdf.xacro imu_joint, relative to chassis_link -->
+          <body name="imu_link" pos="0.2085 -0.2902 0.1681">
+            <inertial
+              mass="0.001"
+              pos="0 0 0"
+              diaginertia="1e-09 1e-09 1e-09"
+            />
+            <site name="imu_site" pos="0 0 0" size="0.01" rgba="0 1 0 1" />
+          </body>
           <body name="ham_assem" pos="0 0 0.278813" euler="1.5708 -1.57079 0">
             <geom
               name="ham_assem"
@@ -574,5 +583,8 @@
   <sensor>
     <force site="tcp_fts_sensor" />
     <torque site="tcp_fts_sensor" />
+    <framequat objtype="site" objname="imu_site" />
+    <gyro site="imu_site" />
+    <accelerometer site="imu_site" />
   </sensor>
 </mujoco>
