diff --git a/src/roboticstoolbox/models/URDF/Fetch.py b/src/roboticstoolbox/models/URDF/Fetch.py index 9a611355..bfacb126 100644 --- a/src/roboticstoolbox/models/URDF/Fetch.py +++ b/src/roboticstoolbox/models/URDF/Fetch.py @@ -1,11 +1,44 @@ #!/usr/bin/env python +import re + import numpy as np from roboticstoolbox.models.URDF.URDFRobot import URDFRobot # from spatialmath import SE3 +def _patch_fetch_urdf(text: str) -> str: + """Work around a broken upstream file served by robot_descriptions. + + Applies to: robot_descriptions v2.0.0's ``fetch_description``, which + clones ``openai/roboschool`` at commit ``c8ee2812`` and exposes + ``roboschool/models_robot/fetch_description/robots/fetch.urdf``. + + That file is not well-formed XML: it has a single + ``...`` block (inside a trailing + ```` element) using the + ``sensor:`` namespace prefix, but no ``xmlns:sensor`` is ever declared + anywhere in the document — old pre-SDF Gazebo camera-sensor syntax + (circa ROS Fuerte/Groovy) that was never valid XML to begin with. + ``roboschool`` is an archived (read-only) GitHub repo, so this can't be + fixed upstream. The block is pure Gazebo simulation config (image + format/size/FOV/clip planes for a camera plugin) with no bearing on + kinematics, dynamics, or geometry, so it's safe to drop entirely. + + If a future robot_descriptions update points at a fixed fork or file, + this patch becomes a no-op (the regex simply won't match) — safe to + remove once confirmed unnecessary. + """ + return re.sub( + r"\s*\s*" + r"\s*\n?", + "\n", + text, + flags=re.DOTALL, + ) + + class Fetch(URDFRobot): """ Class that imports a Fetch URDF model @@ -34,6 +67,7 @@ def __init__(self): "fetch", manufacturer="Fetch", gripper_link_index=11, + patch=_patch_fetch_urdf, ) self.qdlim = np.array([4.0, 4.0, 0.1, 1.25, 1.45, 1.57, 1.52, 1.57, 2.26, 2.26]) diff --git a/src/roboticstoolbox/models/URDF/URDFRobot.py b/src/roboticstoolbox/models/URDF/URDFRobot.py index 565c0550..ad035980 100644 --- a/src/roboticstoolbox/models/URDF/URDFRobot.py +++ b/src/roboticstoolbox/models/URDF/URDFRobot.py @@ -12,7 +12,7 @@ from pathlib import Path import importlib import warnings -from typing import TextIO +from typing import Callable, TextIO import numpy as np from spatialmath import SE3 @@ -258,13 +258,26 @@ def _parse_urdf(urdf_str: str): return elinks, urdf.name -def URDF_file(file: "str | Path | TextIO", model: "str | None" = None) -> tuple: +def URDF_file( + file: "str | Path | TextIO", + model: "str | None" = None, + patch: "Callable[[str], str] | None" = None, +) -> tuple: """Parse a URDF or xacro file, return (elinks, name). ``file`` may be: - an absolute or relative path (str or Path) to a .urdf or .xacro file - a bare name with no suffix, looked up via robot_descriptions - a file-like object whose .read() gives the URDF XML + + ``patch``, if given, is called with the raw file text and must return + the text to actually process. It runs *before* xacro processing/XML + parsing, so it can surgically correct known-broken third-party source + files (e.g. an upstream file with an unexpanded xacro macro that has no + definition anywhere in its own repo, or malformed XML) without touching + the upstream repo or the bundled `rtb-data` copy. See ``Valkyrie.py`` + and ``Fetch.py`` for real examples — each documents exactly which + upstream bug its patch works around. """ import rtbdata @@ -282,21 +295,32 @@ def URDF_file(file: "str | Path | TextIO", model: "str | None" = None) -> tuple: if not file.is_absolute(): file = xacro_root / file resolved_path = file - doc = XacroDoc.from_file(file) + if patch is not None: + # mirrors XacroDoc.from_file()'s own package-discovery step, + # since we bypass from_file() here to patch the text first + packages.walk_up_from(file) + doc = XacroDoc.from_string(patch(file.read_text()), rootdir=file.parent) + else: + doc = XacroDoc.from_file(file) else: - doc = XacroDoc.from_string(file.read()) + text = file.read() + if patch is not None: + text = patch(text) + doc = XacroDoc.from_string(text) elinks, name = _parse_urdf(doc.to_urdf_string()) return elinks, name, resolved_path -def URDF_read(urdf_path: "str | Path") -> tuple: +def URDF_read( + urdf_path: "str | Path", patch: "Callable[[str], str] | None" = None +) -> tuple: """Load a URDF/xacro file, return (elinks, name, filepath). ``filepath`` is the resolved filesystem path that was loaded, or None if - the source was a file-like object. + the source was a file-like object. See ``URDF_file`` for ``patch``. """ - return URDF_file(urdf_path) + return URDF_file(urdf_path, patch=patch) class URDFRobot(Robot): @@ -320,9 +344,10 @@ def __init__( urdf_path: "str | Path", manufacturer: str = "", gripper_link_index: "int | None" = None, + patch: "Callable[[str], str] | None" = None, **kwargs, ): - elinks, name, filepath = URDF_file(urdf_path) + elinks, name, filepath = URDF_file(urdf_path, patch=patch) if gripper_link_index is not None: kwargs["gripper_links"] = elinks[gripper_link_index] super().__init__(elinks, name=name, manufacturer=manufacturer, **kwargs) diff --git a/src/roboticstoolbox/models/URDF/Valkyrie.py b/src/roboticstoolbox/models/URDF/Valkyrie.py index 67d7077b..816fc869 100644 --- a/src/roboticstoolbox/models/URDF/Valkyrie.py +++ b/src/roboticstoolbox/models/URDF/Valkyrie.py @@ -1,11 +1,38 @@ #!/usr/bin/env python +import re + import numpy as np from roboticstoolbox.robot.Link import Link from roboticstoolbox.robot.Robot import Robot from roboticstoolbox.models.URDF.URDFRobot import URDF_read +def _patch_valkyrie_urdf(text: str) -> str: + """Work around a broken upstream file served by robot_descriptions. + + Applies to: robot_descriptions v2.0.0's ``valkyrie_description``, which + clones ``gkjohnson/nasa-urdf-robots`` at commit ``54cdeb1d`` and exposes + ``val_description/model/robots/valkyrie_sim.urdf`` as a (supposedly) + plain URDF file. + + In reality that file still contains one live, unexpanded xacro macro + call: ````. There is no definition of + this macro anywhere in the entire cloned repo (zero ``.xacro`` files + exist in it at all) — it's a gap in the upstream repo itself, not + something robot_descriptions or this loader got wrong. The tag is a + standalone, self-closing element configuring a simulated pelvis IMU + sensor plugin (Gazebo-only, e.g. ``middle_sensor_api_tag``); it isn't + nested inside any ````/````, so it has no bearing on + kinematics, dynamics, or geometry and is safe to drop entirely. + + If a future robot_descriptions/nasa-urdf-robots update actually defines + this macro (or pre-expands it), this patch becomes a no-op (the regex + simply won't match) — safe to remove once confirmed unnecessary. + """ + return re.sub(r"\s*]*/>\n?", "\n", text) + + class Valkyrie(Robot): """ Class that imports a NASA Valkyrie URDF model @@ -36,7 +63,7 @@ def __init__(self, variant="A"): if not variant in "ABCD": raise ValueError("variant must be in the range A-D") - links, name, urdf_filepath = URDF_read("valkyrie") + links, name, urdf_filepath = URDF_read("valkyrie", patch=_patch_valkyrie_urdf) # We wish to add an intermediate link between gripper_r_base and # @gripper_r_finger_r/l