Urdf

__pycache__/
*.py[cod]

.pytest_cache/
.mypy_cache/
.ruff_cache/

.venv/
venv/

dist/
build/
*.egg-info/

.DS_Store

interface:
  display_name: "URDF"
  short_description: "Generate and validate explicit robot URDF outputs."
  default_prompt: "Use $urdf to generate, update, and validate explicit URDF files from Python sources, including robot links, joints, limits, and mesh references without assuming a harness root."

yourdfpy

from __future__ import annotations

import sys
from pathlib import Path

if __package__ in {None, ""}:
    tool_dir = Path(__file__).resolve().parent
    script_root = tool_dir.parent
    sys.path = [path for path in sys.path if Path(path or ".").resolve() != tool_dir]
    sys.path.insert(0, str(script_root))
    from gen_urdf.cli import main
else:
    from .cli import main


if __name__ == "__main__":
    raise SystemExit(main())

from __future__ import annotations

import argparse
import importlib.util
import inspect
import sys
from collections.abc import Sequence
from pathlib import PurePosixPath
from pathlib import Path

from urdf_source import UrdfSourceError, read_urdf_source


def generate_urdf_targets(targets: Sequence[str], *, summary: bool = False) -> int:
    generated_paths = [_generate_target(target) for target in targets]
    if summary:
        _print_summaries(generated_paths)
    return 0


def main(argv: Sequence[str] | None = None) -> int:
    parser = argparse.ArgumentParser(
        prog="gen_urdf",
        description="Generate explicit URDF targets from envelope-returning Python sources.",
    )
    parser.add_argument(
        "targets",
        nargs="+",
        help="Explicit Python source file defining gen_urdf() to generate.",
    )
    parser.add_argument(
        "--summary",
        action="store_true",
        help="Print a compact summary for generated outputs.",
    )
    args = parser.parse_args(list(argv) if argv is not None else None)
    return generate_urdf_targets(args.targets, summary=args.summary)


def _generate_target(target: str) -> Path:
    script_path = Path(target).resolve()
    if script_path.suffix.lower() != ".py":
        raise ValueError(f"{_display_path(script_path)} must be a Python source file")
    if not script_path.is_file():
        raise FileNotFoundError(f"Python source not found: {_display_path(script_path)}")

    module = _load_generator_module(script_path)
    generator = getattr(module, "gen_urdf", None)
    if not callable(generator):
        raise RuntimeError(f"{_display_path(script_path)} does not define callable gen_urdf()")
    if inspect.signature(generator).parameters:
        raise ValueError(f"{_display_path(script_path)} gen_urdf() must not accept arguments")

    envelope = generator()
    if not isinstance(envelope, dict):
        raise TypeError(f"{_display_path(script_path)} gen_urdf() must return a generator envelope dict")

    output_path = _resolve_urdf_output(envelope.get("urdf_output"), script_path=script_path)
    _write_urdf_payload(envelope, output_path=output_path, script_path=script_path)
    if not output_path.exists():
        raise RuntimeError(f"{_display_path(script_path)} did not write {_display_path(output_path)}")
    return output_path


def _load_generator_module(script_path: Path) -> object:
    module_name = (
        "_urdf_tool_"
        + _display_path(script_path).replace("/", "_").replace("\\", "_").replace("-", "_").replace(".", "_")
    )
    module_spec = importlib.util.spec_from_file_location(module_name, script_path)
    if module_spec is None or module_spec.loader is None:
        raise RuntimeError(f"Failed to load generator module from {_display_path(script_path)}")

    module = importlib.util.module_from_spec(module_spec)
    original_sys_path = list(sys.path)
    search_paths = [
        str(Path.cwd().resolve()),
        str(script_path.parent),
    ]
    for candidate in reversed(search_paths):
        if candidate not in sys.path:
            sys.path.insert(0, candidate)

    try:
        sys.modules[module_name] = module
        module_spec.loader.exec_module(module)
    finally:
        sys.path[:] = original_sys_path

    return module


