Urdf

interface:
  display_name: "URDF"
  short_description: "Generate and validate URDF robot descriptions."
  default_prompt: "Use $urdf to generate, update, and validate URDF robot descriptions from Python sources, handing generated URDFs to $cad-viewer when available."

MIT License

Copyright (c) 2026 earthtojake

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.

--editable ./scripts/packages/cadpy_metadata

[build-system]
requires = ["setuptools>=68"]
build-backend = "setuptools.build_meta"

[project]
name = "cadpy-metadata"
version = "0.3.6"
description = "Dependency-free metadata helpers for Python-generated CAD outputs."
requires-python = ">=3.11"
dependencies = []

[tool.setuptools.packages.find]
where = ["src"]
include = ["cadpy_metadata*"]

[tool.setuptools.package-data]
cadpy_metadata = ["py.typed"]

"""Dependency-free metadata helpers for generated CAD-adjacent outputs."""

from cadpy_metadata.generator import (
    GenerationOutput,
    PythonSourceIdentity,
    python_source_identity,
    track_generation_run,
    xml_with_text_to_cad_metadata,
)

__all__ = [
    "GenerationOutput",
    "PythonSourceIdentity",
    "python_source_identity",
    "track_generation_run",
    "xml_with_text_to_cad_metadata",
]

"""Metadata helpers for Python-generated CAD outputs."""

from __future__ import annotations

import contextlib
import hashlib
import json
import os
import re
import threading
import time
import uuid
from dataclasses import dataclass
from datetime import datetime, timezone
from pathlib import Path
from typing import Iterator, Sequence


TEXT_TO_CAD_PREFIX = "cadpy:"
GENERATION_STATUS_SCHEMA_VERSION = 1
GENERATION_LOCK_SUFFIX = ".generation.lock.json"
GENERATION_STATUS_HEARTBEAT_INTERVAL_SEC = 1.0
_ACTIVE_RUN = threading.local()
@dataclass(frozen=True)
class PythonSourceIdentity:
    source_path: str
    source_hash: str


@dataclass(frozen=True)
class GenerationOutput:
    path: Path
    kind: str


def generation_lock_path(output_path: Path | str, run_id: str) -> Path:
    resolved_output = Path(output_path).expanduser().resolve()
    return resolved_output.parent / f".{resolved_output.name}.{_safe_lock_token(run_id)}{GENERATION_LOCK_SUFFIX}"


def python_source_identity(script_path: Path) -> PythonSourceIdentity:
    resolved_script = script_path.expanduser().resolve()
    repo_root = Path.cwd().resolve()
    return PythonSourceIdentity(
        source_path=_manifest_path(resolved_script, repo_root),
        source_hash=_sha256_file(resolved_script),
    )


def track_generation_run(
    *,
    source_path: Path | None,
    generator: str,
    outputs: Sequence[GenerationOutput | tuple[Path, str] | Path | str],
    repo_root: Path | None = None,
) -> contextlib.AbstractContextManager[None]:
    tracker = _GenerationStatusTracker(
        repo_root=repo_root or Path.cwd(),
        source_path=source_path,
        generator=generator,
        outputs=outputs,
    )
    return tracker.run()


