diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
new file mode 100644
index 0000000..76e9f9a
--- /dev/null
+++ b/.vscode/c_cpp_properties.json
@@ -0,0 +1,18 @@
+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceFolder}/**",
+                "~/miniforge3/envs/ros_env/include/**",
+                "/robotics/bcm2835-1.73/**"
+            ],
+            "defines": [],
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "c17",
+            "cppStandard": "gnu++17",
+            "intelliSenseMode": "linux-gcc-arm64"
+        }
+    ],
+    "version": 4
+}
\ No newline at end of file
diff --git a/.vscode/settings.json b/.vscode/settings.json
new file mode 100644
index 0000000..b6cafdb
--- /dev/null
+++ b/.vscode/settings.json
@@ -0,0 +1,92 @@
+{
+    "files.associations": {
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "csignal": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "ctime": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "strstream": "cpp",
+        "bit": "cpp",
+        "*.tcc": "cpp",
+        "bitset": "cpp",
+        "cfenv": "cpp",
+        "chrono": "cpp",
+        "codecvt": "cpp",
+        "compare": "cpp",
+        "complex": "cpp",
+        "concepts": "cpp",
+        "condition_variable": "cpp",
+        "cstdint": "cpp",
+        "deque": "cpp",
+        "forward_list": "cpp",
+        "list": "cpp",
+        "map": "cpp",
+        "set": "cpp",
+        "string": "cpp",
+        "unordered_map": "cpp",
+        "unordered_set": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "functional": "cpp",
+        "iterator": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "ratio": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "fstream": "cpp",
+        "future": "cpp",
+        "initializer_list": "cpp",
+        "iomanip": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "mutex": "cpp",
+        "new": "cpp",
+        "numbers": "cpp",
+        "ostream": "cpp",
+        "semaphore": "cpp",
+        "shared_mutex": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "stop_token": "cpp",
+        "streambuf": "cpp",
+        "thread": "cpp",
+        "cinttypes": "cpp",
+        "typeindex": "cpp",
+        "typeinfo": "cpp",
+        "variant": "cpp"
+    },
+    "ros.distro": "humble",
+    "python.autoComplete.extraPaths": [
+        "/workspaces/DevEnv/repos/Robot2023/install/grr_test/lib/python3.10/site-packages",
+        "/workspaces/DevEnv/repos/Robot2023/install/grr_python_controllers/lib/python3.10/site-packages",
+        "/workspaces/DevEnv/repos/Robot2023/install/grr_bringup/lib/python3.10/site-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/workspaces/DevEnv/repos/Robot2023/install/grr_test/lib/python3.10/site-packages",
+        "/workspaces/DevEnv/repos/Robot2023/install/grr_python_controllers/lib/python3.10/site-packages",
+        "/workspaces/DevEnv/repos/Robot2023/install/grr_bringup/lib/python3.10/site-packages",
+        "/opt/ros/humble/lib/python3.10/site-packages",
+        "/opt/ros/humble/local/lib/python3.10/dist-packages"
+    ]
+}
\ No newline at end of file
diff --git a/src/grr_description/bloodstone.urdf b/src/grr_description/bloodstone.urdf
new file mode 100644
index 0000000..db85f52
--- /dev/null
+++ b/src/grr_description/bloodstone.urdf
@@ -0,0 +1,935 @@
+<?xml version="1.0" ?><!-- =================================================================================== --><!-- |    This document was autogenerated by xacro from /robotics/workspace/install/grr_description/share/grr_description/urdf/robots/bloodstone.urdf.xacro | --><!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | --><!-- =================================================================================== --><robot name="bloodstone"><!-- Specify some standard inertial calculations https://en.wikipedia.org/wiki/List_of_moments_of_inertia --><!-- These make use of xacro's mathematical functionality --><material name="black"><color rgba="0.0 0.0 0.0 1.0"/></material><material name="blue"><color rgba="0.13725490196078433 0.43137254901960786 0.5882352941176471 1.0"/></material><material name="green"><color rgba="0.0 0.8 0.0 1.0"/></material><material name="grey"><color rgba="0.2 0.2 0.2 1.0"/></material><material name="yellow"><color rgba="1.0 0.8431372549019608 0.0 1"/></material><material name="orange"><color rgba="0.9529411764705882 0.5294117647058824 0.1843137254901961 1"/></material><material name="brown"><color rgba="0.8705882352941177 0.8117647058823529 0.7647058823529411 1.0"/></material><material name="red"><color rgba="0.8 0.0 0.0 1.0"/></material><material name="white"><color rgba="1.0 1.0 1.0 1.0"/></material><material name="silver"><color rgba="0.700 0.700 0.700 1.000"/></material><material name="zed2i_mat"><color rgba="0.25 0.25 0.25 0.99"/></material><ros2_control name="bloodstone" type="system"><hardware><plugin>grr_hardware/IsaacDriveHardware</plugin><param name="example_param_hw_start_duration_sec">3.0</param><param name="example_param_hw_stop_duration_sec">3.0</param></hardware><joint name="front_right_mecanum_joint"><command_interface name="velocity"><param name="min">-1</param><param name="max">1</param></command_interface><state_interface name="position"/><state_interface name="velocity"/></joint><joint name="rear_left_mecanum_joint"><command_interface name="velocity"><param name="min">-1</param><param name="max">1</param></command_interface><state_interface name="position"/><state_interface name="velocity"/></joint><joint name="front_left_mecanum_joint"><command_interface name="velocity"><param name="min">-1</param><param name="max">1</param></command_interface><state_interface name="position"/><state_interface name="velocity"/></joint><joint name="rear_right_mecanum_joint"><command_interface name="velocity"><param name="min">-1</param><param name="max">1</param></command_interface><state_interface name="position"/><state_interface name="velocity"/></joint><joint name="small_package_grabber_roller_1_joint"><command_interface name="effort"/><state_interface name="position"/><state_interface name="velocity"/></joint><joint name="small_package_sweeper_roller_joint"><command_interface name="effort"/><state_interface name="position"/><state_interface name="velocity"/></joint></ros2_control><link name="base_link"/><link name="chassis_link"><inertial><origin rpy="0 0 0" xyz="0 0 -0.0675"/><mass value="2.45"/><inertia ixx="0.025954687500000004" ixy="0.0" ixz="0.0" iyy="0.0103359375" iyz="0.0" izz="0.028848750000000003"/></inertial><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/slim_drivebase.stl" scale="0.001 0.001 0.001"/></geometry><material name="blue"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><box size="0.18 0.33 0.0675"/></geometry></collision></link><gazebo reference="chassis"><material>Gazebo/Blue</material></gazebo><joint name="chassis_joint" type="fixed"><parent link="base_link"/><child link="chassis_link"/><origin xyz="0 0 0"/></joint><link name="front_right_mecanum_link"><inertial><origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/><mass value="1.1360000000000001"/><inertia ixx="0.00042284766390000004" ixy="0.0" ixz="0.0" iyy="0.00042284766390000004" iyz="0.0" izz="0.0005117113278"/></inertial><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_wheel_1.stl" scale="0.001 0.001 0.001"/></geometry><material name="orange"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><cylinder length="0.042" radius="0.030015"/></geometry></collision></link><joint name="front_right_mecanum_joint" type="continuous"><origin rpy="0 1.5707963267948966 0" xyz="0.10075 0.10568 0"/><parent link="chassis_link"/><child link="front_right_mecanum_link"/><axis xyz="0.0 0.0 -1.0"/></joint><link name="front_right_roller_0"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="front_right_roller_0_joint" type="continuous"><origin rpy="0.0 0.7853981633974483 0" xyz="0.0 0.030015 0"/><parent link="front_right_mecanum_link"/><child link="front_right_roller_0"/><axis xyz="0.0 0.0 1.0"/></joint><link name="front_right_roller_1"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="front_right_roller_1_joint" type="continuous"><origin rpy="0.5553603672697958 0.5553603672697958 0" xyz="0.02122381003731422 0.021223810037314224 0"/><parent link="front_right_mecanum_link"/><child link="front_right_roller_1"/><axis xyz="0.0 0.0 1.0"/></joint><link name="front_right_roller_2"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="front_right_roller_2_joint" type="continuous"><origin rpy="0.7853981633974483 4.8091767343044744e-17 0" xyz="0.030015 1.8378886838203904e-18 0"/><parent link="front_right_mecanum_link"/><child link="front_right_roller_2"/><axis xyz="0.0 0.0 1.0"/></joint><link name="front_right_roller_3"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="front_right_roller_3_joint" type="continuous"><origin rpy="0.5553603672697958 -0.5553603672697958 0" xyz="0.021223810037314224 -0.02122381003731422 0"/><parent link="front_right_mecanum_link"/><child link="front_right_roller_3"/><axis xyz="0.0 0.0 1.0"/></joint><link name="front_right_roller_4"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="front_right_roller_4_joint" type="continuous"><origin rpy="9.618353468608949e-17 -0.7853981633974483 0" xyz="3.675777367640781e-18 -0.030015 0"/><parent link="front_right_mecanum_link"/><child link="front_right_roller_4"/><axis xyz="0.0 0.0 1.0"/></joint><link name="front_right_roller_5"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="front_right_roller_5_joint" type="continuous"><origin rpy="-0.5553603672697958 -0.5553603672697959 0" xyz="-0.02122381003731422 -0.021223810037314227 0"/><parent link="front_right_mecanum_link"/><child link="front_right_roller_5"/><axis xyz="0.0 0.0 1.0"/></joint><link name="front_right_roller_6"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="front_right_roller_6_joint" type="continuous"><origin rpy="-0.7853981633974483 -1.4427530202913423e-16 0" xyz="-0.030015 -5.513666051461171e-18 0"/><parent link="front_right_mecanum_link"/><child link="front_right_roller_6"/><axis xyz="0.0 0.0 1.0"/></joint><link name="front_right_roller_7"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="front_right_roller_7_joint" type="continuous"><origin rpy="-0.5234033886657797 0.6093783076190201 0.09416051314509408" xyz="-0.021223810037314227 0.02122381003731422 0"/><parent link="front_right_mecanum_link"/><child link="front_right_roller_7"/><axis xyz="0.0 0.0 1.0"/></joint><link name="rear_left_mecanum_link"><inertial><origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/><mass value="1.1360000000000001"/><inertia ixx="0.00042284766390000004" ixy="0.0" ixz="0.0" iyy="0.00042284766390000004" iyz="0.0" izz="0.0005117113278"/></inertial><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_wheel_1.stl" scale="0.001 0.001 0.001"/></geometry><material name="orange"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><cylinder length="0.042" radius="0.030015"/></geometry></collision></link><joint name="rear_left_mecanum_joint" type="continuous"><origin rpy="0 -1.5707963267948966 0" xyz="-0.10075 -0.10568 0"/><parent link="chassis_link"/><child link="rear_left_mecanum_link"/><axis xyz="0.0 0.0 1.0"/></joint><link name="rear_left_roller_0"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="rear_left_roller_0_joint" type="continuous"><origin rpy="0.0 0.7853981633974483 0" xyz="0.0 0.030015 0"/><parent link="rear_left_mecanum_link"/><child link="rear_left_roller_0"/><axis xyz="0.0 0.0 1.0"/></joint><link name="rear_left_roller_1"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="rear_left_roller_1_joint" type="continuous"><origin rpy="0.5553603672697958 0.5553603672697958 0" xyz="0.02122381003731422 0.021223810037314224 0"/><parent link="rear_left_mecanum_link"/><child link="rear_left_roller_1"/><axis xyz="0.0 0.0 1.0"/></joint><link name="rear_left_roller_2"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="rear_left_roller_2_joint" type="continuous"><origin rpy="0.7853981633974483 4.8091767343044744e-17 0" xyz="0.030015 1.8378886838203904e-18 0"/><parent link="rear_left_mecanum_link"/><child link="rear_left_roller_2"/><axis xyz="0.0 0.0 1.0"/></joint><link name="rear_left_roller_3"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="rear_left_roller_3_joint" type="continuous"><origin rpy="0.5553603672697958 -0.5553603672697958 0" xyz="0.021223810037314224 -0.02122381003731422 0"/><parent link="rear_left_mecanum_link"/><child link="rear_left_roller_3"/><axis xyz="0.0 0.0 1.0"/></joint><link name="rear_left_roller_4"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="rear_left_roller_4_joint" type="continuous"><origin rpy="9.618353468608949e-17 -0.7853981633974483 0" xyz="3.675777367640781e-18 -0.030015 0"/><parent link="rear_left_mecanum_link"/><child link="rear_left_roller_4"/><axis xyz="0.0 0.0 1.0"/></joint><link name="rear_left_roller_5"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="rear_left_roller_5_joint" type="continuous"><origin rpy="-0.5553603672697958 -0.5553603672697959 0" xyz="-0.02122381003731422 -0.021223810037314227 0"/><parent link="rear_left_mecanum_link"/><child link="rear_left_roller_5"/><axis xyz="0.0 0.0 1.0"/></joint><link name="rear_left_roller_6"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="rear_left_roller_6_joint" type="continuous"><origin rpy="-0.7853981633974483 -1.4427530202913423e-16 0" xyz="-0.030015 -5.513666051461171e-18 0"/><parent link="rear_left_mecanum_link"/><child link="rear_left_roller_6"/><axis xyz="0.0 0.0 1.0"/></joint><link name="rear_left_roller_7"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="rear_left_roller_7_joint" type="continuous"><origin rpy="-0.5234033886657797 0.6093783076190201 0.09416051314509408" xyz="-0.021223810037314227 0.02122381003731422 0"/><parent link="rear_left_mecanum_link"/><child link="rear_left_roller_7"/><axis xyz="0.0 0.0 1.0"/></joint><link name="front_left_mecanum_link"><inertial><origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/><mass value="1.1360000000000001"/><inertia ixx="0.00042284766390000004" ixy="0.0" ixz="0.0" iyy="0.00042284766390000004" iyz="0.0" izz="0.0005117113278"/></inertial><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_wheel_2.stl" scale="0.001 0.001 0.001"/></geometry><material name="orange"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><cylinder length="0.042" radius="0.030015"/></geometry></collision></link><joint name="front_left_mecanum_joint" type="continuous"><origin rpy="0 1.5707963267948966 0" xyz="-0.10075 0.10568 0"/><parent link="chassis_link"/><child link="front_left_mecanum_link"/><axis xyz="0.0 0.0 -1.0"/></joint><link name="front_left_roller_0"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="front_left_roller_0_joint" type="continuous"><origin rpy="-0.0 -0.7853981633974483 0" xyz="0.0 0.030015 0"/><parent link="front_left_mecanum_link"/><child link="front_left_roller_0"/><axis xyz="0.0 0.0 1.0"/></joint><link name="front_left_roller_1"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="front_left_roller_1_joint" type="continuous"><origin rpy="-0.5553603672697958 -0.5553603672697958 0" xyz="0.02122381003731422 0.021223810037314224 0"/><parent link="front_left_mecanum_link"/><child link="front_left_roller_1"/><axis xyz="0.0 0.0 1.0"/></joint><link name="front_left_roller_2"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="front_left_roller_2_joint" type="continuous"><origin rpy="-0.7853981633974483 -4.8091767343044744e-17 0" xyz="0.030015 1.8378886838203904e-18 0"/><parent link="front_left_mecanum_link"/><child link="front_left_roller_2"/><axis xyz="0.0 0.0 1.0"/></joint><link name="front_left_roller_3"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="front_left_roller_3_joint" type="continuous"><origin rpy="-0.5553603672697958 0.5553603672697958 0" xyz="0.021223810037314224 -0.02122381003731422 0"/><parent link="front_left_mecanum_link"/><child link="front_left_roller_3"/><axis xyz="0.0 0.0 1.0"/></joint><link name="front_left_roller_4"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="front_left_roller_4_joint" type="continuous"><origin rpy="-9.618353468608949e-17 0.7853981633974483 0" xyz="3.675777367640781e-18 -0.030015 0"/><parent link="front_left_mecanum_link"/><child link="front_left_roller_4"/><axis xyz="0.0 0.0 1.0"/></joint><link name="front_left_roller_5"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="front_left_roller_5_joint" type="continuous"><origin rpy="0.5553603672697958 0.5553603672697959 0" xyz="-0.02122381003731422 -0.021223810037314227 0"/><parent link="front_left_mecanum_link"/><child link="front_left_roller_5"/><axis xyz="0.0 0.0 1.0"/></joint><link name="front_left_roller_6"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="front_left_roller_6_joint" type="continuous"><origin rpy="0.7853981633974483 1.4427530202913423e-16 0" xyz="-0.030015 -5.513666051461171e-18 0"/><parent link="front_left_mecanum_link"/><child link="front_left_roller_6"/><axis xyz="0.0 0.0 1.0"/></joint><link name="front_left_roller_7"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="front_left_roller_7_joint" type="continuous"><origin rpy="0.587317345873812 -0.5013424269205712 0.09416051314509408" xyz="-0.021223810037314227 0.02122381003731422 0"/><parent link="front_left_mecanum_link"/><child link="front_left_roller_7"/><axis xyz="0.0 0.0 1.0"/></joint><link name="rear_right_mecanum_link"><inertial><origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/><mass value="1.1360000000000001"/><inertia ixx="0.00042284766390000004" ixy="0.0" ixz="0.0" iyy="0.00042284766390000004" iyz="0.0" izz="0.0005117113278"/></inertial><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_wheel_2.stl" scale="0.001 0.001 0.001"/></geometry><material name="orange"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><cylinder length="0.042" radius="0.030015"/></geometry></collision></link><joint name="rear_right_mecanum_joint" type="continuous"><origin rpy="0 -1.5707963267948966 0" xyz="0.10075 -0.10568 0"/><parent link="chassis_link"/><child link="rear_right_mecanum_link"/><axis xyz="0.0 0.0 1.0"/></joint><link name="rear_right_roller_0"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="rear_right_roller_0_joint" type="continuous"><origin rpy="-0.0 -0.7853981633974483 0" xyz="0.0 0.030015 0"/><parent link="rear_right_mecanum_link"/><child link="rear_right_roller_0"/><axis xyz="0.0 0.0 1.0"/></joint><link name="rear_right_roller_1"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="rear_right_roller_1_joint" type="continuous"><origin rpy="-0.5553603672697958 -0.5553603672697958 0" xyz="0.02122381003731422 0.021223810037314224 0"/><parent link="rear_right_mecanum_link"/><child link="rear_right_roller_1"/><axis xyz="0.0 0.0 1.0"/></joint><link name="rear_right_roller_2"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="rear_right_roller_2_joint" type="continuous"><origin rpy="-0.7853981633974483 -4.8091767343044744e-17 0" xyz="0.030015 1.8378886838203904e-18 0"/><parent link="rear_right_mecanum_link"/><child link="rear_right_roller_2"/><axis xyz="0.0 0.0 1.0"/></joint><link name="rear_right_roller_3"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="rear_right_roller_3_joint" type="continuous"><origin rpy="-0.5553603672697958 0.5553603672697958 0" xyz="0.021223810037314224 -0.02122381003731422 0"/><parent link="rear_right_mecanum_link"/><child link="rear_right_roller_3"/><axis xyz="0.0 0.0 1.0"/></joint><link name="rear_right_roller_4"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="rear_right_roller_4_joint" type="continuous"><origin rpy="-9.618353468608949e-17 0.7853981633974483 0" xyz="3.675777367640781e-18 -0.030015 0"/><parent link="rear_right_mecanum_link"/><child link="rear_right_roller_4"/><axis xyz="0.0 0.0 1.0"/></joint><link name="rear_right_roller_5"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="rear_right_roller_5_joint" type="continuous"><origin rpy="0.5553603672697958 0.5553603672697959 0" xyz="-0.02122381003731422 -0.021223810037314227 0"/><parent link="rear_right_mecanum_link"/><child link="rear_right_roller_5"/><axis xyz="0.0 0.0 1.0"/></joint><link name="rear_right_roller_6"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="rear_right_roller_6_joint" type="continuous"><origin rpy="0.7853981633974483 1.4427530202913423e-16 0" xyz="-0.030015 -5.513666051461171e-18 0"/><parent link="rear_right_mecanum_link"/><child link="rear_right_roller_6"/><axis xyz="0.0 0.0 1.0"/></joint><link name="rear_right_roller_7"><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="grey"/></visual><!-- <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_middle_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_second_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 ${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision>
+      <collision>
+        <origin xyz="0 0 -${2 * roller_length/5}" rpy="0 0 0"/>
+        <geometry>
+          <sphere radius="${roller_third_radius}"/>
+        </geometry>
+      </collision> --><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/mecanum_roller.stl" scale="0.001 0.001 0.001"/></geometry></collision><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="0.07"/><inertia ixx="9.426025000000001e-06" ixy="0.0" ixz="0.0" iyy="9.426025000000001e-06" iyz="0.0" izz="1.9166000000000004e-06"/></inertial></link><joint name="rear_right_roller_7_joint" type="continuous"><origin rpy="0.587317345873812 -0.5013424269205712 0.09416051314509408" xyz="-0.021223810037314227 0.02122381003731422 0"/><parent link="rear_right_mecanum_link"/><child link="rear_right_roller_7"/><axis xyz="0.0 0.0 1.0"/></joint><link name="small_package_grabber_link"><inertial><origin rpy="0 0 0" xyz="0 0 -0.015"/><mass value="2.45"/><inertia ixx="0.025954687500000004" ixy="0.0" ixz="0.0" iyy="0.0103359375" iyz="0.0" izz="0.028848750000000003"/></inertial><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/small_package_grabber.stl" scale="0.001 0.001 0.001"/></geometry><material name="red"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><box size="0.18 0.33 0.0675"/></geometry></collision></link><joint name="small_package_grabber_joint" type="fixed"><parent link="chassis_link"/><child link="small_package_grabber_link"/><origin xyz="-0.03 0.1 0.03"/></joint><link name="small_package_grabber_roller_1_link"><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="2.45"/><inertia ixx="0.04207875" ixy="0.0" ixz="0.0" iyy="0.04207875" iyz="0.0" izz="0.03969"/></inertial><visual><origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/small_package_grabber_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="yellow"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><cylinder length="0.33" radius="0.18"/></geometry></collision></link><joint name="small_package_grabber_roller_1_joint" type="continuous"><origin rpy="0 0 0" xyz="0.035 0.03 -0.04"/><parent link="small_package_grabber_link"/><child link="small_package_grabber_roller_1_link"/><axis xyz="1 0 0"/></joint><link name="small_package_grabber_roller_2_link"><inertial><origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/><mass value="2.45"/><inertia ixx="0.04207875" ixy="0.0" ixz="0.0" iyy="0.04207875" iyz="0.0" izz="0.03969"/></inertial><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/small_package_grabber_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="yellow"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><cylinder length="0.33" radius="0.18"/></geometry></collision></link><joint name="small_package_grabber_roller_2_joint" type="continuous"><origin rpy="0 0 0" xyz="0.035 0.03 0.02"/><parent link="small_package_grabber_link"/><child link="small_package_grabber_roller_2_link"/><axis xyz="1 0 0"/><mimic joint="small_package_grabber_roller_1_joint" multiplier="1" offset="0"/></joint><link name="small_package_grabber_roller_3_link"><inertial><origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/><mass value="2.45"/><inertia ixx="0.04207875" ixy="0.0" ixz="0.0" iyy="0.04207875" iyz="0.0" izz="0.03969"/></inertial><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/small_package_grabber_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="yellow"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><cylinder length="0.33" radius="0.18"/></geometry></collision></link><joint name="small_package_grabber_roller_3_joint" type="continuous"><origin rpy="0 0 0" xyz="0.035 -0.02 -0.055"/><parent link="small_package_grabber_link"/><child link="small_package_grabber_roller_3_link"/><axis xyz="1 0 0"/><mimic joint="small_package_grabber_roller_1_joint" multiplier="-1" offset="0"/></joint><link name="small_package_grabber_roller_4_link"><inertial><origin rpy="0 1.5707963267948966 0" xyz="0 0 0"/><mass value="2.45"/><inertia ixx="0.04207875" ixy="0.0" ixz="0.0" iyy="0.04207875" iyz="0.0" izz="0.03969"/></inertial><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/small_package_grabber_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="yellow"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><cylinder length="0.33" radius="0.18"/></geometry></collision></link><joint name="small_package_grabber_roller_4_joint" type="continuous"><origin rpy="0 0 0" xyz="0.035 -0.03 0.02"/><!-- -0.03 0.1 0.03 --><parent link="small_package_grabber_link"/><child link="small_package_grabber_roller_4_link"/><axis xyz="1 0 0"/><mimic joint="small_package_grabber_roller_1_joint" multiplier="-1" offset="0"/></joint><link name="grr_brain_link"><inertial><origin rpy="0 0 0" xyz="0 0 -0.015"/><mass value="2.45"/><inertia ixx="0.025954687500000004" ixy="0.0" ixz="0.0" iyy="0.0103359375" iyz="0.0" izz="0.028848750000000003"/></inertial><visual><origin rpy="0 0 0" xyz="-0.03 0.1 0.03"/><geometry><mesh filename="package://grr_description/meshes/brain.stl" scale="0.001 0.001 0.001"/></geometry><material name="red"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><box size="0.18 0.33 0.0675"/></geometry></collision></link><joint name="grr_brain_joint" type="fixed"><parent link="chassis_link"/><child link="grr_brain_link"/><origin xyz="-0.09 -0.14 0.07"/></joint><link name="small_package_sweeper_servo_link"><inertial><origin rpy="0 0 0" xyz="0 0 -0.015"/><mass value="2.45"/><inertia ixx="0.025954687500000004" ixy="0.0" ixz="0.0" iyy="0.0103359375" iyz="0.0" izz="0.028848750000000003"/></inertial><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/small_package_sweeper_servo.stl" scale="0.001 0.001 0.001"/></geometry><material name="yellow"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><box size="0.18 0.33 0.0675"/></geometry></collision></link><joint name="small_package_servo_joint" type="fixed"><origin rpy="0 0 0" xyz="-0.035 0.22 0.025"/><parent link="grr_brain_link"/><child link="small_package_sweeper_servo_link"/></joint><link name="small_package_sweeper_link"><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="2.45"/><inertia ixx="0.023565937500000002" ixy="0.0" ixz="0.0" iyy="0.023565937500000002" iyz="0.0" izz="0.03969"/></inertial><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/small_package_sweeper.stl" scale="0.001 0.001 0.001"/></geometry><material name="yellow"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><cylinder length="0.135" radius="0.18"/></geometry></collision></link><joint name="small_package_sweeper_joint" type="revolute"><origin rpy="0 0 0" xyz="-0.01 0.06 -0.06"/><parent link="small_package_sweeper_servo_link"/><child link="small_package_sweeper_link"/><axis xyz="1 0 0"/><limit effort="100" lower="-1.57" upper="1.57" velocity="0.5"/></joint><link name="small_package_sweeper_roller_link"><inertial><origin rpy="0 0 0" xyz="0 0 0"/><mass value="2.45"/><inertia ixx="0.023565937500000002" ixy="0.0" ixz="0.0" iyy="0.023565937500000002" iyz="0.0" izz="0.03969"/></inertial><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/small_package_sweeper_roller.stl" scale="0.001 0.001 0.001"/></geometry><material name="yellow"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><cylinder length="0.135" radius="0.18"/></geometry></collision></link><joint name="small_package_sweeper_roller_joint" type="continuous"><origin rpy="0 0 0" xyz="0 0.05 -0.06"/><parent link="small_package_sweeper_link"/><child link="small_package_sweeper_roller_link"/><axis xyz="0 0 1"/></joint><link name="camera_stand_link"><inertial><origin rpy="0 0 0" xyz="0 0 -0.015"/><mass value="2.45"/><inertia ixx="0.025954687500000004" ixy="0.0" ixz="0.0" iyy="0.0103359375" iyz="0.0" izz="0.028848750000000003"/></inertial><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/camera_stand.stl" scale="0.001 0.001 0.001"/></geometry><material name="green"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><box size="0.18 0.33 0.0675"/></geometry></collision></link><joint name="camera_stand_joint" type="fixed"><parent link="chassis_link"/><child link="camera_stand_link"/><origin xyz="0.0 -0.08 0.145"/></joint><link name="camera_1_link"><inertial><origin rpy="0 0 0" xyz="0 0 -0.015"/><mass value="2.45"/><inertia ixx="0.05109393333333333" ixy="0.0" ixz="0.0" iyy="0.11854427083333333" iyz="0.0" izz="0.06755487083333332"/></inertial><visual><origin rpy="0 0 -1.5707963267948966" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/camera.stl" scale="0.001 0.001 0.001"/></geometry><material name="black"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><box size="0.575 0.016 0.25"/></geometry></collision></link><joint name="camera_1_joint" type="fixed"><parent link="camera_stand_link"/><child link="camera_1_link"/><origin rpy="0 0 0" xyz="0.02 -0.05 0.09"/></joint><link name="camera_2_link"><inertial><origin rpy="0 0 0" xyz="0 0 -0.015"/><mass value="2.45"/><inertia ixx="0.05109393333333333" ixy="0.0" ixz="0.0" iyy="0.11854427083333333" iyz="0.0" izz="0.06755487083333332"/></inertial><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/camera.stl" scale="0.001 0.001 0.001"/></geometry><material name="black"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><box size="0.575 0.016 0.25"/></geometry></collision></link><joint name="camera_2_joint" type="fixed"><parent link="camera_stand_link"/><child link="camera_2_link"/><origin rpy="0 0 0" xyz="0.045 0.04 0.07"/></joint><link name="bridge_latch_link"><inertial><origin rpy="0 0 0" xyz="0 0 -0.015"/><mass value="0.135"/><inertia ixx="0.0015896250000000001" ixy="0.0" ixz="0.0" iyy="0.06875325000000002" iyz="0.0" izz="0.06789262500000001"/></inertial><visual><origin rpy="0 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/bridge_latch.stl" scale="0.001 0.001 0.001"/></geometry><material name="blue"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><box size="2.45 0.18 0.165"/></geometry></collision></link><joint name="bridge_latch_joint" type="fixed"><parent link="camera_stand_link"/><child link="bridge_latch_link"/><origin rpy="0 0 0" xyz="0.0 0.0 0.0"/></joint><link name="bridge_holder_link"><inertial><origin rpy="0 0 0" xyz="0 0 -0.015"/><mass value="0.135"/><inertia ixx="0.0015896250000000001" ixy="0.0" ixz="0.0" iyy="0.0014301562500000002" iyz="0.0" izz="0.00056953125"/></inertial><visual><origin rpy="0 0 0" xyz="0 -0.1 0"/><geometry><mesh filename="package://grr_description/meshes/bridge_holder.stl" scale="0.001 0.001 0.001"/></geometry><material name="yellow"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><box size="0.135 0.18 0.165"/></geometry></collision></link><joint name="bridge_holder_joint" type="fixed"><parent link="chassis_link"/><child link="bridge_holder_link"/><origin rpy="0 0 0" xyz="0.0 -0.04 0.03"/></joint><link name="color_sensor_link"><inertial><origin rpy="0 0 0" xyz="0 0 -0.015"/><mass value="2.45"/><inertia ixx="0.025954687500000004" ixy="0.0" ixz="0.0" iyy="0.0103359375" iyz="0.0" izz="0.028848750000000003"/></inertial><visual><origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/><geometry><mesh filename="package://grr_description/meshes/color_sensor.stl" scale="0.001 0.001 0.001"/></geometry><material name="red"/></visual><collision><origin rpy="0 0 0" xyz="0 0 0"/><geometry><box size="0.18 0.33 0.0675"/></geometry></collision></link><joint name="color_sensor_joint" type="fixed"><parent link="bridge_holder_link"/><child link="color_sensor_link"/><origin rpy="0 0 0" xyz="0.0 -0.11 0.04"/></joint><!-- <xacro:lidar
+    radius="${lidar_radius}"
+    length="${lidar_length}"
+    pos_y="${lidar_joint_pos_y}"
+  /> --><!-- <xacro:zed_camera parent="chassis_link">
+    <origin xyz="0 0.15 0" rpy="0 0 ${pi/2}" />
+  </xacro:zed_camera> --></robot>
\ No newline at end of file
diff --git a/src/grr_guis/grr_guis/main_gui.py b/src/grr_guis/grr_guis/main_gui.py
index 705b4cb..be37d33 100644
--- a/src/grr_guis/grr_guis/main_gui.py
+++ b/src/grr_guis/grr_guis/main_gui.py
@@ -61,7 +61,6 @@ def __init__(self) -> None:
         self.main_pane.addWidget(self.state_machine_button)
         self.main_pane.addWidget(self.start_button)
         
-        
         # self.main_pane.addWidget(self.videoWidget)
         horiz_layout.addLayout(self.main_pane)
         horiz_layout.addLayout(self.middle_pane)
diff --git a/src/grr_hardware_python/grr_hardware_python/robot.py b/src/grr_hardware_python/grr_hardware_python/robot.py
index 27be835..20a3e2e 100644
--- a/src/grr_hardware_python/grr_hardware_python/robot.py
+++ b/src/grr_hardware_python/grr_hardware_python/robot.py
@@ -82,7 +82,6 @@ def __init__(self) -> None:
         # self.left_tof = VL53L4CD(self.tca[3])
         self.down_tof_pub = self.create_publisher(LaserScan, "/grr/down_tof", 10)
         self.left_tof_pub = self.create_publisher(LaserScan, "/grr/left_tof", 10)
-        
         # self.left_tof.start_ranging()
                 
         self.line_follower = Line_Follower(self.i2c)
diff --git a/src/grr_python_controllers/grr_python_controllers/state_machine.py b/src/grr_python_controllers/grr_python_controllers/state_machine.py
index 9f83fce..8f0e196 100644
--- a/src/grr_python_controllers/grr_python_controllers/state_machine.py
+++ b/src/grr_python_controllers/grr_python_controllers/state_machine.py
@@ -28,12 +28,8 @@ def __init__(self):
         super().__init__('State_Machine')
         
         self.promo_display = False
-        self.relay = False
         
         def set_true(): self.promo_display=True
-        def relay_func_on():self.relay = True
-        def relay_func_off():self.relay = False
-        
         
         # self.action_tree = ServoAction(JointState(name=['small_package_sweeper_joint', 'bridge_latch_joint', 'mechansim_thruster_joint', 'mechanism_lift_joint'], position=[0.0, 100.0, 0.0, 100.0]), 1, name="starting positions")
         self.action_tree = ServoAction(JointState(name=['small_package_sweeper_joint', 'bridge_latch_joint', 'mechanism_package_joint', 'mechanism_thruster_joint', 'mechanism_lift_joint'], position=[0.0, 100.0, 50.0, 0.0, 100.0]), 1, name="starting positions")