diff --git a/urc_gazebo/urdf/meshes/collision/base_link_collision.STL b/urc_gazebo/urdf/meshes/collision/base_link_collision.STL
new file mode 100644
index 00000000..ff8fd0f3
Binary files /dev/null and b/urc_gazebo/urdf/meshes/collision/base_link_collision.STL differ
diff --git a/urc_gazebo/urdf/meshes/collision/bicep_collision.STL b/urc_gazebo/urdf/meshes/collision/bicep_collision.STL
new file mode 100644
index 00000000..6e355ce2
Binary files /dev/null and b/urc_gazebo/urdf/meshes/collision/bicep_collision.STL differ
diff --git a/urc_gazebo/urdf/meshes/collision/claw_collision.STL b/urc_gazebo/urdf/meshes/collision/claw_collision.STL
new file mode 100644
index 00000000..19e2c6f7
Binary files /dev/null and b/urc_gazebo/urdf/meshes/collision/claw_collision.STL differ
diff --git a/urc_gazebo/urdf/meshes/collision/elbow_collision.STL b/urc_gazebo/urdf/meshes/collision/elbow_collision.STL
new file mode 100644
index 00000000..9cb6a576
Binary files /dev/null and b/urc_gazebo/urdf/meshes/collision/elbow_collision.STL differ
diff --git a/urc_gazebo/urdf/meshes/collision/left_center_wheel_collision.STL b/urc_gazebo/urdf/meshes/collision/left_center_wheel_collision.STL
new file mode 100644
index 00000000..06776550
Binary files /dev/null and b/urc_gazebo/urdf/meshes/collision/left_center_wheel_collision.STL differ
diff --git a/urc_gazebo/urdf/meshes/collision/left_front_wheel_collision.STL b/urc_gazebo/urdf/meshes/collision/left_front_wheel_collision.STL
new file mode 100644
index 00000000..3b8aec64
Binary files /dev/null and b/urc_gazebo/urdf/meshes/collision/left_front_wheel_collision.STL differ
diff --git a/urc_gazebo/urdf/meshes/collision/left_rear_wheel_collision.STL b/urc_gazebo/urdf/meshes/collision/left_rear_wheel_collision.STL
new file mode 100644
index 00000000..8de57624
Binary files /dev/null and b/urc_gazebo/urdf/meshes/collision/left_rear_wheel_collision.STL differ
diff --git a/urc_gazebo/urdf/meshes/collision/leftbogie_collision.STL b/urc_gazebo/urdf/meshes/collision/leftbogie_collision.STL
new file mode 100644
index 00000000..08957103
Binary files /dev/null and b/urc_gazebo/urdf/meshes/collision/leftbogie_collision.STL differ
diff --git a/urc_gazebo/urdf/meshes/leftgripper_collision.STL b/urc_gazebo/urdf/meshes/collision/leftgripper_collision.STL
similarity index 100%
rename from urc_gazebo/urdf/meshes/leftgripper_collision.STL
rename to urc_gazebo/urdf/meshes/collision/leftgripper_collision.STL
diff --git a/urc_gazebo/urdf/meshes/collision/leftrocker_collision.STL b/urc_gazebo/urdf/meshes/collision/leftrocker_collision.STL
new file mode 100644
index 00000000..e393bda5
Binary files /dev/null and b/urc_gazebo/urdf/meshes/collision/leftrocker_collision.STL differ
diff --git a/urc_gazebo/urdf/meshes/collision/right_center_wheel_collision.STL b/urc_gazebo/urdf/meshes/collision/right_center_wheel_collision.STL
new file mode 100644
index 00000000..f18193ea
Binary files /dev/null and b/urc_gazebo/urdf/meshes/collision/right_center_wheel_collision.STL differ
diff --git a/urc_gazebo/urdf/meshes/collision/right_front_wheel_collision.STL b/urc_gazebo/urdf/meshes/collision/right_front_wheel_collision.STL
new file mode 100644
index 00000000..3c63b40b
Binary files /dev/null and b/urc_gazebo/urdf/meshes/collision/right_front_wheel_collision.STL differ
diff --git a/urc_gazebo/urdf/meshes/collision/right_rear_wheel_collision.STL b/urc_gazebo/urdf/meshes/collision/right_rear_wheel_collision.STL
new file mode 100644
index 00000000..77e15c1b
Binary files /dev/null and b/urc_gazebo/urdf/meshes/collision/right_rear_wheel_collision.STL differ
diff --git a/urc_gazebo/urdf/meshes/collision/rightbogie_collision.STL b/urc_gazebo/urdf/meshes/collision/rightbogie_collision.STL
new file mode 100644
index 00000000..4584ef00
Binary files /dev/null and b/urc_gazebo/urdf/meshes/collision/rightbogie_collision.STL differ
diff --git a/urc_gazebo/urdf/meshes/rightgripper_collision.STL b/urc_gazebo/urdf/meshes/collision/rightgripper_collision.STL
similarity index 100%
rename from urc_gazebo/urdf/meshes/rightgripper_collision.STL
rename to urc_gazebo/urdf/meshes/collision/rightgripper_collision.STL
diff --git a/urc_gazebo/urdf/meshes/collision/rightrocker_collision.STL b/urc_gazebo/urdf/meshes/collision/rightrocker_collision.STL
new file mode 100644
index 00000000..4f135a33
Binary files /dev/null and b/urc_gazebo/urdf/meshes/collision/rightrocker_collision.STL differ
diff --git a/urc_gazebo/urdf/meshes/collision/shoulder_collision.STL b/urc_gazebo/urdf/meshes/collision/shoulder_collision.STL
new file mode 100644
index 00000000..beace34c
Binary files /dev/null and b/urc_gazebo/urdf/meshes/collision/shoulder_collision.STL differ
diff --git a/urc_gazebo/urdf/meshes/collision/spine_collision.STL b/urc_gazebo/urdf/meshes/collision/spine_collision.STL
new file mode 100644
index 00000000..d70ff058
Binary files /dev/null and b/urc_gazebo/urdf/meshes/collision/spine_collision.STL differ
diff --git a/urc_gazebo/urdf/meshes/collision/wrist_collision.STL b/urc_gazebo/urdf/meshes/collision/wrist_collision.STL
new file mode 100644
index 00000000..45ff8fae
Binary files /dev/null and b/urc_gazebo/urdf/meshes/collision/wrist_collision.STL differ
diff --git a/urc_gazebo/urdf/meshes/base_link.STL b/urc_gazebo/urdf/meshes/old/base_link.STL
similarity index 100%
rename from urc_gazebo/urdf/meshes/base_link.STL
rename to urc_gazebo/urdf/meshes/old/base_link.STL
diff --git a/urc_gazebo/urdf/meshes/base_link_collision.STL b/urc_gazebo/urdf/meshes/old/base_link_collision.STL
similarity index 100%
rename from urc_gazebo/urdf/meshes/base_link_collision.STL
rename to urc_gazebo/urdf/meshes/old/base_link_collision.STL
diff --git a/urc_gazebo/urdf/meshes/bicep.STL b/urc_gazebo/urdf/meshes/old/bicep.STL
similarity index 100%
rename from urc_gazebo/urdf/meshes/bicep.STL
rename to urc_gazebo/urdf/meshes/old/bicep.STL
diff --git a/urc_gazebo/urdf/meshes/bicep_collision.STL b/urc_gazebo/urdf/meshes/old/bicep_collision.STL
similarity index 100%
rename from urc_gazebo/urdf/meshes/bicep_collision.STL
rename to urc_gazebo/urdf/meshes/old/bicep_collision.STL
diff --git a/urc_gazebo/urdf/meshes/claw.STL b/urc_gazebo/urdf/meshes/old/claw.STL
similarity index 100%
rename from urc_gazebo/urdf/meshes/claw.STL
rename to urc_gazebo/urdf/meshes/old/claw.STL
diff --git a/urc_gazebo/urdf/meshes/claw_collision.STL b/urc_gazebo/urdf/meshes/old/claw_collision.STL
similarity index 100%
rename from urc_gazebo/urdf/meshes/claw_collision.STL
rename to urc_gazebo/urdf/meshes/old/claw_collision.STL
diff --git a/urc_gazebo/urdf/meshes/elbow.STL b/urc_gazebo/urdf/meshes/old/elbow.STL
similarity index 100%
rename from urc_gazebo/urdf/meshes/elbow.STL
rename to urc_gazebo/urdf/meshes/old/elbow.STL
diff --git a/urc_gazebo/urdf/meshes/elbow_collision.STL b/urc_gazebo/urdf/meshes/old/elbow_collision.STL
similarity index 100%
rename from urc_gazebo/urdf/meshes/elbow_collision.STL
rename to urc_gazebo/urdf/meshes/old/elbow_collision.STL
diff --git a/urc_gazebo/urdf/meshes/forearm.STL b/urc_gazebo/urdf/meshes/old/forearm.STL
similarity index 100%
rename from urc_gazebo/urdf/meshes/forearm.STL
rename to urc_gazebo/urdf/meshes/old/forearm.STL
diff --git a/urc_gazebo/urdf/meshes/forearm_collision.STL b/urc_gazebo/urdf/meshes/old/forearm_collision.STL
similarity index 100%
rename from urc_gazebo/urdf/meshes/forearm_collision.STL
rename to urc_gazebo/urdf/meshes/old/forearm_collision.STL
diff --git a/urc_gazebo/urdf/meshes/leftgripper.STL b/urc_gazebo/urdf/meshes/old/leftgripper.STL
similarity index 100%
rename from urc_gazebo/urdf/meshes/leftgripper.STL
rename to urc_gazebo/urdf/meshes/old/leftgripper.STL
diff --git a/urc_gazebo/urdf/meshes/old/leftgripper_collision.STL b/urc_gazebo/urdf/meshes/old/leftgripper_collision.STL
new file mode 100755
index 00000000..1de812d3
Binary files /dev/null and b/urc_gazebo/urdf/meshes/old/leftgripper_collision.STL differ
diff --git a/urc_gazebo/urdf/meshes/rightgripper.STL b/urc_gazebo/urdf/meshes/old/rightgripper.STL
similarity index 100%
rename from urc_gazebo/urdf/meshes/rightgripper.STL
rename to urc_gazebo/urdf/meshes/old/rightgripper.STL
diff --git a/urc_gazebo/urdf/meshes/old/rightgripper_collision.STL b/urc_gazebo/urdf/meshes/old/rightgripper_collision.STL
new file mode 100755
index 00000000..658227b7
Binary files /dev/null and b/urc_gazebo/urdf/meshes/old/rightgripper_collision.STL differ
diff --git a/urc_gazebo/urdf/meshes/shoulder.STL b/urc_gazebo/urdf/meshes/old/shoulder.STL
similarity index 100%
rename from urc_gazebo/urdf/meshes/shoulder.STL
rename to urc_gazebo/urdf/meshes/old/shoulder.STL
diff --git a/urc_gazebo/urdf/meshes/shoulder_collision.STL b/urc_gazebo/urdf/meshes/old/shoulder_collision.STL
similarity index 100%
rename from urc_gazebo/urdf/meshes/shoulder_collision.STL
rename to urc_gazebo/urdf/meshes/old/shoulder_collision.STL
diff --git a/urc_gazebo/urdf/meshes/wrist.STL b/urc_gazebo/urdf/meshes/old/wrist.STL
similarity index 100%
rename from urc_gazebo/urdf/meshes/wrist.STL
rename to urc_gazebo/urdf/meshes/old/wrist.STL
diff --git a/urc_gazebo/urdf/meshes/wrist_collision.STL b/urc_gazebo/urdf/meshes/old/wrist_collision.STL
similarity index 100%
rename from urc_gazebo/urdf/meshes/wrist_collision.STL
rename to urc_gazebo/urdf/meshes/old/wrist_collision.STL
diff --git a/urc_gazebo/urdf/meshes/visual/base_link.STL b/urc_gazebo/urdf/meshes/visual/base_link.STL
new file mode 100644
index 00000000..b0a457df
Binary files /dev/null and b/urc_gazebo/urdf/meshes/visual/base_link.STL differ
diff --git a/urc_gazebo/urdf/meshes/visual/bicep.STL b/urc_gazebo/urdf/meshes/visual/bicep.STL
new file mode 100644
index 00000000..ea3f71ec
Binary files /dev/null and b/urc_gazebo/urdf/meshes/visual/bicep.STL differ
diff --git a/urc_gazebo/urdf/meshes/visual/claw.STL b/urc_gazebo/urdf/meshes/visual/claw.STL
new file mode 100644
index 00000000..f84e658c
Binary files /dev/null and b/urc_gazebo/urdf/meshes/visual/claw.STL differ
diff --git a/urc_gazebo/urdf/meshes/visual/elbow.STL b/urc_gazebo/urdf/meshes/visual/elbow.STL
new file mode 100644
index 00000000..a9d62535
Binary files /dev/null and b/urc_gazebo/urdf/meshes/visual/elbow.STL differ
diff --git a/urc_gazebo/urdf/meshes/visual/left_center_wheel.STL b/urc_gazebo/urdf/meshes/visual/left_center_wheel.STL
new file mode 100644
index 00000000..f0da61d0
Binary files /dev/null and b/urc_gazebo/urdf/meshes/visual/left_center_wheel.STL differ
diff --git a/urc_gazebo/urdf/meshes/visual/left_front_wheel.STL b/urc_gazebo/urdf/meshes/visual/left_front_wheel.STL
new file mode 100644
index 00000000..e5802504
Binary files /dev/null and b/urc_gazebo/urdf/meshes/visual/left_front_wheel.STL differ
diff --git a/urc_gazebo/urdf/meshes/visual/left_rear_wheel.STL b/urc_gazebo/urdf/meshes/visual/left_rear_wheel.STL
new file mode 100644
index 00000000..ed3e2ae5
Binary files /dev/null and b/urc_gazebo/urdf/meshes/visual/left_rear_wheel.STL differ
diff --git a/urc_gazebo/urdf/meshes/visual/leftbogie.STL b/urc_gazebo/urdf/meshes/visual/leftbogie.STL
new file mode 100644
index 00000000..4918bd5c
Binary files /dev/null and b/urc_gazebo/urdf/meshes/visual/leftbogie.STL differ
diff --git a/urc_gazebo/urdf/meshes/visual/leftgripper.STL b/urc_gazebo/urdf/meshes/visual/leftgripper.STL
new file mode 100644
index 00000000..eacb2e2c
Binary files /dev/null and b/urc_gazebo/urdf/meshes/visual/leftgripper.STL differ
diff --git a/urc_gazebo/urdf/meshes/visual/leftrocker.STL b/urc_gazebo/urdf/meshes/visual/leftrocker.STL
new file mode 100644
index 00000000..364360a8
Binary files /dev/null and b/urc_gazebo/urdf/meshes/visual/leftrocker.STL differ
diff --git a/urc_gazebo/urdf/meshes/visual/right_center_wheel.STL b/urc_gazebo/urdf/meshes/visual/right_center_wheel.STL
new file mode 100644
index 00000000..04b77fae
Binary files /dev/null and b/urc_gazebo/urdf/meshes/visual/right_center_wheel.STL differ
diff --git a/urc_gazebo/urdf/meshes/visual/right_front_wheel.STL b/urc_gazebo/urdf/meshes/visual/right_front_wheel.STL
new file mode 100644
index 00000000..2cd1c325
Binary files /dev/null and b/urc_gazebo/urdf/meshes/visual/right_front_wheel.STL differ
diff --git a/urc_gazebo/urdf/meshes/visual/right_rear_wheel.STL b/urc_gazebo/urdf/meshes/visual/right_rear_wheel.STL
new file mode 100644
index 00000000..51b725b4
Binary files /dev/null and b/urc_gazebo/urdf/meshes/visual/right_rear_wheel.STL differ
diff --git a/urc_gazebo/urdf/meshes/visual/rightbogie.STL b/urc_gazebo/urdf/meshes/visual/rightbogie.STL
new file mode 100644
index 00000000..40ed044e
Binary files /dev/null and b/urc_gazebo/urdf/meshes/visual/rightbogie.STL differ
diff --git a/urc_gazebo/urdf/meshes/visual/rightgripper.STL b/urc_gazebo/urdf/meshes/visual/rightgripper.STL
new file mode 100644
index 00000000..19077784
Binary files /dev/null and b/urc_gazebo/urdf/meshes/visual/rightgripper.STL differ
diff --git a/urc_gazebo/urdf/meshes/visual/rightrocker.STL b/urc_gazebo/urdf/meshes/visual/rightrocker.STL
new file mode 100644
index 00000000..c9356db0
Binary files /dev/null and b/urc_gazebo/urdf/meshes/visual/rightrocker.STL differ
diff --git a/urc_gazebo/urdf/meshes/visual/shoulder.STL b/urc_gazebo/urdf/meshes/visual/shoulder.STL
new file mode 100644
index 00000000..de17c0ad
Binary files /dev/null and b/urc_gazebo/urdf/meshes/visual/shoulder.STL differ
diff --git a/urc_gazebo/urdf/meshes/visual/spine.STL b/urc_gazebo/urdf/meshes/visual/spine.STL
new file mode 100644
index 00000000..dd781ccd
Binary files /dev/null and b/urc_gazebo/urdf/meshes/visual/spine.STL differ
diff --git a/urc_gazebo/urdf/meshes/visual/wrist.STL b/urc_gazebo/urdf/meshes/visual/wrist.STL
new file mode 100644
index 00000000..6774e7a2
Binary files /dev/null and b/urc_gazebo/urdf/meshes/visual/wrist.STL differ
diff --git a/urc_hw_description/urdf/ros2_control.urdf b/urc_hw_description/urdf/ros2_control.urdf
index 829393b0..9dab3f40 100644
--- a/urc_hw_description/urdf/ros2_control.urdf
+++ b/urc_hw_description/urdf/ros2_control.urdf
@@ -52,4 +52,76 @@
             <state_interface name="cell8_voltage" />
         </sensor> -->
     </ros2_control>