class _GenerationStatusTracker:
    def __init__(
        self,
        *,
        repo_root: Path,
        source_path: Path | None,
        generator: str,
        outputs: Sequence[GenerationOutput | tuple[Path, str] | Path | str],
    ) -> None:
        self.repo_root = repo_root.expanduser().resolve()
        self.source_path = source_path.expanduser().resolve() if source_path is not None else None
        self.generator = str(generator or "").strip()
        self.outputs = tuple(_normalize_generation_output(output) for output in outputs)
        self.run_id = f"{os.getpid()}-{uuid.uuid4().hex}"
        self.status_paths = tuple(
            dict.fromkeys(
                generation_lock_path(output.path, self.run_id)
                for output in self.outputs
                if output.path is not None
            )
        )
        self.status_path = self.status_paths[0] if self.status_paths else None
        self.started_at = _now_iso()
        self.stop = threading.Event()
        self.thread: threading.Thread | None = None

    @contextlib.contextmanager
    def run(self) -> Iterator[None]:
        active_depth = int(getattr(_ACTIVE_RUN, "depth", 0) or 0)
        if active_depth > 0:
            yield
            return

        _ACTIVE_RUN.depth = active_depth + 1
        self._start()
        try:
            yield
        finally:
            try:
                self._finish()
            finally:
                _ACTIVE_RUN.depth = active_depth

    def _start(self) -> None:
        if not self.status_paths:
            return
        self._write_status()
        self.thread = threading.Thread(target=self._heartbeat, daemon=True)
        self.thread.start()

    def _finish(self) -> None:
        self.stop.set()
        if self.thread is not None:
            self.thread.join(timeout=0.25)
        for status_path in self.status_paths:
            try:
                status_path.unlink()
            except FileNotFoundError:
                pass
            except OSError:
                self._write_status(status="finished", status_paths=(status_path,))

    def _heartbeat(self) -> None:
        while not self.stop.wait(GENERATION_STATUS_HEARTBEAT_INTERVAL_SEC):
            self._write_status()

    def _write_status(
        self,
        *,
        status: str = "running",
        status_paths: Sequence[Path] | None = None,
    ) -> None:
        for status_path in tuple(status_paths or self.status_paths):
            try:
                status_path.parent.mkdir(parents=True, exist_ok=True)
                tmp_path = status_path.with_name(f"{status_path.name}.{os.getpid()}.tmp")
                payload = self._status_payload(status=status, status_path=status_path)
                tmp_path.write_text(json.dumps(payload, sort_keys=True) + "\n", encoding="utf-8")
                tmp_path.replace(status_path)
            except OSError:
                pass

    def _status_payload(self, *, status: str, status_path: Path) -> dict[str, object]:
        base_dir = status_path.parent
        return {
            "schemaVersion": GENERATION_STATUS_SCHEMA_VERSION,
            "id": self.run_id,
            "status": status,
            "pid": os.getpid(),
            "startedAt": self.started_at,
            "updatedAt": _now_iso(),
            "sourcePath": _display_generation_path(self.source_path, base_dir),
            "generator": self.generator,
            "outputs": [
                {
                    "path": _display_generation_path(output.path, base_dir),
                    "kind": output.kind,
                }
                for output in self.outputs
                if output.path is not None
            ],
        }


def xml_with_text_to_cad_metadata(
    xml_text: str,
    identity: PythonSourceIdentity,
    *,
    output_path: Path | None = None,
    source_path: Path | None = None,
) -> str:
    text = _strip_text_to_cad_metadata_comments(str(xml_text or ""))
    metadata_source_path = identity.source_path
    if output_path is not None and source_path is not None:
        metadata_source_path = _display_generation_path(source_path, Path(output_path).expanduser().resolve().parent)
    comment_block = "".join(
        f"<!-- {TEXT_TO_CAD_PREFIX}{key}={value} -->\n"
        for key, value in (
            ("sourcePath", metadata_source_path),
            ("sourceHash", identity.source_hash),
        )
        if value
    )
    declaration = re.match(r"\s*<\?xml[^>]*\?>\s*", text)
    if declaration:
        insert_at = declaration.end()
        return text[:insert_at] + comment_block + text[insert_at:]
    return comment_block + text


def _normalize_generation_output(output: GenerationOutput | tuple[Path, str] | Path | str) -> GenerationOutput:
    if isinstance(output, GenerationOutput):
        return output
    if isinstance(output, tuple):
        output_path, kind = output
        return GenerationOutput(path=Path(output_path), kind=str(kind or "").strip())
    path = Path(output)
    return GenerationOutput(path=path, kind=path.suffix.lower().replace(".", "") or "output")


def _safe_lock_token(value: object) -> str:
    token = "".join(
        character if character.isalnum() or character in {"-", "_"} else "_"
        for character in str(value or "").strip()
    ).strip("._")
    return token or uuid.uuid4().hex


def _display_generation_path(path: Path | None, repo_root: Path) -> str:
    if path is None:
        return ""
    resolved = path.expanduser().resolve()
    return os.path.relpath(resolved, start=repo_root.expanduser().resolve()).replace(os.sep, "/")


def _now_iso() -> str:
    return datetime.fromtimestamp(time.time(), tz=timezone.utc).isoformat().replace("+00:00", "Z")