def _resolve_urdf_output(raw_value: object, *, script_path: Path) -> Path:
    if not isinstance(raw_value, str) or not raw_value.strip():
        raise ValueError(f"{_display_path(script_path)} gen_urdf() envelope urdf_output must be a non-empty string")
    value = raw_value.strip()
    if "\\" in value:
        raise ValueError(f"{_display_path(script_path)} gen_urdf() envelope urdf_output must use POSIX '/' separators")
    pure = PurePosixPath(value)
    if pure.is_absolute() or any(part in {"", "."} for part in pure.parts):
        raise ValueError(f"{_display_path(script_path)} gen_urdf() envelope urdf_output must be relative")
    output_path = (script_path.parent / Path(*pure.parts)).resolve()
    if output_path.suffix.lower() != ".urdf":
        raise ValueError(f"{_display_path(script_path)} gen_urdf() envelope urdf_output must end in .urdf")
    return output_path


def _write_urdf_payload(envelope: dict[str, object], *, output_path: Path, script_path: Path) -> None:
    xml = envelope.get("xml")
    if not isinstance(xml, str):
        raise TypeError(
            f"{_display_path(script_path)} gen_urdf() envelope field 'xml' must be a string, "
            f"got {type(xml).__name__}"
        )
    output_path.parent.mkdir(parents=True, exist_ok=True)
    text = xml if xml.endswith("\n") else xml + "\n"
    output_path.write_text(text, encoding="utf-8")
    print(f"Wrote URDF: {output_path}")


def _print_summaries(paths: Sequence[Path]) -> None:
    for path in paths:
        try:
            urdf_source = read_urdf_source(path)
        except (UrdfSourceError, ValueError) as exc:
            print(f"summary {_display_path(path)}: unavailable ({exc})")
            continue
        print(
            f"{_display_path(path)}: robot={urdf_source.robot_name} "
            f"links={len(urdf_source.links)} joints={len(urdf_source.joints)}"
        )


def _display_path(path: Path) -> str:
    resolved = path.resolve()
    try:
        return resolved.relative_to(Path.cwd().resolve()).as_posix()
    except ValueError:
        return resolved.as_posix()

import tempfile
import unittest
from pathlib import Path
from unittest import mock

from gen_urdf import cli


class GenUrdfCliTests(unittest.TestCase):
    def test_requires_explicit_target(self) -> None:
        with self.assertRaises(SystemExit) as cm:
            cli.main([])
        self.assertEqual(2, cm.exception.code)

    def test_rejects_root_option(self) -> None:
        with self.assertRaises(SystemExit) as cm:
            cli.main(["--root", "models/samples"])
        self.assertEqual(2, cm.exception.code)

    def test_passes_targets_in_order(self) -> None:
        with mock.patch.object(cli, "generate_urdf_targets", return_value=0) as generate:
            self.assertEqual(0, cli.main(["sample_robot.py", "other.py", "--summary"]))

        generate.assert_called_once_with(["sample_robot.py", "other.py"], summary=True)

    def test_generates_urdf_without_cad_skill_imports(self) -> None:
        with tempfile.TemporaryDirectory(prefix="tmp-gen-urdf-") as tempdir:
            source_path = Path(tempdir) / "sample_robot.py"
            source_path.write_text(
                "\n".join(
                    [
                        "def gen_urdf():",
                        "    return {",
                        "        'xml': '<robot name=\"sample\"><link name=\"base_link\" /></robot>',",
                        "        'urdf_output': 'sample_robot.urdf',",
                        "    }",
                        "",
                    ]
                ),
                encoding="utf-8",
            )

            self.assertEqual(0, cli.generate_urdf_targets([str(source_path)]))

            self.assertEqual(
                '<robot name="sample"><link name="base_link" /></robot>\n',
                (Path(tempdir) / "sample_robot.urdf").read_text(encoding="utf-8"),
            )


if __name__ == "__main__":
    unittest.main()

import tempfile
import unittest
from pathlib import Path
from unittest.mock import Mock, patch

from urdf_source import UrdfSourceError, read_urdf_source


