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 @@
|
||||
"""SDFormat/SDF 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 sdf.cli import main
|
||||
else:
|
||||
from .cli import main
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,405 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from collections.abc import Callable, Mapping, Sequence
|
||||
import math
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
Number = int | float
|
||||
PoseValues = Sequence[Number] | ET.Element | None
|
||||
|
||||
def fmt_float(value: object) -> str:
|
||||
number = _finite_float(value, "value")
|
||||
if number == 0:
|
||||
return "0"
|
||||
if 1e-4 <= abs(number) < 1e9:
|
||||
text_value = f"{number:.12f}".rstrip("0").rstrip(".")
|
||||
return text_value or "0"
|
||||
return f"{number:.12g}"
|
||||
|
||||
|
||||
def fmt_vector(values: Sequence[object]) -> str:
|
||||
return " ".join(fmt_float(value) for value in values)
|
||||
|
||||
|
||||
def text(parent: ET.Element, tag: str, value: object, attrib: Mapping[str, object] | None = None) -> ET.Element:
|
||||
child = ET.SubElement(parent, tag, _string_attrib(attrib))
|
||||
child.text = str(value)
|
||||
return child
|
||||
|
||||
|
||||
def sdf_root(version: str = "1.12") -> ET.Element:
|
||||
version_text = str(version or "").strip()
|
||||
if not version_text:
|
||||
raise ValueError("SDF version must be non-empty")
|
||||
return ET.Element("sdf", {"version": version_text})
|
||||
|
||||
|
||||
def world(parent: ET.Element, name: str) -> ET.Element:
|
||||
return ET.SubElement(parent, "world", {"name": _required_name(name, "world")})
|
||||
|
||||
|
||||
def model(
|
||||
parent: ET.Element,
|
||||
name: str,
|
||||
*,
|
||||
static: bool | None = None,
|
||||
pose: PoseValues = None,
|
||||
) -> ET.Element:
|
||||
model_element = ET.SubElement(parent, "model", {"name": _required_name(name, "model")})
|
||||
if static is not None:
|
||||
text(model_element, "static", "true" if static else "false")
|
||||
_append_pose(model_element, pose)
|
||||
return model_element
|
||||
|
||||
|
||||
def frame(
|
||||
parent: ET.Element,
|
||||
name: str,
|
||||
*,
|
||||
attached_to: str | None = None,
|
||||
pose: PoseValues = None,
|
||||
) -> ET.Element:
|
||||
attrib = {"name": _required_name(name, "frame")}
|
||||
if attached_to:
|
||||
attrib["attached_to"] = str(attached_to)
|
||||
frame_element = ET.SubElement(parent, "frame", attrib)
|
||||
_append_pose(frame_element, pose)
|
||||
return frame_element
|
||||
|
||||
|
||||
def link(parent: ET.Element, name: str, *, pose: PoseValues = None, inertial: Mapping[str, object] | None = None) -> ET.Element:
|
||||
link_element = ET.SubElement(parent, "link", {"name": _required_name(name, "link")})
|
||||
_append_pose(link_element, pose)
|
||||
if inertial:
|
||||
inertial_element = inertial_helper(
|
||||
link_element,
|
||||
inertial["mass"],
|
||||
inertial["inertia"],
|
||||
pose=inertial.get("pose"),
|
||||
)
|
||||
del inertial_element
|
||||
return link_element
|
||||
|
||||
|
||||
def joint(
|
||||
parent: ET.Element,
|
||||
name: str,
|
||||
joint_type: str,
|
||||
parent_link: str,
|
||||
child_link: str,
|
||||
*,
|
||||
pose: PoseValues = None,
|
||||
axis_xyz: Sequence[object] | None = None,
|
||||
axis_expressed_in: str | None = None,
|
||||
axis2_xyz: Sequence[object] | None = None,
|
||||
limits: Mapping[str, object] | None = None,
|
||||
) -> ET.Element:
|
||||
joint_element = ET.SubElement(
|
||||
parent,
|
||||
"joint",
|
||||
{
|
||||
"name": _required_name(name, "joint"),
|
||||
"type": _required_name(joint_type, "joint type"),
|
||||
},
|
||||
)
|
||||
text(joint_element, "parent", _required_name(parent_link, "joint parent"))
|
||||
text(joint_element, "child", _required_name(child_link, "joint child"))
|
||||
_append_pose(joint_element, pose)
|
||||
if axis_xyz is not None:
|
||||
axis_element = axis(joint_element, axis_xyz, expressed_in=axis_expressed_in)
|
||||
if limits:
|
||||
_limit(axis_element, limits)
|
||||
if axis2_xyz is not None:
|
||||
axis2_element = axis(joint_element, axis2_xyz, expressed_in=axis_expressed_in, tag="axis2")
|
||||
if limits and "axis2" in limits and isinstance(limits["axis2"], Mapping):
|
||||
_limit(axis2_element, limits["axis2"])
|
||||
return joint_element
|
||||
|
||||
|
||||
def pose(
|
||||
parent: ET.Element,
|
||||
xyz: Sequence[object] = (0, 0, 0),
|
||||
rpy: Sequence[object] = (0, 0, 0),
|
||||
*,
|
||||
relative_to: str | None = None,
|
||||
rotation_format: str = "euler_rpy",
|
||||
degrees: bool = False,
|
||||
) -> ET.Element:
|
||||
if rotation_format != "euler_rpy":
|
||||
raise ValueError("pose() supports rotation_format='euler_rpy'; use quat_pose() for quaternions")
|
||||
values = [*_vector(xyz, 3, "pose xyz"), *_vector(rpy, 3, "pose rpy")]
|
||||
attrib: dict[str, object] = {}
|
||||
if relative_to:
|
||||
attrib["relative_to"] = str(relative_to)
|
||||
if degrees:
|
||||
attrib["degrees"] = "true"
|
||||
return text(parent, "pose", fmt_vector(values), attrib)
|
||||
|
||||
|
||||
def quat_pose(
|
||||
parent: ET.Element,
|
||||
xyz: Sequence[object] = (0, 0, 0),
|
||||
quat_xyzw: Sequence[object] = (0, 0, 0, 1),
|
||||
*,
|
||||
relative_to: str | None = None,
|
||||
) -> ET.Element:
|
||||
quat = _vector(quat_xyzw, 4, "pose quaternion")
|
||||
if _norm(quat) == 0:
|
||||
raise ValueError("pose quaternion must be nonzero")
|
||||
attrib: dict[str, object] = {"rotation_format": "quat_xyzw"}
|
||||
if relative_to:
|
||||
attrib["relative_to"] = str(relative_to)
|
||||
return text(parent, "pose", fmt_vector([*_vector(xyz, 3, "pose xyz"), *quat]), attrib)
|
||||
|
||||
|
||||
def axis(
|
||||
parent: ET.Element,
|
||||
xyz: Sequence[object] = (0, 0, 1),
|
||||
*,
|
||||
expressed_in: str | None = None,
|
||||
tag: str = "axis",
|
||||
) -> ET.Element:
|
||||
values = _vector(xyz, 3, f"{tag} xyz")
|
||||
if _norm(values) == 0:
|
||||
raise ValueError(f"{tag} vector must be nonzero")
|
||||
axis_element = ET.SubElement(parent, tag)
|
||||
attrib = {"expressed_in": str(expressed_in)} if expressed_in else None
|
||||
text(axis_element, "xyz", fmt_vector(values), attrib)
|
||||
return axis_element
|
||||
|
||||
|
||||
def visual(
|
||||
parent: ET.Element,
|
||||
name: str,
|
||||
*,
|
||||
pose: PoseValues = None,
|
||||
geometry_builder: Callable[[ET.Element], object] | None = None,
|
||||
) -> ET.Element:
|
||||
owner = ET.SubElement(parent, "visual", {"name": _required_name(name, "visual")})
|
||||
_append_pose(owner, pose)
|
||||
_call_geometry_builder(owner, geometry_builder)
|
||||
return owner
|
||||
|
||||
|
||||
def collision(
|
||||
parent: ET.Element,
|
||||
name: str,
|
||||
*,
|
||||
pose: PoseValues = None,
|
||||
geometry_builder: Callable[[ET.Element], object] | None = None,
|
||||
) -> ET.Element:
|
||||
owner = ET.SubElement(parent, "collision", {"name": _required_name(name, "collision")})
|
||||
_append_pose(owner, pose)
|
||||
_call_geometry_builder(owner, geometry_builder)
|
||||
return owner
|
||||
|
||||
|
||||
def box(parent: ET.Element, size_xyz: Sequence[object]) -> ET.Element:
|
||||
values = _positive_vector(size_xyz, 3, "box size")
|
||||
box_element = ET.SubElement(_geometry_parent(parent), "box")
|
||||
text(box_element, "size", fmt_vector(values))
|
||||
return box_element
|
||||
|
||||
|
||||
def sphere(parent: ET.Element, radius: object) -> ET.Element:
|
||||
sphere_element = ET.SubElement(_geometry_parent(parent), "sphere")
|
||||
text(sphere_element, "radius", fmt_float(_positive_float(radius, "sphere radius")))
|
||||
return sphere_element
|
||||
|
||||
|
||||
def cylinder(parent: ET.Element, radius: object, length: object) -> ET.Element:
|
||||
cylinder_element = ET.SubElement(_geometry_parent(parent), "cylinder")
|
||||
text(cylinder_element, "radius", fmt_float(_positive_float(radius, "cylinder radius")))
|
||||
text(cylinder_element, "length", fmt_float(_positive_float(length, "cylinder length")))
|
||||
return cylinder_element
|
||||
|
||||
|
||||
def capsule(parent: ET.Element, radius: object, length: object) -> ET.Element:
|
||||
capsule_element = ET.SubElement(_geometry_parent(parent), "capsule")
|
||||
text(capsule_element, "radius", fmt_float(_positive_float(radius, "capsule radius")))
|
||||
text(capsule_element, "length", fmt_float(_positive_float(length, "capsule length")))
|
||||
return capsule_element
|
||||
|
||||
|
||||
def mesh(parent: ET.Element, uri: str, *, scale: Sequence[object] | None = None) -> ET.Element:
|
||||
uri_text = str(uri or "").strip()
|
||||
if not uri_text:
|
||||
raise ValueError("mesh uri must be non-empty")
|
||||
mesh_element = ET.SubElement(_geometry_parent(parent), "mesh")
|
||||
text(mesh_element, "uri", uri_text)
|
||||
if scale is not None:
|
||||
text(mesh_element, "scale", fmt_vector(_positive_vector(scale, 3, "mesh scale")))
|
||||
return mesh_element
|
||||
|
||||
|
||||
def inertial(parent: ET.Element, mass: object, inertia: Mapping[str, object] | Sequence[object], *, pose: PoseValues = None) -> ET.Element:
|
||||
return inertial_helper(parent, mass, inertia, pose=pose)
|
||||
|
||||
|
||||
def sensor(
|
||||
parent: ET.Element,
|
||||
name: str,
|
||||
sensor_type: str,
|
||||
*,
|
||||
pose: PoseValues = None,
|
||||
topic: str | None = None,
|
||||
update_rate: object | None = None,
|
||||
) -> ET.Element:
|
||||
sensor_element = ET.SubElement(
|
||||
parent,
|
||||
"sensor",
|
||||
{"name": _required_name(name, "sensor"), "type": _required_name(sensor_type, "sensor type")},
|
||||
)
|
||||
_append_pose(sensor_element, pose)
|
||||
if topic:
|
||||
text(sensor_element, "topic", topic)
|
||||
if update_rate is not None:
|
||||
rate = _finite_float(update_rate, "sensor update_rate")
|
||||
if rate < 0:
|
||||
raise ValueError("sensor update_rate must be non-negative")
|
||||
text(sensor_element, "update_rate", fmt_float(rate))
|
||||
return sensor_element
|
||||
|
||||
|
||||
def plugin(
|
||||
parent: ET.Element,
|
||||
name: str,
|
||||
filename: str,
|
||||
params: Mapping[str, object] | None = None,
|
||||
**extra_params: object,
|
||||
) -> ET.Element:
|
||||
plugin_element = ET.SubElement(
|
||||
parent,
|
||||
"plugin",
|
||||
{
|
||||
"name": _required_name(name, "plugin"),
|
||||
"filename": _required_name(filename, "plugin filename"),
|
||||
},
|
||||
)
|
||||
for key, value in {**dict(params or {}), **extra_params}.items():
|
||||
text(plugin_element, str(key), value)
|
||||
return plugin_element
|
||||
|
||||
|
||||
def include(parent: ET.Element, uri: str, *, name: str | None = None, pose: PoseValues = None) -> ET.Element:
|
||||
include_element = ET.SubElement(parent, "include")
|
||||
text(include_element, "uri", _required_name(uri, "include uri"))
|
||||
if name:
|
||||
text(include_element, "name", name)
|
||||
_append_pose(include_element, pose)
|
||||
return include_element
|
||||
|
||||
|
||||
def inertial_helper(
|
||||
parent: ET.Element,
|
||||
mass: object,
|
||||
inertia: Mapping[str, object] | Sequence[object],
|
||||
*,
|
||||
pose: PoseValues = None,
|
||||
) -> ET.Element:
|
||||
inertial_element = ET.SubElement(parent, "inertial")
|
||||
_append_pose(inertial_element, pose)
|
||||
text(inertial_element, "mass", fmt_float(_positive_float(mass, "inertial mass")))
|
||||
inertia_element = ET.SubElement(inertial_element, "inertia")
|
||||
for key, value in _inertia_items(inertia).items():
|
||||
text(inertia_element, key, fmt_float(_finite_float(value, f"inertia {key}")))
|
||||
return inertial_element
|
||||
|
||||
|
||||
def _limit(parent: ET.Element, values: Mapping[str, object]) -> ET.Element:
|
||||
limit_element = ET.SubElement(parent, "limit")
|
||||
for key in ("lower", "upper", "effort", "velocity"):
|
||||
if key in values:
|
||||
text(limit_element, key, fmt_float(_finite_float(values[key], f"limit {key}")))
|
||||
return limit_element
|
||||
|
||||
|
||||
def _append_pose(parent: ET.Element, value: PoseValues) -> None:
|
||||
if value is None:
|
||||
return
|
||||
if isinstance(value, ET.Element):
|
||||
parent.append(value)
|
||||
return
|
||||
if len(value) == 6:
|
||||
pose(parent, xyz=value[:3], rpy=value[3:])
|
||||
return
|
||||
if len(value) == 7:
|
||||
quat_pose(parent, xyz=value[:3], quat_xyzw=value[3:])
|
||||
return
|
||||
raise ValueError("pose values must contain 6 Euler values or 7 quaternion values")
|
||||
|
||||
|
||||
def _call_geometry_builder(
|
||||
owner: ET.Element,
|
||||
geometry_builder: Callable[[ET.Element], object] | None,
|
||||
) -> None:
|
||||
if geometry_builder is None:
|
||||
return
|
||||
geometry_builder(_geometry_parent(owner))
|
||||
|
||||
|
||||
def _geometry_parent(parent: ET.Element) -> ET.Element:
|
||||
if _local_name(parent.tag) == "geometry":
|
||||
return parent
|
||||
if _local_name(parent.tag) in {"visual", "collision"}:
|
||||
existing = next((child for child in list(parent) if _local_name(child.tag) == "geometry"), None)
|
||||
return existing if existing is not None else ET.SubElement(parent, "geometry")
|
||||
return ET.SubElement(parent, "geometry")
|
||||
|
||||
|
||||
def _inertia_items(inertia: Mapping[str, object] | Sequence[object]) -> dict[str, object]:
|
||||
keys = ("ixx", "iyy", "izz", "ixy", "ixz", "iyz")
|
||||
if isinstance(inertia, Mapping):
|
||||
missing = [key for key in keys if key not in inertia]
|
||||
if missing:
|
||||
raise ValueError(f"inertia is missing component(s): {', '.join(missing)}")
|
||||
return {key: inertia[key] for key in keys}
|
||||
if len(inertia) != 6:
|
||||
raise ValueError("inertia sequence must contain ixx, iyy, izz, ixy, ixz, iyz")
|
||||
return dict(zip(keys, inertia))
|
||||
|
||||
|
||||
def _vector(values: Sequence[object], expected_len: int, label: str) -> list[float]:
|
||||
if len(values) != expected_len:
|
||||
raise ValueError(f"{label} must contain {expected_len} values")
|
||||
return [_finite_float(value, label) for value in values]
|
||||
|
||||
|
||||
def _positive_vector(values: Sequence[object], expected_len: int, label: str) -> list[float]:
|
||||
return [_positive_float(value, label) for value in _vector(values, expected_len, label)]
|
||||
|
||||
|
||||
def _finite_float(value: object, label: str) -> float:
|
||||
try:
|
||||
number = float(value)
|
||||
except (TypeError, ValueError) as exc:
|
||||
raise ValueError(f"{label} must be numeric") from exc
|
||||
if not math.isfinite(number):
|
||||
raise ValueError(f"{label} must be finite")
|
||||
return number
|
||||
|
||||
|
||||
def _positive_float(value: object, label: str) -> float:
|
||||
number = _finite_float(value, label)
|
||||
if number <= 0:
|
||||
raise ValueError(f"{label} must be positive")
|
||||
return number
|
||||
|
||||
|
||||
def _norm(values: Sequence[float]) -> float:
|
||||
return math.sqrt(sum(value * value for value in values))
|
||||
|
||||
|
||||
def _required_name(value: object, label: str) -> str:
|
||||
text_value = str(value or "").strip()
|
||||
if not text_value:
|
||||
raise ValueError(f"{label} must be non-empty")
|
||||
return text_value
|
||||
|
||||
|
||||
def _string_attrib(attrib: Mapping[str, object] | None) -> dict[str, str]:
|
||||
return {str(key): str(value) for key, value in dict(attrib or {}).items() if value is not None}
|
||||
|
||||
|
||||
def _local_name(tag: object) -> str:
|
||||
return str(tag).rsplit("}", 1)[-1]
|
||||
@@ -0,0 +1,387 @@
|
||||
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 sdf.external import run_gz_sdf_check
|
||||
from sdf.findings import Finding, ValidationResult, format_findings
|
||||
from sdf.source import SdfSourceError
|
||||
from sdf.validation import validate_sdf_xml
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _TargetSpec:
|
||||
source_path: Path
|
||||
output_path: Path
|
||||
|
||||
|
||||
def generate_sdf_targets(
|
||||
targets: Sequence[str],
|
||||
*,
|
||||
output: str | Path | None = None,
|
||||
gz_check: str = "auto",
|
||||
strict: bool = False,
|
||||
) -> 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,
|
||||
gz_check=gz_check,
|
||||
strict=strict,
|
||||
)
|
||||
return 0
|
||||
|
||||
|
||||
def main(argv: Sequence[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(
|
||||
prog="sdf",
|
||||
description="Generate explicit SDFormat/SDF targets from Python sources.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"targets",
|
||||
nargs="+",
|
||||
help="Explicit Python source file or SOURCE.py=OUTPUT.sdf pair defining gen_sdf() to generate.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"-o",
|
||||
"--output",
|
||||
metavar="PATH",
|
||||
help="Write the generated SDF file to this path. Valid only with one plain Python target.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--gz-check",
|
||||
choices=("auto", "required", "never"),
|
||||
default="auto",
|
||||
help="Optionally run 'gz sdf --check' before writing output.",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--strict",
|
||||
action="store_true",
|
||||
help="Treat bundled validation warnings and generator envelope warnings as failures.",
|
||||
)
|
||||
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_sdf_targets(args.targets, output=args.output, gz_check=args.gz_check, strict=args.strict)
|
||||
|
||||
|
||||
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("sdf --output cannot be combined with SOURCE=OUTPUT targets")
|
||||
if output is not None and len(targets) != 1:
|
||||
raise ValueError("sdf --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(".sdf")
|
||||
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("sdf 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("sdf output must be a non-empty path")
|
||||
if "\\" in value:
|
||||
raise ValueError("sdf 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() != ".sdf":
|
||||
raise ValueError("sdf output must end in .sdf")
|
||||
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"sdf 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, gz_check: str, strict: bool) -> 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_sdf",
|
||||
outputs=[GenerationOutput(output_path, "sdf")],
|
||||
):
|
||||
return _generate_target_inner(script_path, output_path=output_path, gz_check=gz_check, strict=strict)
|
||||
|
||||
|
||||
def _generate_target_inner(script_path: Path, *, output_path: Path, gz_check: str, strict: bool) -> Path:
|
||||
|
||||
module = _load_generator_module(script_path)
|
||||
generator = getattr(module, "gen_sdf", None)
|
||||
if not callable(generator):
|
||||
raise RuntimeError(f"{_display_path(script_path)} does not define callable gen_sdf()")
|
||||
if inspect.signature(generator).parameters:
|
||||
raise ValueError(f"{_display_path(script_path)} gen_sdf() must not accept arguments")
|
||||
|
||||
payload = _normalize_sdf_payload(generator(), script_path=script_path)
|
||||
xml_text = _payload_xml(payload, script_path=script_path)
|
||||
|
||||
validation = validate_sdf_xml(
|
||||
xml_text,
|
||||
source_path=output_path,
|
||||
base_dir=output_path.parent,
|
||||
metadata=payload.get("metadata") if isinstance(payload.get("metadata"), dict) else None,
|
||||
)
|
||||
validation.warnings.extend(_payload_warning_findings(payload, script_path=script_path))
|
||||
_raise_for_failed_generation(validation, strict=strict)
|
||||
|
||||
external = run_gz_sdf_check(xml_text, output_path=output_path, mode=gz_check)
|
||||
validation.extend(external)
|
||||
_raise_for_failed_generation(validation, strict=strict)
|
||||
|
||||
_write_sdf_payload(payload, output_path=output_path, script_path=script_path)
|
||||
_print_generation_report(payload, validation)
|
||||
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 = (
|
||||
"_sdf_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_sdf_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_sdf() must return an SDF XML root element, XML string, "
|
||||
"or generator envelope dict"
|
||||
)
|
||||
allowed_fields = {"xml", "metadata", "assumptions", "warnings"}
|
||||
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_sdf() envelope has unsupported field(s): {joined}")
|
||||
if "xml" not in raw_payload:
|
||||
raise TypeError(f"{_display_path(script_path)} gen_sdf() envelope must define 'xml'")
|
||||
payload = dict(raw_payload)
|
||||
payload["xml"] = _normalize_xml_value(payload["xml"], script_path=script_path, label="gen_sdf() envelope field 'xml'")
|
||||
payload["metadata"] = _normalize_metadata(payload.get("metadata"), script_path=script_path)
|
||||
payload["assumptions"] = _normalize_report_items(payload.get("assumptions"), field="assumptions", script_path=script_path)
|
||||
payload["warnings"] = _normalize_report_items(payload.get("warnings"), field="warnings", script_path=script_path)
|
||||
return payload
|
||||
|
||||
|
||||
def _write_sdf_payload(payload: dict[str, object], *, output_path: Path, script_path: Path) -> None:
|
||||
xml = _payload_xml(payload, script_path=script_path)
|
||||
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 SDF: {output_path}")
|
||||
|
||||
|
||||
def _payload_xml(payload: dict[str, object], *, script_path: Path) -> str:
|
||||
xml = payload.get("xml")
|
||||
if not isinstance(xml, str):
|
||||
raise TypeError(
|
||||
f"{_display_path(script_path)} gen_sdf() envelope field 'xml' must be a string, "
|
||||
f"got {type(xml).__name__}"
|
||||
)
|
||||
return xml
|
||||
|
||||
|
||||
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 _normalize_metadata(raw_metadata: object, *, script_path: Path) -> dict[str, object]:
|
||||
if raw_metadata is None:
|
||||
return {}
|
||||
if not isinstance(raw_metadata, dict):
|
||||
raise TypeError(f"{_display_path(script_path)} gen_sdf() envelope field 'metadata' must be a dict")
|
||||
metadata: dict[str, object] = {}
|
||||
for key, value in raw_metadata.items():
|
||||
if not isinstance(value, (str, int, float, bool, type(None))):
|
||||
raise TypeError(
|
||||
f"{_display_path(script_path)} gen_sdf() metadata value for {key!r} must be a scalar"
|
||||
)
|
||||
metadata[str(key)] = value
|
||||
return metadata
|
||||
|
||||
|
||||
def _normalize_report_items(raw_items: object, *, field: str, script_path: Path) -> list[dict[str, str]]:
|
||||
if raw_items is None:
|
||||
return []
|
||||
if isinstance(raw_items, (str, bytes)) or not isinstance(raw_items, Sequence):
|
||||
raise TypeError(f"{_display_path(script_path)} gen_sdf() envelope field '{field}' must be a list")
|
||||
normalized: list[dict[str, str]] = []
|
||||
default_code = "generator_assumption" if field == "assumptions" else "generator_warning"
|
||||
for item in raw_items:
|
||||
if isinstance(item, str):
|
||||
message = item.strip()
|
||||
code = default_code
|
||||
source = ""
|
||||
elif isinstance(item, dict):
|
||||
message = str(item.get("message") or "").strip()
|
||||
code = str(item.get("code") or default_code).strip()
|
||||
source = str(item.get("source") or "").strip()
|
||||
else:
|
||||
raise TypeError(
|
||||
f"{_display_path(script_path)} gen_sdf() envelope field '{field}' entries must be strings or dicts"
|
||||
)
|
||||
if not message:
|
||||
raise TypeError(f"{_display_path(script_path)} gen_sdf() envelope field '{field}' has an empty message")
|
||||
normalized.append({"code": code, "message": message, "source": source})
|
||||
return normalized
|
||||
|
||||
|
||||
def _payload_warning_findings(payload: dict[str, object], *, script_path: Path) -> list[Finding]:
|
||||
warnings = payload.get("warnings")
|
||||
if not isinstance(warnings, list):
|
||||
return []
|
||||
findings: list[Finding] = []
|
||||
for item in warnings:
|
||||
if not isinstance(item, dict):
|
||||
continue
|
||||
source = item.get("source")
|
||||
findings.append(
|
||||
Finding(
|
||||
severity="warning",
|
||||
code=str(item.get("code") or "generator_warning"),
|
||||
message=str(item.get("message") or ""),
|
||||
path=_display_path(script_path),
|
||||
hint=f"Source: {source}" if source else None,
|
||||
)
|
||||
)
|
||||
return findings
|
||||
|
||||
|
||||
def _raise_for_failed_generation(validation: ValidationResult, *, strict: bool) -> None:
|
||||
findings = validation.errors + (validation.warnings if strict else [])
|
||||
if findings:
|
||||
raise SdfSourceError(format_findings(findings))
|
||||
|
||||
|
||||
def _print_generation_report(payload: dict[str, object], validation: ValidationResult) -> None:
|
||||
assumptions = payload.get("assumptions")
|
||||
if isinstance(assumptions, list):
|
||||
for item in assumptions:
|
||||
if not isinstance(item, dict):
|
||||
continue
|
||||
source = f" Source: {item['source']}." if item.get("source") else ""
|
||||
message = str(item["message"])
|
||||
ending = "" if message.endswith(".") else "."
|
||||
print(f"Assumption [{item['code']}]: {message}{ending}{source}")
|
||||
for finding in [*validation.warnings, *validation.infos]:
|
||||
print(finding.format())
|
||||
|
||||
|
||||
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,55 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
import shutil
|
||||
import subprocess
|
||||
import tempfile
|
||||
|
||||
from .findings import ValidationResult
|
||||
|
||||
GzCheckMode = str
|
||||
|
||||
|
||||
def run_gz_sdf_check(xml_text: str, *, output_path: Path, mode: GzCheckMode = "auto") -> ValidationResult:
|
||||
result = ValidationResult()
|
||||
normalized_mode = str(mode or "auto").strip().lower()
|
||||
if normalized_mode not in {"auto", "required", "never"}:
|
||||
raise ValueError("gz_check must be one of: auto, required, never")
|
||||
if normalized_mode == "never":
|
||||
result.add("info", "gz_check_skipped", "gz sdf --check skipped by request")
|
||||
return result
|
||||
|
||||
gz_path = shutil.which("gz")
|
||||
if gz_path is None:
|
||||
severity = "error" if normalized_mode == "required" else "warning"
|
||||
result.add(
|
||||
severity,
|
||||
"gz_check_unavailable",
|
||||
"gz sdf --check skipped because 'gz' is not on PATH",
|
||||
)
|
||||
return result
|
||||
|
||||
output_parent = output_path.resolve().parent
|
||||
output_parent.mkdir(parents=True, exist_ok=True)
|
||||
with tempfile.NamedTemporaryFile("w", encoding="utf-8", suffix=".sdf", dir=output_parent, delete=False) as handle:
|
||||
temp_path = Path(handle.name)
|
||||
handle.write(xml_text if xml_text.endswith("\n") else xml_text + "\n")
|
||||
try:
|
||||
completed = subprocess.run(
|
||||
[gz_path, "sdf", "--check", str(temp_path)],
|
||||
check=False,
|
||||
capture_output=True,
|
||||
text=True,
|
||||
)
|
||||
finally:
|
||||
temp_path.unlink(missing_ok=True)
|
||||
|
||||
if completed.returncode != 0:
|
||||
details = (completed.stderr or completed.stdout or "").strip()
|
||||
message = "gz sdf --check failed"
|
||||
if details:
|
||||
message = f"{message}: {details}"
|
||||
result.add("error", "gz_check_failed", message)
|
||||
else:
|
||||
result.add("info", "gz_check_passed", "gz sdf --check passed")
|
||||
return result
|
||||
@@ -0,0 +1,61 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass, field
|
||||
from typing import Literal
|
||||
|
||||
Severity = Literal["error", "warning", "info"]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class Finding:
|
||||
severity: Severity
|
||||
code: str
|
||||
message: str
|
||||
path: str | None = None
|
||||
hint: str | None = None
|
||||
|
||||
def format(self) -> str:
|
||||
location = f" at {self.path}" if self.path else ""
|
||||
hint = f" Hint: {self.hint}" if self.hint else ""
|
||||
return f"{self.severity}: {self.code}{location}: {self.message}{hint}"
|
||||
|
||||
|
||||
@dataclass
|
||||
class ValidationResult:
|
||||
errors: list[Finding] = field(default_factory=list)
|
||||
warnings: list[Finding] = field(default_factory=list)
|
||||
infos: list[Finding] = field(default_factory=list)
|
||||
|
||||
@property
|
||||
def ok(self) -> bool:
|
||||
return not self.errors
|
||||
|
||||
def add(
|
||||
self,
|
||||
severity: Severity,
|
||||
code: str,
|
||||
message: str,
|
||||
*,
|
||||
path: str | None = None,
|
||||
hint: str | None = None,
|
||||
) -> Finding:
|
||||
finding = Finding(severity=severity, code=code, message=message, path=path, hint=hint)
|
||||
if severity == "error":
|
||||
self.errors.append(finding)
|
||||
elif severity == "warning":
|
||||
self.warnings.append(finding)
|
||||
else:
|
||||
self.infos.append(finding)
|
||||
return finding
|
||||
|
||||
def extend(self, other: ValidationResult) -> None:
|
||||
self.errors.extend(other.errors)
|
||||
self.warnings.extend(other.warnings)
|
||||
self.infos.extend(other.infos)
|
||||
|
||||
def all_findings(self) -> list[Finding]:
|
||||
return [*self.errors, *self.warnings, *self.infos]
|
||||
|
||||
|
||||
def format_findings(findings: list[Finding]) -> str:
|
||||
return "\n".join(finding.format() for finding in findings)
|
||||
@@ -0,0 +1,309 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from urllib.parse import unquote, urlparse
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
from .validation import raise_for_validation_errors, validate_sdf_root
|
||||
|
||||
SDF_SUFFIX = ".sdf"
|
||||
EXTERNAL_URI_SCHEMES = {"model", "package", "http", "https", "fuel"}
|
||||
|
||||
|
||||
class SdfSourceError(ValueError):
|
||||
pass
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SdfJoint:
|
||||
name: str
|
||||
joint_type: str
|
||||
parent_link: str
|
||||
child_link: str
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class SdfSource:
|
||||
file_ref: str
|
||||
source_path: Path
|
||||
version: str
|
||||
model_names: tuple[str, ...]
|
||||
world_names: tuple[str, ...]
|
||||
links: tuple[str, ...]
|
||||
joints: tuple[SdfJoint, ...]
|
||||
mesh_paths: tuple[Path, ...]
|
||||
visual_mesh_paths: tuple[Path, ...] = ()
|
||||
collision_mesh_paths: tuple[Path, ...] = ()
|
||||
|
||||
|
||||
def file_ref_from_sdf_path(sdf_path: Path) -> str:
|
||||
resolved = sdf_path.resolve()
|
||||
if resolved.suffix.lower() != SDF_SUFFIX:
|
||||
raise SdfSourceError(f"{resolved} is not an SDF source file")
|
||||
return _relative_to_repo(resolved)
|
||||
|
||||
|
||||
def read_sdf_source(sdf_path: Path) -> SdfSource:
|
||||
resolved_path = sdf_path.resolve()
|
||||
if resolved_path.suffix.lower() != SDF_SUFFIX:
|
||||
raise SdfSourceError(f"{resolved_path} is not an SDF source file")
|
||||
|
||||
try:
|
||||
root = ET.fromstring(resolved_path.read_text(encoding="utf-8"))
|
||||
except (OSError, ET.ParseError) as exc:
|
||||
raise SdfSourceError(f"{_relative_to_repo(resolved_path)} could not be parsed as SDF XML") from exc
|
||||
return parse_sdf_root(root, source_path=resolved_path, base_dir=resolved_path.parent)
|
||||
|
||||
|
||||
def parse_sdf_xml(xml_text: str, *, source_path: Path, base_dir: Path | None = None) -> SdfSource:
|
||||
try:
|
||||
root = ET.fromstring(xml_text)
|
||||
except ET.ParseError as exc:
|
||||
raise SdfSourceError(f"{_relative_to_repo(source_path)} could not be parsed as SDF XML") from exc
|
||||
return parse_sdf_root(root, source_path=source_path, base_dir=base_dir)
|
||||
|
||||
|
||||
def parse_sdf_root(root: ET.Element, *, source_path: Path, base_dir: Path | None = None) -> SdfSource:
|
||||
resolved_path = source_path.resolve()
|
||||
resolved_base_dir = Path(base_dir).resolve() if base_dir is not None else resolved_path.parent
|
||||
validation = validate_sdf_root(root, source_path=resolved_path, base_dir=resolved_base_dir)
|
||||
raise_for_validation_errors(validation)
|
||||
|
||||
if _local_name(root.tag) != "sdf":
|
||||
raise SdfSourceError(f"{_relative_to_repo(resolved_path)} root element must be <sdf>")
|
||||
version = str(root.attrib.get("version") or "").strip()
|
||||
if not version:
|
||||
raise SdfSourceError(f"{_relative_to_repo(resolved_path)} SDF version is required")
|
||||
|
||||
world_elements = _children(root, "world")
|
||||
world_names = [_required_name(world, source_path=resolved_path, label="world") for world in world_elements]
|
||||
_raise_on_duplicates(world_names, source_path=resolved_path, label="world")
|
||||
|
||||
model_elements = list(_children(root, "model"))
|
||||
for world in world_elements:
|
||||
model_elements.extend(_children(world, "model"))
|
||||
|
||||
model_names: list[str] = []
|
||||
links: list[str] = []
|
||||
joints: list[SdfJoint] = []
|
||||
visual_mesh_paths: list[Path] = []
|
||||
collision_mesh_paths: list[Path] = []
|
||||
|
||||
for model_element in model_elements:
|
||||
model_name = _required_name(model_element, source_path=resolved_path, label="model")
|
||||
model_names.append(model_name)
|
||||
model_links, model_joints, model_visual_meshes, model_collision_meshes = _read_model(
|
||||
model_element,
|
||||
source_path=resolved_path,
|
||||
base_dir=resolved_base_dir,
|
||||
model_name=model_name,
|
||||
)
|
||||
links.extend(model_links)
|
||||
joints.extend(model_joints)
|
||||
visual_mesh_paths.extend(model_visual_meshes)
|
||||
collision_mesh_paths.extend(model_collision_meshes)
|
||||
|
||||
return SdfSource(
|
||||
file_ref=file_ref_from_sdf_path(resolved_path),
|
||||
source_path=resolved_path,
|
||||
version=version,
|
||||
model_names=tuple(model_names),
|
||||
world_names=tuple(world_names),
|
||||
links=tuple(links),
|
||||
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 _read_model(
|
||||
model_element: ET.Element,
|
||||
*,
|
||||
source_path: Path,
|
||||
base_dir: Path,
|
||||
model_name: str,
|
||||
) -> tuple[list[str], list[SdfJoint], list[Path], list[Path]]:
|
||||
link_names = [
|
||||
_required_name(link_element, source_path=source_path, label=f"model {model_name!r} link")
|
||||
for link_element in _children(model_element, "link")
|
||||
]
|
||||
_raise_on_duplicates(link_names, source_path=source_path, label=f"model {model_name!r} link")
|
||||
link_name_set = set(link_names)
|
||||
|
||||
visual_mesh_paths: list[Path] = []
|
||||
collision_mesh_paths: list[Path] = []
|
||||
for link_element in _children(model_element, "link"):
|
||||
visual_mesh_paths.extend(
|
||||
_geometry_mesh_paths(
|
||||
link_element,
|
||||
element_name="visual",
|
||||
source_path=source_path,
|
||||
base_dir=base_dir,
|
||||
)
|
||||
)
|
||||
collision_mesh_paths.extend(
|
||||
_geometry_mesh_paths(
|
||||
link_element,
|
||||
element_name="collision",
|
||||
source_path=source_path,
|
||||
base_dir=base_dir,
|
||||
)
|
||||
)
|
||||
|
||||
joint_names: list[str] = []
|
||||
joints: list[SdfJoint] = []
|
||||
for joint_element in _children(model_element, "joint"):
|
||||
joint_name = _required_name(joint_element, source_path=source_path, label=f"model {model_name!r} joint")
|
||||
joint_names.append(joint_name)
|
||||
joint_type = str(joint_element.attrib.get("type") or "").strip()
|
||||
if not joint_type:
|
||||
raise SdfSourceError(
|
||||
f"{_relative_to_repo(source_path)} model {model_name!r} joint {joint_name!r} type is required"
|
||||
)
|
||||
parent_link = _required_child_text(
|
||||
joint_element,
|
||||
"parent",
|
||||
source_path=source_path,
|
||||
label=f"model {model_name!r} joint {joint_name!r} parent",
|
||||
)
|
||||
child_link = _required_child_text(
|
||||
joint_element,
|
||||
"child",
|
||||
source_path=source_path,
|
||||
label=f"model {model_name!r} joint {joint_name!r} child",
|
||||
)
|
||||
_validate_link_reference(
|
||||
parent_link,
|
||||
link_names=link_name_set,
|
||||
source_path=source_path,
|
||||
context=f"model {model_name!r} joint {joint_name!r} parent",
|
||||
)
|
||||
_validate_link_reference(
|
||||
child_link,
|
||||
link_names=link_name_set,
|
||||
source_path=source_path,
|
||||
context=f"model {model_name!r} joint {joint_name!r} child",
|
||||
allow_world=False,
|
||||
)
|
||||
joints.append(
|
||||
SdfJoint(
|
||||
name=joint_name,
|
||||
joint_type=joint_type,
|
||||
parent_link=parent_link,
|
||||
child_link=child_link,
|
||||
)
|
||||
)
|
||||
_raise_on_duplicates(joint_names, source_path=source_path, label=f"model {model_name!r} joint")
|
||||
|
||||
return link_names, joints, visual_mesh_paths, collision_mesh_paths
|
||||
|
||||
|
||||
def _children(parent: ET.Element, tag_name: str) -> list[ET.Element]:
|
||||
return [child for child in list(parent) if _local_name(child.tag) == tag_name]
|
||||
|
||||
|
||||
def _local_name(tag: str) -> str:
|
||||
return str(tag).rsplit("}", 1)[-1]
|
||||
|
||||
|
||||
def _required_name(element: ET.Element, *, source_path: Path, label: str) -> str:
|
||||
name = str(element.attrib.get("name") or "").strip()
|
||||
if not name:
|
||||
raise SdfSourceError(f"{_relative_to_repo(source_path)} {label} name is required")
|
||||
return name
|
||||
|
||||
|
||||
def _required_child_text(
|
||||
parent: ET.Element,
|
||||
tag_name: str,
|
||||
*,
|
||||
source_path: Path,
|
||||
label: str,
|
||||
) -> str:
|
||||
element = next(iter(_children(parent, tag_name)), None)
|
||||
value = str(element.text if element is not None else "").strip()
|
||||
if not value:
|
||||
raise SdfSourceError(f"{_relative_to_repo(source_path)} {label} is required")
|
||||
return value
|
||||
|
||||
|
||||
def _validate_link_reference(
|
||||
link_ref: str,
|
||||
*,
|
||||
link_names: set[str],
|
||||
source_path: Path,
|
||||
context: str,
|
||||
allow_world: bool = True,
|
||||
) -> None:
|
||||
if allow_world and link_ref == "world":
|
||||
return
|
||||
if "::" in link_ref:
|
||||
return
|
||||
if link_ref not in link_names:
|
||||
raise SdfSourceError(f"{_relative_to_repo(source_path)} {context} references missing link {link_ref!r}")
|
||||
|
||||
|
||||
def _geometry_mesh_paths(
|
||||
link_element: ET.Element,
|
||||
*,
|
||||
element_name: str,
|
||||
source_path: Path,
|
||||
base_dir: Path,
|
||||
) -> list[Path]:
|
||||
mesh_paths: list[Path] = []
|
||||
for geometry_owner in _children(link_element, element_name):
|
||||
geometry_element = next(iter(_children(geometry_owner, "geometry")), None)
|
||||
if geometry_element is None:
|
||||
continue
|
||||
mesh_element = next(iter(_children(geometry_element, "mesh")), None)
|
||||
if mesh_element is None:
|
||||
continue
|
||||
uri = _required_child_text(
|
||||
mesh_element,
|
||||
"uri",
|
||||
source_path=source_path,
|
||||
label=f"{element_name} mesh uri",
|
||||
)
|
||||
mesh_path = _resolve_local_mesh_uri(uri, base_dir=base_dir)
|
||||
if mesh_path is not None:
|
||||
if not mesh_path.is_file():
|
||||
raise SdfSourceError(
|
||||
f"{_relative_to_repo(source_path)} references missing mesh file: {uri!r}"
|
||||
)
|
||||
mesh_paths.append(mesh_path)
|
||||
return mesh_paths
|
||||
|
||||
|
||||
def _resolve_local_mesh_uri(uri: str, *, base_dir: Path) -> Path | None:
|
||||
parsed = urlparse(uri)
|
||||
if parsed.scheme in EXTERNAL_URI_SCHEMES:
|
||||
return None
|
||||
if parsed.scheme and parsed.scheme != "file":
|
||||
return None
|
||||
if parsed.scheme == "file":
|
||||
return Path(unquote(parsed.path)).resolve()
|
||||
return (base_dir / uri).resolve()
|
||||
|
||||
|
||||
def _raise_on_duplicates(values: list[str], *, source_path: Path, label: str) -> None:
|
||||
seen: set[str] = set()
|
||||
duplicates: set[str] = set()
|
||||
for value in values:
|
||||
if value in seen:
|
||||
duplicates.add(value)
|
||||
continue
|
||||
seen.add(value)
|
||||
if duplicates:
|
||||
duplicate_text = ", ".join(repr(item) for item in sorted(duplicates))
|
||||
raise SdfSourceError(
|
||||
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()
|
||||
@@ -0,0 +1,846 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from collections.abc import Mapping
|
||||
import math
|
||||
from pathlib import Path
|
||||
import re
|
||||
from urllib.parse import unquote, urlparse
|
||||
import xml.etree.ElementTree as ET
|
||||
|
||||
from .findings import ValidationResult, format_findings
|
||||
|
||||
EXTERNAL_URI_SCHEMES = {"model", "package", "http", "https", "fuel"}
|
||||
COMMON_JOINT_TYPES = {
|
||||
"ball",
|
||||
"continuous",
|
||||
"fixed",
|
||||
"gearbox",
|
||||
"prismatic",
|
||||
"revolute",
|
||||
"revolute2",
|
||||
"screw",
|
||||
"universal",
|
||||
}
|
||||
SECOND_AXIS_JOINT_TYPES = {"revolute2", "universal"}
|
||||
BOOLEAN_VALUES = {"0", "1", "true", "false", "yes", "no", "on", "off"}
|
||||
TRUE_VALUES = {"1", "true", "yes", "on"}
|
||||
POSE_TOLERANCE = 1e-12
|
||||
UNIT_TOLERANCE = 1e-6
|
||||
PSD_TOLERANCE = 1e-9
|
||||
|
||||
|
||||
def validate_sdf_xml(
|
||||
xml_text: str,
|
||||
*,
|
||||
source_path: Path,
|
||||
base_dir: Path | None = None,
|
||||
metadata: Mapping[str, object] | None = None,
|
||||
) -> ValidationResult:
|
||||
result = ValidationResult()
|
||||
try:
|
||||
root = ET.fromstring(xml_text)
|
||||
except ET.ParseError as exc:
|
||||
result.add(
|
||||
"error",
|
||||
"invalid_xml",
|
||||
f"{_display_path(source_path)} could not be parsed as SDF XML: {exc}",
|
||||
path="/",
|
||||
)
|
||||
return result
|
||||
result.extend(validate_sdf_root(root, source_path=source_path, base_dir=base_dir, metadata=metadata))
|
||||
return result
|
||||
|
||||
|
||||
def validate_sdf_root(
|
||||
root: ET.Element,
|
||||
*,
|
||||
source_path: Path,
|
||||
base_dir: Path | None = None,
|
||||
metadata: Mapping[str, object] | None = None,
|
||||
) -> ValidationResult:
|
||||
del metadata
|
||||
result = ValidationResult()
|
||||
resolved_path = source_path.resolve()
|
||||
resolved_base_dir = Path(base_dir).resolve() if base_dir is not None else resolved_path.parent
|
||||
|
||||
if _local_name(root.tag) != "sdf":
|
||||
result.add(
|
||||
"error",
|
||||
"invalid_root",
|
||||
f"{_display_path(resolved_path)} root element must be <sdf>",
|
||||
path="/",
|
||||
)
|
||||
return result
|
||||
|
||||
version = str(root.attrib.get("version") or "").strip()
|
||||
if not version:
|
||||
result.add(
|
||||
"error",
|
||||
"missing_version",
|
||||
f"{_display_path(resolved_path)} SDF version is required",
|
||||
path="/sdf",
|
||||
)
|
||||
elif not re.match(r"^\d+\.\d+(?:\.\d+)?$", version):
|
||||
result.add(
|
||||
"warning",
|
||||
"unusual_version",
|
||||
f"SDF version {version!r} does not look like major.minor",
|
||||
path="/sdf",
|
||||
)
|
||||
|
||||
meaningful_tags = {"model", "world", "actor", "light", "include", "plugin"}
|
||||
if not any(_local_name(child.tag) in meaningful_tags for child in list(root)):
|
||||
result.add(
|
||||
"error",
|
||||
"empty_document",
|
||||
f"{_display_path(resolved_path)} must define at least one <model>, <world>, or other SDF content",
|
||||
path="/sdf",
|
||||
)
|
||||
|
||||
_check_unique_named(_children(root, "world"), result, "/sdf", "world")
|
||||
_check_unique_named(_children(root, "model"), result, "/sdf", "model")
|
||||
|
||||
for include_element in _children(root, "include"):
|
||||
_validate_include(include_element, result, "/sdf/include")
|
||||
for plugin_element in _children(root, "plugin"):
|
||||
_validate_plugin(plugin_element, result, "/sdf/plugin")
|
||||
for world_element in _children(root, "world"):
|
||||
_validate_world(world_element, result, resolved_base_dir)
|
||||
for model_element in _children(root, "model"):
|
||||
_validate_model(model_element, result, resolved_base_dir, parent_targets={"world"})
|
||||
|
||||
return result
|
||||
|
||||
|
||||
def raise_for_validation_errors(result: ValidationResult, *, strict: bool = False) -> None:
|
||||
findings = result.errors + (result.warnings if strict else [])
|
||||
if findings:
|
||||
from .source import SdfSourceError
|
||||
|
||||
raise SdfSourceError(format_findings(findings))
|
||||
|
||||
|
||||
def _validate_world(world_element: ET.Element, result: ValidationResult, base_dir: Path) -> None:
|
||||
world_name = _required_name(world_element, result, _path("world", world_element), "world")
|
||||
world_path = _path("world", world_element, fallback="/sdf/world")
|
||||
if not world_name:
|
||||
world_name = "world"
|
||||
targets = {"world", world_name}
|
||||
|
||||
_check_unique_named(_children(world_element, "model"), result, world_path, "model")
|
||||
_check_unique_named(_children(world_element, "frame"), result, world_path, "frame")
|
||||
|
||||
frames = _children(world_element, "frame")
|
||||
frame_names = _names(frames)
|
||||
targets.update(frame_names)
|
||||
for frame_element in frames:
|
||||
_validate_frame(frame_element, result, targets, world_path)
|
||||
_validate_frame_cycles(frames, result, world_path)
|
||||
|
||||
for pose_element in _children(world_element, "pose"):
|
||||
_validate_pose(pose_element, result, f"{world_path}/pose", targets)
|
||||
for include_element in _children(world_element, "include"):
|
||||
_validate_include(include_element, result, f"{world_path}/include")
|
||||
for plugin_element in _children(world_element, "plugin"):
|
||||
_validate_plugin(plugin_element, result, f"{world_path}/plugin")
|
||||
for model_element in _children(world_element, "model"):
|
||||
_validate_model(model_element, result, base_dir, parent_targets=targets)
|
||||
|
||||
|
||||
def _validate_model(
|
||||
model_element: ET.Element,
|
||||
result: ValidationResult,
|
||||
base_dir: Path,
|
||||
*,
|
||||
parent_targets: set[str],
|
||||
) -> None:
|
||||
model_name = _required_name(model_element, result, _path("model", model_element), "model")
|
||||
model_path = _path("model", model_element, fallback="/sdf/model")
|
||||
if not model_name:
|
||||
model_name = "model"
|
||||
|
||||
link_elements = _children(model_element, "link")
|
||||
joint_elements = _children(model_element, "joint")
|
||||
frame_elements = _children(model_element, "frame")
|
||||
nested_model_elements = _children(model_element, "model")
|
||||
plugin_elements = _children(model_element, "plugin")
|
||||
|
||||
_check_unique_named(link_elements, result, model_path, "link")
|
||||
_check_unique_named(joint_elements, result, model_path, "joint")
|
||||
_check_unique_named(frame_elements, result, model_path, "frame")
|
||||
_check_unique_named(nested_model_elements, result, model_path, "model")
|
||||
|
||||
link_names = set(_names(link_elements))
|
||||
frame_names = set(_names(frame_elements))
|
||||
nested_model_names = set(_names(nested_model_elements))
|
||||
local_targets = {
|
||||
"world",
|
||||
"__model__",
|
||||
model_name,
|
||||
*parent_targets,
|
||||
*link_names,
|
||||
*frame_names,
|
||||
*nested_model_names,
|
||||
}
|
||||
|
||||
for pose_element in _children(model_element, "pose"):
|
||||
_validate_pose(pose_element, result, f"{model_path}/pose", local_targets)
|
||||
|
||||
for static_element in _children(model_element, "static"):
|
||||
_validate_boolean_text(static_element, result, f"{model_path}/static", "static")
|
||||
|
||||
for frame_element in frame_elements:
|
||||
_validate_frame(frame_element, result, local_targets, model_path)
|
||||
_validate_frame_cycles(frame_elements, result, model_path)
|
||||
|
||||
child_links = _joint_child_links(joint_elements)
|
||||
|
||||
for link_element in link_elements:
|
||||
_validate_link(link_element, result, base_dir, local_targets, model_path)
|
||||
for joint_element in joint_elements:
|
||||
_validate_joint(joint_element, result, link_names, local_targets, model_path)
|
||||
for include_element in _children(model_element, "include"):
|
||||
_validate_include(include_element, result, f"{model_path}/include")
|
||||
for plugin_element in plugin_elements:
|
||||
_validate_plugin(plugin_element, result, f"{model_path}/plugin")
|
||||
|
||||
if not _model_is_static(model_element):
|
||||
referenced_links = set(child_links)
|
||||
referenced_links.update(_joint_parent_links(joint_elements) - {"world"})
|
||||
for link_element in link_elements:
|
||||
name = _name(link_element)
|
||||
if (
|
||||
name
|
||||
and name in referenced_links
|
||||
and _children(link_element, "collision")
|
||||
and not _children(link_element, "inertial")
|
||||
):
|
||||
result.add(
|
||||
"warning",
|
||||
"missing_inertial",
|
||||
f"dynamic link {name!r} has collision geometry but no inertial data",
|
||||
path=f"{model_path}/link[@name='{name}']",
|
||||
)
|
||||
|
||||
for nested_model in nested_model_elements:
|
||||
_validate_model(nested_model, result, base_dir, parent_targets=local_targets)
|
||||
|
||||
|
||||
def _validate_link(
|
||||
link_element: ET.Element,
|
||||
result: ValidationResult,
|
||||
base_dir: Path,
|
||||
targets: set[str],
|
||||
model_path: str,
|
||||
) -> None:
|
||||
link_name = _required_name(link_element, result, f"{model_path}/link", "link")
|
||||
link_path = f"{model_path}/link[@name='{link_name}']" if link_name else f"{model_path}/link"
|
||||
|
||||
for pose_element in _children(link_element, "pose"):
|
||||
_validate_pose(pose_element, result, f"{link_path}/pose", targets)
|
||||
|
||||
_check_unique_named(_children(link_element, "visual"), result, link_path, "visual")
|
||||
_check_unique_named(_children(link_element, "collision"), result, link_path, "collision")
|
||||
_check_unique_named(_children(link_element, "sensor"), result, link_path, "sensor")
|
||||
|
||||
visual_meshes: set[str] = set()
|
||||
for visual_element in _children(link_element, "visual"):
|
||||
visual_meshes.update(_validate_geometry_owner(visual_element, result, base_dir, targets, f"{link_path}/visual"))
|
||||
for collision_element in _children(link_element, "collision"):
|
||||
collision_meshes = _validate_geometry_owner(
|
||||
collision_element,
|
||||
result,
|
||||
base_dir,
|
||||
targets,
|
||||
f"{link_path}/collision",
|
||||
)
|
||||
for mesh_uri in collision_meshes:
|
||||
if mesh_uri in visual_meshes:
|
||||
result.add(
|
||||
"warning",
|
||||
"collision_reuses_visual_mesh",
|
||||
f"collision geometry reuses visual mesh URI {mesh_uri!r}",
|
||||
path=f"{link_path}/collision",
|
||||
hint="Use simplified collision geometry for physics when possible.",
|
||||
)
|
||||
for inertial_element in _children(link_element, "inertial"):
|
||||
_validate_inertial(inertial_element, result, targets, f"{link_path}/inertial")
|
||||
for sensor_element in _children(link_element, "sensor"):
|
||||
_validate_sensor(sensor_element, result, targets, f"{link_path}/sensor")
|
||||
|
||||
|
||||
def _validate_frame(frame_element: ET.Element, result: ValidationResult, targets: set[str], owner_path: str) -> None:
|
||||
frame_name = _required_name(frame_element, result, f"{owner_path}/frame", "frame")
|
||||
frame_path = f"{owner_path}/frame[@name='{frame_name}']" if frame_name else f"{owner_path}/frame"
|
||||
attached_to = str(frame_element.attrib.get("attached_to") or "").strip()
|
||||
if attached_to:
|
||||
_check_reference(attached_to, result, frame_path, "attached_to", targets)
|
||||
for pose_element in _children(frame_element, "pose"):
|
||||
_validate_pose(pose_element, result, f"{frame_path}/pose", targets)
|
||||
|
||||
|
||||
def _validate_joint(
|
||||
joint_element: ET.Element,
|
||||
result: ValidationResult,
|
||||
link_names: set[str],
|
||||
targets: set[str],
|
||||
model_path: str,
|
||||
) -> None:
|
||||
joint_name = _required_name(joint_element, result, f"{model_path}/joint", "joint")
|
||||
joint_path = f"{model_path}/joint[@name='{joint_name}']" if joint_name else f"{model_path}/joint"
|
||||
joint_type = str(joint_element.attrib.get("type") or "").strip()
|
||||
if not joint_type:
|
||||
result.add("error", "missing_joint_type", f"{joint_path} type is required", path=joint_path)
|
||||
elif joint_type not in COMMON_JOINT_TYPES:
|
||||
result.add("error", "unknown_joint_type", f"unsupported SDF joint type {joint_type!r}", path=joint_path)
|
||||
|
||||
parent_link = _required_child_text(joint_element, "parent", result, f"{joint_path}/parent", "joint parent")
|
||||
child_link = _required_child_text(joint_element, "child", result, f"{joint_path}/child", "joint child")
|
||||
if parent_link:
|
||||
_validate_link_reference(parent_link, result, link_names, f"{joint_path}/parent", allow_world=True)
|
||||
if child_link:
|
||||
_validate_link_reference(child_link, result, link_names, f"{joint_path}/child", allow_world=False)
|
||||
|
||||
for pose_element in _children(joint_element, "pose"):
|
||||
_validate_pose(pose_element, result, f"{joint_path}/pose", targets)
|
||||
|
||||
axis_elements = _children(joint_element, "axis")
|
||||
axis2_elements = _children(joint_element, "axis2")
|
||||
if joint_type in {"continuous", "revolute", "prismatic"} and not axis_elements:
|
||||
result.add(
|
||||
"warning",
|
||||
"missing_joint_axis",
|
||||
f"{joint_type} joint {joint_name!r} does not declare an explicit axis",
|
||||
path=joint_path,
|
||||
)
|
||||
for axis_element in axis_elements:
|
||||
_validate_axis(axis_element, result, targets, f"{joint_path}/axis", joint_type=joint_type, axis_name="axis")
|
||||
for axis2_element in axis2_elements:
|
||||
if joint_type not in SECOND_AXIS_JOINT_TYPES:
|
||||
result.add(
|
||||
"error",
|
||||
"unsupported_axis2",
|
||||
f"joint type {joint_type!r} does not support axis2 in the lightweight validator",
|
||||
path=f"{joint_path}/axis2",
|
||||
)
|
||||
_validate_axis(axis2_element, result, targets, f"{joint_path}/axis2", joint_type=joint_type, axis_name="axis2")
|
||||
|
||||
|
||||
def _validate_axis(
|
||||
axis_element: ET.Element,
|
||||
result: ValidationResult,
|
||||
targets: set[str],
|
||||
axis_path: str,
|
||||
*,
|
||||
joint_type: str,
|
||||
axis_name: str,
|
||||
) -> None:
|
||||
del axis_name
|
||||
xyz_element = _first_child(axis_element, "xyz")
|
||||
if xyz_element is None:
|
||||
result.add("error", "missing_axis_xyz", "joint axis must contain <xyz>", path=axis_path)
|
||||
else:
|
||||
values = _parse_number_text(str(xyz_element.text or ""), 3, result, f"{axis_path}/xyz", "axis xyz")
|
||||
if values is not None:
|
||||
norm = _norm(values)
|
||||
if norm == 0:
|
||||
result.add("error", "zero_axis", "joint axis vector must be nonzero", path=f"{axis_path}/xyz")
|
||||
elif abs(norm - 1.0) > UNIT_TOLERANCE:
|
||||
result.add(
|
||||
"warning",
|
||||
"non_unit_axis",
|
||||
f"joint axis vector length is {norm:.6g}, not 1",
|
||||
path=f"{axis_path}/xyz",
|
||||
)
|
||||
expressed_in = str(xyz_element.attrib.get("expressed_in") or axis_element.attrib.get("expressed_in") or "").strip()
|
||||
if expressed_in:
|
||||
_check_reference(expressed_in, result, f"{axis_path}/xyz", "expressed_in", targets)
|
||||
|
||||
for limit_element in _children(axis_element, "limit"):
|
||||
lower = _optional_number_child(limit_element, "lower", result, f"{axis_path}/limit/lower", allow_infinite=True)
|
||||
upper = _optional_number_child(limit_element, "upper", result, f"{axis_path}/limit/upper", allow_infinite=True)
|
||||
if lower is not None and upper is not None and lower > upper:
|
||||
result.add("error", "invalid_joint_limit", "joint lower limit exceeds upper limit", path=f"{axis_path}/limit")
|
||||
for tag in ("effort", "velocity", "stiffness", "dissipation"):
|
||||
_optional_number_child(limit_element, tag, result, f"{axis_path}/limit/{tag}", allow_infinite=True)
|
||||
if joint_type == "continuous" and (lower is not None or upper is not None):
|
||||
result.add(
|
||||
"warning",
|
||||
"continuous_joint_limits",
|
||||
"continuous joints usually should not use finite position limits",
|
||||
path=f"{axis_path}/limit",
|
||||
)
|
||||
|
||||
|
||||
def _validate_geometry_owner(
|
||||
owner: ET.Element,
|
||||
result: ValidationResult,
|
||||
base_dir: Path,
|
||||
targets: set[str],
|
||||
owner_path: str,
|
||||
) -> set[str]:
|
||||
owner_name = _required_name(owner, result, owner_path, _local_name(owner.tag))
|
||||
path = f"{owner_path}[@name='{owner_name}']" if owner_name else owner_path
|
||||
for pose_element in _children(owner, "pose"):
|
||||
_validate_pose(pose_element, result, f"{path}/pose", targets)
|
||||
|
||||
geometry_elements = _children(owner, "geometry")
|
||||
if len(geometry_elements) != 1:
|
||||
result.add(
|
||||
"error",
|
||||
"invalid_geometry_count",
|
||||
f"{_local_name(owner.tag)} must contain exactly one <geometry>",
|
||||
path=path,
|
||||
)
|
||||
return set()
|
||||
|
||||
geometry = geometry_elements[0]
|
||||
known_children = [
|
||||
child for child in list(geometry) if _local_name(child.tag) in {"box", "sphere", "cylinder", "capsule", "plane", "mesh"}
|
||||
]
|
||||
if len(known_children) != 1:
|
||||
result.add(
|
||||
"error",
|
||||
"invalid_geometry_shape",
|
||||
"geometry must contain exactly one recognized primitive or mesh child",
|
||||
path=f"{path}/geometry",
|
||||
)
|
||||
return set()
|
||||
|
||||
shape = known_children[0]
|
||||
shape_name = _local_name(shape.tag)
|
||||
if shape_name == "box":
|
||||
_validate_positive_vector_child(shape, "size", 3, result, f"{path}/geometry/box/size")
|
||||
elif shape_name == "sphere":
|
||||
_validate_positive_number_child(shape, "radius", result, f"{path}/geometry/sphere/radius")
|
||||
elif shape_name == "cylinder":
|
||||
_validate_positive_number_child(shape, "radius", result, f"{path}/geometry/cylinder/radius")
|
||||
_validate_positive_number_child(shape, "length", result, f"{path}/geometry/cylinder/length")
|
||||
elif shape_name == "capsule":
|
||||
_validate_positive_number_child(shape, "radius", result, f"{path}/geometry/capsule/radius")
|
||||
_validate_positive_number_child(shape, "length", result, f"{path}/geometry/capsule/length")
|
||||
elif shape_name == "plane":
|
||||
_validate_positive_vector_child(shape, "size", 2, result, f"{path}/geometry/plane/size")
|
||||
elif shape_name == "mesh":
|
||||
uri = _required_child_text(shape, "uri", result, f"{path}/geometry/mesh/uri", "mesh uri")
|
||||
_validate_positive_vector_child(shape, "scale", 3, result, f"{path}/geometry/mesh/scale", required=False)
|
||||
if uri:
|
||||
_validate_mesh_uri(uri, result, base_dir, f"{path}/geometry/mesh/uri")
|
||||
return {uri}
|
||||
return set()
|
||||
|
||||
|
||||
def _validate_inertial(inertial_element: ET.Element, result: ValidationResult, targets: set[str], inertial_path: str) -> None:
|
||||
for pose_element in _children(inertial_element, "pose"):
|
||||
_validate_pose(pose_element, result, f"{inertial_path}/pose", targets)
|
||||
mass = _optional_number_child(inertial_element, "mass", result, f"{inertial_path}/mass", required=True)
|
||||
if mass is not None and mass <= 0:
|
||||
result.add("error", "invalid_mass", "inertial mass must be positive", path=f"{inertial_path}/mass")
|
||||
|
||||
inertia_element = _first_child(inertial_element, "inertia")
|
||||
if inertia_element is None:
|
||||
result.add("warning", "missing_inertia_matrix", "inertial element has mass but no inertia matrix", path=inertial_path)
|
||||
return
|
||||
components: dict[str, float] = {}
|
||||
for tag in ("ixx", "iyy", "izz", "ixy", "ixz", "iyz"):
|
||||
value = _optional_number_child(inertia_element, tag, result, f"{inertial_path}/inertia/{tag}", required=True)
|
||||
if value is not None:
|
||||
components[tag] = value
|
||||
if len(components) != 6:
|
||||
return
|
||||
if not _inertia_is_psd(components):
|
||||
result.add(
|
||||
"error",
|
||||
"invalid_inertia_matrix",
|
||||
"inertia matrix must be positive semidefinite within tolerance",
|
||||
path=f"{inertial_path}/inertia",
|
||||
)
|
||||
|
||||
|
||||
def _validate_sensor(sensor_element: ET.Element, result: ValidationResult, targets: set[str], sensor_path: str) -> None:
|
||||
sensor_name = _required_name(sensor_element, result, sensor_path, "sensor")
|
||||
path = f"{sensor_path}[@name='{sensor_name}']" if sensor_name else sensor_path
|
||||
sensor_type = str(sensor_element.attrib.get("type") or "").strip()
|
||||
if not sensor_type:
|
||||
result.add("error", "missing_sensor_type", "sensor type is required", path=path)
|
||||
for pose_element in _children(sensor_element, "pose"):
|
||||
_validate_pose(pose_element, result, f"{path}/pose", targets)
|
||||
update_rate = _optional_number_child(sensor_element, "update_rate", result, f"{path}/update_rate")
|
||||
if update_rate is not None and update_rate < 0:
|
||||
result.add("error", "invalid_sensor_update_rate", "sensor update_rate must be non-negative", path=f"{path}/update_rate")
|
||||
|
||||
|
||||
def _validate_plugin(plugin_element: ET.Element, result: ValidationResult, plugin_path: str) -> None:
|
||||
filename = str(plugin_element.attrib.get("filename") or "").strip()
|
||||
if not filename:
|
||||
result.add("error", "missing_plugin_filename", "plugin filename is required", path=plugin_path)
|
||||
if "name" in plugin_element.attrib and not str(plugin_element.attrib.get("name") or "").strip():
|
||||
result.add("error", "missing_plugin_name", "plugin name is empty", path=plugin_path)
|
||||
elif "name" not in plugin_element.attrib:
|
||||
result.add("warning", "missing_plugin_name", "plugin name is omitted", path=plugin_path)
|
||||
|
||||
|
||||
def _validate_include(include_element: ET.Element, result: ValidationResult, include_path: str) -> None:
|
||||
_required_child_text(include_element, "uri", result, f"{include_path}/uri", "include uri")
|
||||
|
||||
|
||||
def _validate_pose(pose_element: ET.Element, result: ValidationResult, pose_path: str, targets: set[str]) -> None:
|
||||
rotation_format = str(pose_element.attrib.get("rotation_format") or "euler_rpy").strip()
|
||||
if rotation_format == "euler_rpy":
|
||||
values = _parse_number_text(str(pose_element.text or ""), 6, result, pose_path, "pose")
|
||||
elif rotation_format == "quat_xyzw":
|
||||
values = _parse_number_text(str(pose_element.text or ""), 7, result, pose_path, "pose")
|
||||
if values is not None:
|
||||
quat = values[3:]
|
||||
norm = _norm(quat)
|
||||
if norm == 0:
|
||||
result.add("error", "zero_quaternion", "quaternion pose has zero norm", path=pose_path)
|
||||
elif abs(norm - 1.0) > UNIT_TOLERANCE:
|
||||
result.add("warning", "non_unit_quaternion", f"quaternion pose norm is {norm:.6g}, not 1", path=pose_path)
|
||||
else:
|
||||
result.add("error", "unsupported_rotation_format", f"unsupported pose rotation_format {rotation_format!r}", path=pose_path)
|
||||
values = None
|
||||
|
||||
degrees = str(pose_element.attrib.get("degrees") or "").strip().lower()
|
||||
if degrees and degrees not in BOOLEAN_VALUES:
|
||||
result.add("error", "invalid_pose_degrees", "pose degrees attribute must be boolean-like", path=pose_path)
|
||||
elif degrees in TRUE_VALUES:
|
||||
result.add("warning", "pose_uses_degrees", "pose uses degrees=true; SDF defaults to radians", path=pose_path)
|
||||
|
||||
relative_to = str(pose_element.attrib.get("relative_to") or "").strip()
|
||||
if values is not None and _nontrivial_pose(values) and not relative_to:
|
||||
result.add(
|
||||
"warning",
|
||||
"pose_missing_relative_to",
|
||||
"nontrivial pose omits explicit relative_to",
|
||||
path=pose_path,
|
||||
)
|
||||
if relative_to:
|
||||
_check_reference(relative_to, result, pose_path, "relative_to", targets)
|
||||
|
||||
|
||||
def _validate_frame_cycles(frames: list[ET.Element], result: ValidationResult, owner_path: str) -> None:
|
||||
frame_names = set(_names(frames))
|
||||
edges: dict[str, str] = {}
|
||||
for frame in frames:
|
||||
name = _name(frame)
|
||||
target = str(frame.attrib.get("attached_to") or "").strip()
|
||||
if not target:
|
||||
pose_element = _first_child(frame, "pose")
|
||||
target = str(pose_element.attrib.get("relative_to") or "").strip() if pose_element is not None else ""
|
||||
if name and target in frame_names:
|
||||
edges[name] = target
|
||||
|
||||
visiting: set[str] = set()
|
||||
visited: set[str] = set()
|
||||
|
||||
def visit(frame_name: str, stack: list[str]) -> None:
|
||||
if frame_name in visited:
|
||||
return
|
||||
if frame_name in visiting:
|
||||
cycle = " -> ".join([*stack, frame_name])
|
||||
result.add("error", "frame_cycle", f"frame graph contains a cycle: {cycle}", path=f"{owner_path}/frame")
|
||||
return
|
||||
visiting.add(frame_name)
|
||||
target = edges.get(frame_name)
|
||||
if target:
|
||||
visit(target, [*stack, frame_name])
|
||||
visiting.remove(frame_name)
|
||||
visited.add(frame_name)
|
||||
|
||||
for frame_name in edges:
|
||||
visit(frame_name, [])
|
||||
|
||||
|
||||
def _validate_mesh_uri(uri: str, result: ValidationResult, base_dir: Path, uri_path: str) -> None:
|
||||
parsed = urlparse(uri)
|
||||
if parsed.scheme in EXTERNAL_URI_SCHEMES:
|
||||
return
|
||||
if parsed.scheme and parsed.scheme != "file":
|
||||
result.add(
|
||||
"warning",
|
||||
"unresolved_mesh_uri_scheme",
|
||||
f"mesh URI scheme {parsed.scheme!r} is not resolved by bundled validation",
|
||||
path=uri_path,
|
||||
)
|
||||
return
|
||||
mesh_path = Path(unquote(parsed.path)).resolve() if parsed.scheme == "file" else (base_dir / uri).resolve()
|
||||
if not mesh_path.is_file():
|
||||
result.add(
|
||||
"error",
|
||||
"missing_mesh_file",
|
||||
f"references missing mesh file: {uri!r}",
|
||||
path=uri_path,
|
||||
)
|
||||
|
||||
|
||||
def _validate_link_reference(
|
||||
link_ref: str,
|
||||
result: ValidationResult,
|
||||
link_names: set[str],
|
||||
path: str,
|
||||
*,
|
||||
allow_world: bool,
|
||||
) -> None:
|
||||
if link_ref == "world":
|
||||
if allow_world:
|
||||
return
|
||||
result.add("error", "invalid_joint_child", "joint child may not be world", path=path)
|
||||
return
|
||||
if "::" in link_ref:
|
||||
result.add(
|
||||
"warning",
|
||||
"scoped_link_reference_unchecked",
|
||||
f"scoped link reference {link_ref!r} was not fully resolved by bundled validation",
|
||||
path=path,
|
||||
)
|
||||
return
|
||||
if link_ref not in link_names:
|
||||
result.add("error", "missing_link_reference", f"references missing link {link_ref!r}", path=path)
|
||||
|
||||
|
||||
def _check_reference(ref: str, result: ValidationResult, path: str, attr: str, targets: set[str]) -> None:
|
||||
if "::" in ref:
|
||||
if any(part == "" for part in ref.split("::")):
|
||||
result.add("error", "invalid_scoped_reference", f"{attr} has malformed scoped reference {ref!r}", path=path)
|
||||
else:
|
||||
result.add(
|
||||
"warning",
|
||||
"scoped_reference_unchecked",
|
||||
f"{attr} scoped reference {ref!r} was not fully resolved by bundled validation",
|
||||
path=path,
|
||||
)
|
||||
return
|
||||
if ref not in targets:
|
||||
result.add("error", "unresolved_reference", f"{attr} references unknown frame or link {ref!r}", path=path)
|
||||
|
||||
|
||||
def _check_unique_named(elements: list[ET.Element], result: ValidationResult, scope_path: str, label: str) -> None:
|
||||
seen: set[str] = set()
|
||||
duplicates: set[str] = set()
|
||||
for element in elements:
|
||||
name = _required_name(element, result, f"{scope_path}/{label}", label)
|
||||
if not name:
|
||||
continue
|
||||
if name in seen:
|
||||
duplicates.add(name)
|
||||
seen.add(name)
|
||||
if duplicates:
|
||||
duplicate_text = ", ".join(repr(item) for item in sorted(duplicates))
|
||||
result.add(
|
||||
"error",
|
||||
"duplicate_names",
|
||||
f"{label} names contain duplicates {duplicate_text}",
|
||||
path=f"{scope_path}/{label}",
|
||||
)
|
||||
|
||||
|
||||
def _validate_positive_vector_child(
|
||||
parent: ET.Element,
|
||||
tag: str,
|
||||
expected_len: int,
|
||||
result: ValidationResult,
|
||||
path: str,
|
||||
*,
|
||||
required: bool = True,
|
||||
) -> list[float] | None:
|
||||
child = _first_child(parent, tag)
|
||||
if child is None:
|
||||
if required:
|
||||
result.add("error", "missing_numeric_vector", f"{tag} is required", path=path)
|
||||
return None
|
||||
values = _parse_number_text(str(child.text or ""), expected_len, result, path, tag)
|
||||
if values is None:
|
||||
return None
|
||||
for value in values:
|
||||
if value <= 0:
|
||||
result.add("error", "invalid_dimension", f"{tag} values must be positive", path=path)
|
||||
break
|
||||
return values
|
||||
|
||||
|
||||
def _validate_positive_number_child(parent: ET.Element, tag: str, result: ValidationResult, path: str) -> float | None:
|
||||
value = _optional_number_child(parent, tag, result, path, required=True)
|
||||
if value is not None and value <= 0:
|
||||
result.add("error", "invalid_dimension", f"{tag} must be positive", path=path)
|
||||
return value
|
||||
|
||||
|
||||
def _optional_number_child(
|
||||
parent: ET.Element,
|
||||
tag: str,
|
||||
result: ValidationResult,
|
||||
path: str,
|
||||
*,
|
||||
required: bool = False,
|
||||
allow_infinite: bool = False,
|
||||
) -> float | None:
|
||||
child = _first_child(parent, tag)
|
||||
if child is None or str(child.text or "").strip() == "":
|
||||
if required:
|
||||
result.add("error", "missing_number", f"{tag} is required", path=path)
|
||||
return None
|
||||
return _parse_optional_number(str(child.text or ""), result, path, tag, required=required, allow_infinite=allow_infinite)
|
||||
|
||||
|
||||
def _parse_optional_number(
|
||||
value: str,
|
||||
result: ValidationResult,
|
||||
path: str,
|
||||
label: str,
|
||||
*,
|
||||
required: bool = False,
|
||||
allow_infinite: bool = False,
|
||||
) -> float | None:
|
||||
value = str(value or "").strip()
|
||||
if not value:
|
||||
if required:
|
||||
result.add("error", "missing_number", f"{label} is required", path=path)
|
||||
return None
|
||||
try:
|
||||
number = float(value)
|
||||
except ValueError:
|
||||
result.add("error", "invalid_number", f"{label} must be numeric", path=path)
|
||||
return None
|
||||
if allow_infinite:
|
||||
if math.isnan(number):
|
||||
result.add("error", "invalid_number", f"{label} must not be NaN", path=path)
|
||||
return None
|
||||
elif not math.isfinite(number):
|
||||
result.add("error", "invalid_number", f"{label} must be finite", path=path)
|
||||
return None
|
||||
return number
|
||||
|
||||
|
||||
def _parse_number_text(
|
||||
text: str,
|
||||
expected_len: int,
|
||||
result: ValidationResult,
|
||||
path: str,
|
||||
label: str,
|
||||
) -> list[float] | None:
|
||||
parts = str(text or "").split()
|
||||
if len(parts) != expected_len:
|
||||
result.add("error", "invalid_numeric_vector", f"{label} must contain {expected_len} numeric values", path=path)
|
||||
return None
|
||||
values: list[float] = []
|
||||
for part in parts:
|
||||
try:
|
||||
value = float(part)
|
||||
except ValueError:
|
||||
result.add("error", "invalid_number", f"{label} contains a non-numeric value {part!r}", path=path)
|
||||
return None
|
||||
if not math.isfinite(value):
|
||||
result.add("error", "invalid_number", f"{label} values must be finite", path=path)
|
||||
return None
|
||||
values.append(value)
|
||||
return values
|
||||
|
||||
|
||||
def _validate_boolean_text(element: ET.Element, result: ValidationResult, path: str, label: str) -> None:
|
||||
value = str(element.text or "").strip().lower()
|
||||
if value and value not in BOOLEAN_VALUES:
|
||||
result.add("error", "invalid_boolean", f"{label} must be boolean-like", path=path)
|
||||
|
||||
|
||||
def _required_name(element: ET.Element, result: ValidationResult, path: str, label: str) -> str:
|
||||
name = _name(element)
|
||||
if not name:
|
||||
result.add("error", "missing_name", f"{label} name is required", path=path)
|
||||
return name
|
||||
|
||||
|
||||
def _required_child_text(
|
||||
parent: ET.Element,
|
||||
tag: str,
|
||||
result: ValidationResult,
|
||||
path: str,
|
||||
label: str,
|
||||
) -> str:
|
||||
value = _child_text(parent, tag)
|
||||
if not value:
|
||||
result.add("error", "missing_child_text", f"{label} is required", path=path)
|
||||
return value
|
||||
|
||||
|
||||
def _joint_child_links(joints: list[ET.Element]) -> set[str]:
|
||||
return {_child_text(joint, "child") for joint in joints if _child_text(joint, "child") and "::" not in _child_text(joint, "child")}
|
||||
|
||||
|
||||
def _joint_parent_links(joints: list[ET.Element]) -> set[str]:
|
||||
return {_child_text(joint, "parent") for joint in joints if _child_text(joint, "parent") and "::" not in _child_text(joint, "parent")}
|
||||
|
||||
|
||||
def _model_is_static(model_element: ET.Element) -> bool:
|
||||
value = _child_text(model_element, "static").lower()
|
||||
return value in TRUE_VALUES
|
||||
|
||||
|
||||
def _inertia_is_psd(values: dict[str, float]) -> bool:
|
||||
ixx = values["ixx"]
|
||||
iyy = values["iyy"]
|
||||
izz = values["izz"]
|
||||
ixy = values["ixy"]
|
||||
ixz = values["ixz"]
|
||||
iyz = values["iyz"]
|
||||
minor_xy = (ixx * iyy) - (ixy * ixy)
|
||||
minor_xz = (ixx * izz) - (ixz * ixz)
|
||||
minor_yz = (iyy * izz) - (iyz * iyz)
|
||||
det = (ixx * iyy * izz) + (2 * ixy * ixz * iyz) - (ixx * iyz * iyz) - (iyy * ixz * ixz) - (izz * ixy * ixy)
|
||||
return (
|
||||
ixx >= -PSD_TOLERANCE
|
||||
and iyy >= -PSD_TOLERANCE
|
||||
and izz >= -PSD_TOLERANCE
|
||||
and minor_xy >= -PSD_TOLERANCE
|
||||
and minor_xz >= -PSD_TOLERANCE
|
||||
and minor_yz >= -PSD_TOLERANCE
|
||||
and det >= -PSD_TOLERANCE
|
||||
)
|
||||
|
||||
|
||||
def _nontrivial_pose(values: list[float]) -> bool:
|
||||
return any(abs(value) > POSE_TOLERANCE for value in values)
|
||||
|
||||
|
||||
def _norm(values: list[float]) -> float:
|
||||
return math.sqrt(sum(value * value for value in values))
|
||||
|
||||
|
||||
def _children(parent: ET.Element, tag_name: str) -> list[ET.Element]:
|
||||
return [child for child in list(parent) if _local_name(child.tag) == tag_name]
|
||||
|
||||
|
||||
def _first_child(parent: ET.Element, tag_name: str) -> ET.Element | None:
|
||||
return next(iter(_children(parent, tag_name)), None)
|
||||
|
||||
|
||||
def _child_text(parent: ET.Element, tag_name: str) -> str:
|
||||
child = _first_child(parent, tag_name)
|
||||
return str(child.text if child is not None else "").strip()
|
||||
|
||||
|
||||
def _name(element: ET.Element) -> str:
|
||||
return str(element.attrib.get("name") or "").strip()
|
||||
|
||||
|
||||
def _names(elements: list[ET.Element]) -> list[str]:
|
||||
return [_name(element) for element in elements if _name(element)]
|
||||
|
||||
|
||||
def _local_name(tag: object) -> str:
|
||||
return str(tag).rsplit("}", 1)[-1]
|
||||
|
||||
|
||||
def _path(tag: str, element: ET.Element, *, fallback: str | None = None) -> str:
|
||||
name = _name(element)
|
||||
if name:
|
||||
return f"/sdf/{tag}[@name='{name}']"
|
||||
return fallback or f"/sdf/{tag}"
|
||||
|
||||
|
||||
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()
|
||||
Reference in New Issue
Block a user