def _strip_text_to_cad_metadata_comments(xml_text: str) -> str:
    return re.sub(
        r"\s*<!--\s*cadpy:source[A-Za-z]*=[\s\S]*?-->\s*",
        "\n",
        xml_text,
    ).lstrip("\n")


def _manifest_path(path: Path, repo_root: Path) -> str:
    resolved = path.resolve()
    try:
        return resolved.relative_to(repo_root).as_posix()
    except ValueError:
        return resolved.as_posix()


def _sha256_file(path: Path) -> str:
    digest = hashlib.sha256()
    with path.open("rb") as handle:
        for chunk in iter(lambda: handle.read(1024 * 1024), b""):
            digest.update(chunk)
    return digest.hexdigest()

"""URDF generation and validation helpers."""

from __future__ import annotations

import sys
from pathlib import Path

if __package__ in {None, ""}:
    scripts_dir = Path(__file__).resolve().parents[1]
    if str(scripts_dir) not in sys.path:
        sys.path.insert(0, str(scripts_dir))
    from 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 dataclasses import dataclass
from pathlib import Path
import xml.etree.ElementTree as ET

SCRIPTS_DIR = Path(__file__).resolve().parents[1]
if __package__ in {None, ""}:
    sys.path.insert(0, str(SCRIPTS_DIR))

PACKAGES_DIR = SCRIPTS_DIR / "packages"
CADPY_METADATA_SRC_DIR = PACKAGES_DIR / "cadpy_metadata" / "src"
if str(PACKAGES_DIR) not in sys.path:
    sys.path.insert(0, str(PACKAGES_DIR))
if str(CADPY_METADATA_SRC_DIR) not in sys.path:
    sys.path.insert(0, str(CADPY_METADATA_SRC_DIR))

from cadpy_metadata import (
    GenerationOutput,
    python_source_identity,
    track_generation_run,
    xml_with_text_to_cad_metadata,
)
from urdf.source import read_urdf_source


@dataclass(frozen=True)
class _TargetSpec:
    source_path: Path
    output_path: Path


def generate_urdf_targets(targets: Sequence[str], *, output: str | Path | None = None) -> int:
    target_specs = _resolve_target_specs(targets, output=output)
    _validate_unique_outputs(target_specs)
    for target_spec in target_specs:
        _generate_target(target_spec.source_path, output_path=target_spec.output_path)
    return 0


def main(argv: Sequence[str] | None = None) -> int:
    parser = argparse.ArgumentParser(
        prog="urdf",
        description="Generate explicit URDF targets from Python sources.",
    )
    parser.add_argument(
        "targets",
        nargs="+",
        help="Explicit Python source file or SOURCE.py=OUTPUT.urdf pair defining gen_urdf() to generate.",
    )
    parser.add_argument(
        "-o",
        "--output",
        metavar="PATH",
        help="Write the generated URDF file to this path. Valid only with one plain Python target.",
    )
    args = parser.parse_args(list(argv) if argv is not None else None)
    if args.output is not None:
        if _targets_include_output_pairs(args.targets):
            parser.error("--output cannot be combined with SOURCE=OUTPUT targets")
        if len(args.targets) != 1:
            parser.error("--output can only be used with exactly one target")
    return generate_urdf_targets(args.targets, output=args.output)


def _resolve_target_specs(targets: Sequence[str], *, output: str | Path | None = None) -> list[_TargetSpec]:
    if output is not None and _targets_include_output_pairs(targets):
        raise ValueError("urdf --output cannot be combined with SOURCE=OUTPUT targets")
    if output is not None and len(targets) != 1:
        raise ValueError("urdf --output can only be used with exactly one target")

    specs: list[_TargetSpec] = []
    for raw_target in targets:
        target_text = str(raw_target or "").strip()
        if "=" in target_text:
            raw_source, raw_output = target_text.split("=", 1)
            source_path = _resolve_source_path(raw_source)
            output_path = _resolve_cli_output_path(raw_output)
        else:
            source_path = _resolve_source_path(target_text)
            output_path = _resolve_cli_output_path(output) if output is not None else source_path.with_suffix(".urdf")
        specs.append(_TargetSpec(source_path=source_path, output_path=output_path))
    return specs


