diff --git a/src/sherpa/urdf/components/imu.xacro b/src/sherpa/urdf/components/imu.xacro
new file mode 100644
index 0000000..80f93be
--- /dev/null
+++ b/src/sherpa/urdf/components/imu.xacro
@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <xacro:macro name="imu" params="name parent xyz rpy frequency">
+
+        <link name="${name}_link">
+            <inertial>
+                <mass value="0.01"/>
+                <inertia 
+                    ixx="0.001828"
+                    ixy="0.0"
+                    ixz="0.0"
+                    iyy="0.001828"
+                    iyz="0.0"
+                    izz="0.003528"
+                />
+            </inertial>
+        </link>
+        <joint name="${name}_joint" type="fixed">
+            <parent link="${parent}_link"/>
+            <child link="${name}_link"/>
+            <origin xyz="${xyz}" rpy="${rpy}"/>
+        </joint>
+        <gazebo reference="${name}_link">
+            <sensor name="${name}" type="imu">
+                <always_on>1</always_on>
+                <update_rate>${frequency}</update_rate>
+                <gz_frame_id>${name}_link</gz_frame_id>
+                <visualize>true</visualize>
+                <topic>/imu</topic>
+            </sensor>
+        </gazebo>
+    </xacro:macro>
+</robot>
\ No newline at end of file
diff --git a/src/sherpa/urdf/components/mecanum_wheel.xacro b/src/sherpa/urdf/components/mecanum_wheel.xacro
index 8a74a00..58f5523 100644
--- a/src/sherpa/urdf/components/mecanum_wheel.xacro
+++ b/src/sherpa/urdf/components/mecanum_wheel.xacro
@@ -13,11 +13,11 @@
 
         <!-- get left or right wheel -->
         <xacro:if value="${direction == 'l'}">
-            <xacro:property name="mesh" value="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_left.STL"/>
+            <xacro:property name="mesh" value="package://sherpa/urdf/meshes//mecanum_wheel_left.STL"/>
             <xacro:property name="friction_dir" value="-1"/>
         </xacro:if>
         <xacro:if value="${direction == 'r'}">
-            <xacro:property name="mesh" value="https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift/tip/files/meshes/mecanum_wheel_right.STL"/>
+            <xacro:property name="mesh" value="package://sherpa/urdf/meshes//mecanum_wheel_right.STL"/>
             <xacro:property name="friction_dir" value="1"/>
         </xacro:if>
 
@@ -46,18 +46,18 @@
                 <material name="mecanum_wheel_material"/>
             </visual>
             <collision name='collision'>
-            <geometry>
-                <sphere radius="${radius}"/>
-            </geometry>
-            <surface>
-                <friction>
-                <ode>
-                    <mu>1.2</mu>
-                    <mu2>0</mu2>
-                    <fdir1 gz:expressed_in="${parent}">1 ${friction_dir} 0</fdir1>
-                </ode>
-                </friction>
-            </surface>
+                <geometry>
+                    <sphere radius="${radius}"/>
+                </geometry>
+                <surface>
+                    <friction>
+                    <ode>
+                        <mu>1.2</mu>
+                        <mu2>0</mu2>
+                        <fdir1 gz:expressed_in="${parent}_link">1 ${friction_dir} 0</fdir1>
+                    </ode>
+                    </friction>
+                </surface>
             </collision>
         </link>
         <joint name='${name}_joint' type='continuous'>
diff --git a/src/sherpa/urdf/components/ouster_lidar.xacro b/src/sherpa/urdf/components/ouster_lidar.xacro
index 280fd66..6061d23 100644
--- a/src/sherpa/urdf/components/ouster_lidar.xacro
+++ b/src/sherpa/urdf/components/ouster_lidar.xacro
@@ -140,11 +140,6 @@
         </joint>
         <gazebo reference="${name}_link">
             <sensor name="${name}" type="gpu_lidar">
-                <plugin
-                    filename="gz-sim-sensors-system"
-                    name="gz::sim::systems::Sensors">
-                    <render_engine>ogre2</render_engine>
-                </plugin>
                 <update_rate>${frequency}</update_rate>
                 <topic>/velodyne_points</topic>
                 <gz_frame_id>${name}_link</gz_frame_id>