class UrdfSourceTests(unittest.TestCase):
    def setUp(self) -> None:
        self._tempdir = tempfile.TemporaryDirectory(prefix="tmp-urdf-source-")
        self.temp_root = Path(self._tempdir.name)

    def tearDown(self) -> None:
        self._tempdir.cleanup()

    def _cad_ref(self, name: str) -> str:
        return (self.temp_root / f"{name}.urdf").resolve().as_posix()

    def _write_mesh(self, name: str) -> Path:
        mesh_path = self.temp_root / f"{name}.stl"
        mesh_path.write_text("solid empty\nendsolid empty\n", encoding="utf-8")
        return mesh_path

    def _write_urdf(self, name: str, body: str) -> Path:
        urdf_path = self.temp_root / f"{name}.urdf"
        urdf_path.write_text(body.strip() + "\n", encoding="utf-8")
        script_path = self.temp_root / f"{name}.py"
        if not script_path.exists():
            script_path.write_text(
                "\n".join(
                    [
                        "def gen_step():",
                        f"    return {{'instances': [], 'step_output': {f'{name}.step'!r}}}",
                        "",
                        "def gen_urdf():",
                        f"    return {{'xml': '', 'urdf_output': {f'{name}.urdf'!r}}}",
                        "",
                    ]
                ),
                encoding="utf-8",
            )
        return urdf_path

    def test_read_urdf_source_accepts_valid_mesh_robot(self) -> None:
        mesh_path = self._write_mesh("base")
        source_path = self._write_urdf(
            "robot",
            f"""
            <robot name="sample-robot">
              <link name="base_link">
                <visual>
                  <geometry>
                    <mesh filename="{mesh_path.name}" scale="0.001 0.001 0.001" />
                  </geometry>
                </visual>
              </link>
            </robot>
            """,
        )

        source = read_urdf_source(source_path)

        self.assertEqual(self._cad_ref("robot"), source.file_ref)
        self.assertEqual("sample-robot", source.robot_name)
        self.assertEqual("base_link", source.root_link)
        self.assertEqual(("base_link",), source.links)
        self.assertEqual(0, len(source.joints))
        self.assertEqual((mesh_path.resolve(),), source.mesh_paths)

    def test_read_urdf_source_validates_with_yourdfpy_without_loading_meshes(self) -> None:
        source_path = self._write_urdf(
            "robot",
            """
            <robot name="sample-robot">
              <link name="base_link" />
            </robot>
            """,
        )
        urdf = Mock()
        urdf.validate.return_value = True
        urdf.errors = []

        with patch("yourdfpy.URDF.load", return_value=urdf) as load:
            read_urdf_source(source_path)

        load.assert_called_once()
        self.assertEqual(str(source_path.resolve()), load.call_args.args[0])
        self.assertFalse(load.call_args.kwargs["build_scene_graph"])
        self.assertFalse(load.call_args.kwargs["build_collision_scene_graph"])
        self.assertFalse(load.call_args.kwargs["load_meshes"])
        self.assertFalse(load.call_args.kwargs["load_collision_meshes"])
        urdf.validate.assert_called_once_with()

    def test_read_urdf_source_reports_yourdfpy_validation_errors(self) -> None:
        source_path = self._write_urdf(
            "robot",
            """
            <robot name="sample-robot">
              <link name="base_link" />
            </robot>
            """,
        )
        urdf = Mock()
        urdf.validate.return_value = False
        urdf.errors = ["bad joint"]

        with patch("yourdfpy.URDF.load", return_value=urdf):
            with self.assertRaisesRegex(UrdfSourceError, "bad joint"):
                read_urdf_source(source_path)

    def test_read_urdf_source_rejects_duplicate_links(self) -> None:
        mesh_path = self._write_mesh("base")
        source_path = self._write_urdf(
            "robot",
            f"""
            <robot name="sample-robot">
              <link name="base_link">
                <visual>
                  <geometry>
                    <mesh filename="{mesh_path.name}" />
                  </geometry>
                </visual>
              </link>
              <link name="base_link" />
            </robot>
            """,
        )

        with self.assertRaisesRegex(UrdfSourceError, "duplicates"):
            read_urdf_source(source_path)

    def test_read_urdf_source_rejects_missing_mesh(self) -> None:
        source_path = self._write_urdf(
            "robot",
            """
            <robot name="sample-robot">
              <link name="base_link">
                <visual>
                  <geometry>
                    <mesh filename="does-not-exist.stl" />
                  </geometry>
                </visual>
              </link>
            </robot>
            """,
        )

        with self.assertRaisesRegex(UrdfSourceError, "missing mesh file"):
            read_urdf_source(source_path)

    def test_read_urdf_source_rejects_unsupported_joint_type(self) -> None:
        mesh_path = self._write_mesh("base")
        source_path = self._write_urdf(
            "robot",
            f"""
            <robot name="sample-robot">
              <link name="base_link">
                <visual>
                  <geometry>
                    <mesh filename="{mesh_path.name}" />
                  </geometry>
                </visual>
              </link>
              <link name="arm_link" />
              <joint name="base_to_arm" type="prismatic">
                <parent link="base_link" />
                <child link="arm_link" />
              </joint>
            </robot>
            """,
        )

        with self.assertRaisesRegex(UrdfSourceError, "unsupported type"):
            read_urdf_source(source_path)

    def test_file_ref_ignores_neighbor_step_toml(self) -> None:
        source_path = self._write_urdf(
            "robot",
            """
            <robot name="sample-robot">
              <link name="base_link" />
            </robot>
            """,
        )
        stale_path = self.temp_root / "robot.step.toml"
        stale_path.write_text(
            "\n".join(
                [
                    'kind = "part"',
                    'source = "robot.urdf"',
                ]
            )
            + "\n",
            encoding="utf-8",
        )

        source = read_urdf_source(source_path)

        self.assertEqual(source_path.resolve().as_posix(), source.file_ref)