def _resolve_source_path(raw_source: object) -> Path:
    value = str(raw_source or "").strip()
    if not value:
        raise ValueError("urdf target source must be a non-empty path")
    source_path = Path(value).expanduser()
    return source_path.resolve() if source_path.is_absolute() else (Path.cwd() / source_path).resolve()


def _resolve_cli_output_path(raw_output: object) -> Path:
    value = str(raw_output or "").strip()
    if not value:
        raise ValueError("urdf output must be a non-empty path")
    if "\\" in value:
        raise ValueError("urdf output must use POSIX '/' separators")
    output_path = Path(value).expanduser()
    resolved = output_path.resolve() if output_path.is_absolute() else (Path.cwd() / output_path).resolve()
    if resolved.suffix.lower() != ".urdf":
        raise ValueError("urdf output must end in .urdf")
    return resolved


def _targets_include_output_pairs(targets: Sequence[str]) -> bool:
    return any("=" in str(target or "") for target in targets)


def _validate_unique_outputs(target_specs: Sequence[_TargetSpec]) -> None:
    seen: dict[Path, Path] = {}
    for target_spec in target_specs:
        output_path = target_spec.output_path.resolve()
        previous = seen.get(output_path)
        if previous is not None:
            raise ValueError(f"urdf output path is used more than once: {_display_path(target_spec.output_path)}")
        seen[output_path] = target_spec.output_path


def _generate_target(script_path: Path, *, output_path: Path) -> Path:
    script_path = script_path.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)}")

    with track_generation_run(
        source_path=script_path,
        generator="gen_urdf",
        outputs=[GenerationOutput(output_path, "urdf")],
    ):
        return _generate_target_inner(script_path, output_path=output_path)


def _generate_target_inner(script_path: Path, *, output_path: Path) -> 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")

    payload = _normalize_urdf_payload(generator(), script_path=script_path)
    _write_urdf_payload(payload, 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)}")
    read_urdf_source(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 _normalize_urdf_payload(raw_payload: object, *, script_path: Path) -> dict[str, object]:
    if _is_xml_element(raw_payload):
        return {"xml": _serialize_xml_element(raw_payload)}
    if isinstance(raw_payload, str):
        return {"xml": raw_payload}
    if not isinstance(raw_payload, dict):
        raise TypeError(
            f"{_display_path(script_path)} gen_urdf() must return a URDF XML root element, XML string, "
            "or generator envelope dict"
        )
    allowed_fields = {"xml"}
    extra_fields = sorted(str(key) for key in raw_payload if key not in allowed_fields)
    if extra_fields:
        joined = ", ".join(extra_fields)
        raise TypeError(f"{_display_path(script_path)} gen_urdf() envelope has unsupported field(s): {joined}")
    if "xml" not in raw_payload:
        raise TypeError(f"{_display_path(script_path)} gen_urdf() envelope must define 'xml'")
    payload = dict(raw_payload)
    payload["xml"] = _normalize_xml_value(payload["xml"], script_path=script_path, label="gen_urdf() envelope field 'xml'")
    return payload


def _write_urdf_payload(payload: dict[str, object], *, output_path: Path, script_path: Path) -> None:
    xml = payload.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)
    identity = python_source_identity(script_path)
    xml = xml_with_text_to_cad_metadata(xml, identity, output_path=output_path, source_path=script_path)
    text = xml if xml.endswith("\n") else xml + "\n"
    output_path.write_text(text, encoding="utf-8")
    print(f"Wrote URDF: {output_path}")


def _normalize_xml_value(raw_xml: object, *, script_path: Path, label: str) -> str:
    if _is_xml_element(raw_xml):
        return _serialize_xml_element(raw_xml)
    if isinstance(raw_xml, str):
        return raw_xml
    raise TypeError(
        f"{_display_path(script_path)} {label} must be an xml.etree.ElementTree.Element or string, "
        f"got {type(raw_xml).__name__}"
    )


def _is_xml_element(value: object) -> bool:
    return isinstance(value, ET.Element)


def _serialize_xml_element(root: ET.Element) -> str:
    ET.indent(root, space="  ")
    body = ET.tostring(root, encoding="unicode", short_empty_elements=True)
    return f'<?xml version="1.0"?>\n{body}'


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()


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

from __future__ import annotations