-</robot>
\ No newline at end of file
+</robot>
+
+
+
+
+ <!-- mock_components/GenericSystem -->
+   <!-- gazebo_ros2_control/GazeboSystem -->
+   <!-- <ros2_control name="GazeboSimSystem" type="system">
+      <hardware>
+         <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+      </hardware>
+      <joint name="shoulderjoint">
+         <command_interface name="position" />
+         <state_interface name="position">
+            <param name="initial_value">0.0</param>
+         </state_interface>
+         <state_interface name="velocity" />
+         <state_interface name="effort" />
+      </joint>
+      <joint name="bicepjoint">
+         <command_interface name="position" />
+         <state_interface name="position">
+            <param name="initial_value">0.0</param>
+         </state_interface>
+         <state_interface name="velocity" />
+         <state_interface name="effort" />
+      </joint>
+      <joint name="elbowjoint">
+         <command_interface name="position" />
+         <state_interface name="position">
+            <param name="initial_value">0.0</param>
+         </state_interface>
+         <state_interface name="velocity" />
+         <state_interface name="effort" />
+      </joint>
+      <joint name="wristjoint">
+         <command_interface name="position" />
+         <state_interface name="position">
+            <param name="initial_value">0.0</param>
+         </state_interface>
+         <state_interface name="velocity" />
+         <state_interface name="effort" />
+      </joint>
+      <joint name="clawjoint">
+         <command_interface name="position" />
+         <state_interface name="position">
+            <param name="initial_value">0.0</param>
+         </state_interface>
+         <state_interface name="velocity" />
+         <state_interface name="effort" />
+      </joint>
+      <joint name="leftgripperjoint">
+         <command_interface name="position" />
+         <state_interface name="position">
+            <param name="initial_value">0.0</param>
+         </state_interface>
+         <state_interface name="velocity" />
+         <state_interface name="effort" />
+      </joint>
+      <joint name="rightgripperjoint">
+         <command_interface name="position" />
+         <state_interface name="position">
+            <param name="initial_value">0.0</param>
+         </state_interface>
+         <state_interface name="velocity" />
+         <state_interface name="effort" />
+      </joint>
+   </ros2_control> -->
+   <!-- <gazebo>
+      <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
+         <parameters>file://$(find urc_gazebo)/urdf/config/RoboticArm_params.yaml</parameters>
+      </plugin>
+   </gazebo> -->
\ No newline at end of file
diff --git a/urc_hw_description/urdf/walli.xacro b/urc_hw_description/urdf/walli.xacro
index d44439f9..aecaf70b 100644
--- a/urc_hw_description/urdf/walli.xacro
+++ b/urc_hw_description/urdf/walli.xacro
@@ -1,537 +1,581 @@
-<?xml version='1.0'?>
-<robot name="walli" xmlns:xacro="http://www.ros.org/wiki/xacro">
-  <xacro:include filename="$(find urc_hw_description)/urdf/walli_prop.xacro" />
-  <xacro:include filename="$(find urc_hw_description)/urdf/walli_inertials.xacro" />
-  <xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro" />
-  <xacro:include filename="$(find urc_hw_description)/urdf/walli_arm.urdf" />
+<?xml version="1.0" encoding="UTF-8"?>
+<robot name="walli" xmlns:xacro="http://ros.org/wiki/xacro">
 
-  <!-- Rover Base Link -->
-  <link name="base_link">
-    <visual>
-      <geometry>
-        <box size="${base_length} ${base_width} ${base_height}" />
-      </geometry>
-      <material name="Cyan">
-        <color rgba="0 1.0 1.0 1.0" />
-      </material>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="${base_length} ${base_width} ${base_height}" />
-      </geometry>
-    </collision>
-    <xacro:box_inertia m="15" w="${base_width}" d="${base_length}" h="${base_height}" />
-  </link>
-  <gazebo reference="base_link">
-    <material>Gazebo/Red</material>
-  </gazebo>
+  <!-- These includes do not work if we use the file://$(find urc_hw_description) format -->
+  <!-- <xacro:include filename="$(find urc_hw_description)/urdf/walli_prop.xacro"/>
+  <xacro:include filename="$(find urc_hw_description)/urdf/walli_inertials.xacro"/>
+  <xacro:include filename="$(find urc_hw_description)/urdf/walli_sensors.xacro"/> -->
 
-
-  <!-- Wheels -->
-  <xacro:macro name="wheel" params="prefix x_reflect y_reflect">
-    <link name="${prefix}_link">
+   <link name="base_link">
+      <inertial>
+         <origin xyz="-0.22771 0.0020794 0.13424" rpy="0 0 0" />
+         <mass value="2.485" />
+         <inertia ixx="0.017201" ixy="-2.0986E-11" ixz="5.8899E-06" iyy="0.0028906" iyz="5.0336E-07" izz="0.019124" />
+      </inertial>
       <visual>
-        <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
-        <geometry>
-          <cylinder radius="${wheel_radius}" length="${wheel_width}" />
-        </geometry>
-        <material name="Gray">
-          <color rgba="0.5 0.5 0.5 1.0" />
-        </material>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/visual/base_link.STL" />
+         </geometry>
       </visual>
       <collision>
-        <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
-        <geometry>
-          <cylinder radius="${wheel_radius}" length="${wheel_width}" />
-        </geometry>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/collision/base_link_collision.STL" />
+         </geometry>
       </collision>
-      <xacro:cylinder_inertia m="0.5" r="${wheel_radius}" h="${wheel_width}" />
-    </link>
-    <joint name="${prefix}_joint" type="continuous">
+   </link>
+   <link name="rightrocker">
+      <inertial>
+         <origin xyz="0.062687 0.034616 -0.016048" rpy="0 0 0" />
+         <mass value="0.57523" />
+         <inertia ixx="0.00040232" ixy="-8.4972E-11" ixz="-0.00023706" iyy="0.00095582" iyz="-4.2951E-08" izz="0.00061776" />
+      </inertial>
+      <visual>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/visual/rightrocker.STL" />
+         </geometry>
+      </visual>
+      <collision>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/collision/rightrocker_collision.STL" />
+         </geometry>
+      </collision>
+   </link>
+   <joint name="rightrockerjoint" type="fixed">
+      <origin xyz="-0.194 -0.33715 0.080295" rpy="0 0 0" />
       <parent link="base_link" />
