first commit
This commit is contained in:
@@ -0,0 +1,17 @@
|
||||
[build-system]
|
||||
requires = ["setuptools>=68"]
|
||||
build-backend = "setuptools.build_meta"
|
||||
|
||||
[project]
|
||||
name = "cadpy-metadata"
|
||||
version = "0.3.9"
|
||||
description = "Dependency-free metadata helpers for Python-generated CAD outputs."
|
||||
requires-python = ">=3.12"
|
||||
dependencies = []
|
||||
|
||||
[tool.setuptools.packages.find]
|
||||
where = ["src"]
|
||||
include = ["cadpy_metadata*"]
|
||||
|
||||
[tool.setuptools.package-data]
|
||||
cadpy_metadata = ["py.typed"]
|
||||
+17
@@ -0,0 +1,17 @@
|
||||
"""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",
|
||||
]
|
||||
+246
@@ -0,0 +1,246 @@
|
||||
"""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()
|
||||
+1
@@ -0,0 +1 @@
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
"""URDF generation and validation helpers."""
|
||||
@@ -0,0 +1,16 @@
|
||||
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())
|
||||
@@ -0,0 +1,253 @@
|
||||
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())
|
||||
@@ -0,0 +1,698 @@
|
||||
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()
|
||||
Reference in New Issue
Block a user