from dataclasses import dataclass
from enum import Enum
from math import degrees, isfinite
from pathlib import Path, PurePosixPath
from urllib.parse import unquote, urlparse
import warnings
import xml.etree.ElementTree as ET

URDF_SUFFIX = ".urdf"
SUPPORTED_JOINT_TYPES = {"fixed", "continuous", "revolute", "prismatic"}
SUPPORTED_GEOMETRY_TAGS = {"box", "cylinder", "mesh", "sphere"}


class UrdfSourceError(ValueError):
    pass


class UrdfSourceWarning(UserWarning):
    pass


class MeshUriKind(Enum):
    LOCAL_RELATIVE = "local_relative"
    LOCAL_ABSOLUTE = "local_absolute"
    PACKAGE = "package"
    REMOTE = "remote"


@dataclass(frozen=True)
class MeshReference:
    uri: str
    kind: MeshUriKind
    path: Path | None = None
    package_name: str | None = None
    package_path: PurePosixPath | None = None


@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, ...]
    visual_mesh_paths: tuple[Path, ...] = ()
    collision_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, *, package_map: dict[str, Path] | None = None) -> 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)
    _validate_link_inertials(root, source_path=resolved_path)

    visual_mesh_paths: list[Path] = []
    collision_mesh_paths: list[Path] = []
    for link_element in root.findall("link"):
        visual_mesh_paths.extend(
            _geometry_mesh_paths(
                link_element,
                element_name="visual",
                source_path=resolved_path,
                package_map=package_map,
            )
        )
        collision_mesh_paths.extend(
            _geometry_mesh_paths(
                link_element,
                element_name="collision",
                source_path=resolved_path,
                package_map=package_map,
            )
        )

    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}"
            )
        _validate_origin(
            joint_element.find("origin"),
            source_path=resolved_path,
            label=f"joint {name!r} origin",
        )
        _validate_joint_axis(joint_element, joint_type=joint_type, source_path=resolved_path)
        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"
        )

    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(visual_mesh_paths + collision_mesh_paths),
        visual_mesh_paths=tuple(visual_mesh_paths),
        collision_mesh_paths=tuple(collision_mesh_paths),
    )


def _validate_link_inertials(root: ET.Element, *, source_path: Path) -> None:
    for link_element in root.findall("link"):
        link_name = str(link_element.attrib.get("name") or "").strip()
        inertial_element = link_element.find("inertial")
        if inertial_element is None:
            continue
        _validate_origin(
            inertial_element.find("origin"),
            source_path=source_path,
            label=f"link {link_name!r} inertial origin",
        )
        mass_element = inertial_element.find("mass")
        if mass_element is None:
            raise UrdfSourceError(
                f"{_relative_to_repo(source_path)} link {link_name!r} inertial requires <mass>"
            )
        mass = _required_float_attr(
            mass_element,
            "value",
            source_path=source_path,
            label=f"link {link_name!r} inertial mass",
        )
        if mass <= 0.0:
            raise UrdfSourceError(
                f"{_relative_to_repo(source_path)} link {link_name!r} inertial mass must be positive"
            )

        inertia_element = inertial_element.find("inertia")
        if inertia_element is None:
            raise UrdfSourceError(
                f"{_relative_to_repo(source_path)} link {link_name!r} inertial requires <inertia>"
            )
        ixx = _required_float_attr(
            inertia_element,
            "ixx",
            source_path=source_path,
            label=f"link {link_name!r} inertia ixx",
        )
        ixy = _required_float_attr(
            inertia_element,
            "ixy",
            source_path=source_path,
            label=f"link {link_name!r} inertia ixy",
        )
        ixz = _required_float_attr(
            inertia_element,
            "ixz",
            source_path=source_path,
            label=f"link {link_name!r} inertia ixz",
        )
        iyy = _required_float_attr(
            inertia_element,
            "iyy",
            source_path=source_path,
            label=f"link {link_name!r} inertia iyy",
        )
        iyz = _required_float_attr(
            inertia_element,
            "iyz",
            source_path=source_path,
            label=f"link {link_name!r} inertia iyz",
        )
        izz = _required_float_attr(
            inertia_element,
            "izz",
            source_path=source_path,
            label=f"link {link_name!r} inertia izz",
        )
        _validate_inertia_values(
            link_name,
            ixx=ixx,
            ixy=ixy,
            ixz=ixz,
            iyy=iyy,
            iyz=iyz,
            izz=izz,
            source_path=source_path,
        )


