diff --git a/open_phantom/utils/handle_urdf.py b/open_phantom/utils/handle_urdf.py
index f0d2093..31c34db 100644
--- a/open_phantom/utils/handle_urdf.py
+++ b/open_phantom/utils/handle_urdf.py
@@ -1,4 +1,5 @@
 import os
+import re
 import xml.dom.minidom as minidom
 import xml.etree.ElementTree as ET
 
@@ -11,23 +12,25 @@ If the mesh file is not found, the original path is used.
 """
 
 
-def fix_path(path: str, urdf_dir: str) -> str:
+def fix_path(path: str, robot_base_dir: str) -> str:
     if path.startswith("package://"):
         parts = path[len("package://") :].split("/", 1)
         if len(parts) == 2:
             package_name, rel_path = parts
 
-            # Try potential locations for the mesh
+            # Try potential locations for the mesh with relative paths
             potential_paths = [
-                os.path.join(urdf_dir, rel_path),
-                os.path.join(urdf_dir, "../meshes", rel_path),
-                os.path.join(urdf_dir, f"../{package_name}", rel_path),
-                os.path.join(urdf_dir, "../..", rel_path),
+                os.path.join("meshes", rel_path),
+                os.path.join(package_name, rel_path),
+                rel_path,
             ]
 
-            for possible_path in potential_paths:
-                if os.path.exists(possible_path):
-                    return possible_path
+            for possible_rel_path in potential_paths:
+                # Check if file exists with this relative path
+                full_path = os.path.join(robot_base_dir, possible_rel_path)
+                if os.path.exists(full_path):
+                    # Return the relative path from urdf directory
+                    return os.path.join("..", possible_rel_path)
 
         print(f"Failed to find mesh for package path: {path}")
 
@@ -76,16 +79,13 @@ def format_xml(root: ET.Element, fixed_path: str) -> None:
 
 
 def handle_urdf(urdf_path: str) -> str:
-    # Check if URDF is valid
-    if not os.path.exists(urdf_path):
-        print(f"Invalid URDF path: {urdf_path}")
-        return None
-
-    # TODO: Add check to see if URDF needs to be processed
+    assert os.path.exists(urdf_path), f"Invalid URDF path: {urdf_path}"
 
     try:
         urdf_dir = os.path.dirname(urdf_path)
-        root, fixed_path = fix_mesh_paths(urdf_path, urdf_dir)
+        robot_base_dir = os.path.dirname(urdf_dir)
+
+        root, fixed_path = fix_mesh_paths(urdf_path, robot_base_dir)
         format_xml(root, fixed_path)
         print(f"Successfully processed URDF: {fixed_path}")
         return fixed_path
@@ -99,6 +99,6 @@ if __name__ == "__main__":
     cwd = os.getcwd()
     urdf_path = os.path.join(
         cwd,
-        "open_phantom/urdf/SO_5DOF_ARM100_05d.SLDASM/urdf/SO_5DOF_ARM100_05d.SLDASM.urdf",
+        "urdf/SO_5DOF_ARM100_05d.SLDASM/urdf/SO_5DOF_ARM100_05d.SLDASM.urdf",
     )
     handle_urdf(urdf_path)
diff --git a/urdf/SO_5DOF_ARM100_05d.SLDASM/urdf/SO_5DOF_ARM100_05d.SLDASM_fixed.urdf b/urdf/SO_5DOF_ARM100_05d.SLDASM/urdf/SO_5DOF_ARM100_05d.SLDASM_fixed.urdf
index eeead45..1ce809d 100644
--- a/urdf/SO_5DOF_ARM100_05d.SLDASM/urdf/SO_5DOF_ARM100_05d.SLDASM_fixed.urdf
+++ b/urdf/SO_5DOF_ARM100_05d.SLDASM/urdf/SO_5DOF_ARM100_05d.SLDASM_fixed.urdf
@@ -9,7 +9,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Base.STL"/>
+        <mesh filename="../meshes/Base.STL"/>
       </geometry>
       <material name="">
         <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
@@ -18,7 +18,7 @@
     <collision>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Base.STL"/>
+        <mesh filename="../meshes/Base.STL"/>
       </geometry>
     </collision>
   </link>
@@ -31,7 +31,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Shoulder_Rotation_Pitch.STL"/>
+        <mesh filename="../meshes/Shoulder_Rotation_Pitch.STL"/>
       </geometry>
       <material name="">
         <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
@@ -40,7 +40,7 @@
     <collision>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Shoulder_Rotation_Pitch.STL"/>
+        <mesh filename="../meshes/Shoulder_Rotation_Pitch.STL"/>
       </geometry>
     </collision>
   </link>
@@ -59,7 +59,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Upper_Arm.STL"/>
+        <mesh filename="../meshes/Upper_Arm.STL"/>
       </geometry>
       <material name="">
         <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
@@ -68,7 +68,7 @@
     <collision>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Upper_Arm.STL"/>
+        <mesh filename="../meshes/Upper_Arm.STL"/>
       </geometry>
     </collision>
   </link>
@@ -87,7 +87,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Lower_Arm.STL"/>
+        <mesh filename="../meshes/Lower_Arm.STL"/>
       </geometry>
       <material name="">
         <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
@@ -96,7 +96,7 @@
     <collision>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Lower_Arm.STL"/>
+        <mesh filename="../meshes/Lower_Arm.STL"/>
       </geometry>
     </collision>
   </link>
@@ -115,7 +115,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Wrist_Pitch_Roll.STL"/>
+        <mesh filename="../meshes/Wrist_Pitch_Roll.STL"/>
       </geometry>
       <material name="">
         <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
@@ -124,7 +124,7 @@
     <collision>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Wrist_Pitch_Roll.STL"/>
+        <mesh filename="../meshes/Wrist_Pitch_Roll.STL"/>
       </geometry>
     </collision>
   </link>
@@ -143,7 +143,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Fixed_Gripper.STL"/>
+        <mesh filename="../meshes/Fixed_Gripper.STL"/>
       </geometry>
       <material name="">
         <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
@@ -152,7 +152,7 @@
     <collision>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Fixed_Gripper.STL"/>
+        <mesh filename="../meshes/Fixed_Gripper.STL"/>
       </geometry>
     </collision>
   </link>
@@ -171,7 +171,7 @@
     <visual>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Moving_Jaw.STL"/>
+        <mesh filename="../meshes/Moving_Jaw.STL"/>
       </geometry>
       <material name="">
         <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
@@ -180,7 +180,7 @@
     <collision>
       <origin xyz="0 0 0" rpy="0 0 0"/>
       <geometry>
-        <mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Moving_Jaw.STL"/>
+        <mesh filename="../meshes/Moving_Jaw.STL"/>
       </geometry>
     </collision>
   </link>