if __name__ == "__main__":
    unittest.main()

from __future__ import annotations

from dataclasses import dataclass
from math import degrees
from pathlib import Path
import xml.etree.ElementTree as ET

URDF_SUFFIX = ".urdf"
SUPPORTED_JOINT_TYPES = {"fixed", "continuous", "revolute"}
SUPPORTED_MESH_SUFFIXES = {".stl"}


class UrdfSourceError(ValueError):
    pass


@dataclass(frozen=True)
class UrdfJoint:
    name: str
    joint_type: str
    parent_link: str
    child_link: str
    min_value_deg: float | None
    max_value_deg: float | None


@dataclass(frozen=True)
class UrdfSource:
    file_ref: str
    source_path: Path
    robot_name: str
    root_link: str
    links: tuple[str, ...]
    joints: tuple[UrdfJoint, ...]
    mesh_paths: tuple[Path, ...]


def file_ref_from_urdf_path(urdf_path: Path) -> str:
    resolved = urdf_path.resolve()
    if resolved.suffix.lower() != URDF_SUFFIX:
        raise UrdfSourceError(f"{resolved} is not a URDF source file")
    return _relative_to_repo(resolved)


def read_urdf_source(urdf_path: Path) -> UrdfSource:
    resolved_path = urdf_path.resolve()
    if resolved_path.suffix.lower() != URDF_SUFFIX:
        raise UrdfSourceError(f"{resolved_path} is not a URDF source file")

    try:
        root = ET.fromstring(resolved_path.read_text(encoding="utf-8"))
    except (OSError, ET.ParseError) as exc:
        raise UrdfSourceError(f"{_relative_to_repo(resolved_path)} could not be parsed as URDF XML") from exc

    if root.tag != "robot":
        raise UrdfSourceError(f"{_relative_to_repo(resolved_path)} root element must be <robot>")
    robot_name = str(root.attrib.get("name") or "").strip()
    if not robot_name:
        raise UrdfSourceError(f"{_relative_to_repo(resolved_path)} robot name is required")

    link_names = []
    for link_element in root.findall("link"):
        name = str(link_element.attrib.get("name") or "").strip()
        if not name:
            raise UrdfSourceError(f"{_relative_to_repo(resolved_path)} link name is required")
        link_names.append(name)
    if not link_names:
        raise UrdfSourceError(f"{_relative_to_repo(resolved_path)} must define at least one link")
    _raise_on_duplicates(link_names, source_path=resolved_path, label="link")
    link_name_set = set(link_names)

    mesh_paths: list[Path] = []
    for link_element in root.findall("link"):
        for visual_element in link_element.findall("visual"):
            geometry_element = visual_element.find("geometry")
            if geometry_element is None:
                continue
            mesh_element = geometry_element.find("mesh")
            if mesh_element is None:
                raise UrdfSourceError(
                    f"{_relative_to_repo(resolved_path)} only mesh visual geometry is supported"
                )
            filename = str(mesh_element.attrib.get("filename") or "").strip()
            if not filename:
                raise UrdfSourceError(f"{_relative_to_repo(resolved_path)} mesh filename is required")
            mesh_path = _resolve_mesh_path(filename, source_path=resolved_path)
            if mesh_path.suffix.lower() not in SUPPORTED_MESH_SUFFIXES:
                raise UrdfSourceError(
                    f"{_relative_to_repo(resolved_path)} only STL mesh geometry is supported: {filename!r}"
                )
            if not mesh_path.is_file():
                raise UrdfSourceError(
                    f"{_relative_to_repo(resolved_path)} references missing mesh file: {filename!r}"
                )
            mesh_paths.append(mesh_path)

    joints = []
    joint_names = []
    parent_by_child: dict[str, str] = {}
    children = set()
    joints_by_parent: dict[str, list[str]] = {}
    for joint_element in root.findall("joint"):
        name = str(joint_element.attrib.get("name") or "").strip()
        if not name:
            raise UrdfSourceError(f"{_relative_to_repo(resolved_path)} joint name is required")
        joint_names.append(name)
        joint_type = str(joint_element.attrib.get("type") or "").strip().lower()
        if joint_type not in SUPPORTED_JOINT_TYPES:
            raise UrdfSourceError(
                f"{_relative_to_repo(resolved_path)} joint {name!r} uses unsupported type {joint_type!r}"
            )
        parent_element = joint_element.find("parent")
        child_element = joint_element.find("child")
        parent_link = str(parent_element.attrib.get("link") if parent_element is not None else "").strip()
        child_link = str(child_element.attrib.get("link") if child_element is not None else "").strip()
        if not parent_link or not child_link:
            raise UrdfSourceError(
                f"{_relative_to_repo(resolved_path)} joint {name!r} must define parent and child links"
            )
        if parent_link not in link_name_set:
            raise UrdfSourceError(
                f"{_relative_to_repo(resolved_path)} joint {name!r} references missing parent link {parent_link!r}"
            )
        if child_link not in link_name_set:
            raise UrdfSourceError(
                f"{_relative_to_repo(resolved_path)} joint {name!r} references missing child link {child_link!r}"
            )
        if child_link in parent_by_child:
            raise UrdfSourceError(
                f"{_relative_to_repo(resolved_path)} link {child_link!r} has multiple parents"
            )
        parent_by_child[child_link] = parent_link
        children.add(child_link)
        joints_by_parent.setdefault(parent_link, []).append(child_link)
        min_value_deg, max_value_deg = _joint_limits_deg(joint_element, joint_type=joint_type, source_path=resolved_path)
        joints.append(
            UrdfJoint(
                name=name,
                joint_type=joint_type,
                parent_link=parent_link,
                child_link=child_link,
                min_value_deg=min_value_deg,
                max_value_deg=max_value_deg,
            )
        )

    _raise_on_duplicates(joint_names, source_path=resolved_path, label="joint")
    root_candidates = [link_name for link_name in link_names if link_name not in children]
    if len(root_candidates) != 1:
        raise UrdfSourceError(
            f"{_relative_to_repo(resolved_path)} must form a single rooted tree; found roots {root_candidates!r}"
        )
    root_link = root_candidates[0]

    visited: set[str] = set()
    visiting: set[str] = set()

    def visit(link_name: str) -> None:
        if link_name in visited:
            return
        if link_name in visiting:
            raise UrdfSourceError(f"{_relative_to_repo(resolved_path)} joint graph contains a cycle")
        visiting.add(link_name)
        for child_link in joints_by_parent.get(link_name, ()):
            visit(child_link)
        visiting.remove(link_name)
        visited.add(link_name)

    visit(root_link)
    if visited != link_name_set:
        missing_links = sorted(link_name_set - visited)
        raise UrdfSourceError(
            f"{_relative_to_repo(resolved_path)} leaves links disconnected from the root: {missing_links!r}"
        )
    if len(joints) != len(link_names) - 1:
        raise UrdfSourceError(
            f"{_relative_to_repo(resolved_path)} must form a tree with exactly links-1 joints"
        )

    _validate_with_yourdfpy(resolved_path)

    return UrdfSource(
        file_ref=file_ref_from_urdf_path(resolved_path),
        source_path=resolved_path,
        robot_name=robot_name,
        root_link=root_link,
        links=tuple(link_names),
        joints=tuple(joints),
        mesh_paths=tuple(mesh_paths),
    )