def _required_float_attr(
    element: ET.Element,
    attr_name: str,
    *,
    source_path: Path,
    label: str,
) -> float:
    try:
        value = element.attrib[attr_name]
    except KeyError as exc:
        raise UrdfSourceError(
            f"{_relative_to_repo(source_path)} {label} requires {attr_name!r}"
        ) from exc
    try:
        parsed = float(value)
    except ValueError as exc:
        raise UrdfSourceError(f"{_relative_to_repo(source_path)} {label} is invalid") from exc
    if not isfinite(parsed):
        raise UrdfSourceError(f"{_relative_to_repo(source_path)} {label} must be finite")
    return parsed


def _validate_inertia_values(
    link_name: str,
    *,
    ixx: float,
    ixy: float,
    ixz: float,
    iyy: float,
    iyz: float,
    izz: float,
    source_path: Path,
) -> None:
    del ixy, ixz, iyz
    if ixx <= 0.0 or iyy <= 0.0 or izz <= 0.0:
        raise UrdfSourceError(
            f"{_relative_to_repo(source_path)} link {link_name!r} inertia diagonal values must be positive"
        )
    tolerance = 1e-12
    if ixx + iyy + tolerance < izz or ixx + izz + tolerance < iyy or iyy + izz + tolerance < ixx:
        raise UrdfSourceError(
            f"{_relative_to_repo(source_path)} link {link_name!r} inertia violates triangle inequalities"
        )


def _geometry_mesh_paths(
    link_element: ET.Element,
    *,
    element_name: str,
    source_path: Path,
    package_map: dict[str, Path] | None,
) -> list[Path]:
    mesh_paths: list[Path] = []
    link_name = str(link_element.attrib.get("name") or "").strip()
    for geometry_owner in link_element.findall(element_name):
        _validate_origin(
            geometry_owner.find("origin"),
            source_path=source_path,
            label=f"link {link_name!r} {element_name} origin",
        )
        geometry_element = geometry_owner.find("geometry")
        if geometry_element is None:
            raise UrdfSourceError(
                f"{_relative_to_repo(source_path)} link {link_name!r} {element_name} requires <geometry>"
            )
        geometry_child = _validate_geometry_element(
            geometry_element,
            source_path=source_path,
            label=f"link {link_name!r} {element_name} geometry",
        )
        if geometry_child.tag != "mesh":
            continue
        mesh_path = _validated_mesh_path(geometry_child, source_path=source_path, package_map=package_map)
        if mesh_path is not None:
            mesh_paths.append(mesh_path)
    return mesh_paths


def _validated_mesh_path(
    mesh_element: ET.Element,
    *,
    source_path: Path,
    package_map: dict[str, Path] | None,
) -> Path | None:
    filename = str(mesh_element.attrib.get("filename") or "").strip()
    if not filename:
        raise UrdfSourceError(f"{_relative_to_repo(source_path)} mesh filename is required")
    mesh_ref = classify_mesh_uri(filename)
    _validate_mesh_scale(mesh_element, source_path=source_path, filename=filename)
    mesh_path = resolve_mesh_uri(filename, package_map=package_map)
    if mesh_path is None:
        _warn_unresolved_mesh_uri(filename, mesh_ref=mesh_ref, source_path=source_path)
        return None
    if mesh_ref.kind == MeshUriKind.LOCAL_RELATIVE:
        mesh_path = (source_path.parent / mesh_path).resolve()
    if not mesh_path.is_file():
        raise UrdfSourceError(
            f"{_relative_to_repo(source_path)} references missing mesh file: {filename!r}"
        )
    return mesh_path


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)} {joint_type} 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)} {joint_type} 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)} {joint_type} joint {joint_element.attrib.get('name', '')!r} has invalid limits"
        ) from exc
    if not isfinite(lower) or not isfinite(upper):
        raise UrdfSourceError(
            f"{_relative_to_repo(source_path)} {joint_type} joint {joint_element.attrib.get('name', '')!r} limits must be finite"
        )
    if lower > upper:
        raise UrdfSourceError(
            f"{_relative_to_repo(source_path)} {joint_type} joint {joint_element.attrib.get('name', '')!r} lower limit exceeds upper limit"
        )
    _validate_optional_float_attr(
        limit_element,
        "effort",
        source_path=source_path,
        label=f"{joint_type} joint {joint_element.attrib.get('name', '')!r} effort limit",
    )
    _validate_optional_float_attr(
        limit_element,
        "velocity",
        source_path=source_path,
        label=f"{joint_type} joint {joint_element.attrib.get('name', '')!r} velocity limit",
    )
    if joint_type == "prismatic":
        return lower, upper
    return degrees(lower), degrees(upper)