-      <child link="${prefix}_link" />
-      <origin xyz="${x_reflect*wheel_xoff} ${y_reflect*(base_width/2+wheel_ygap)} ${-wheel_zoff}"
-        rpy="0 0 0" />
+      <child link="rightrocker" />
+      <axis xyz="0 0 0" />
+      <dynamics damping="60" friction="4" />
+   </joint>
+   <link name="right_rear_wheel">
+      <inertial>
+         <origin xyz="0.0010497 0.10248 0.00012788" rpy="0 0 0" />
+         <mass value="0.65429" />
+         <inertia ixx="0.00040499" ixy="-2.9713E-08" ixz="-2.8347E-07" iyy="0.00055081" iyz="2.6154E-08" izz="0.00040532" />
+      </inertial>
+      <visual>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/visual/right_rear_wheel.STL" />
+         </geometry>
+      </visual>
+      <collision>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/collision/right_rear_wheel_collision.STL" />
+         </geometry>
+      </collision>
+   </link>
+   <joint name="right_rear_wheel_joint" type="continuous">
+      <origin xyz="-0.36316 -0.17103 -0.19158" rpy="0 0 0" />
+      <parent link="rightrocker" />
+      <child link="right_rear_wheel" />
+      <axis xyz="0 -1 0" />
+      <dynamics damping="4" friction="20" />
+   </joint>
+   <link name="rightbogie">
+      <inertial>
+         <origin xyz="-6.8787E-09 0.032385 -0.015429" rpy="0 0 0" />
+         <mass value="0.66758" />
+         <inertia ixx="0.00019756" ixy="-5.9804E-10" ixz="9.4624E-11" iyy="0.00044836" iyz="2.6509E-13" izz="0.0003846" />
+      </inertial>
+      <visual>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/visual/rightbogie.STL" />
+         </geometry>
+      </visual>
+      <collision>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/collision/rightbogie_collision.STL" />
+         </geometry>
+      </collision>
+   </link>
+   <joint name="rightbogiejoint" type="fixed">
+      <origin xyz="0.24177 -0.00254 -0.074203" rpy="0 0 0" />
+      <parent link="rightrocker" />
+      <child link="rightbogie" />
+      <axis xyz="0 0 0" />
+      <dynamics damping="60" friction="4" />
+   </joint>
+   <link name="right_front_wheel">
+      <inertial>
+         <origin xyz="-0.00066285 0.10248 0.00082496" rpy="0 0 0" />
+         <mass value="0.65429" />
+         <inertia ixx="0.000405" ixy="-2.1882E-08" ixz="2.5112E-07" iyy="0.00055081" iyz="-4.9845E-08" izz="0.00040531" />
+      </inertial>
+      <visual>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/visual/right_front_wheel.STL" />
+         </geometry>
+      </visual>
+      <collision>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/collision/right_front_wheel_collision.STL" />
+         </geometry>
+      </collision>
+   </link>
+   <joint name="right_front_wheel_joint" type="continuous">
+      <origin xyz="0.20139 -0.16849 -0.11671" rpy="0 0 0" />
+      <parent link="rightbogie" />
+      <child link="right_front_wheel" />
+      <axis xyz="0 -1 0" />
+      <dynamics damping="4" friction="20" />
+   </joint>
+   <link name="right_center_wheel">
+      <inertial>
+         <origin xyz="0.0010444 0.10248 0.00016665" rpy="0 0 0" />
+         <mass value="0.65429" />
+         <inertia ixx="0.00040501" ixy="-3.1023E-08" ixz="-2.9568E-07" iyy="0.00055081" iyz="2.5206E-08" izz="0.0004053" />
+      </inertial>
+      <visual>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/visual/right_center_wheel.STL" />
+         </geometry>
+      </visual>
+      <collision>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/collision/right_center_wheel_collision.STL" />
+         </geometry>
+      </collision>
+   </link>
+   <joint name="right_center_wheel_joint" type="continuous">
+      <origin xyz="-0.20177 -0.16849 -0.11606" rpy="0 0 0" />
+      <parent link="rightbogie" />
+      <child link="right_center_wheel" />
+      <axis xyz="0 -1 0" />
+      <dynamics damping="4" friction="20" />
+   </joint>
+   <link name="leftrocker">
+      <inertial>
+         <origin xyz="0.065435 -0.034981 -0.01685" rpy="0 0 0" />
+         <mass value="0.58415" />
+         <inertia ixx="0.0004044" ixy="-5.259E-11" ixz="-0.00023722" iyy="0.00095925" iyz="-4.2951E-08" izz="0.00061912" />
+      </inertial>
+      <visual>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/visual/leftrocker.STL" />
+         </geometry>
+      </visual>
+      <collision>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/collision/leftrocker_collision.STL" />
+         </geometry>
+      </collision>
+   </link>
+   <joint name="leftrockerjoint" type="fixed">
+      <origin xyz="-0.19241 0.33715 0.080257" rpy="0 0 0" />
+      <parent link="base_link" />
+      <child link="leftrocker" />
+      <axis xyz="0 0 0" />
+      <dynamics damping="60" friction="4" />
+   </joint>
+   <link name="left_rear_wheel">
+      <inertial>
+         <origin xyz="0.00069452 -0.10248 0.00079886" rpy="0 0 0" />
+         <mass value="0.65433" />
+         <inertia ixx="0.00040509" ixy="-4.8144E-09" ixz="-2.5925E-07" iyy="0.00055097" iyz="4.0749E-08" izz="0.00040552" />
+      </inertial>
+      <visual>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/visual/left_rear_wheel.STL" />
+         </geometry>
+      </visual>
+      <collision>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/collision/left_rear_wheel_collision.STL" />
+         </geometry>
+      </collision>
+   </link>
+   <joint name="left_rear_wheel_joint" type="continuous">
+      <origin xyz="-0.36266 0.17103 -0.19248" rpy="0 0 0" />
+      <parent link="leftrocker" />
+      <child link="left_rear_wheel" />
       <axis xyz="0 1 0" />
-      <limit effort="${axle_eff_limit}" velocity="${axle_vel_limit}" />
-      <dynamics damping="0" friction="4.0" />
-    </joint>
-    <gazebo reference="${prefix}_link">
-      <material>Gazebo/Black</material>
-    </gazebo>
-  </xacro:macro>
-  <xacro:wheel prefix="left_front_wheel" x_reflect="1" y_reflect="1" />
-  <xacro:wheel prefix="left_center_wheel" x_reflect="0" y_reflect="1" />
-  <xacro:wheel prefix="left_rear_wheel" x_reflect="-1" y_reflect="1" />
-  <xacro:wheel prefix="right_front_wheel" x_reflect="1" y_reflect="-1" />
-  <xacro:wheel prefix="right_center_wheel" x_reflect="0" y_reflect="-1" />
-  <xacro:wheel prefix="right_rear_wheel" x_reflect="-1" y_reflect="-1" />
-
-  <!-- Robot Footprint -->
-  <link name="base_footprint" />
-  <joint name="base_joint" type="fixed">
-    <parent link="base_link" />
-    <child link="base_footprint" />
-    <origin xyz="0.0 0.0 ${-(wheel_radius+wheel_zoff)}" rpy="0 0 0" />
-  </joint>
-
-  <!-- Arm Joint -->
-  <joint name="arm_to_rover" type="fixed">
-    <origin rpy="0 0 0" xyz="0 0 0" />
-    <parent link="base_link" />
-    <child link="arm_base_link" />
-    <dynamics damping="60.0" friction="4.0" />
-  </joint>
-
-  <!-- IMU -->
-  <link name="imu_link">
-    <visual>
-      <geometry>
-        <box size="0.1 0.1 0.1" />
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="0.1 0.1 0.1" />
-      </geometry>
-    </collision>
-    <xacro:box_inertia m="0.1" w="0.1" d="0.1" h="0.1" />
-  </link>
-  <joint name="imu_joint" type="fixed">
-    <axis xyz="1 0 0" />
-    <origin xyz="0 0 0.05" />
-    <parent link="base_link" />
-    <child link="imu_link" />
-  </joint>
-  <gazebo reference="imu_link">
-    <sensor name="imu_sensor" type="imu">
-      <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
-        <ros>
-          <namespace>/imu</namespace>
-          <remapping>~/out:=data</remapping>
-        </ros>
-        <initial_orientation_as_reference>false</initial_orientation_as_reference>
-      </plugin>
-      <always_on>true</always_on>
-      <update_rate>100</update_rate>
-      <visualize>true</visualize>
-      <imu>
-        <angular_velocity>
-          <x>
-            <noise type="gaussian">
-              <mean>0.0</mean>
-              <stddev>2e-4</stddev>
-              <bias_mean>0.0000075</bias_mean>
-              <bias_stddev>0.0000008</bias_stddev>
-            </noise>
-          </x>
-          <y>
-            <noise type="gaussian">
-              <mean>0.0</mean>
-              <stddev>2e-4</stddev>
-              <bias_mean>0.0000075</bias_mean>
-              <bias_stddev>0.0000008</bias_stddev>
-            </noise>
-          </y>
-          <z>
-            <noise type="gaussian">
-              <mean>0.0</mean>
-              <stddev>2e-4</stddev>
-              <bias_mean>0.0000075</bias_mean>
-              <bias_stddev>0.0000008</bias_stddev>
-            </noise>
-          </z>
-        </angular_velocity>
-        <linear_acceleration>
-          <x>
-            <noise type="gaussian">
-              <mean>0.0</mean>
-              <stddev>1.7e-2</stddev>
-              <bias_mean>0.1</bias_mean>
-              <bias_stddev>0.001</bias_stddev>
-            </noise>
-          </x>
-          <y>
-            <noise type="gaussian">
-              <mean>0.0</mean>
-              <stddev>1.7e-2</stddev>
-              <bias_mean>0.1</bias_mean>
-              <bias_stddev>0.001</bias_stddev>
-            </noise>
-          </y>
-          <z>
-            <noise type="gaussian">
-              <mean>0.0</mean>
-              <stddev>1.7e-2</stddev>
-              <bias_mean>0.1</bias_mean>
-              <bias_stddev>0.001</bias_stddev>
-            </noise>
-          </z>
-        </linear_acceleration>
-      </imu>
-    </sensor>
-  </gazebo>
-
-
-  <gazebo>
-    <plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
-      <ros>
-        <namespace>/sensor</namespace>
-      </ros>
-      <!-- wheels -->
-      <left_joint>left_front_wheel_joint</left_joint>
-      <right_joint>right_front_wheel_joint</right_joint>
-      <!-- kinematics -->
-      <wheel_separation>0.4</wheel_separation>
-      <wheel_diameter>0.2</wheel_diameter>
-      <!-- limits -->
-      <max_wheel_torque>20</max_wheel_torque>
-      <max_wheel_acceleration>1.0</max_wheel_acceleration>
-      <!-- output -->
-      <publish_odom>true</publish_odom>
-      <publish_odom_tf>false</publish_odom_tf>
-      <publish_wheel_tf>true</publish_wheel_tf>
-      <odometry_frame>odom</odometry_frame>
-      <robot_base_frame>base_link</robot_base_frame>
-    </plugin>
-  </gazebo>
-
-  <!-- GPS -->
-  <link name="gps">
-    <visual>
-      <geometry>
-        <box size="0.1 0.1 0.1" />
-      </geometry>
-    </visual>
-    <collision>
-      <geometry>
-        <box size="0.1 0.1 0.1" />
-      </geometry>
-    </collision>
-    <xacro:box_inertia m="0.1" w="0.1" d="0.1" h="0.1" />
-  </link>
-  <joint name="gps_joint" type="fixed">
-    <axis xyz="1 0 0" />
-    <origin xyz="0 0 0.05" />
-    <parent link="base_link" />
-    <child link="gps" />
-  </joint>
-  <gazebo reference="gps">
-    <sensor name="gps" type="gps">
-      <plugin filename="libgazebo_ros_gps_sensor.so" name="gps_plugin">
-        <ros>
-          <namespace>/gps</namespace>
-          <remapping>~/out:=data</remapping>
-        </ros>
-        <frame_name>gps</frame_name>
-      </plugin>
-      <always_on>true</always_on>
-      <updateRate>30</updateRate>
-    </sensor>
-  </gazebo>
-
-  <!-- LIDAR -->
-  <link name="lidar_link">
-    <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="0.125" />
-      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
-    </inertial>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.0508" length="0.055" />
-      </geometry>
-    </collision>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <cylinder radius="0.0508" length="0.055" />
-      </geometry>
-    </visual>
-  </link>
-  <joint name="lidar_joint" type="fixed">
-    <parent link="base_link" />
-    <child link="lidar_link" />
-    <origin xyz="0.0 0.0 0.1" rpy="0.0 0.0 0.0" />
-  </joint>
-  <!-- <xacro:VLP-16 parent="lidar_link" name="lidar" topic="/scan" hz="${lidar_hz}"
-  samples="${lidar_samples}" gpu="false">
-    <origin xyz="${lidar_x} 0.0 ${lidar_z}" rpy="0 ${lidar_pitch} 0" />
-  </xacro:VLP-16> -->
-  <gazebo reference="lidar_link">
-    <sensor name="lidar" type="ray">
-      <always_on>true</always_on>
-      <visualize>true</visualize>
-      <update_rate>5</update_rate>
-      <ray>
-        <scan>
-          <horizontal>
-            <samples>360</samples>
-            <resolution>1.000000</resolution>
-            <min_angle>0.000000</min_angle>
-            <max_angle>6.280000</max_angle>
-          </horizontal>
-        </scan>
-        <range>
-          <min>0.12</min>
-          <max>3.5</max>
-          <resolution>0.015000</resolution>
-        </range>
-        <noise>
-          <type>gaussian</type>
-          <mean>0.0</mean>
-          <stddev>0.01</stddev>
-        </noise>
-      </ray>
-      <plugin name="scan" filename="libgazebo_ros_ray_sensor.so">
-        <ros>
-          <remapping>~/out:=scan</remapping>
-        </ros>
-        <output_type>sensor_msgs/LaserScan</output_type>
-        <frame_name>lidar_link</frame_name>
-      </plugin>
-    </sensor>
-  </gazebo>
-
-  <!-- May want to change this to a depth camera -->
-  <xacro:macro name="camera" params="orientation origin_x origin_y origin_z yaw">
-    <link name="cam/${orientation}">
+      <dynamics damping="4" friction="20" />
+   </joint>
+   <link name="leftbogie">
+      <inertial>
+         <origin xyz="-6.8787E-09 -0.032385 -0.015429" rpy="0 0 0" />
+         <mass value="0.66758" />
+         <inertia ixx="0.00019756" ixy="-5.9804E-10" ixz="9.4624E-11" iyy="0.00044836" iyz="2.6509E-13" izz="0.0003846" />
+      </inertial>
       <visual>
-        <geometry>
-          <box size="0.01 0.01 0.01" />
-        </geometry>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/visual/leftbogie.STL" />
+         </geometry>
       </visual>
       <collision>
-        <geometry>
-          <box size="0.01 0.01 0.01" />
-        </geometry>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/collision/leftbogie_collision.STL" />
+         </geometry>
       </collision>
