mirror of
https://github.com/luckyrobots/open_phantom.git
synced 2025-04-02 18:16:40 +00:00
fix urdf handler
This commit is contained in:
Ethan Clark
parent
d6c61b2bb4
commit
55317d3ac2
@ -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)
|
||||
|
||||
@ -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>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user