first commit
This commit is contained in:
+17
@@ -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 @@
|
||||
"""SRDF 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 srdf.cli import main
|
||||
else:
|
||||
from .cli import main
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,671 @@
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import importlib.util
|
||||
import inspect
|
||||
from math import isfinite
|
||||
import os
|
||||
import sys
|
||||
import warnings
|
||||
from collections.abc import Sequence
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path, PurePosixPath
|
||||
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 srdf.source import LEGACY_EXPLORER_NAMESPACE, SRDF_METADATA_NAMESPACE, SrdfSource, SrdfSourceError, parse_srdf_xml
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _TargetSpec:
|
||||
source_path: Path
|
||||
output_path: Path
|
||||
|
||||
|
||||
def generate_srdf_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="srdf",
|
||||
description="Generate explicit MoveIt2 SRDF targets from Python sources.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"targets",
|
||||
nargs="+",
|
||||
help="Explicit Python source file or SOURCE.py=OUTPUT.srdf pair defining gen_srdf() to generate.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"-o",
|
||||
"--output",
|
||||
metavar="PATH",
|
||||
help="Write the generated SRDF 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_srdf_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("srdf --output cannot be combined with SOURCE=OUTPUT targets")
|
||||
if output is not None and len(targets) != 1:
|
||||
raise ValueError("srdf --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(".srdf")
|
||||
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("srdf 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("srdf output must be a non-empty path")
|
||||
if "\\" in value:
|
||||
raise ValueError("srdf 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() != ".srdf":
|
||||
raise ValueError("srdf output must end in .srdf")
|
||||
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"srdf 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_srdf",
|
||||
outputs=[GenerationOutput(output_path, "srdf")],
|
||||
):
|
||||
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_srdf", None)
|
||||
if not callable(generator):
|
||||
raise RuntimeError(f"{_display_path(script_path)} does not define callable gen_srdf()")
|
||||
if inspect.signature(generator).parameters:
|
||||
raise ValueError(f"{_display_path(script_path)} gen_srdf() must not accept arguments")
|
||||
|
||||
payload = _normalize_srdf_payload(generator(), script_path=script_path)
|
||||
xml = str(payload["xml"])
|
||||
urdf_path = _resolve_relative_file(payload["urdf"], source_path=script_path, suffix=".urdf", label="urdf")
|
||||
urdf_robot = _read_urdf_robot(urdf_path)
|
||||
xml = _xml_with_linked_urdf_metadata(xml, output_path=output_path, urdf_path=urdf_path)
|
||||
srdf_source = parse_srdf_xml(xml, source_path=output_path)
|
||||
_validate_linked_urdf_ref(
|
||||
srdf_source,
|
||||
expected_urdf_ref=_relative_posix_path(urdf_path, output_path.parent),
|
||||
)
|
||||
_validate_srdf_against_urdf(srdf_source, urdf_robot=urdf_robot)
|
||||
_write_srdf_payload(xml, 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 = (
|
||||
"_srdf_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_srdf_payload(raw_payload: object, *, script_path: Path) -> dict[str, object]:
|
||||
if not isinstance(raw_payload, dict):
|
||||
raise TypeError(f"{_display_path(script_path)} gen_srdf() must return an envelope dict")
|
||||
allowed_fields = {"xml", "urdf"}
|
||||
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_srdf() envelope has unsupported field(s): {joined}")
|
||||
if "xml" not in raw_payload:
|
||||
raise TypeError(f"{_display_path(script_path)} gen_srdf() envelope must define 'xml'")
|
||||
if "urdf" not in raw_payload:
|
||||
raise TypeError(f"{_display_path(script_path)} gen_srdf() envelope must define 'urdf'")
|
||||
xml = _normalize_xml_value(raw_payload.get("xml"), script_path=script_path, label="gen_srdf() envelope field 'xml'")
|
||||
if not xml.strip():
|
||||
raise TypeError(f"{_display_path(script_path)} gen_srdf() envelope field 'xml' must be non-empty")
|
||||
if not isinstance(raw_payload.get("urdf"), str) or not str(raw_payload.get("urdf")).strip():
|
||||
raise TypeError(f"{_display_path(script_path)} gen_srdf() envelope field 'urdf' must be a non-empty string")
|
||||
return {
|
||||
"xml": xml,
|
||||
"urdf": str(raw_payload["urdf"]).strip(),
|
||||
}
|
||||
|
||||
|
||||
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 _resolve_relative_file(raw_value: object, *, source_path: Path, suffix: str, label: str) -> Path:
|
||||
if not isinstance(raw_value, str) or not raw_value.strip():
|
||||
raise ValueError(f"{label} must be a non-empty relative path")
|
||||
value = raw_value.strip()
|
||||
if "\\" in value:
|
||||
raise ValueError(f"{label} must use POSIX '/' separators")
|
||||
pure = PurePosixPath(value)
|
||||
if pure.is_absolute() or any(part in {"", "."} for part in pure.parts):
|
||||
raise ValueError(f"{label} must be a relative path")
|
||||
path = (source_path.parent / Path(*pure.parts)).resolve()
|
||||
if path.suffix.lower() != suffix:
|
||||
raise ValueError(f"{label} must end in {suffix}")
|
||||
if not path.is_file():
|
||||
raise FileNotFoundError(f"{label} file does not exist: {_display_path(path)}")
|
||||
return path
|
||||
|
||||
|
||||
def _read_urdf_robot(urdf_path: Path) -> dict[str, object]:
|
||||
try:
|
||||
root = ET.parse(urdf_path).getroot()
|
||||
except ET.ParseError as exc:
|
||||
raise ValueError(f"URDF is invalid XML: {_display_path(urdf_path)}") from exc
|
||||
if root.tag != "robot":
|
||||
raise ValueError("URDF root must be <robot>")
|
||||
robot_name = str(root.get("name") or "").strip()
|
||||
links = {
|
||||
str(link.get("name") or "").strip()
|
||||
for link in root.findall("link")
|
||||
if str(link.get("name") or "").strip()
|
||||
}
|
||||
joints: dict[str, dict[str, object]] = {}
|
||||
for joint in root.findall("joint"):
|
||||
name = str(joint.get("name") or "").strip()
|
||||
if not name:
|
||||
continue
|
||||
parent_element = joint.find("parent")
|
||||
child_element = joint.find("child")
|
||||
joint_type = str(joint.get("type") or "").strip()
|
||||
lower: float | None = None
|
||||
upper: float | None = None
|
||||
limit_element = joint.find("limit")
|
||||
if limit_element is not None and joint_type in {"revolute", "prismatic"}:
|
||||
lower = _optional_finite_float(limit_element.get("lower"))
|
||||
upper = _optional_finite_float(limit_element.get("upper"))
|
||||
joints[name] = {
|
||||
"type": joint_type,
|
||||
"parent": str(parent_element.get("link") if parent_element is not None else "").strip(),
|
||||
"child": str(child_element.get("link") if child_element is not None else "").strip(),
|
||||
"lower": lower,
|
||||
"upper": upper,
|
||||
"mimic": joint.find("mimic") is not None,
|
||||
}
|
||||
if not robot_name:
|
||||
raise ValueError("URDF robot name is required")
|
||||
return {"name": robot_name, "links": links, "joints": joints}
|
||||
|
||||
|
||||
def _validate_srdf_against_urdf(srdf_source: SrdfSource, *, urdf_robot: dict[str, object]) -> None:
|
||||
urdf_name = str(urdf_robot["name"])
|
||||
if srdf_source.robot_name != urdf_name:
|
||||
raise SrdfSourceError(f"SRDF robot name {srdf_source.robot_name!r} must match URDF robot name {urdf_name!r}")
|
||||
links = urdf_robot["links"]
|
||||
joints = urdf_robot["joints"]
|
||||
assert isinstance(links, set)
|
||||
assert isinstance(joints, dict)
|
||||
group_names = {group.name for group in srdf_source.planning_groups}
|
||||
groups_by_name = {group.name: group for group in srdf_source.planning_groups}
|
||||
if not group_names:
|
||||
raise SrdfSourceError("SRDF must define at least one planning group")
|
||||
|
||||
for group in srdf_source.planning_groups:
|
||||
if not group.joint_names and not group.link_names and not group.chains and not group.subgroups:
|
||||
raise SrdfSourceError(f"SRDF planning group {group.name!r} must define joints, links, chains, or subgroups")
|
||||
_validate_names_exist(group.joint_names, set(joints), label=f"planning group {group.name!r} joint")
|
||||
_validate_names_exist(group.link_names, links, label=f"planning group {group.name!r} link")
|
||||
_validate_names_exist(group.subgroups, group_names, label=f"planning group {group.name!r} subgroup")
|
||||
for chain in group.chains:
|
||||
if chain.base_link not in links:
|
||||
raise SrdfSourceError(f"planning group {group.name!r} chain references missing base_link {chain.base_link!r}")
|
||||
if chain.tip_link not in links:
|
||||
raise SrdfSourceError(f"planning group {group.name!r} chain references missing tip_link {chain.tip_link!r}")
|
||||
|
||||
for end_effector in srdf_source.end_effectors:
|
||||
if end_effector.parent_link not in links:
|
||||
raise SrdfSourceError(f"end_effector {end_effector.name!r} references missing parent_link {end_effector.parent_link!r}")
|
||||
if end_effector.group not in group_names:
|
||||
raise SrdfSourceError(f"end_effector {end_effector.name!r} references missing group {end_effector.group!r}")
|
||||
if end_effector.parent_group and end_effector.parent_group not in group_names:
|
||||
raise SrdfSourceError(
|
||||
f"end_effector {end_effector.name!r} references missing parent_group {end_effector.parent_group!r}"
|
||||
)
|
||||
_validate_end_effector_topology(
|
||||
end_effector,
|
||||
groups_by_name=groups_by_name,
|
||||
urdf_robot=urdf_robot,
|
||||
)
|
||||
|
||||
for group_state in srdf_source.group_states:
|
||||
if group_state.group not in group_names:
|
||||
raise SrdfSourceError(f"group_state {group_state.name!r} references missing group {group_state.group!r}")
|
||||
group_joint_names = set(_joint_names_for_group(groups_by_name[group_state.group], urdf_robot=urdf_robot, groups_by_name=groups_by_name))
|
||||
for joint_name, value in group_state.joint_values_by_name.items():
|
||||
if joint_name not in joints:
|
||||
raise SrdfSourceError(f"group_state {group_state.name!r} joint references missing name {joint_name!r}")
|
||||
if joint_name not in group_joint_names:
|
||||
raise SrdfSourceError(
|
||||
f"group_state {group_state.name!r} joint {joint_name!r} is not in group {group_state.group!r}"
|
||||
)
|
||||
_validate_group_state_joint_value(group_state.name, joint_name, value, joints[joint_name])
|
||||
|
||||
for pair in srdf_source.disabled_collision_pairs:
|
||||
_validate_names_exist((pair.link1, pair.link2), links, label="disable_collisions link")
|
||||
_warn_on_many_manual_disabled_pairs(srdf_source.disabled_collision_pairs)
|
||||
|
||||
|
||||
def _optional_finite_float(value: object) -> float | None:
|
||||
if value is None:
|
||||
return None
|
||||
try:
|
||||
parsed = float(value)
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
return parsed if isfinite(parsed) else None
|
||||
|
||||
|
||||
def _joint_path_for_chain(urdf_robot: dict[str, object], *, base_link: str, tip_link: str) -> list[str]:
|
||||
if not base_link or not tip_link or base_link == tip_link:
|
||||
return []
|
||||
joints = urdf_robot.get("joints")
|
||||
if not isinstance(joints, dict):
|
||||
return []
|
||||
by_parent: dict[str, list[tuple[str, dict[str, object]]]] = {}
|
||||
for joint_name, joint in joints.items():
|
||||
if not isinstance(joint, dict):
|
||||
continue
|
||||
parent = str(joint.get("parent") or "").strip()
|
||||
child = str(joint.get("child") or "").strip()
|
||||
if parent and child:
|
||||
by_parent.setdefault(parent, []).append((str(joint_name), joint))
|
||||
|
||||
stack: list[tuple[str, list[str]]] = [(base_link, [])]
|
||||
visited: set[str] = set()
|
||||
while stack:
|
||||
link_name, path = stack.pop()
|
||||
if link_name == tip_link:
|
||||
return path
|
||||
if link_name in visited:
|
||||
continue
|
||||
visited.add(link_name)
|
||||
for joint_name, joint in reversed(by_parent.get(link_name, [])):
|
||||
child = str(joint.get("child") or "").strip()
|
||||
if child:
|
||||
stack.append((child, [*path, joint_name]))
|
||||
return []
|
||||
|
||||
|
||||
def _joint_names_for_group(
|
||||
group: object,
|
||||
*,
|
||||
urdf_robot: dict[str, object],
|
||||
groups_by_name: dict[str, object],
|
||||
visited: set[str] | None = None,
|
||||
) -> list[str]:
|
||||
names: list[str] = []
|
||||
joints = urdf_robot.get("joints")
|
||||
if not isinstance(joints, dict):
|
||||
return names
|
||||
|
||||
for joint_name in getattr(group, "joint_names", ()):
|
||||
joint = joints.get(joint_name)
|
||||
if isinstance(joint, dict) and str(joint.get("type") or "") != "fixed" and not bool(joint.get("mimic")):
|
||||
_append_unique(names, [joint_name])
|
||||
|
||||
for chain in getattr(group, "chains", ()):
|
||||
chain_joint_names = []
|
||||
for joint_name in _joint_path_for_chain(
|
||||
urdf_robot,
|
||||
base_link=str(getattr(chain, "base_link", "") or ""),
|
||||
tip_link=str(getattr(chain, "tip_link", "") or ""),
|
||||
):
|
||||
joint = joints.get(joint_name)
|
||||
if isinstance(joint, dict) and str(joint.get("type") or "") != "fixed" and not bool(joint.get("mimic")):
|
||||
chain_joint_names.append(joint_name)
|
||||
_append_unique(names, chain_joint_names)
|
||||
|
||||
if visited is None:
|
||||
visited = set()
|
||||
group_name = str(getattr(group, "name", "") or "")
|
||||
if group_name:
|
||||
visited.add(group_name)
|
||||
for subgroup_name in getattr(group, "subgroups", ()):
|
||||
subgroup_key = str(subgroup_name or "").strip()
|
||||
if not subgroup_key or subgroup_key in visited:
|
||||
continue
|
||||
subgroup = groups_by_name.get(subgroup_key)
|
||||
if subgroup is not None:
|
||||
_append_unique(
|
||||
names,
|
||||
_joint_names_for_group(
|
||||
subgroup,
|
||||
urdf_robot=urdf_robot,
|
||||
groups_by_name=groups_by_name,
|
||||
visited=visited,
|
||||
),
|
||||
)
|
||||
return names
|
||||
|
||||
|
||||
def _link_names_for_group(
|
||||
group: object,
|
||||
*,
|
||||
urdf_robot: dict[str, object],
|
||||
groups_by_name: dict[str, object],
|
||||
visited: set[str] | None = None,
|
||||
) -> set[str]:
|
||||
links = set(str(link_name) for link_name in getattr(group, "link_names", ()))
|
||||
joints = urdf_robot.get("joints")
|
||||
if not isinstance(joints, dict):
|
||||
return links
|
||||
|
||||
def add_joint_links(joint_name: str) -> None:
|
||||
joint = joints.get(joint_name)
|
||||
if not isinstance(joint, dict):
|
||||
return
|
||||
child = str(joint.get("child") or "").strip()
|
||||
if child:
|
||||
links.add(child)
|
||||
|
||||
for joint_name in getattr(group, "joint_names", ()):
|
||||
add_joint_links(str(joint_name))
|
||||
for chain in getattr(group, "chains", ()):
|
||||
links.add(str(getattr(chain, "tip_link", "") or ""))
|
||||
for joint_name in _joint_path_for_chain(
|
||||
urdf_robot,
|
||||
base_link=str(getattr(chain, "base_link", "") or ""),
|
||||
tip_link=str(getattr(chain, "tip_link", "") or ""),
|
||||
):
|
||||
add_joint_links(joint_name)
|
||||
|
||||
if visited is None:
|
||||
visited = set()
|
||||
group_name = str(getattr(group, "name", "") or "")
|
||||
if group_name:
|
||||
visited.add(group_name)
|
||||
for subgroup_name in getattr(group, "subgroups", ()):
|
||||
subgroup_key = str(subgroup_name or "").strip()
|
||||
if not subgroup_key or subgroup_key in visited:
|
||||
continue
|
||||
subgroup = groups_by_name.get(subgroup_key)
|
||||
if subgroup is not None:
|
||||
links.update(
|
||||
_link_names_for_group(
|
||||
subgroup,
|
||||
urdf_robot=urdf_robot,
|
||||
groups_by_name=groups_by_name,
|
||||
visited=visited,
|
||||
)
|
||||
)
|
||||
links.discard("")
|
||||
return links
|
||||
|
||||
|
||||
def _joint_adjacent_to_any_link(urdf_robot: dict[str, object], parent_link: str, child_links: set[str]) -> bool:
|
||||
joints = urdf_robot.get("joints")
|
||||
if not isinstance(joints, dict):
|
||||
return False
|
||||
for joint in joints.values():
|
||||
if not isinstance(joint, dict):
|
||||
continue
|
||||
parent = str(joint.get("parent") or "").strip()
|
||||
child = str(joint.get("child") or "").strip()
|
||||
if (parent == parent_link and child in child_links) or (child == parent_link and parent in child_links):
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
def _validate_end_effector_topology(
|
||||
end_effector: object,
|
||||
*,
|
||||
groups_by_name: dict[str, object],
|
||||
urdf_robot: dict[str, object],
|
||||
) -> None:
|
||||
group_name = str(getattr(end_effector, "group", "") or "")
|
||||
parent_group_name = str(getattr(end_effector, "parent_group", "") or "")
|
||||
parent_link = str(getattr(end_effector, "parent_link", "") or "")
|
||||
if not group_name or group_name not in groups_by_name:
|
||||
return
|
||||
|
||||
end_effector_links = _link_names_for_group(
|
||||
groups_by_name[group_name],
|
||||
urdf_robot=urdf_robot,
|
||||
groups_by_name=groups_by_name,
|
||||
)
|
||||
if parent_group_name and parent_group_name in groups_by_name:
|
||||
parent_group_links = _link_names_for_group(
|
||||
groups_by_name[parent_group_name],
|
||||
urdf_robot=urdf_robot,
|
||||
groups_by_name=groups_by_name,
|
||||
)
|
||||
overlap = sorted(end_effector_links & parent_group_links)
|
||||
if overlap:
|
||||
raise SrdfSourceError(
|
||||
f"end_effector {getattr(end_effector, 'name', '')!r} group shares link(s) with parent_group: {overlap!r}"
|
||||
)
|
||||
if parent_link and parent_link not in parent_group_links:
|
||||
raise SrdfSourceError(
|
||||
f"end_effector {getattr(end_effector, 'name', '')!r} parent_link {parent_link!r} is not in parent_group {parent_group_name!r}"
|
||||
)
|
||||
if end_effector_links and parent_link not in end_effector_links and not _joint_adjacent_to_any_link(
|
||||
urdf_robot,
|
||||
parent_link,
|
||||
end_effector_links,
|
||||
):
|
||||
raise SrdfSourceError(
|
||||
f"end_effector {getattr(end_effector, 'name', '')!r} parent_link {parent_link!r} is not adjacent to its group"
|
||||
)
|
||||
|
||||
|
||||
def _validate_group_state_joint_value(
|
||||
state_name: str,
|
||||
joint_name: str,
|
||||
value: float,
|
||||
joint: object,
|
||||
) -> None:
|
||||
if not isinstance(joint, dict):
|
||||
return
|
||||
joint_type = str(joint.get("type") or "").strip()
|
||||
if joint_type == "fixed":
|
||||
raise SrdfSourceError(f"group_state {state_name!r} cannot set fixed joint {joint_name!r}")
|
||||
if bool(joint.get("mimic")):
|
||||
raise SrdfSourceError(f"group_state {state_name!r} cannot set mimic joint {joint_name!r}")
|
||||
if joint_type == "continuous":
|
||||
return
|
||||
lower = joint.get("lower")
|
||||
upper = joint.get("upper")
|
||||
if isinstance(lower, float) and value < lower:
|
||||
raise SrdfSourceError(f"group_state {state_name!r} joint {joint_name!r} is below its URDF lower limit")
|
||||
if isinstance(upper, float) and value > upper:
|
||||
raise SrdfSourceError(f"group_state {state_name!r} joint {joint_name!r} is above its URDF upper limit")
|
||||
|
||||
|
||||
def _append_unique(target: list[str], values: list[str]) -> None:
|
||||
seen = set(target)
|
||||
for value in values:
|
||||
if value not in seen:
|
||||
target.append(value)
|
||||
seen.add(value)
|
||||
|
||||
|
||||
def _warn_on_many_manual_disabled_pairs(pairs: object) -> None:
|
||||
manual_count = sum(1 for pair in pairs if getattr(pair, "source", "") == "manual")
|
||||
if manual_count >= 25:
|
||||
warnings.warn(
|
||||
f"SRDF contains {manual_count} manually reasoned disabled collision pairs; prefer sampled/setup-assistant provenance.",
|
||||
stacklevel=3,
|
||||
)
|
||||
|
||||
|
||||
def _validate_linked_urdf_ref(srdf_source: SrdfSource, *, expected_urdf_ref: str) -> None:
|
||||
urdf_ref = str(srdf_source.urdf_ref or "").strip()
|
||||
if not urdf_ref:
|
||||
raise SrdfSourceError("SRDF must include <tcad:urdf path=\"...\"/> metadata")
|
||||
if PurePosixPath(urdf_ref) != PurePosixPath(expected_urdf_ref):
|
||||
raise SrdfSourceError(
|
||||
f"SRDF tcad:urdf path {urdf_ref!r} must match gen_srdf() envelope urdf {expected_urdf_ref!r}"
|
||||
)
|
||||
|
||||
|
||||
def _relative_posix_path(target_path: Path, start_dir: Path) -> str:
|
||||
return Path(os.path.relpath(Path(target_path).resolve(), Path(start_dir).resolve())).as_posix()
|
||||
|
||||
|
||||
def _xml_with_linked_urdf_metadata(xml: str, *, output_path: Path, urdf_path: Path) -> str:
|
||||
try:
|
||||
root = ET.fromstring(xml)
|
||||
except ET.ParseError as exc:
|
||||
raise SrdfSourceError(f"{_display_path(output_path)} could not be parsed as SRDF XML") from exc
|
||||
if root.tag != "robot":
|
||||
raise SrdfSourceError(f"{_display_path(output_path)} root element must be <robot>")
|
||||
ET.register_namespace("tcad", SRDF_METADATA_NAMESPACE)
|
||||
ET.register_namespace("explorer", LEGACY_EXPLORER_NAMESPACE)
|
||||
metadata_tag = f"{{{SRDF_METADATA_NAMESPACE}}}urdf"
|
||||
legacy_metadata_tag = f"{{{LEGACY_EXPLORER_NAMESPACE}}}urdf"
|
||||
metadata_element = None
|
||||
for child in list(root):
|
||||
if child.tag in {metadata_tag, legacy_metadata_tag} or str(child.tag or "") in {"tcad:urdf", "explorer:urdf"}:
|
||||
metadata_element = child
|
||||
break
|
||||
if metadata_element is None:
|
||||
metadata_element = ET.Element(metadata_tag)
|
||||
root.insert(0, metadata_element)
|
||||
else:
|
||||
metadata_element.tag = metadata_tag
|
||||
metadata_element.set("path", _relative_posix_path(urdf_path, output_path.parent))
|
||||
if root.text is None:
|
||||
root.text = "\n "
|
||||
if metadata_element.tail is None:
|
||||
metadata_element.tail = "\n "
|
||||
return _serialize_xml_element(root)
|
||||
|
||||
|
||||
def _validate_names_exist(names: object, allowed: set[str], *, label: str) -> None:
|
||||
for name in names:
|
||||
if name not in allowed:
|
||||
raise SrdfSourceError(f"{label} references missing name {name!r}")
|
||||
|
||||
|
||||
def _write_srdf_payload(xml: str, *, output_path: Path, script_path: Path) -> None:
|
||||
output_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
xml = xml_with_text_to_cad_metadata(
|
||||
xml,
|
||||
python_source_identity(script_path),
|
||||
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 SRDF: {output_path}")
|
||||
|
||||
|
||||
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,277 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from math import isfinite
|
||||
from pathlib import Path
|
||||
from typing import Literal
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
SRDF_SUFFIX = ".srdf"
|
||||
SRDF_METADATA_NAMESPACE = "https://text-to-cad.dev/srdf"
|
||||
LEGACY_EXPLORER_NAMESPACE = "https://text-to-cad.dev/explorer"
|
||||
|
||||
|
||||
class SrdfSourceError(ValueError):
|
||||
pass
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SrdfChain:
|
||||
base_link: str
|
||||
tip_link: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SrdfPlanningGroup:
|
||||
name: str
|
||||
joint_names: tuple[str, ...]
|
||||
link_names: tuple[str, ...]
|
||||
chains: tuple[SrdfChain, ...]
|
||||
subgroups: tuple[str, ...]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SrdfEndEffector:
|
||||
name: str
|
||||
parent_link: str
|
||||
group: str
|
||||
parent_group: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SrdfGroupState:
|
||||
name: str
|
||||
group: str
|
||||
joint_values_by_name: dict[str, float]
|
||||
|
||||
@property
|
||||
def joint_values_by_name_rad(self) -> dict[str, float]:
|
||||
"""Deprecated compatibility alias. SRDF values use URDF-native units."""
|
||||
return self.joint_values_by_name
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SrdfDisabledCollisionPair:
|
||||
link1: str
|
||||
link2: str
|
||||
reason: str
|
||||
source: Literal["adjacent", "sampled", "manual", "setup_assistant", "assumed"] = "manual"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SrdfSource:
|
||||
file_ref: str
|
||||
source_path: Path
|
||||
robot_name: str
|
||||
urdf_ref: str
|
||||
planning_groups: tuple[SrdfPlanningGroup, ...]
|
||||
end_effectors: tuple[SrdfEndEffector, ...]
|
||||
group_states: tuple[SrdfGroupState, ...]
|
||||
disabled_collision_pairs: tuple[SrdfDisabledCollisionPair, ...]
|
||||
|
||||
|
||||
def file_ref_from_srdf_path(srdf_path: Path) -> str:
|
||||
resolved = srdf_path.resolve()
|
||||
if resolved.suffix.lower() != SRDF_SUFFIX:
|
||||
raise SrdfSourceError(f"{resolved} is not an SRDF source file")
|
||||
return _relative_to_repo(resolved)
|
||||
|
||||
|
||||
def read_srdf_source(srdf_path: Path) -> SrdfSource:
|
||||
resolved_path = srdf_path.resolve()
|
||||
if resolved_path.suffix.lower() != SRDF_SUFFIX:
|
||||
raise SrdfSourceError(f"{resolved_path} is not an SRDF source file")
|
||||
try:
|
||||
root = ET.fromstring(resolved_path.read_text(encoding="utf-8"))
|
||||
except (OSError, ET.ParseError) as exc:
|
||||
raise SrdfSourceError(f"{_relative_to_repo(resolved_path)} could not be parsed as SRDF XML") from exc
|
||||
return parse_srdf_root(root, source_path=resolved_path)
|
||||
|
||||
|
||||
def parse_srdf_xml(xml_text: str, *, source_path: Path) -> SrdfSource:
|
||||
try:
|
||||
root = ET.fromstring(xml_text)
|
||||
except ET.ParseError as exc:
|
||||
raise SrdfSourceError(f"{_relative_to_repo(source_path)} could not be parsed as SRDF XML") from exc
|
||||
return parse_srdf_root(root, source_path=source_path)
|
||||
|
||||
|
||||
def parse_srdf_root(root: ET.Element, *, source_path: Path) -> SrdfSource:
|
||||
if root.tag != "robot":
|
||||
raise SrdfSourceError(f"{_relative_to_repo(source_path)} root element must be <robot>")
|
||||
robot_name = str(root.attrib.get("name") or "").strip()
|
||||
if not robot_name:
|
||||
raise SrdfSourceError(f"{_relative_to_repo(source_path)} robot name is required")
|
||||
urdf_ref = _linked_urdf_ref(root)
|
||||
|
||||
planning_groups: list[SrdfPlanningGroup] = []
|
||||
group_names: list[str] = []
|
||||
for group_element in root.findall("group"):
|
||||
name = str(group_element.attrib.get("name") or "").strip()
|
||||
if not name:
|
||||
raise SrdfSourceError(f"{_relative_to_repo(source_path)} group name is required")
|
||||
group_names.append(name)
|
||||
planning_groups.append(
|
||||
SrdfPlanningGroup(
|
||||
name=name,
|
||||
joint_names=_child_names(group_element, "joint"),
|
||||
link_names=_child_names(group_element, "link"),
|
||||
chains=tuple(
|
||||
SrdfChain(
|
||||
base_link=str(chain.attrib.get("base_link") or "").strip(),
|
||||
tip_link=str(chain.attrib.get("tip_link") or "").strip(),
|
||||
)
|
||||
for chain in group_element.findall("chain")
|
||||
),
|
||||
subgroups=_child_names(group_element, "group"),
|
||||
)
|
||||
)
|
||||
_raise_on_duplicates(group_names, source_path=source_path, label="group")
|
||||
|
||||
end_effectors: list[SrdfEndEffector] = []
|
||||
end_effector_names: list[str] = []
|
||||
for element in root.findall("end_effector"):
|
||||
name = str(element.attrib.get("name") or "").strip()
|
||||
if not name:
|
||||
raise SrdfSourceError(f"{_relative_to_repo(source_path)} end_effector name is required")
|
||||
end_effector_names.append(name)
|
||||
end_effectors.append(
|
||||
SrdfEndEffector(
|
||||
name=name,
|
||||
parent_link=str(element.attrib.get("parent_link") or "").strip(),
|
||||
group=str(element.attrib.get("group") or "").strip(),
|
||||
parent_group=str(element.attrib.get("parent_group") or "").strip(),
|
||||
)
|
||||
)
|
||||
_raise_on_duplicates(end_effector_names, source_path=source_path, label="end_effector")
|
||||
|
||||
group_states: list[SrdfGroupState] = []
|
||||
group_state_keys: list[str] = []
|
||||
for element in root.findall("group_state"):
|
||||
name = str(element.attrib.get("name") or "").strip()
|
||||
group = str(element.attrib.get("group") or "").strip()
|
||||
if not name or not group:
|
||||
raise SrdfSourceError(f"{_relative_to_repo(source_path)} group_state requires name and group")
|
||||
group_state_keys.append(f"{group}/{name}")
|
||||
joint_values: dict[str, float] = {}
|
||||
joint_names: list[str] = []
|
||||
for joint_element in element.findall("joint"):
|
||||
joint_name = str(joint_element.attrib.get("name") or "").strip()
|
||||
if not joint_name:
|
||||
raise SrdfSourceError(f"{_relative_to_repo(source_path)} group_state joint name is required")
|
||||
joint_names.append(joint_name)
|
||||
try:
|
||||
joint_value = float(joint_element.attrib.get("value"))
|
||||
except (TypeError, ValueError) as exc:
|
||||
raise SrdfSourceError(
|
||||
f"{_relative_to_repo(source_path)} group_state {name!r} joint {joint_name!r} value must be numeric"
|
||||
) from exc
|
||||
if not isfinite(joint_value):
|
||||
raise SrdfSourceError(
|
||||
f"{_relative_to_repo(source_path)} group_state {name!r} joint {joint_name!r} value must be finite"
|
||||
)
|
||||
joint_values[joint_name] = joint_value
|
||||
_raise_on_duplicates(joint_names, source_path=source_path, label=f"group_state {name!r} joint")
|
||||
group_states.append(SrdfGroupState(name=name, group=group, joint_values_by_name=joint_values))
|
||||
_raise_on_duplicates(group_state_keys, source_path=source_path, label="group_state")
|
||||
|
||||
disabled_pairs: list[SrdfDisabledCollisionPair] = []
|
||||
pair_keys: list[str] = []
|
||||
for element in root.findall("disable_collisions"):
|
||||
link1 = str(element.attrib.get("link1") or "").strip()
|
||||
link2 = str(element.attrib.get("link2") or "").strip()
|
||||
if not link1 or not link2:
|
||||
raise SrdfSourceError(f"{_relative_to_repo(source_path)} disable_collisions requires link1 and link2")
|
||||
if link1 == link2:
|
||||
raise SrdfSourceError(f"{_relative_to_repo(source_path)} disable_collisions cannot repeat the same link")
|
||||
reason = str(element.attrib.get("reason") or "").strip()
|
||||
if not reason:
|
||||
raise SrdfSourceError(
|
||||
f"{_relative_to_repo(source_path)} disable_collisions for {link1!r}/{link2!r} requires a reason"
|
||||
)
|
||||
pair_key = "/".join(sorted((link1, link2)))
|
||||
pair_keys.append(pair_key)
|
||||
disabled_pairs.append(
|
||||
SrdfDisabledCollisionPair(
|
||||
link1=link1,
|
||||
link2=link2,
|
||||
reason=reason,
|
||||
source=_disabled_collision_source(reason),
|
||||
)
|
||||
)
|
||||
_raise_on_duplicates(pair_keys, source_path=source_path, label="disable_collisions")
|
||||
|
||||
return SrdfSource(
|
||||
file_ref=file_ref_from_srdf_path(source_path),
|
||||
source_path=source_path.resolve(),
|
||||
robot_name=robot_name,
|
||||
urdf_ref=urdf_ref,
|
||||
planning_groups=tuple(planning_groups),
|
||||
end_effectors=tuple(end_effectors),
|
||||
group_states=tuple(group_states),
|
||||
disabled_collision_pairs=tuple(disabled_pairs),
|
||||
)
|
||||
|
||||
|
||||
def _local_name(tag: str) -> str:
|
||||
return str(tag or "").rsplit("}", 1)[-1].split(":", 1)[-1]
|
||||
|
||||
|
||||
def _linked_urdf_ref(root: ET.Element) -> str:
|
||||
for child in list(root):
|
||||
if _local_name(child.tag) != "urdf":
|
||||
continue
|
||||
tag = str(child.tag or "")
|
||||
if (
|
||||
tag.startswith(f"{{{SRDF_METADATA_NAMESPACE}}}")
|
||||
or tag.startswith(f"{{{LEGACY_EXPLORER_NAMESPACE}}}")
|
||||
or tag.startswith("tcad:")
|
||||
or tag.startswith("explorer:")
|
||||
):
|
||||
return str(child.attrib.get("path") or "").strip()
|
||||
return ""
|
||||
|
||||
|
||||
def _child_names(element: ET.Element, tag: str) -> tuple[str, ...]:
|
||||
names: list[str] = []
|
||||
for child in element.findall(tag):
|
||||
name = str(child.attrib.get("name") or "").strip()
|
||||
if name:
|
||||
names.append(name)
|
||||
return tuple(names)
|
||||
|
||||
|
||||
def _raise_on_duplicates(names: list[str], *, source_path: Path, label: str) -> None:
|
||||
seen = set()
|
||||
duplicates = []
|
||||
for name in names:
|
||||
if name in seen:
|
||||
duplicates.append(name)
|
||||
seen.add(name)
|
||||
if duplicates:
|
||||
raise SrdfSourceError(
|
||||
f"{_relative_to_repo(source_path)} has duplicate {label} name(s): {sorted(set(duplicates))!r}"
|
||||
)
|
||||
|
||||
|
||||
def _disabled_collision_source(
|
||||
reason: str,
|
||||
) -> Literal["adjacent", "sampled", "manual", "setup_assistant", "assumed"]:
|
||||
normalized = str(reason or "").strip().lower()
|
||||
if "adjacent" in normalized:
|
||||
return "adjacent"
|
||||
if any(token in normalized for token in ("never", "always", "sample", "default")):
|
||||
return "sampled"
|
||||
if "setup" in normalized or "assistant" in normalized:
|
||||
return "setup_assistant"
|
||||
if "assum" in normalized:
|
||||
return "assumed"
|
||||
return "manual"
|
||||
|
||||
|
||||
def _relative_to_repo(path: Path) -> str:
|
||||
resolved = path.resolve()
|
||||
try:
|
||||
return resolved.relative_to(Path.cwd().resolve()).as_posix()
|
||||
except ValueError:
|
||||
return resolved.as_posix()
|
||||
Reference in New Issue
Block a user