-      <xacro:box_inertia m="0.01" w="0.01" d="0.01" h="0.01" />
-    </link>
-    <joint name="${orientation}_cam_joint" type="fixed">
-      <axis xyz="1 0 0" />
-      <!-- Camera Orientation-->
-      <origin xyz="${origin_x} ${origin_y} ${origin_z}" rpy="0 0 ${yaw}" />
+   </link>
+   <joint name="leftbogiejoint" type="fixed">
+      <origin xyz="0.24182 0.00254 -0.073984" rpy="0 0 0" />
+      <parent link="leftrocker" />
+      <child link="leftbogie" />
+      <axis xyz="0 0 0" />
+      <dynamics damping="60" friction="4" />
+   </joint>
+   <link name="left_front_wheel">
+      <inertial>
+         <origin xyz="-0.001042 -0.10248 0.00016516" rpy="0 0 0" />
+         <mass value="0.65433" />
+         <inertia ixx="0.00040512" ixy="-1.5424E-08" ixz="2.7516E-07" iyy="0.00055097" iyz="-3.4895E-08" izz="0.00040549" />
+      </inertial>
+      <visual>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/visual/left_front_wheel.STL" />
+         </geometry>
+      </visual>
+      <collision>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/collision/left_front_wheel_collision.STL" />
+         </geometry>
+      </collision>
+   </link>
+   <joint name="left_front_wheel_joint" type="continuous">
+      <origin xyz="0.20177 0.16849 -0.11606" rpy="0 0 0" />
+      <parent link="leftbogie" />
+      <child link="left_front_wheel" />
+      <axis xyz="0 1 0" />
+      <dynamics damping="4" friction="20" />
+   </joint>
+   <link name="left_center_wheel">
+      <inertial>
+         <origin xyz="0.00066514 -0.10248 0.00082348" rpy="0 0 0" />
+         <mass value="0.65433" />
+         <inertia ixx="0.00040511" ixy="-6.2827E-09" ixz="-2.7164E-07" iyy="0.00055097" iyz="4.0155E-08" izz="0.0004055" />
+      </inertial>
+      <visual>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/visual/left_center_wheel.STL" />
+         </geometry>
+      </visual>
+      <collision>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/collision/left_center_wheel_collision.STL" />
+         </geometry>
+      </collision>
+   </link>
+   <joint name="left_center_wheel_joint" type="continuous">
+      <origin xyz="-0.20139 0.16849 -0.11671" rpy="0 0 0" />
+      <parent link="leftbogie" />
+      <child link="left_center_wheel" />
+      <axis xyz="0 1 0" />
+      <dynamics damping="4" friction="20" />
+   </joint>
+   <link name="spine">
+      <inertial>
+         <origin xyz="-4.2872E-05 -0.00027225 -0.21303" rpy="0 0 0" />
+         <mass value="0.9682" />
+         <inertia ixx="0.01233" ixy="-4.9034E-07" ixz="-3.3052E-10" iyy="0.012329" iyz="-6.338E-06" izz="0.00039532" />
+      </inertial>
+      <visual>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/visual/spine.STL" />
+         </geometry>
+      </visual>
+      <collision>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/collision/spine_collision.STL" />
+         </geometry>
+      </collision>
+   </link>
+   <joint name="spinejoint" type="fixed">
+      <origin xyz="0.000513666061109697 0.00028388502121135 0.630050810000002" rpy="0 0 0" />
       <parent link="base_link" />
-      <child link="cam/${orientation}" />
-    </joint>
-    <gazebo reference="cam/${orientation}">
-      <sensor name="${orientation}_sensor" type="camera">
-        <plugin name="${orientation}_sensor" filename="libgazebo_ros_camera.so">
-          <ros>
-            <namespace>/image</namespace>
-            <remapping>~/image_raw:=${orientation}_img</remapping>
-            <remapping>~/camera_info:=${orientation}_camera_info</remapping>
-          </ros>
-          <initial_orientation_as_reference>false</initial_orientation_as_reference>
-          <camera_name>${orientation}_camera</camera_name>
-          <frame_name>${orientation}_tf</frame_name>
-          <hack_baseline>0.07</hack_baseline>
-        </plugin>
-        <always_on>true</always_on>
-        <update_rate>100</update_rate>
-        <visualize>true</visualize>
-        <camera name="cam/${orientation}">
-          <horizontal_fov>1.3962634</horizontal_fov>
-          <image>
-            <width>800</width>
-            <height>800</height>
-            <format>R8G8B8</format>
-          </image>
-          <clip>
-            <near>0.02</near>
-            <far>300</far>
-          </clip>
-          <noise>
-            <type>gaussian</type>
-            <mean>0</mean>
-            <stddev>0.007</stddev>
-          </noise>
-        </camera>
-      </sensor>
-    </gazebo>
-  </xacro:macro>
-  <xacro:camera orientation="center" origin_x="0.1" origin_y="0" origin_z="0" yaw="-0.14" />
-  <xacro:camera orientation="left" origin_x="0" origin_y="0.1" origin_z="0" yaw="1.5" />
-  <xacro:camera orientation="right" origin_x="0" origin_y="-0.1" origin_z="0" yaw="-1.5" />
-
-
-  <!-- Depth Camera (addition for Nav2) -->
-  <link name="camera_link">
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.015 0.130 0.022" />
-      </geometry>
-    </visual>
-
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <box size="0.015 0.130 0.022" />
-      </geometry>
-    </collision>
-
-    <inertial>
+      <child link="spine" />
+      <axis xyz="0 0 0" />
+   </joint>
+   <link name="shoulder">
+      <inertial>
+         <origin xyz="0.020518 0.053038 -0.052389" rpy="0 0 0" />
+         <mass value="1.3264" />
+         <inertia ixx="0.0025022" ixy="4.7083E-07" ixz="8.2027E-08" iyy="0.00092659" iyz="2.0751E-06" izz="0.0020646" />
+      </inertial>
+      <visual>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/visual/shoulder.STL" />
+         </geometry>
+      </visual>
+      <collision>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/collision/shoulder_collision.STL" />
+         </geometry>
+      </collision>
+   </link>
+   <joint name="shoulderjoint" type="revolute">
       <origin xyz="0 0 0" rpy="0 0 0" />
