luckyrobots/open_phantom
13
0
Fork 0
mirror of https://github.com/luckyrobots/open_phantom.git synced 2025-04-03 10:32:19 +00:00
Code Issues 4 Packages Projects Releases Wiki Activity

fix urdf handler

Browse Source
This commit is contained in:
Ethan Clark 2025-03-25 09:53:58 -07:00
parent d6c61b2bb4
commit 55317d3ac2
2 changed files with 31 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
open_phantom/utils/handle_urdf.py
View File

@ -1,4 +1,5 @@
import os import os
import re
import xml.dom.minidom as minidom import xml.dom.minidom as minidom
import xml.etree.ElementTree as ET 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://"): if path.startswith("package://"):
parts = path[len("package://") :].split("/", 1) parts = path[len("package://") :].split("/", 1)
if len(parts) == 2: if len(parts) == 2:
package_name, rel_path = parts package_name, rel_path = parts
# Try potential locations for the mesh # Try potential locations for the mesh with relative paths
potential_paths = [ potential_paths = [
os.path.join(urdf_dir, rel_path), os.path.join("meshes", rel_path),
os.path.join(urdf_dir, "../meshes", rel_path), os.path.join(package_name, rel_path),
os.path.join(urdf_dir, f"../{package_name}", rel_path), rel_path,
os.path.join(urdf_dir, "../..", rel_path),
] ]
for possible_path in potential_paths: for possible_rel_path in potential_paths:
if os.path.exists(possible_path): # Check if file exists with this relative path
return possible_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}") 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: def handle_urdf(urdf_path: str) -> str:
# Check if URDF is valid assert os.path.exists(urdf_path), f"Invalid URDF path: {urdf_path}"
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
try: try:
urdf_dir = os.path.dirname(urdf_path) 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) format_xml(root, fixed_path)
print(f"Successfully processed URDF: {fixed_path}") print(f"Successfully processed URDF: {fixed_path}")
return fixed_path return fixed_path
@ -99,6 +99,6 @@ if __name__ == "__main__":
cwd = os.getcwd() cwd = os.getcwd()
urdf_path = os.path.join( urdf_path = os.path.join(
cwd, 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) handle_urdf(urdf_path)

28
urdf/SO_5DOF_ARM100_05d.SLDASM/urdf/SO_5DOF_ARM100_05d.SLDASM_fixed.urdf
View File

@ -9,7 +9,7 @@
<visual> <visual>
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Base.STL"/> <mesh filename="../meshes/Base.STL"/>
</geometry> </geometry>
<material name=""> <material name="">
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/> <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
@ -18,7 +18,7 @@
<collision> <collision>
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Base.STL"/> <mesh filename="../meshes/Base.STL"/>
</geometry> </geometry>
</collision> </collision>
</link> </link>
@ -31,7 +31,7 @@
<visual> <visual>
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Shoulder_Rotation_Pitch.STL"/> <mesh filename="../meshes/Shoulder_Rotation_Pitch.STL"/>
</geometry> </geometry>
<material name=""> <material name="">
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/> <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
@ -40,7 +40,7 @@
<collision> <collision>
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Shoulder_Rotation_Pitch.STL"/> <mesh filename="../meshes/Shoulder_Rotation_Pitch.STL"/>
</geometry> </geometry>
</collision> </collision>
</link> </link>
@ -59,7 +59,7 @@
<visual> <visual>
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Upper_Arm.STL"/> <mesh filename="../meshes/Upper_Arm.STL"/>
</geometry> </geometry>
<material name=""> <material name="">
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/> <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
@ -68,7 +68,7 @@
<collision> <collision>
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Upper_Arm.STL"/> <mesh filename="../meshes/Upper_Arm.STL"/>
</geometry> </geometry>
</collision> </collision>
</link> </link>
@ -87,7 +87,7 @@
<visual> <visual>
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Lower_Arm.STL"/> <mesh filename="../meshes/Lower_Arm.STL"/>
</geometry> </geometry>
<material name=""> <material name="">
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/> <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
@ -96,7 +96,7 @@
<collision> <collision>
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Lower_Arm.STL"/> <mesh filename="../meshes/Lower_Arm.STL"/>
</geometry> </geometry>
</collision> </collision>
</link> </link>
@ -115,7 +115,7 @@
<visual> <visual>
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Wrist_Pitch_Roll.STL"/> <mesh filename="../meshes/Wrist_Pitch_Roll.STL"/>
</geometry> </geometry>
<material name=""> <material name="">
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/> <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
@ -124,7 +124,7 @@
<collision> <collision>
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Wrist_Pitch_Roll.STL"/> <mesh filename="../meshes/Wrist_Pitch_Roll.STL"/>
</geometry> </geometry>
</collision> </collision>
</link> </link>
@ -143,7 +143,7 @@
<visual> <visual>
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Fixed_Gripper.STL"/> <mesh filename="../meshes/Fixed_Gripper.STL"/>
</geometry> </geometry>
<material name=""> <material name="">
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/> <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
@ -152,7 +152,7 @@
<collision> <collision>
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Fixed_Gripper.STL"/> <mesh filename="../meshes/Fixed_Gripper.STL"/>
</geometry> </geometry>
</collision> </collision>
</link> </link>
@ -171,7 +171,7 @@
<visual> <visual>
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Moving_Jaw.STL"/> <mesh filename="../meshes/Moving_Jaw.STL"/>
</geometry> </geometry>
<material name=""> <material name="">
<color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/> <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/>
@ -180,7 +180,7 @@
<collision> <collision>
<origin xyz="0 0 0" rpy="0 0 0"/> <origin xyz="0 0 0" rpy="0 0 0"/>
<geometry> <geometry>
<mesh filename="package://SO_5DOF_ARM100_05d.SLDASM/meshes/Moving_Jaw.STL"/> <mesh filename="../meshes/Moving_Jaw.STL"/>
</geometry> </geometry>
</collision> </collision>
</link> </link>