diff --git a/src/sherpa/urdf/meshes/mecanum_wheel_left.STL b/src/sherpa/urdf/meshes/mecanum_wheel_left.STL
new file mode 100644
index 0000000..35d6a9e
Binary files /dev/null and b/src/sherpa/urdf/meshes/mecanum_wheel_left.STL differ
diff --git a/src/sherpa/urdf/meshes/mecanum_wheel_right.STL b/src/sherpa/urdf/meshes/mecanum_wheel_right.STL
new file mode 100644
index 0000000..ddca9ba
Binary files /dev/null and b/src/sherpa/urdf/meshes/mecanum_wheel_right.STL differ
diff --git a/src/sherpa/urdf/sherpa.xacro b/src/sherpa/urdf/sherpa.xacro
old mode 100755
new mode 100644
index e9bdf30..eb3d166
--- a/src/sherpa/urdf/sherpa.xacro
+++ b/src/sherpa/urdf/sherpa.xacro
@@ -1,5 +1,11 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="sherpa">   
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="sherpa">  
+    <gazebo>
+        <plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="libgz_ros2_control-system.so">
+            <parameters>$(find sherpa)/config/mecanum_controller.yaml</parameters>
+        </plugin>
+    </gazebo>
+
     <link name="base_link">
         <inertial>
             <mass value="15"/>
@@ -14,9 +20,11 @@
         </inertial>
     </link>
 
+    <xacro:include filename="components/imu.xacro"/>
     <xacro:include filename="components/mecanum_wheel.xacro"/>
     <xacro:include filename="components/ouster_lidar.xacro"/>
 
+    <xacro:imu name="imu" parent="base" xyz="0 0 0.156" rpy="0 0 0" frequency="300"/>
     <xacro:ouster_lidar name="lidar" parent="base" xyz="0 0 0.15" rpy="0 0 0" model="vlp16" frequency="10" topic="/points"/>
     
     <xacro:mecanum_wheel name="wheel_fr" parent="base" xyz=" 0.11 -0.11 0" rpy="0 0 0" direction="r" radius="0.04" mass="0.25"/>
@@ -24,17 +32,33 @@
     <xacro:mecanum_wheel name="wheel_br" parent="base" xyz="-0.11 -0.11 0" rpy="0 0 0" direction="l" radius="0.04" mass="0.25"/>
     <xacro:mecanum_wheel name="wheel_bl" parent="base" xyz="-0.11  0.11 0" rpy="0 0 0" direction="r" radius="0.04" mass="0.25"/>
 
-    <gazebo>
-        <plugin filename="gz-sim-mecanum-drive-system" name="gz::sim::systems::MecanumDrive">
-            <front_left_joint>wheel_fl_joint</front_left_joint>
-            <front_right_joint>wheel_fr_joint</front_right_joint>
-            <back_left_joint>wheel_bl_joint</back_left_joint>
-            <back_right_joint>wheel_br_joint</back_right_joint>
-            <wheel_separation>0.22</wheel_separation>
-            <wheelbase>0.22</wheelbase>
-            <wheel_radius>0.04</wheel_radius>
-            <min_acceleration>-5</min_acceleration>
-            <max_acceleration>5</max_acceleration>
-        </plugin>
-    </gazebo>
+    <ros2_control name="SherpaSystem" type="system">
+        <hardware>
+            <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+        </hardware>
+        
+        <joint name="wheel_fl_joint">
+            <command_interface name="velocity"/>
+            <state_interface name="velocity"/>
+            <state_interface name="position"/>
+        </joint>
+
+        <joint name="wheel_fr_joint">
+            <command_interface name="velocity"/>
+            <state_interface name="velocity"/>
+            <state_interface name="position"/>
+        </joint>
+
+        <joint name="wheel_bl_joint">
+            <command_interface name="velocity"/>
+            <state_interface name="velocity"/>
+            <state_interface name="position"/>
+        </joint>
+
+        <joint name="wheel_br_joint">
+            <command_interface name="velocity"/>
+            <state_interface name="velocity"/>
+            <state_interface name="position"/>
+        </joint>
+    </ros2_control>
 </robot>
\ No newline at end of file