def _validate_optional_float_attr(
    element: ET.Element,
    attr_name: str,
    *,
    source_path: Path,
    label: str,
) -> float | None:
    if attr_name not in element.attrib:
        return None
    return _required_float_attr(element, attr_name, source_path=source_path, label=label)


def _validate_origin(origin_element: ET.Element | None, *, source_path: Path, label: str) -> None:
    if origin_element is None:
        return
    if "xyz" in origin_element.attrib:
        _required_float_vector_attr(
            origin_element,
            "xyz",
            expected_count=3,
            source_path=source_path,
            label=f"{label} xyz",
        )
    if "rpy" in origin_element.attrib:
        _required_float_vector_attr(
            origin_element,
            "rpy",
            expected_count=3,
            source_path=source_path,
            label=f"{label} rpy",
        )


def _validate_joint_axis(joint_element: ET.Element, *, joint_type: str, source_path: Path) -> None:
    axis_element = joint_element.find("axis")
    if axis_element is None:
        return
    axis = _required_float_vector_attr(
        axis_element,
        "xyz",
        expected_count=3,
        source_path=source_path,
        label=f"joint {joint_element.attrib.get('name', '')!r} axis",
    )
    if joint_type != "fixed" and all(component == 0.0 for component in axis):
        raise UrdfSourceError(
            f"{_relative_to_repo(source_path)} joint {joint_element.attrib.get('name', '')!r} axis must be nonzero"
        )


def _validate_geometry_element(geometry_element: ET.Element, *, source_path: Path, label: str) -> ET.Element:
    geometry_children = list(geometry_element)
    if len(geometry_children) != 1:
        supported = ", ".join(sorted(SUPPORTED_GEOMETRY_TAGS))
        raise UrdfSourceError(
            f"{_relative_to_repo(source_path)} {label} must define exactly one geometry element: {supported}"
        )
    geometry_child = geometry_children[0]
    if geometry_child.tag not in SUPPORTED_GEOMETRY_TAGS:
        supported = ", ".join(sorted(SUPPORTED_GEOMETRY_TAGS))
        raise UrdfSourceError(
            f"{_relative_to_repo(source_path)} {label} must use one of: {supported}"
        )
    _validate_geometry_child(geometry_child, source_path=source_path, label=label)
    return geometry_child


def _validate_geometry_child(geometry_child: ET.Element, *, source_path: Path, label: str) -> None:
    if geometry_child.tag == "mesh":
        return
    if geometry_child.tag == "box":
        _required_positive_float_vector_attr(
            geometry_child,
            "size",
            expected_count=3,
            source_path=source_path,
            label=f"{label} box size",
        )
        return
    if geometry_child.tag == "cylinder":
        _required_positive_float_attr(
            geometry_child,
            "radius",
            source_path=source_path,
            label=f"{label} cylinder radius",
        )
        _required_positive_float_attr(
            geometry_child,
            "length",
            source_path=source_path,
            label=f"{label} cylinder length",
        )
        return
    if geometry_child.tag == "sphere":
        _required_positive_float_attr(
            geometry_child,
            "radius",
            source_path=source_path,
            label=f"{label} sphere radius",
        )


def _validate_mesh_scale(mesh_element: ET.Element, *, source_path: Path, filename: str) -> None:
    if "scale" not in mesh_element.attrib:
        return
    _required_positive_float_vector_attr(
        mesh_element,
        "scale",
        expected_count=3,
        source_path=source_path,
        label=f"mesh {filename!r} scale",
    )