-      <mass value="0.035" />
-      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
-    </inertial>
-  </link>
-
-  <joint name="camera_joint" type="fixed">
-    <parent link="base_link" />
-    <child link="camera_link" />
-    <origin xyz="0.1 0 0.03" rpy="0 0 0" />
-  </joint>
-
-  <link name="camera_depth_frame" />
-
-  <joint name="camera_depth_joint" type="fixed">
-    <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}" />
-    <parent link="camera_link" />
-    <child link="camera_depth_frame" />
-  </joint>
-
-  <gazebo reference="camera_link">
-    <sensor name="depth_camera" type="depth">
-      <visualize>true</visualize>
-      <update_rate>30.0</update_rate>
-      <camera name="camera">
-        <horizontal_fov>1.047198</horizontal_fov>
-        <image>
-          <width>640</width>
-          <height>480</height>
-          <format>R8G8B8</format>
-        </image>
-        <clip>
-          <near>0.05</near>
-          <far>3</far>
-        </clip>
-      </camera>
-      <plugin name="depth_camera_controller" filename="libgazebo_ros_camera.so">
-        <baseline>0.2</baseline>
-        <alwaysOn>true</alwaysOn>
-        <updateRate>0.0</updateRate>
-        <frame_name>camera_depth_frame</frame_name>
-        <pointCloudCutoff>0.5</pointCloudCutoff>
-        <pointCloudCutoffMax>3.0</pointCloudCutoffMax>
-        <distortionK1>0</distortionK1>
-        <distortionK2>0</distortionK2>
-        <distortionK3>0</distortionK3>
-        <distortionT1>0</distortionT1>
-        <distortionT2>0</distortionT2>
-        <CxPrime>0</CxPrime>
-        <Cx>0</Cx>
-        <Cy>0</Cy>
-        <focalLength>0</focalLength>
-        <hackBaseline>0</hackBaseline>
-      </plugin>
-    </sensor>
-  </gazebo>
-
-
-  <gazebo>
-    <sensor name="imu" type="imu">
-      <always_on>true</always_on>
-      <update_rate>30</update_rate>
-      <plugin name="imu" filename="libgazebo_ros_imu_sensor.so">
-        <ros>
-          <namespace>/imu</namespace>
-          <argument>~/out:=data</argument>
-        </ros>
-        <frame_name>imu</frame_name>
-      </plugin>
-    </sensor>
-    <plugin name="gazebo_ros_p3d" filename="libgazebo_ros_p3d.so">
-      <alwaysOn>true</alwaysOn>
-      <update_rate>30.0</update_rate>
-      <ros>
-        <namespace>/ground_truth</namespace>
-        <remapping>odom:=state_raw</remapping>
-      </ros>
-      <body_name>base_link</body_name>
-      <frame_name>world</frame_name>
-
-      <xyz_offset>0 0 0</xyz_offset>
-      <rpy_offset>0.0 0.0 0.0</rpy_offset>
-      <gaussian_noise>0.00</gaussian_noise>
-    </plugin>
-  </gazebo>
-
-</robot>
-
-<!-- Mock/Untested ros2 control scheme for operating the rover with ros2 control.
-    This matches how the control is supposed to be set up, but is commented out
-    because the way the urdf is designed, it interferes with the arm control. -->
-<!--
-    <ros2_control name="rover_control" type="system">
-    <hardware>
-      <plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
-    </hardware>
-    <joint name="left_bogie_front_axle">
-      <command_interface name="velocity">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <state_interface name="velocity"/>
-    </joint>
-    <joint name="left_bogie_rear_axle">
-      <command_interface name="velocity">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <state_interface name="velocity"/>
-    </joint>
-    <joint name="left_rocker_rear_axle">
-      <command_interface name="velocity">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <state_interface name="velocity"/>
-    </joint>
-    <joint name="right_bogie_front_axle">
-      <command_interface name="velocity">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <state_interface name="velocity"/>
-    </joint>
-    <joint name="right_bogie_rear_axle">
-      <command_interface name="velocity">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <state_interface name="velocity"/>
-    </joint>
-    <joint name="right_rocker_rear_axle">
-      <command_interface name="velocity">
-        <param name="min">-1</param>
-        <param name="max">1</param>
-      </command_interface>
-      <state_interface name="velocity"/>
-    </joint>
-    <transmission name="rover_transmission">
-      <plugin>transmission_interface/SimpleTransmission</plugin>
-      <joint name="left_bogie_front_axle" role="left_bogie_front_axle">
-        <mechanical_reduction>1</mechanical_reduction>
-      </joint>
-      <joint name="left_bogie_rear_axle" role="left_bogie_rear_axle">
-        <mechanical_reduction>1</mechanical_reduction>
-      </joint>
-      <joint name="left_rocker_rear_axle" role="left_rocker_rear_axle">
-        <mechanical_reduction>1</mechanical_reduction>
-      </joint>
-      <joint name="right_bogie_front_axle" role="right_bogie_front_axle">
-        <mechanical_reduction>1</mechanical_reduction>
-      </joint>
-      <joint name="right_bogie_rear_axle" role="right_bogie_rear_axle">
-        <mechanical_reduction>1</mechanical_reduction>
-      </joint>
-      <joint name="right_rocker_rear_axle" role="right_rocket_rear_axle">
-        <mechanical_reduction>1</mechanical_reduction>
-      </joint>
-    </transmission>
-    </ros2_control>
-    -->
\ No newline at end of file
+      <parent link="spine" />
+      <child link="shoulder" />
+      <axis xyz="0 0 1" />
+      <limit lower="-3.14" upper="3.14" effort="0" velocity="0" />
+      <dynamics damping="4" friction="4" />
+   </joint>
+   <link name="bicep">
+      <inertial>
+         <origin xyz="0.12731 0.047938 0.11952" rpy="0 0 0" />
+         <mass value="0.56899" />
+         <inertia ixx="0.0028177" ixy="-3.8181E-18" ixz="-0.0028792" iyy="0.0058314" iyz="-3.5192E-18" izz="0.0031814" />
+      </inertial>
+      <visual>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/visual/bicep.STL" />
+         </geometry>
+      </visual>
+      <collision>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/collision/bicep_collision.STL" />
+         </geometry>
+      </collision>
+   </link>
+   <joint name="bicepjoint" type="revolute">
+      <origin xyz="0.046323 -0.047405 -0.052413" rpy="0 0 0" />
+      <parent link="shoulder" />
+      <child link="bicep" />
+      <axis xyz="0 -1 0" />
+      <limit lower="-3.14" upper="3.14" effort="0" velocity="0" />
+      <dynamics damping="60" friction="4" />
+   </joint>
+   <link name="elbow">
+      <inertial>
+         <origin xyz="0.057399 0.088305 0.031812" rpy="0 0 0" />
+         <mass value="1.7547" />
+         <inertia ixx="0.0021164" ixy="2.1484E-06" ixz="-1.1917E-05" iyy="0.004637" iyz="-4.7083E-07" izz="0.0062321" />
+      </inertial>
+      <visual>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/visual/elbow.STL" />
+         </geometry>
+      </visual>
+      <collision>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/collision/elbow_collision.STL" />
+         </geometry>
+      </collision>
+   </link>
+   <joint name="elbowjoint" type="revolute">
+      <origin xyz="0.25459 8.2754E-05 0.23905" rpy="0 0 0" />
+      <parent link="bicep" />
+      <child link="elbow" />
+      <axis xyz="0 -1 0" />
+      <limit lower="-3.14" upper="3.14" effort="0" velocity="0" />
+      <dynamics damping="60" friction="4" />
+   </joint>
+   <link name="wrist">
+      <inertial>
+         <origin xyz="-0.015666 0.09009 0.02975" rpy="0 0 0" />
+         <mass value="1.641" />
+         <inertia ixx="0.0020915" ixy="3.1784E-05" ixz="-5.5989E-08" iyy="0.0013998" iyz="-4.6737E-07" izz="0.0029203" />
+      </inertial>
+      <visual>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/visual/wrist.STL" />
+         </geometry>
+      </visual>
+      <collision>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/collision/wrist_collision.STL" />
+         </geometry>
+      </collision>
+   </link>
+   <joint name="wristjoint" type="revolute">
+      <origin xyz="0.35396 0.0035964 0.048704" rpy="0 0 0" />
+      <parent link="elbow" />
+      <child link="wrist" />
+      <axis xyz="0 -1 0" />
+      <limit lower="-3.14" upper="3.14" effort="0" velocity="0" />
+      <dynamics damping="4" friction="4" />
+   </joint>
+   <link name="claw">
+      <inertial>
+         <origin xyz="-0.12039 -3.2707E-05 8.7622E-05" rpy="0 0 0" />
+         <mass value="0.68517" />
+         <inertia ixx="0.00085866" ixy="-3.3295E-07" ixz="3.4281E-08" iyy="0.00076618" iyz="3.4515E-08" izz="0.0015578" />
+      </inertial>
+      <visual>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/visual/claw.STL" />
+         </geometry>
+      </visual>
+      <collision>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/collision/claw_collision.STL" />
+         </geometry>
+      </collision>
+   </link>
+   <joint name="clawjoint" type="revolute">
+      <origin xyz="0.34674 0.047161 0.046323" rpy="0 0 0" />
+      <parent link="wrist" />
+      <child link="claw" />
+      <axis xyz="1 0 0" />
+      <limit lower="-3.14" upper="3.14" effort="0" velocity="0" />
+      <dynamics damping="4" friction="4" />
+   </joint>
+   <link name="rightgripper">
+      <inertial>
+         <origin xyz="0.10255 -0.010123 -0.013053" rpy="0 0 0" />
+         <mass value="0.3358" />
+         <inertia ixx="0.00018107" ixy="-4.531E-05" ixz="-1.2298E-19" iyy="0.00059927" iyz="7.8324E-19" izz="0.00066862" />
+      </inertial>
+      <visual>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/visual/rightgripper.STL" />
+         </geometry>
+      </visual>
+      <collision>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/collision/rightgripper_collision.STL" />
+         </geometry>
+      </collision>
+   </link>
+   <joint name="rightgripperjoint" type="revolute">
+      <origin xyz="-0.030801 -0.080243 0.013053" rpy="0 0 0" />
+      <parent link="claw" />
+      <child link="rightgripper" />
+      <axis xyz="0 0 -1" />
+      <limit lower="-1.57" upper="0.6" effort="0" velocity="0" />
+      <dynamics damping="4" friction="2" />
+   </joint>
+   <link name="leftgripper">
+      <inertial>
+         <origin xyz="0.10255 0.010123 -0.013053" rpy="0 0 0" />
+         <mass value="0.3358" />
+         <inertia ixx="0.00018107" ixy="4.531E-05" ixz="3.3128E-18" iyy="0.00059927" iyz="5.2882E-19" izz="0.00066862" />
+      </inertial>
+      <visual>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/visual/leftgripper.STL" />
+         </geometry>
+      </visual>
+      <collision>
+         <origin xyz="0 0 0" rpy="0 0 0" />
+         <geometry>
+            <mesh filename="file://$(find urc_gazebo)/urdf/meshes/collision/leftgripper_collision.STL" />
+         </geometry>
+      </collision>
+   </link>
+   <joint name="leftgripperjoint" type="revolute">
+      <origin xyz="-0.030801 0.080243 0.013053" rpy="0 0 0" />
+      <parent link="claw" />
+      <child link="leftgripper" />
+      <axis xyz="0 0 1" />
+      <limit lower="-0.6" upper="1.57" effort="0" velocity="0" />
+      <dynamics damping="4" friction="2" />
+      <mimic joint="rightgripperjoint" multiplier="1" offset="0" />
+   </joint>
+   <gazebo reference="base_link">
+      <material>Gazebo/Grey</material>
+   </gazebo>
+   <gazebo reference="rightrocker">
+      <material>Gazebo/Grey</material>
+   </gazebo>
+   <gazebo reference="right_rear_wheel">
+      <material>Gazebo/Black</material>
+   </gazebo>
+   <gazebo reference="rightbogie">
+      <material>Gazebo/Grey</material>
+   </gazebo>
+   <gazebo reference="right_front_wheel">
+      <material>Gazebo/Black</material>
+   </gazebo>
+   <gazebo reference="right_center_wheel">
+      <material>Gazebo/Black</material>
+   </gazebo>
+   <gazebo reference="leftrocker">
+      <material>Gazebo/Grey</material>
+   </gazebo>
+   <gazebo reference="left_rear_wheel">
+      <material>Gazebo/Black</material>
+   </gazebo>
+   <gazebo reference="leftbogie">
+      <material>Gazebo/Grey</material>
+   </gazebo>
+   <gazebo reference="left_front_wheel">
+      <material>Gazebo/Black</material>
+   </gazebo>
+   <gazebo reference="left_center_wheel">
+      <material>Gazebo/Black</material>
+   </gazebo>
+   <gazebo reference="spine">
+      <material>Gazebo/Grey</material>
+   </gazebo>
+   <gazebo reference="shoulder">
+      <material>Gazebo/Grey</material>
+   </gazebo>
+   <gazebo reference="bicep">
+      <material>Gazebo/Grey</material>
+   </gazebo>
+   <gazebo reference="elbow">
+      <material>Gazebo/Grey</material>
+   </gazebo>
+   <gazebo reference="wrist">
+      <material>Gazebo/Grey</material>
+   </gazebo>
+   <gazebo reference="claw">
+      <material>Gazebo/Grey</material>
+   </gazebo>
+   <gazebo reference="rightgripper">
+      <material>Gazebo/Grey</material>
+   </gazebo>
+   <gazebo reference="leftgripper">
+      <material>Gazebo/Grey</material>
+   </gazebo>
+   <gazebo reference="shoulderjoint">
+      <implicitSpringDamper>true</implicitSpringDamper>
+   </gazebo>
+   <gazebo reference="bicepjoint">
+      <implicitSpringDamper>true</implicitSpringDamper>
+   </gazebo>
+   <gazebo reference="elbowjoint">
+      <implicitSpringDamper>true</implicitSpringDamper>
+   </gazebo>
+   <gazebo reference="wristjoint">
+      <implicitSpringDamper>true</implicitSpringDamper>
+   </gazebo>
+   <gazebo reference="clawjoint">
+      <implicitSpringDamper>true</implicitSpringDamper>
+   </gazebo>
+   <gazebo reference="leftgripperjoint">
+      <implicitSpringDamper>true</implicitSpringDamper>
+   </gazebo>
+   <gazebo reference="rightgripperjoint">
+      <implicitSpringDamper>true</implicitSpringDamper>
+   </gazebo>
+</robot>
\ No newline at end of file
diff --git a/urc_hw_description/urdf/walli_arm.urdf b/urc_hw_description/urdf/walli_arm.urdf
deleted file mode 100755
index d5414741..00000000
--- a/urc_hw_description/urdf/walli_arm.urdf
+++ /dev/null
@@ -1,400 +0,0 @@
-<?xml version="1.0" encoding="utf-8"?>
-<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
-     Commit Version: 1.6.0-4-g7f85cfe  Build Version: 1.6.7995.38578
-     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
-<robot name="walli_arm">
-  <link name="arm_base_link">
-    <inertial>
-      <origin xyz="-8.1195E-05 0.025555 0.31181" rpy="0 0 0" />
-      <mass value="0.56817" />
-      <inertia ixx="0.010817" ixy="4.4773E-13" ixz="-4.0161E-12"
-        iyy="0.010818" iyz="1.3408E-13" izz="0.00040167" />
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="file://$(find urc_gazebo)/urdf/meshes/base_link.STL" />
-      </geometry>
-      <material name="">
-        <color rgba="0.75294 0.75294 0.75294 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="file://$(find urc_gazebo)/urdf/meshes/base_link_collision.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <link name="shoulder">
-    <inertial>
-      <origin xyz="-0.04837 -0.018689 -0.068105" rpy="0 0 0" />
-      <mass value="3.586" />
-      <inertia ixx="0.004611" ixy="1.9578E-06" ixz="-4.7127E-06"
-        iyy="0.0037191" iyz="-4.489E-06" izz="0.0029735" />
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="file://$(find urc_gazebo)/urdf/meshes/shoulder.STL" />
-      </geometry>
-      <material name="">
-        <color rgba="0.75294 0.75294 0.75294 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="file://$(find urc_gazebo)/urdf/meshes/shoulder_collision.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint name="shoulderjoint" type="continuous">
-    <origin xyz="0.00028389 0.0254 0.62708" rpy="0 0 0" />
-    <parent link="arm_base_link" />
-    <child link="shoulder" />
-    <axis xyz="0 0 -1" />
-    <dynamics damping="60.0" friction="4.0" />
-  </joint>
-  <link name="bicep">
-  <inertial>
-    <origin xyz="0.12104 0.17465 -1.6653E-16" rpy="0 0 0" />
-    <mass value="0.55725" />
-    <inertia ixx="0.0061511" ixy="5.9124E-19" ixz="-2.0963E-21"
-      iyy="6.7641E-05" iyz="-9.6466E-19" izz="0.0061546" />
-  </inertial>
-  <visual>
-    <origin
-      xyz="0 0 0"
-      rpy="0 0 0" />
-    <geometry>
-      <mesh
-        filename="file://$(find urc_gazebo)/urdf/meshes/bicep.STL" />
-    </geometry>
-    <material
-      name="">
-      <color
-        rgba="0.75294 0.75294 0.75294 1" />
-    </material>
-  </visual>
-  <collision>
-    <origin xyz="0 0 0" rpy="0 0 0" />
-    <geometry>
-      <mesh filename="file://$(find urc_gazebo)/urdf/meshes/bicep_collision.STL" />
-    </geometry>
-  </collision>
-</link>
-<joint name="bicepjoint" type="revolute">
-  <origin xyz="-0.12131 0.046323 -0.052413" rpy="0 0 0" />
-  <parent link="shoulder" />
-  <child link="bicep" />
-  <axis xyz="1 0 0" />
-  <limit lower="-1.57" upper="1.57" effort="0" velocity="0" />
-  <dynamics damping="60.0" friction="4.0" />
-</joint>
-<link name="elbow">
-  <inertial>
-    <origin xyz="-0.06786 0.023863 0.062716" rpy="0 0 0" />
-    <mass value="3.1709" />
-    <inertia ixx="0.0039723" ixy="1.1762E-06" ixz="1.6958E-06"
-      iyy="0.0039893" iyz="6.6104E-06" izz="0.0017383" />
-  </inertial>
-  <visual>
-    <origin xyz="0 0 0" rpy="0 0 0" />
-    <geometry>
-      <mesh filename="file://$(find urc_gazebo)/urdf/meshes/elbow.STL" />
-    </geometry>
-    <material name="">
-      <color rgba="0.75294 0.75294 0.75294 1" />
-    </material>
-  </visual>
-  <collision>
-    <origin xyz="0 0 0" rpy="0 0 0" />
-    <geometry>
-      <mesh filename="file://$(find urc_gazebo)/urdf/meshes/elbow_collision.STL" />
-    </geometry>
-  </collision>
-  </link>
-  <joint name="elbowjoint" type="revolute">
-    <origin xyz="0.24206 0.34925 0" rpy="0 0 0" />
-    <parent link="bicep" />
-    <child link="elbow" />
-    <axis xyz="1 0 0" />
-    <limit lower="-1.57" upper="1.57" effort="0" velocity="0" />
-    <dynamics damping="60.0" friction="4.0" />
-  </joint>
-  <link name="forearm">
-    <inertial>
-      <origin xyz="-0.00011067 9.3127E-05 -0.45483" rpy="0 0 0" />
-      <mass value="0.53926" />
-      <inertia ixx="0.0024603" ixy="-2.3325E-08" ixz="-3.8937E-11"
-        iyy="0.0024892" iyz="3.5055E-11" izz="0.00029125" />
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="file://$(find urc_gazebo)/urdf/meshes/forearm.STL" />
-      </geometry>
-      <material name="">
-        <color rgba="0.75294 0.75294 0.75294 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="file://$(find urc_gazebo)/urdf/meshes/forearm_collision.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint name="forearmjoint" type="continuous">
-    <origin xyz="-0.12103 0.046323 0.23626" rpy="0 0 0" />
-    <parent link="elbow" />
-    <child link="forearm" />
-    <axis xyz="0 0 -1" />
-    <dynamics damping="60.0" friction="4.0" />
-  </joint>
-  <link name="wrist">
-    <inertial>
-      <origin xyz="-0.043952 -0.063003 -0.0020958" rpy="0 0 0" />
-      <mass value="2.4599" />
-      <inertia ixx="0.0011935" ixy="-4.0524E-07" ixz="-2.2366E-06"
-        iyy="0.0018813" iyz="-1.0459E-06" izz="0.0019208" />
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="file://$(find urc_gazebo)/urdf/meshes/wrist.STL" />
-      </geometry>
-      <material name="">
-        <color rgba="0.75294 0.75294 0.75294 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="file://$(find urc_gazebo)/urdf/meshes/wrist_collision.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint name="wristjoint" type="revolute">
-    <origin xyz="0 0.11071 -0.63096" rpy="0 0 0" />
-    <parent link="forearm" />
-    <child link="wrist" />
-    <axis xyz="0 -1 0" />
-    <limit lower="-1.57" upper="1.57" effort="0" velocity="0" />
-    <dynamics damping="60.0" friction="4.0" />
-  </joint>
-  <link name="claw">
-    <inertial>
-      <origin xyz="0.42253 3.8235E-07 6.7396E-05" rpy="0 0 0" />
-      <mass value="1.4352" />
-      <inertia ixx="0.0010292" ixy="1.5887E-07" ixz="1.398E-07"
-        iyy="0.0012655" iyz="1.8778E-08" izz="0.0020608" />
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="file://$(find urc_gazebo)/urdf/meshes/claw.STL" />
-      </geometry>
-      <material name="">
-        <color rgba="0.75294 0.75294 0.75294 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="file://$(find urc_gazebo)/urdf/meshes/claw_collision.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint name="clawjoint" type="continuous">
-    <origin xyz="-0.23582 -0.11071 -0.034131" rpy="0 0 0" />
-    <parent link="wrist" />
-    <child link="claw" />
-    <axis xyz="1 0 0" />
-    <dynamics damping="60.0" friction="4.0" />
-  </joint>
-  <link name="leftgripper">
-    <inertial>
-      <origin xyz="0.11047 -0.031729 -0.011112" rpy="0 0 0" />
-      <mass value="0.3358" />
-      <inertia ixx="0.00013218" ixy="7.9073E-05" ixz="2.0634E-18"
-        iyy="0.00064815" iyz="-5.0458E-19" izz="0.00066862" />
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="file://$(find urc_gazebo)/urdf/meshes/leftgripper.STL" />
-      </geometry>
-      <material name="">
-        <color rgba="0.89804 0.91765 0.92941 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="file://$(find urc_gazebo)/urdf/meshes/leftgripper_collision.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint name="leftgripperjoint" type="revolute">
-    <origin xyz="0.55008 0.080202 0.011112" rpy="0 0 0" />
-    <parent link="claw" />
-    <child link="leftgripper" />
-    <axis xyz="0 0 1" />
-    <limit lower="-0.6" upper="1.57" effort="0" velocity="0" />
-    <dynamics damping="60.0" friction="4.0" />
-  </joint>
-  <link name="rightgripper">
-    <inertial>
-      <origin xyz="0.11047 0.031729 -0.0127" rpy="0 0 0" />
-      <mass value="0.3358" />
-      <inertia ixx="0.00013218" ixy="-7.9073E-05" ixz="-1.0118E-18"
-        iyy="0.00064815" iyz="-3.1698E-19" izz="0.00066862" />
-    </inertial>
-    <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="file://$(find urc_gazebo)/urdf/meshes/rightgripper.STL" />
-      </geometry>
-      <material name="">
-        <color rgba="0.50196 0.50196 0.50196 1" />
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
-      <geometry>
-        <mesh filename="file://$(find urc_gazebo)/urdf/meshes/rightgripper_collision.STL" />
-      </geometry>
-    </collision>
-  </link>
-  <joint name="rightgripperjoint" type="revolute">
-    <origin xyz="0.55008 -0.080283 0.0127" rpy="0 0 0" />
-    <parent link="claw" />
-    <child link="rightgripper" />
-    <axis xyz="0 0 -1" />
-    <limit lower="-1.57" upper="0.6" effort="0" velocity="0" />
-    <dynamics damping="60.0" friction="4.0" />
-    <mimic joint="leftgripperjoint" multiplier="1" offset="0" />
-  </joint>
-  
-  <gazebo reference="shoulderjoint">
-    <implicitSpringDamper>true</implicitSpringDamper>
-  </gazebo>
-
-  <gazebo reference="bicepjoint">
-    <implicitSpringDamper>true</implicitSpringDamper>
-  </gazebo>
-
-  <gazebo reference="elbowjoint">
-    <implicitSpringDamper>true</implicitSpringDamper>
-  </gazebo>
-
-  <gazebo reference="forearmjoint">
-    <implicitSpringDamper>true</implicitSpringDamper>
-  </gazebo>
-
-  <gazebo reference="wristjoint">
-    <implicitSpringDamper>true</implicitSpringDamper>
-  </gazebo>
-
-  <gazebo reference="clawjoint">
-    <implicitSpringDamper>true</implicitSpringDamper>
-  </gazebo>
-
-  <gazebo reference="leftgripperjoint">
-    <implicitSpringDamper>true</implicitSpringDamper>
-  </gazebo>
-
-  <gazebo reference="rightgripperjoint">
-    <implicitSpringDamper>true</implicitSpringDamper>
-  </gazebo>
-  
-  <ros2_control name="arm_control" type="system">
-    <hardware>
-      <!-- TODO make a launch param to switch between real/fake hardware. You can only use ONE. -->
-      <!--Use for functionality in Gazebo-->
-      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
-      <!--Use for functionality in RViz-->
-      <!-- <plugin>mock_components/GenericSystem</plugin> -->
-      <plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
-    </hardware>
-
-      <joint name ="shoulderjoint">
-        <command_interface name="position" />
-        <state_interface name="position">
-          <param name="initial_value">0.0</param>
-        </state_interface>
-        <state_interface name="velocity"/>
-        <state_interface name="effort"/>
-      </joint>
-      
-      <joint name ="bicepjoint">
-        <command_interface name="position" />
-        <state_interface name="position">
-          <param name="initial_value">0.0</param>
-        </state_interface>
-        <state_interface name="velocity"/>
-        <state_interface name="effort"/>
-      </joint>
-
-      <joint name ="elbowjoint">
-        <command_interface name="position" />
-        <state_interface name="position">
-          <param name="initial_value">0.0</param>
-        </state_interface>
-        <state_interface name="velocity"/>
-        <state_interface name="effort"/>
-      </joint>
-	<joint name ="forearmjoint">
-        <command_interface name="position" />
-        <state_interface name="position">
-          <param name="initial_value">0.0</param>
-        </state_interface>
-        <state_interface name="velocity"/>
-        <state_interface name="effort"/>
-      </joint>
-	<joint name ="wristjoint">
-        <command_interface name="position" />
-        <state_interface name="position">
-          <param name="initial_value">0.0</param>
-        </state_interface>
-        <state_interface name="velocity"/>
-        <state_interface name="effort"/>
-      </joint>
-      
-      <joint name ="clawjoint">
-        <command_interface name="position" />
-        <state_interface name="position">
-          <param name="initial_value">0.0</param>
-        </state_interface>
-        <state_interface name="velocity"/>
-        <state_interface name="effort"/>
-      </joint>
-      
-      <joint name ="leftgripperjoint">
-        <command_interface name="position" />
-        <state_interface name="position">
-          <param name="initial_value">0.0</param>
-        </state_interface>
-        <state_interface name="velocity"/>
-        <state_interface name="effort"/>
-      </joint>
-      
-      <joint name ="rightgripperjoint">
-        <command_interface name="position" />
-        <state_interface name="position">
-          <param name="initial_value">0.0</param>
-        </state_interface>
-        <state_interface name="velocity"/>
-        <state_interface name="effort"/>
-      </joint>
-    </ros2_control>
-    
-    <gazebo>
-      <plugin name ="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
-        <parameters>file://$(find urc_gazebo)/urdf/config/RoboticArm_params.yaml</parameters>
-      </plugin>
-    </gazebo>
-
-</robot>
diff --git a/urc_hw_description/urdf/walli_old.xacro b/urc_hw_description/urdf/walli_old.xacro
new file mode 100644
index 00000000..d44439f9
--- /dev/null
+++ b/urc_hw_description/urdf/walli_old.xacro
@@ -0,0 +1,537 @@
+<?xml version='1.0'?>
+<robot name="walli" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:include filename="$(find urc_hw_description)/urdf/walli_prop.xacro" />
+  <xacro:include filename="$(find urc_hw_description)/urdf/walli_inertials.xacro" />
+  <xacro:include filename="$(find velodyne_description)/urdf/VLP-16.urdf.xacro" />
+  <xacro:include filename="$(find urc_hw_description)/urdf/walli_arm.urdf" />
+
+  <!-- Rover Base Link -->
+  <link name="base_link">
+    <visual>
+      <geometry>
+        <box size="${base_length} ${base_width} ${base_height}" />
+      </geometry>
+      <material name="Cyan">
+        <color rgba="0 1.0 1.0 1.0" />
+      </material>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="${base_length} ${base_width} ${base_height}" />
+      </geometry>
+    </collision>
+    <xacro:box_inertia m="15" w="${base_width}" d="${base_length}" h="${base_height}" />
+  </link>
+  <gazebo reference="base_link">
+    <material>Gazebo/Red</material>
+  </gazebo>
+
+
+  <!-- Wheels -->
+  <xacro:macro name="wheel" params="prefix x_reflect y_reflect">
+    <link name="${prefix}_link">
+      <visual>
+        <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
+        <geometry>
+          <cylinder radius="${wheel_radius}" length="${wheel_width}" />
+        </geometry>
+        <material name="Gray">
+          <color rgba="0.5 0.5 0.5 1.0" />
+        </material>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="${pi/2} 0 0" />
+        <geometry>
+          <cylinder radius="${wheel_radius}" length="${wheel_width}" />
+        </geometry>
+      </collision>
+      <xacro:cylinder_inertia m="0.5" r="${wheel_radius}" h="${wheel_width}" />
+    </link>
+    <joint name="${prefix}_joint" type="continuous">
+      <parent link="base_link" />
+      <child link="${prefix}_link" />
+      <origin xyz="${x_reflect*wheel_xoff} ${y_reflect*(base_width/2+wheel_ygap)} ${-wheel_zoff}"
+        rpy="0 0 0" />
+      <axis xyz="0 1 0" />
+      <limit effort="${axle_eff_limit}" velocity="${axle_vel_limit}" />
+      <dynamics damping="0" friction="4.0" />
+    </joint>
+    <gazebo reference="${prefix}_link">
+      <material>Gazebo/Black</material>
+    </gazebo>
+  </xacro:macro>
+  <xacro:wheel prefix="left_front_wheel" x_reflect="1" y_reflect="1" />
+  <xacro:wheel prefix="left_center_wheel" x_reflect="0" y_reflect="1" />
+  <xacro:wheel prefix="left_rear_wheel" x_reflect="-1" y_reflect="1" />
+  <xacro:wheel prefix="right_front_wheel" x_reflect="1" y_reflect="-1" />
+  <xacro:wheel prefix="right_center_wheel" x_reflect="0" y_reflect="-1" />
+  <xacro:wheel prefix="right_rear_wheel" x_reflect="-1" y_reflect="-1" />
+
+  <!-- Robot Footprint -->
+  <link name="base_footprint" />
+  <joint name="base_joint" type="fixed">
+    <parent link="base_link" />
+    <child link="base_footprint" />
+    <origin xyz="0.0 0.0 ${-(wheel_radius+wheel_zoff)}" rpy="0 0 0" />
+  </joint>
+
+  <!-- Arm Joint -->
+  <joint name="arm_to_rover" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0" />
+    <parent link="base_link" />
+    <child link="arm_base_link" />
+    <dynamics damping="60.0" friction="4.0" />
+  </joint>
+
+  <!-- IMU -->
+  <link name="imu_link">
+    <visual>
+      <geometry>
+        <box size="0.1 0.1 0.1" />
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.1 0.1 0.1" />
+      </geometry>
+    </collision>
+    <xacro:box_inertia m="0.1" w="0.1" d="0.1" h="0.1" />
+  </link>
+  <joint name="imu_joint" type="fixed">
+    <axis xyz="1 0 0" />
+    <origin xyz="0 0 0.05" />
+    <parent link="base_link" />
+    <child link="imu_link" />
+  </joint>
+  <gazebo reference="imu_link">
+    <sensor name="imu_sensor" type="imu">
+      <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
+        <ros>
+          <namespace>/imu</namespace>
+          <remapping>~/out:=data</remapping>
+        </ros>
+        <initial_orientation_as_reference>false</initial_orientation_as_reference>
+      </plugin>
+      <always_on>true</always_on>
+      <update_rate>100</update_rate>
+      <visualize>true</visualize>
+      <imu>
+        <angular_velocity>
+          <x>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>2e-4</stddev>
+              <bias_mean>0.0000075</bias_mean>
+              <bias_stddev>0.0000008</bias_stddev>
+            </noise>
+          </x>
+          <y>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>2e-4</stddev>
+              <bias_mean>0.0000075</bias_mean>
+              <bias_stddev>0.0000008</bias_stddev>
+            </noise>
+          </y>
+          <z>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>2e-4</stddev>
+              <bias_mean>0.0000075</bias_mean>
+              <bias_stddev>0.0000008</bias_stddev>
+            </noise>
+          </z>
+        </angular_velocity>
+        <linear_acceleration>
+          <x>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>1.7e-2</stddev>
+              <bias_mean>0.1</bias_mean>
+              <bias_stddev>0.001</bias_stddev>
+            </noise>
+          </x>
+          <y>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>1.7e-2</stddev>
+              <bias_mean>0.1</bias_mean>
+              <bias_stddev>0.001</bias_stddev>
+            </noise>
+          </y>
+          <z>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>1.7e-2</stddev>
+              <bias_mean>0.1</bias_mean>
+              <bias_stddev>0.001</bias_stddev>
+            </noise>
+          </z>
+        </linear_acceleration>
+      </imu>
+    </sensor>
+  </gazebo>
+
+
+  <gazebo>
+    <plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
+      <ros>
+        <namespace>/sensor</namespace>
+      </ros>
+      <!-- wheels -->
+      <left_joint>left_front_wheel_joint</left_joint>
+      <right_joint>right_front_wheel_joint</right_joint>
+      <!-- kinematics -->
+      <wheel_separation>0.4</wheel_separation>
+      <wheel_diameter>0.2</wheel_diameter>
+      <!-- limits -->
+      <max_wheel_torque>20</max_wheel_torque>
+      <max_wheel_acceleration>1.0</max_wheel_acceleration>
+      <!-- output -->
+      <publish_odom>true</publish_odom>
+      <publish_odom_tf>false</publish_odom_tf>
+      <publish_wheel_tf>true</publish_wheel_tf>
+      <odometry_frame>odom</odometry_frame>
+      <robot_base_frame>base_link</robot_base_frame>
+    </plugin>
+  </gazebo>
+
+  <!-- GPS -->
+  <link name="gps">
+    <visual>
+      <geometry>
+        <box size="0.1 0.1 0.1" />
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.1 0.1 0.1" />
+      </geometry>
+    </collision>
+    <xacro:box_inertia m="0.1" w="0.1" d="0.1" h="0.1" />
+  </link>
+  <joint name="gps_joint" type="fixed">
+    <axis xyz="1 0 0" />
+    <origin xyz="0 0 0.05" />
+    <parent link="base_link" />
+    <child link="gps" />
+  </joint>
+  <gazebo reference="gps">
+    <sensor name="gps" type="gps">
+      <plugin filename="libgazebo_ros_gps_sensor.so" name="gps_plugin">
+        <ros>
+          <namespace>/gps</namespace>
+          <remapping>~/out:=data</remapping>
+        </ros>
+        <frame_name>gps</frame_name>
+      </plugin>
+      <always_on>true</always_on>
+      <updateRate>30</updateRate>
+    </sensor>
+  </gazebo>
+
+  <!-- LIDAR -->
+  <link name="lidar_link">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <mass value="0.125" />
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
+    </inertial>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <cylinder radius="0.0508" length="0.055" />
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <cylinder radius="0.0508" length="0.055" />
+      </geometry>
+    </visual>
+  </link>
+  <joint name="lidar_joint" type="fixed">
+    <parent link="base_link" />
+    <child link="lidar_link" />
+    <origin xyz="0.0 0.0 0.1" rpy="0.0 0.0 0.0" />
+  </joint>
+  <!-- <xacro:VLP-16 parent="lidar_link" name="lidar" topic="/scan" hz="${lidar_hz}"
+  samples="${lidar_samples}" gpu="false">
+    <origin xyz="${lidar_x} 0.0 ${lidar_z}" rpy="0 ${lidar_pitch} 0" />
+  </xacro:VLP-16> -->
+  <gazebo reference="lidar_link">
+    <sensor name="lidar" type="ray">
+      <always_on>true</always_on>
+      <visualize>true</visualize>
+      <update_rate>5</update_rate>
+      <ray>
+        <scan>
+          <horizontal>
+            <samples>360</samples>
+            <resolution>1.000000</resolution>
+            <min_angle>0.000000</min_angle>
+            <max_angle>6.280000</max_angle>
+          </horizontal>
+        </scan>
+        <range>
+          <min>0.12</min>
+          <max>3.5</max>
+          <resolution>0.015000</resolution>
+        </range>
+        <noise>
+          <type>gaussian</type>
+          <mean>0.0</mean>
+          <stddev>0.01</stddev>
+        </noise>
+      </ray>
+      <plugin name="scan" filename="libgazebo_ros_ray_sensor.so">
+        <ros>
+          <remapping>~/out:=scan</remapping>
+        </ros>
+        <output_type>sensor_msgs/LaserScan</output_type>
+        <frame_name>lidar_link</frame_name>
+      </plugin>
+    </sensor>
+  </gazebo>
+
+  <!-- May want to change this to a depth camera -->
+  <xacro:macro name="camera" params="orientation origin_x origin_y origin_z yaw">
+    <link name="cam/${orientation}">
+      <visual>
+        <geometry>
+          <box size="0.01 0.01 0.01" />
+        </geometry>
+      </visual>
+      <collision>
+        <geometry>
+          <box size="0.01 0.01 0.01" />
+        </geometry>
+      </collision>
+      <xacro:box_inertia m="0.01" w="0.01" d="0.01" h="0.01" />
+    </link>
+    <joint name="${orientation}_cam_joint" type="fixed">
+      <axis xyz="1 0 0" />
+      <!-- Camera Orientation-->
+      <origin xyz="${origin_x} ${origin_y} ${origin_z}" rpy="0 0 ${yaw}" />
+      <parent link="base_link" />
+      <child link="cam/${orientation}" />
+    </joint>
+    <gazebo reference="cam/${orientation}">
+      <sensor name="${orientation}_sensor" type="camera">
+        <plugin name="${orientation}_sensor" filename="libgazebo_ros_camera.so">
+          <ros>
+            <namespace>/image</namespace>
+            <remapping>~/image_raw:=${orientation}_img</remapping>
+            <remapping>~/camera_info:=${orientation}_camera_info</remapping>
+          </ros>
+          <initial_orientation_as_reference>false</initial_orientation_as_reference>
+          <camera_name>${orientation}_camera</camera_name>
+          <frame_name>${orientation}_tf</frame_name>
+          <hack_baseline>0.07</hack_baseline>
+        </plugin>
+        <always_on>true</always_on>
+        <update_rate>100</update_rate>
+        <visualize>true</visualize>
+        <camera name="cam/${orientation}">
+          <horizontal_fov>1.3962634</horizontal_fov>
+          <image>
+            <width>800</width>
+            <height>800</height>
+            <format>R8G8B8</format>
+          </image>
+          <clip>
+            <near>0.02</near>
+            <far>300</far>
+          </clip>
+          <noise>
+            <type>gaussian</type>
+            <mean>0</mean>
+            <stddev>0.007</stddev>
+          </noise>
+        </camera>
+      </sensor>
+    </gazebo>
+  </xacro:macro>
+  <xacro:camera orientation="center" origin_x="0.1" origin_y="0" origin_z="0" yaw="-0.14" />
+  <xacro:camera orientation="left" origin_x="0" origin_y="0.1" origin_z="0" yaw="1.5" />
+  <xacro:camera orientation="right" origin_x="0" origin_y="-0.1" origin_z="0" yaw="-1.5" />
+
+
+  <!-- Depth Camera (addition for Nav2) -->
+  <link name="camera_link">
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <box size="0.015 0.130 0.022" />
+      </geometry>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <box size="0.015 0.130 0.022" />
+      </geometry>
+    </collision>
+
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <mass value="0.035" />
+      <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001" />
+    </inertial>
+  </link>
+
+  <joint name="camera_joint" type="fixed">
+    <parent link="base_link" />
+    <child link="camera_link" />
+    <origin xyz="0.1 0 0.03" rpy="0 0 0" />
+  </joint>
+
+  <link name="camera_depth_frame" />
+
+  <joint name="camera_depth_joint" type="fixed">
+    <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}" />
+    <parent link="camera_link" />
+    <child link="camera_depth_frame" />
+  </joint>
+
+  <gazebo reference="camera_link">
+    <sensor name="depth_camera" type="depth">
+      <visualize>true</visualize>
+      <update_rate>30.0</update_rate>
+      <camera name="camera">
+        <horizontal_fov>1.047198</horizontal_fov>
+        <image>
+          <width>640</width>
+          <height>480</height>
+          <format>R8G8B8</format>
+        </image>
+        <clip>
+          <near>0.05</near>
+          <far>3</far>
+        </clip>
+      </camera>
+      <plugin name="depth_camera_controller" filename="libgazebo_ros_camera.so">
+        <baseline>0.2</baseline>
+        <alwaysOn>true</alwaysOn>
+        <updateRate>0.0</updateRate>
+        <frame_name>camera_depth_frame</frame_name>
+        <pointCloudCutoff>0.5</pointCloudCutoff>
+        <pointCloudCutoffMax>3.0</pointCloudCutoffMax>
+        <distortionK1>0</distortionK1>
+        <distortionK2>0</distortionK2>
+        <distortionK3>0</distortionK3>
+        <distortionT1>0</distortionT1>
+        <distortionT2>0</distortionT2>
+        <CxPrime>0</CxPrime>
+        <Cx>0</Cx>
+        <Cy>0</Cy>
+        <focalLength>0</focalLength>
+        <hackBaseline>0</hackBaseline>
+      </plugin>
+    </sensor>
+  </gazebo>
+
+
+  <gazebo>
+    <sensor name="imu" type="imu">
+      <always_on>true</always_on>
+      <update_rate>30</update_rate>
+      <plugin name="imu" filename="libgazebo_ros_imu_sensor.so">
+        <ros>
+          <namespace>/imu</namespace>
+          <argument>~/out:=data</argument>
+        </ros>
+        <frame_name>imu</frame_name>
+      </plugin>
+    </sensor>
+    <plugin name="gazebo_ros_p3d" filename="libgazebo_ros_p3d.so">
+      <alwaysOn>true</alwaysOn>
+      <update_rate>30.0</update_rate>
+      <ros>
+        <namespace>/ground_truth</namespace>
+        <remapping>odom:=state_raw</remapping>
+      </ros>
+      <body_name>base_link</body_name>
+      <frame_name>world</frame_name>
+
+      <xyz_offset>0 0 0</xyz_offset>
+      <rpy_offset>0.0 0.0 0.0</rpy_offset>
+      <gaussian_noise>0.00</gaussian_noise>
+    </plugin>
+  </gazebo>
+
+</robot>
+
+<!-- Mock/Untested ros2 control scheme for operating the rover with ros2 control.
+    This matches how the control is supposed to be set up, but is commented out
+    because the way the urdf is designed, it interferes with the arm control. -->
+<!--
+    <ros2_control name="rover_control" type="system">
+    <hardware>
+      <plugin>ros2_control_demo_hardware/VelocityActuatorHardware</plugin>
+    </hardware>
+    <joint name="left_bogie_front_axle">
+      <command_interface name="velocity">
+        <param name="min">-1</param>
+        <param name="max">1</param>
+      </command_interface>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="left_bogie_rear_axle">
+      <command_interface name="velocity">
+        <param name="min">-1</param>
+        <param name="max">1</param>
+      </command_interface>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="left_rocker_rear_axle">
+      <command_interface name="velocity">
+        <param name="min">-1</param>
+        <param name="max">1</param>
+      </command_interface>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="right_bogie_front_axle">
+      <command_interface name="velocity">
+        <param name="min">-1</param>
+        <param name="max">1</param>
+      </command_interface>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="right_bogie_rear_axle">
+      <command_interface name="velocity">
+        <param name="min">-1</param>
+        <param name="max">1</param>
+      </command_interface>
+      <state_interface name="velocity"/>
+    </joint>
+    <joint name="right_rocker_rear_axle">
+      <command_interface name="velocity">
+        <param name="min">-1</param>
+        <param name="max">1</param>
+      </command_interface>
+      <state_interface name="velocity"/>
+    </joint>
+    <transmission name="rover_transmission">
+      <plugin>transmission_interface/SimpleTransmission</plugin>
+      <joint name="left_bogie_front_axle" role="left_bogie_front_axle">
+        <mechanical_reduction>1</mechanical_reduction>
+      </joint>
+      <joint name="left_bogie_rear_axle" role="left_bogie_rear_axle">
+        <mechanical_reduction>1</mechanical_reduction>
+      </joint>
+      <joint name="left_rocker_rear_axle" role="left_rocker_rear_axle">
+        <mechanical_reduction>1</mechanical_reduction>
+      </joint>
+      <joint name="right_bogie_front_axle" role="right_bogie_front_axle">
+        <mechanical_reduction>1</mechanical_reduction>
+      </joint>
+      <joint name="right_bogie_rear_axle" role="right_bogie_rear_axle">
+        <mechanical_reduction>1</mechanical_reduction>
+      </joint>
+      <joint name="right_rocker_rear_axle" role="right_rocket_rear_axle">
+        <mechanical_reduction>1</mechanical_reduction>
+      </joint>
+    </transmission>
+    </ros2_control>
+    -->
\ No newline at end of file
diff --git a/urc_hw_description/urdf/walli_sensors.xacro b/urc_hw_description/urdf/walli_sensors.xacro
new file mode 100644
index 00000000..dbe0e123
--- /dev/null
+++ b/urc_hw_description/urdf/walli_sensors.xacro
@@ -0,0 +1,376 @@
+<?xml version="1.0"?>
+<robot name="walli_sensors" xmlns:xacro="http://ros.org/wiki/xacro">
+<!-- IMU -->
+  <link name="imu_link">
+    <visual>
+      <geometry>
+        <box size="0.1 0.1 0.1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.1 0.1 0.1"/>
+      </geometry>
+    </collision>
+    <xacro:box_inertia m="0.1" w="0.1" d="0.1" h="0.1"/>
+  </link>
+  <joint name="imu_joint" type="fixed">
+		<axis xyz="1 0 0"/>
+		<origin xyz="0 0 0.05"/>
+		<parent link="base_link"/>
+		<child link="imu_link"/>
+	</joint>
+	<gazebo reference="imu_link">
+    <sensor name="imu_sensor" type="imu">
+      <plugin filename="libgazebo_ros_imu_sensor.so" name="imu_plugin">
+        <ros>
+          <namespace>/imu</namespace>
+          <remapping>~/out:=data</remapping>
+        </ros>
+        <initial_orientation_as_reference>false</initial_orientation_as_reference>
+      </plugin>
+      <always_on>true</always_on>
+      <update_rate>100</update_rate>
+      <visualize>true</visualize>
+     <imu>
+     	<angular_velocity>
+     	  <x>
+     	    <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>2e-4</stddev>
+              <bias_mean>0.0000075</bias_mean>
+              <bias_stddev>0.0000008</bias_stddev>
+            </noise>
+          </x>
+          <y>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>2e-4</stddev>
+              <bias_mean>0.0000075</bias_mean>
+              <bias_stddev>0.0000008</bias_stddev>
+            </noise>
+          </y>
+          <z>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>2e-4</stddev>
+              <bias_mean>0.0000075</bias_mean>
+              <bias_stddev>0.0000008</bias_stddev>
+            </noise>
+          </z>
+        </angular_velocity>
+        <linear_acceleration>
+          <x>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>1.7e-2</stddev>
+              <bias_mean>0.1</bias_mean>
+              <bias_stddev>0.001</bias_stddev>
+            </noise>
+          </x>
+          <y>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>1.7e-2</stddev>
+              <bias_mean>0.1</bias_mean>
+              <bias_stddev>0.001</bias_stddev>
+            </noise>
+          </y>
+          <z>
+            <noise type="gaussian">
+              <mean>0.0</mean>
+              <stddev>1.7e-2</stddev>
+              <bias_mean>0.1</bias_mean>
+              <bias_stddev>0.001</bias_stddev>
+            </noise>
+          </z>
+        </linear_acceleration>
+      </imu>
+    </sensor>
+  </gazebo>
+
+
+  <gazebo>
+  <plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
+    <ros>
+      <namespace>/sensor</namespace>
+    </ros>
+    <!-- wheels -->
+    <left_joint>left_front_wheel_joint</left_joint>
+    <right_joint>right_front_wheel_joint</right_joint>
+    <!-- kinematics -->
+    <wheel_separation>0.4</wheel_separation>
+    <wheel_diameter>0.2</wheel_diameter>
+    <!-- limits -->
+    <max_wheel_torque>20</max_wheel_torque>
+    <max_wheel_acceleration>1.0</max_wheel_acceleration>
+    <!-- output -->
+    <publish_odom>true</publish_odom>
+    <publish_odom_tf>false</publish_odom_tf>
+    <publish_wheel_tf>true</publish_wheel_tf>
+    <odometry_frame>odom</odometry_frame>
+    <robot_base_frame>base_link</robot_base_frame>
+  </plugin>
+</gazebo>
+
+  <!-- GPS -->
+  <link name="gps">
+    <visual>
+      <geometry>
+        <box size="0.1 0.1 0.1"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.1 0.1 0.1"/>
+      </geometry>
+    </collision>
+    <xacro:box_inertia m="0.1" w="0.1" d="0.1" h="0.1"/>
+  </link>
+   <joint name="gps_joint" type="fixed">
+		<axis xyz="1 0 0"/>
+		<origin xyz="0 0 0.05"/>
+		<parent link="base_link"/>
+		<child link="gps"/>
+	</joint>
+  <gazebo reference="gps">
+    <sensor name="gps" type="gps">
+      <plugin filename="libgazebo_ros_gps_sensor.so" name="gps_plugin">
+          <ros>
+            <namespace>/gps</namespace>
+            <remapping>~/out:=data</remapping>
+          </ros>
+          <frame_name>gps</frame_name>
+        </plugin>
+        <always_on>true</always_on>
+        <updateRate>30</updateRate>
+    </sensor>
+  </gazebo>
+
+   <!-- LIDAR -->
+  <link name="lidar_link">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0.125"/>
+      <inertia ixx="0.001"  ixy="0"  ixz="0" iyy="0.001" iyz="0" izz="0.001" />
+    </inertial>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.0508" length="0.055"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <cylinder radius="0.0508" length="0.055"/>
+      </geometry>
+    </visual>
+  </link>
+  <joint name="lidar_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="lidar_link"/>
+    <origin xyz="0.0 0.0 0.1" rpy="0.0 0.0 0.0"/>
+  </joint>
+  <!-- <xacro:VLP-16 parent="lidar_link" name="lidar" topic="/scan" hz="${lidar_hz}" samples="${lidar_samples}" gpu="false">
+    <origin xyz="${lidar_x} 0.0 ${lidar_z}" rpy="0 ${lidar_pitch} 0" />
+  </xacro:VLP-16> -->
+  <gazebo reference="lidar_link">
+  <sensor name="lidar" type="ray">
+    <always_on>true</always_on>
+    <visualize>true</visualize>
+    <update_rate>5</update_rate>
+    <ray>
+      <scan>
+        <horizontal>
+          <samples>360</samples>
+          <resolution>1.000000</resolution>
+          <min_angle>0.000000</min_angle>
+          <max_angle>6.280000</max_angle>
+        </horizontal>
+      </scan>
+      <range>
+        <min>0.12</min>
+        <max>3.5</max>
+        <resolution>0.015000</resolution>
+      </range>
+      <noise>
+        <type>gaussian</type>
+        <mean>0.0</mean>
+        <stddev>0.01</stddev>
+      </noise>
+    </ray>
+    <plugin name="scan" filename="libgazebo_ros_ray_sensor.so">
+      <ros>
+        <remapping>~/out:=scan</remapping>
+      </ros>
+      <output_type>sensor_msgs/LaserScan</output_type>
+      <frame_name>lidar_link</frame_name>
+    </plugin>
+  </sensor>
+</gazebo>
+
+  <!-- May want to change this to a depth camera -->
+  <xacro:macro name="camera" params="orientation origin_x origin_y origin_z yaw">
+    <link name="cam/${orientation}">
+      <visual>
+        <geometry>
+          <box size="0.01 0.01 0.01"/>
+        </geometry>
+      </visual>
+      <collision>
+        <geometry>
+          <box size="0.01 0.01 0.01"/>
+        </geometry>
+      </collision>
+      <xacro:box_inertia m="0.01" w="0.01" d="0.01" h="0.01"/>
+    </link>
+    <joint name="${orientation}_cam_joint" type="fixed">
+  	  <axis xyz="1 0 0"/>
+      <!-- Camera Orientation-->
+	    <origin xyz="${origin_x} ${origin_y} ${origin_z}" rpy = "0 0 ${yaw}"/>
+	    <parent link="base_link"/>
+	    <child link="cam/${orientation}"/>
+    </joint>
+    <gazebo reference="cam/${orientation}">
+      <sensor name="${orientation}_sensor" type="camera">
+        <plugin name="${orientation}_sensor" filename="libgazebo_ros_camera.so">
+          <ros>
+            <namespace>/image</namespace>
+            <remapping>~/image_raw:=${orientation}_img</remapping>
+            <remapping>~/camera_info:=${orientation}_camera_info</remapping>
+          </ros>
+          <initial_orientation_as_reference>false</initial_orientation_as_reference>
+          <camera_name>${orientation}_camera</camera_name>
+          <frame_name>${orientation}_tf</frame_name>
+          <hack_baseline>0.07</hack_baseline>
+        </plugin>
+        <always_on>true</always_on>
+        <update_rate>100</update_rate>
+        <visualize>true</visualize>
+        <camera name="cam/${orientation}">
+          <horizontal_fov>1.3962634</horizontal_fov>
+          <image>
+            <width>800</width>
+            <height>800</height>
+            <format>R8G8B8</format>
+          </image>
+          <clip>
+            <near>0.02</near>
+            <far>300</far>
+          </clip>
+          <noise>
+            <type>gaussian</type>
+            <mean>0</mean>
+            <stddev>0.007</stddev>
+          </noise>
+        </camera>
+      </sensor>
+    </gazebo>
+  </xacro:macro>
+  <xacro:camera orientation="center" origin_x="0.1" origin_y="0"    origin_z="0" yaw="-0.14"/>
+  <xacro:camera orientation="left"   origin_x="0"    origin_y="0.1"  origin_z="0"    yaw="1.5"/>
+  <xacro:camera orientation="right"  origin_x="0"    origin_y="-0.1" origin_z="0"    yaw="-1.5"/>
+
+
+<!-- Depth Camera (addition for Nav2) -->
+<link name="camera_link">
+  <visual>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <geometry>
+      <box size="0.015 0.130 0.022"/>
+    </geometry>
+  </visual>
+
+  <collision>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <geometry>
+      <box size="0.015 0.130 0.022"/>
+    </geometry>
+  </collision>
+
+  <inertial>
+    <origin xyz="0 0 0" rpy="0 0 0"/>
+    <mass value="0.035"/>
+    <inertia ixx="0.001"  ixy="0"  ixz="0" iyy="0.001" iyz="0" izz="0.001" />
+  </inertial>
+</link>
+
+<joint name="camera_joint" type="fixed">
+  <parent link="base_link"/>
+  <child link="camera_link"/>
+  <origin xyz="0.1 0 0.03" rpy="0 0 0"/>
+</joint>
+
+<link name="camera_depth_frame"/>
+
+<joint name="camera_depth_joint" type="fixed">
+  <origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
+  <parent link="camera_link"/>
+  <child link="camera_depth_frame"/>
+</joint>
+
+<gazebo reference="camera_link">
+  <sensor name="depth_camera" type="depth">
+    <visualize>true</visualize>
+    <update_rate>30.0</update_rate>
+    <camera name="camera">
+      <horizontal_fov>1.047198</horizontal_fov>
+      <image>
+        <width>640</width>
+        <height>480</height>
+        <format>R8G8B8</format>
+      </image>
+      <clip>
+        <near>0.05</near>
+        <far>3</far>
+      </clip>
+    </camera>
+    <plugin name="depth_camera_controller" filename="libgazebo_ros_camera.so">
+      <baseline>0.2</baseline>
+      <alwaysOn>true</alwaysOn>
+      <updateRate>0.0</updateRate>
+      <frame_name>camera_depth_frame</frame_name>
+      <pointCloudCutoff>0.5</pointCloudCutoff>
+      <pointCloudCutoffMax>3.0</pointCloudCutoffMax>
+      <distortionK1>0</distortionK1>
+      <distortionK2>0</distortionK2>
+      <distortionK3>0</distortionK3>
+      <distortionT1>0</distortionT1>
+      <distortionT2>0</distortionT2>
+      <CxPrime>0</CxPrime>
+      <Cx>0</Cx>
+      <Cy>0</Cy>
+      <focalLength>0</focalLength>
+      <hackBaseline>0</hackBaseline>
+    </plugin>
+  </sensor>
+</gazebo>
+  <gazebo>
+    <sensor name="imu" type="imu">
+      <always_on>true</always_on>
+      <update_rate>30</update_rate>
+      <plugin name="imu" filename="libgazebo_ros_imu_sensor.so">
+        <ros>
+          <namespace>/imu</namespace>
+          <argument>~/out:=data</argument>
+        </ros>
+        <frame_name>imu</frame_name>
+      </plugin>
+    </sensor>            
+    <plugin name="gazebo_ros_p3d" filename="libgazebo_ros_p3d.so">
+      <alwaysOn>true</alwaysOn>
+      <update_rate>30.0</update_rate>
+      <ros>
+        <namespace>/ground_truth</namespace>
+        <remapping>odom:=state_raw</remapping>
+      </ros>
+      <body_name>base_link</body_name>
+      <frame_name>world</frame_name>
+
+      <xyz_offset>0 0 0</xyz_offset>
+      <rpy_offset>0.0 0.0 0.0</rpy_offset>
+      <gaussian_noise>0.00</gaussian_noise>
+    </plugin>
+  </gazebo>
+</robot>
\ No newline at end of file