def _validate_with_yourdfpy(source_path: Path) -> None:
    try:
        from yourdfpy import URDF
    except ModuleNotFoundError as exc:
        raise UrdfSourceError(
            f"{_relative_to_repo(source_path)} requires yourdfpy for URDF validation; "
            "install it in the CAD Python environment"
        ) from exc

    try:
        urdf = URDF.load(
            str(source_path),
            build_scene_graph=False,
            build_collision_scene_graph=False,
            load_meshes=False,
            load_collision_meshes=False,
        )
        is_valid = urdf.validate()
    except Exception as exc:
        raise UrdfSourceError(f"{_relative_to_repo(source_path)} failed yourdfpy validation: {exc}") from exc

    if not is_valid:
        errors = "; ".join(str(error) for error in urdf.errors) or "unknown error"
        raise UrdfSourceError(
            f"{_relative_to_repo(source_path)} failed yourdfpy validation: {errors}"
        )


def _joint_limits_deg(
    joint_element: ET.Element,
    *,
    joint_type: str,
    source_path: Path,
) -> tuple[float | None, float | None]:
    if joint_type == "fixed":
        return 0.0, 0.0
    if joint_type == "continuous":
        return -180.0, 180.0
    limit_element = joint_element.find("limit")
    if limit_element is None:
        raise UrdfSourceError(
            f"{_relative_to_repo(source_path)} revolute joint {joint_element.attrib.get('name', '')!r} requires <limit>"
        )
    try:
        lower = float(limit_element.attrib["lower"])
        upper = float(limit_element.attrib["upper"])
    except KeyError as exc:
        raise UrdfSourceError(
            f"{_relative_to_repo(source_path)} revolute joint {joint_element.attrib.get('name', '')!r} requires lower and upper limits"
        ) from exc
    except ValueError as exc:
        raise UrdfSourceError(
            f"{_relative_to_repo(source_path)} revolute joint {joint_element.attrib.get('name', '')!r} has invalid limits"
        ) from exc
    return degrees(lower), degrees(upper)


def _resolve_mesh_path(filename: str, *, source_path: Path) -> Path:
    if filename.startswith("package://"):
        relative = filename.removeprefix("package://").lstrip("/")
        return (Path.cwd() / relative).resolve()
    return (source_path.parent / filename).resolve()


def _raise_on_duplicates(values: list[str], *, source_path: Path, label: str) -> None:
    seen: set[str] = set()
    duplicates: set[str] = set()
    for value in values:
        if value in seen:
            duplicates.add(value)
            continue
        seen.add(value)
    if duplicates:
        duplicate_text = ", ".join(repr(item) for item in sorted(duplicates))
        raise UrdfSourceError(
            f"{_relative_to_repo(source_path)} {label} names contain duplicates {duplicate_text}"
        )


def _relative_to_repo(path: Path) -> str:
    try:
        return path.resolve().relative_to(Path.cwd().resolve()).as_posix()
    except ValueError:
        return path.resolve().as_posix()