def _required_positive_float_attr(
    element: ET.Element,
    attr_name: str,
    *,
    source_path: Path,
    label: str,
) -> float:
    value = _required_float_attr(element, attr_name, source_path=source_path, label=label)
    if value <= 0.0:
        raise UrdfSourceError(f"{_relative_to_repo(source_path)} {label} must be positive")
    return value


def _required_float_vector_attr(
    element: ET.Element,
    attr_name: str,
    *,
    expected_count: int,
    source_path: Path,
    label: str,
) -> tuple[float, ...]:
    try:
        raw_value = element.attrib[attr_name]
    except KeyError as exc:
        raise UrdfSourceError(
            f"{_relative_to_repo(source_path)} {label} requires {attr_name!r}"
        ) from exc
    raw_parts = raw_value.split()
    if len(raw_parts) != expected_count:
        raise UrdfSourceError(
            f"{_relative_to_repo(source_path)} {label} must have {expected_count} values"
        )
    try:
        values = tuple(float(part) for part in raw_parts)
    except ValueError as exc:
        raise UrdfSourceError(f"{_relative_to_repo(source_path)} {label} is invalid") from exc
    if not all(isfinite(value) for value in values):
        raise UrdfSourceError(f"{_relative_to_repo(source_path)} {label} values must be finite")
    return values


def _required_positive_float_vector_attr(
    element: ET.Element,
    attr_name: str,
    *,
    expected_count: int,
    source_path: Path,
    label: str,
) -> tuple[float, ...]:
    values = _required_float_vector_attr(
        element,
        attr_name,
        expected_count=expected_count,
        source_path=source_path,
        label=label,
    )
    if any(value <= 0.0 for value in values):
        raise UrdfSourceError(f"{_relative_to_repo(source_path)} {label} values must be positive")
    return values


def classify_mesh_uri(uri: str) -> MeshReference:
    value = str(uri or "").strip()
    parsed = urlparse(value)

    if parsed.scheme == "package":
        package_name = unquote(parsed.netloc).strip()
        package_path = PurePosixPath(unquote(parsed.path).lstrip("/"))
        if not package_name or package_path.as_posix() == "." or ".." in package_path.parts:
            raise UrdfSourceError(f"package mesh URI is invalid: {uri!r}")
        return MeshReference(
            uri=value,
            kind=MeshUriKind.PACKAGE,
            package_name=package_name,
            package_path=package_path,
        )

    if parsed.scheme == "file":
        file_path = Path(unquote(parsed.path))
        if file_path.is_absolute():
            return MeshReference(uri=value, kind=MeshUriKind.LOCAL_ABSOLUTE, path=file_path.resolve())
        return MeshReference(uri=value, kind=MeshUriKind.LOCAL_RELATIVE, path=file_path)

    if parsed.scheme:
        return MeshReference(uri=value, kind=MeshUriKind.REMOTE)

    local_path = Path(value)
    if local_path.is_absolute():
        return MeshReference(uri=value, kind=MeshUriKind.LOCAL_ABSOLUTE, path=local_path.resolve())
    return MeshReference(uri=value, kind=MeshUriKind.LOCAL_RELATIVE, path=local_path)


def resolve_mesh_uri(uri: str, package_map: dict[str, Path] | None = None) -> Path | None:
    mesh_ref = classify_mesh_uri(uri)
    if mesh_ref.kind in {MeshUriKind.LOCAL_RELATIVE, MeshUriKind.LOCAL_ABSOLUTE}:
        return mesh_ref.path
    if mesh_ref.kind == MeshUriKind.PACKAGE:
        package_root = (package_map or {}).get(str(mesh_ref.package_name))
        if package_root is None or mesh_ref.package_path is None:
            return None
        return (Path(package_root).expanduser() / Path(*mesh_ref.package_path.parts)).resolve()
    return None


def _warn_unresolved_mesh_uri(uri: str, *, mesh_ref: MeshReference, source_path: Path) -> None:
    if mesh_ref.kind == MeshUriKind.PACKAGE:
        message = f"WARN: {uri} syntax is valid but was not resolved."
    else:
        message = f"WARN: {uri} is not a local mesh URI and was not resolved."
    warnings.warn(f"{_relative_to_repo(source_path)} {message}", UrdfSourceWarning, stacklevel=3)


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()