lockwire 0.2.0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- lockwire/__init__.py +9 -0
- lockwire/__main__.py +9 -0
- lockwire/advice.py +119 -0
- lockwire/api.py +1216 -0
- lockwire/api_payload.py +985 -0
- lockwire/bench.py +182 -0
- lockwire/blame.py +100 -0
- lockwire/catalog.py +100 -0
- lockwire/catalog_seed.yaml +545 -0
- lockwire/checks.py +3483 -0
- lockwire/cli.py +1799 -0
- lockwire/cli_line.py +73 -0
- lockwire/constraints.py +313 -0
- lockwire/copy.py +302 -0
- lockwire/describe.py +682 -0
- lockwire/edit.py +2098 -0
- lockwire/emit_ts.py +150 -0
- lockwire/export_mhs.py +115 -0
- lockwire/factdiff.py +254 -0
- lockwire/forces.py +334 -0
- lockwire/gitref.py +188 -0
- lockwire/hw.py +388 -0
- lockwire/mcp_server.py +276 -0
- lockwire/mechanism.py +379 -0
- lockwire/mesh_dae.py +555 -0
- lockwire/mesh_simple.py +198 -0
- lockwire/page.py +1485 -0
- lockwire/plan.py +849 -0
- lockwire/plan_payload.py +151 -0
- lockwire/plugins.py +70 -0
- lockwire/report.py +848 -0
- lockwire/report_html.py +707 -0
- lockwire/report_json.py +168 -0
- lockwire/review_draft.py +315 -0
- lockwire/scan.py +732 -0
- lockwire/scene.py +535 -0
- lockwire/scene_import.py +309 -0
- lockwire/scene_urdf.py +750 -0
- lockwire/scene_view.py +376 -0
- lockwire/schema.py +3616 -0
- lockwire/sdf.py +240 -0
- lockwire/serve.py +1256 -0
- lockwire/start_operation.py +92 -0
- lockwire/statics.py +77 -0
- lockwire/units.py +115 -0
- lockwire/urdf.py +348 -0
- lockwire/vcs.py +402 -0
- lockwire/whatif.py +161 -0
- lockwire-0.2.0.dist-info/METADATA +192 -0
- lockwire-0.2.0.dist-info/RECORD +52 -0
- lockwire-0.2.0.dist-info/WHEEL +4 -0
- lockwire-0.2.0.dist-info/entry_points.txt +3 -0
|
@@ -0,0 +1,92 @@
|
|
|
1
|
+
"""The shared contract-only start operation; desktop callers never generate HTML."""
|
|
2
|
+
|
|
3
|
+
from __future__ import annotations
|
|
4
|
+
|
|
5
|
+
import hashlib
|
|
6
|
+
import os
|
|
7
|
+
import tempfile
|
|
8
|
+
import xml.etree.ElementTree as ET
|
|
9
|
+
from pathlib import Path
|
|
10
|
+
|
|
11
|
+
from lockwire.edit import _contract_lock, contract_revision, relative_source
|
|
12
|
+
from lockwire.scene import SceneRefusal
|
|
13
|
+
from lockwire.scene_urdf import _resolve_mesh_path, mesh_root
|
|
14
|
+
from lockwire.schema import ArtifactRef, Contract, RobotInfo, save_contract
|
|
15
|
+
from lockwire.sdf import parse_sdf
|
|
16
|
+
from lockwire.urdf import _refuse_doctype_entity, parse_urdf
|
|
17
|
+
|
|
18
|
+
|
|
19
|
+
def _find_meshes(root: ET.Element, source: Path, target: Path) -> tuple[list[ArtifactRef], str]:
|
|
20
|
+
"""The model's mesh files as sha256-pinned refs inside the robot's package folder.
|
|
21
|
+
|
|
22
|
+
All-or-nothing: the scene loader refuses a mesh it cannot verify, so a partial list
|
|
23
|
+
would break the whole scene. Anything missing or outside the package comes back as a note.
|
|
24
|
+
"""
|
|
25
|
+
uris = sorted({u for m in root.iter("mesh") if (u := m.get("filename") or m.findtext("uri"))})
|
|
26
|
+
folder, found, problems = mesh_root(source, target.parent), {}, []
|
|
27
|
+
for uri in uris:
|
|
28
|
+
try:
|
|
29
|
+
path = _resolve_mesh_path(uri, source.parent, source, (source.parent, folder)).resolve()
|
|
30
|
+
found[path.relative_to(folder).as_posix()] = path
|
|
31
|
+
except (SceneRefusal, ValueError):
|
|
32
|
+
problems.append(uri)
|
|
33
|
+
if problems:
|
|
34
|
+
return [], (f"{len(problems)} of {len(uris)} mesh files not found inside the robot's package folder "
|
|
35
|
+
f"(e.g. {problems[0]}); the 3D model is not recorded.")
|
|
36
|
+
refs = [ArtifactRef(path=rel, sha256=hashlib.sha256(p.read_bytes()).hexdigest())
|
|
37
|
+
for rel, p in sorted(found.items())]
|
|
38
|
+
return refs, ""
|
|
39
|
+
|
|
40
|
+
|
|
41
|
+
def start_contract(
|
|
42
|
+
urdf: Path | str,
|
|
43
|
+
*,
|
|
44
|
+
out: Path | str | None = None,
|
|
45
|
+
overwrite: bool = False,
|
|
46
|
+
expected_revision: str | None = None,
|
|
47
|
+
) -> dict[str, str | bool]:
|
|
48
|
+
"""Create once, or replace only the exact bytes the caller observed."""
|
|
49
|
+
source = Path(urdf).resolve()
|
|
50
|
+
target = Path(out).resolve() if out is not None else source.parent / "lockwire.yaml"
|
|
51
|
+
if source == target:
|
|
52
|
+
raise ValueError("model and lockfile paths must differ")
|
|
53
|
+
with _contract_lock(target):
|
|
54
|
+
exists = target.exists()
|
|
55
|
+
if exists and not overwrite:
|
|
56
|
+
return {"path": str(target), "revision": contract_revision(target), "written": False}
|
|
57
|
+
if overwrite and (
|
|
58
|
+
not exists or expected_revision is None or expected_revision != contract_revision(target)
|
|
59
|
+
):
|
|
60
|
+
raise ValueError("lockfile changed since it was opened; reload before overwriting")
|
|
61
|
+
raw = source.read_bytes()
|
|
62
|
+
_refuse_doctype_entity(raw, source)
|
|
63
|
+
xml = ET.fromstring(raw)
|
|
64
|
+
model = parse_sdf(source) if xml.tag == "sdf" else parse_urdf(source)
|
|
65
|
+
spelling = relative_source(target, source)
|
|
66
|
+
meshes, note = _find_meshes(xml, source, target)
|
|
67
|
+
model = model.model_copy(update={
|
|
68
|
+
"external_assets": meshes,
|
|
69
|
+
"source": spelling,
|
|
70
|
+
"provenance": model.provenance.model_copy(update={"source": spelling}),
|
|
71
|
+
})
|
|
72
|
+
contract = Contract(robot=RobotInfo(name=source.stem), model=model)
|
|
73
|
+
fd, temporary = tempfile.mkstemp(prefix=f".{target.name}.", dir=target.parent)
|
|
74
|
+
os.close(fd)
|
|
75
|
+
try:
|
|
76
|
+
save_contract(contract, Path(temporary))
|
|
77
|
+
# Refuse even a non-cooperating writer that changed bytes during parsing.
|
|
78
|
+
if exists and contract_revision(target) != expected_revision:
|
|
79
|
+
raise ValueError("lockfile changed since it was opened; reload before overwriting")
|
|
80
|
+
if exists:
|
|
81
|
+
os.replace(temporary, target)
|
|
82
|
+
else:
|
|
83
|
+
# Installing a new file must never replace a writer racing this start.
|
|
84
|
+
try:
|
|
85
|
+
os.link(temporary, target)
|
|
86
|
+
except FileExistsError as exc:
|
|
87
|
+
raise ValueError("lockfile appeared during start; open it before overwriting") from exc
|
|
88
|
+
finally:
|
|
89
|
+
Path(temporary).unlink(missing_ok=True)
|
|
90
|
+
done: dict[str, str | bool] = {
|
|
91
|
+
"path": str(target), "revision": contract_revision(target), "written": True}
|
|
92
|
+
return {**done, "note": note} if note else done
|
lockwire/statics.py
ADDED
|
@@ -0,0 +1,77 @@
|
|
|
1
|
+
"""Rigid-body statics for stage one — support reactions, determinate sets only.
|
|
2
|
+
|
|
3
|
+
``docs/design/e5_solver_choice.md`` chose **numpy-stiffness** by
|
|
4
|
+
measurement. This module is the *determinate subset* of that choice, and the
|
|
5
|
+
narrowing is deliberate, not a substitution:
|
|
6
|
+
|
|
7
|
+
* For a **statically determinate** support set the reactions follow from
|
|
8
|
+
equilibrium alone and are **independent of the stiffness EI**. The solver study measured
|
|
9
|
+
exactly that — its ``equilibrium-statics`` candidate matched the textbook
|
|
10
|
+
reactions to 1.9e-16 — and rejected it only because it cannot produce
|
|
11
|
+
*deflections* or answer a *redundant* set.
|
|
12
|
+
* For a **redundant** set the reactions *do* depend on EI. The contract schema
|
|
13
|
+
is frozen and declares no Young's modulus, no section and no area on
|
|
14
|
+
``Link`` or ``Physical``, so there is nothing to solve a redundant set with.
|
|
15
|
+
Those read NOT SCORED naming why, never a plausible-looking guess.
|
|
16
|
+
|
|
17
|
+
Consequently a stiffness solve has no input it could consume here, and a beam
|
|
18
|
+
deflection is not offered at all. What that document lists as unproven —
|
|
19
|
+
plates, shear-dominated stubby members, out-of-plane frames, dynamics — is
|
|
20
|
+
likewise out of stage one and named in the payload rather than omitted.
|
|
21
|
+
|
|
22
|
+
Sign convention: reactions are positive upward, against gravity acting along
|
|
23
|
+
the root frame's -Z. Positions are scalars on one horizontal span axis.
|
|
24
|
+
"""
|
|
25
|
+
|
|
26
|
+
from __future__ import annotations
|
|
27
|
+
|
|
28
|
+
STANDARD_G = 9.81
|
|
29
|
+
"""m/s^2. The same standard-gravity figure preflight's cross-signal stage
|
|
30
|
+
scores an accelerometer's |g| against (``preflight/stages/cross_signal.py``);
|
|
31
|
+
kept literal here so this module imports no preflight stage."""
|
|
32
|
+
|
|
33
|
+
|
|
34
|
+
class Indeterminate(ValueError):
|
|
35
|
+
"""The support set cannot be answered from the contract alone."""
|
|
36
|
+
|
|
37
|
+
|
|
38
|
+
def weight_n(mass_kg: float, g: float = STANDARD_G) -> float:
|
|
39
|
+
"""Weight in newtons of a mass in kilograms."""
|
|
40
|
+
return mass_kg * g
|
|
41
|
+
|
|
42
|
+
|
|
43
|
+
def support_reactions(
|
|
44
|
+
load_n: float, load_pos_m: float, support_pos_m: list[float]
|
|
45
|
+
) -> list[tuple[float, float]]:
|
|
46
|
+
"""Reaction (force N, moment N*m) at each support, in the order given.
|
|
47
|
+
|
|
48
|
+
One support is a fixed interface: it carries the whole load plus the moment
|
|
49
|
+
``W * (load_pos - support)`` — case 1 in reaction form.
|
|
50
|
+
|
|
51
|
+
Two supports are the lever rule — case 3, the two-bolt bracket. Each
|
|
52
|
+
is a simple support, so each carries force and no moment; a support behind
|
|
53
|
+
the load correctly comes out negative (it pulls down).
|
|
54
|
+
|
|
55
|
+
Three or more supports on one rigid body are statically indeterminate: the
|
|
56
|
+
split depends on stiffness the contract does not declare. Raises
|
|
57
|
+
``Indeterminate`` rather than returning a number.
|
|
58
|
+
"""
|
|
59
|
+
count = len(support_pos_m)
|
|
60
|
+
if count == 0:
|
|
61
|
+
raise Indeterminate("no mount point is declared, so nothing reacts the load")
|
|
62
|
+
if count == 1:
|
|
63
|
+
return [(load_n, load_n * (load_pos_m - support_pos_m[0]))]
|
|
64
|
+
if count == 2:
|
|
65
|
+
near, far = support_pos_m
|
|
66
|
+
if near == far:
|
|
67
|
+
raise Indeterminate(
|
|
68
|
+
"both mount points sit at the same place on the span axis, "
|
|
69
|
+
"so the load split between them is undetermined"
|
|
70
|
+
)
|
|
71
|
+
reaction_far = load_n * (load_pos_m - near) / (far - near)
|
|
72
|
+
return [(load_n - reaction_far, 0.0), (reaction_far, 0.0)]
|
|
73
|
+
raise Indeterminate(
|
|
74
|
+
f"{count} mount points on one rigid body is statically indeterminate; the load "
|
|
75
|
+
"split depends on the structure's stiffness (Young's modulus and section), which "
|
|
76
|
+
"the contract schema does not declare"
|
|
77
|
+
)
|
lockwire/units.py
ADDED
|
@@ -0,0 +1,115 @@
|
|
|
1
|
+
"""Minimal unit handling: enough to sum and compare contract quantities.
|
|
2
|
+
|
|
3
|
+
Deliberately a table, not a units library. Every unit the schema accepts is
|
|
4
|
+
listed here; an unknown unit fails validation instead of passing through.
|
|
5
|
+
"""
|
|
6
|
+
|
|
7
|
+
import math
|
|
8
|
+
|
|
9
|
+
# unit -> (dimension, factor to the dimension's base unit)
|
|
10
|
+
_UNITS: dict[str, tuple[str, float]] = {
|
|
11
|
+
# mass -> kg
|
|
12
|
+
"kg": ("mass", 1.0),
|
|
13
|
+
"g": ("mass", 1e-3),
|
|
14
|
+
"mg": ("mass", 1e-6),
|
|
15
|
+
# power -> W
|
|
16
|
+
"W": ("power", 1.0),
|
|
17
|
+
"mW": ("power", 1e-3),
|
|
18
|
+
"kW": ("power", 1e3),
|
|
19
|
+
# energy -> Wh (Wh over J: battery math reads naturally)
|
|
20
|
+
"Wh": ("energy", 1.0),
|
|
21
|
+
"mWh": ("energy", 1e-3),
|
|
22
|
+
"kWh": ("energy", 1e3),
|
|
23
|
+
"J": ("energy", 1.0 / 3600.0),
|
|
24
|
+
# time -> s
|
|
25
|
+
"s": ("time", 1.0),
|
|
26
|
+
"ms": ("time", 1e-3),
|
|
27
|
+
"min": ("time", 60.0),
|
|
28
|
+
"h": ("time", 3600.0),
|
|
29
|
+
# rate -> Hz
|
|
30
|
+
"Hz": ("rate", 1.0),
|
|
31
|
+
# electrical
|
|
32
|
+
"V": ("voltage", 1.0),
|
|
33
|
+
"mV": ("voltage", 1e-3),
|
|
34
|
+
"A": ("current", 1.0),
|
|
35
|
+
"mA": ("current", 1e-3),
|
|
36
|
+
# length -> m
|
|
37
|
+
"m": ("length", 1.0),
|
|
38
|
+
"cm": ("length", 1e-2),
|
|
39
|
+
"mm": ("length", 1e-3),
|
|
40
|
+
"m^2": ("area", 1.0),
|
|
41
|
+
"m^4": ("second_moment", 1.0),
|
|
42
|
+
# temperature (absolute compare only — never summed)
|
|
43
|
+
"C": ("temperature", 1.0),
|
|
44
|
+
"degC": ("temperature", 1.0), # bag spelling for the same Celsius scale
|
|
45
|
+
# pressure -> hPa
|
|
46
|
+
"Pa": ("pressure", 0.01),
|
|
47
|
+
"hPa": ("pressure", 1.0),
|
|
48
|
+
"kPa": ("pressure", 10.0),
|
|
49
|
+
"MPa": ("pressure", 1e4),
|
|
50
|
+
"GPa": ("pressure", 1e7),
|
|
51
|
+
# scalar angular measure -> rad (not semantic position or orientation roles)
|
|
52
|
+
"deg": ("angle", math.pi / 180.0),
|
|
53
|
+
# kinematics
|
|
54
|
+
"m/s^2": ("acceleration", 1.0),
|
|
55
|
+
"m/s": ("velocity", 1.0),
|
|
56
|
+
"rad/s": ("angular_rate", 1.0),
|
|
57
|
+
"rad/s^2": ("angular_acceleration", 1.0),
|
|
58
|
+
"rad": ("angle", 1.0),
|
|
59
|
+
"N": ("force", 1.0),
|
|
60
|
+
"N*m": ("torque", 1.0),
|
|
61
|
+
"ohm": ("resistance", 1.0),
|
|
62
|
+
"kg*m^2": ("inertia", 1.0),
|
|
63
|
+
"kg/m^3": ("density", 1.0),
|
|
64
|
+
"N/m": ("linear_stiffness", 1.0),
|
|
65
|
+
"N*m/rad": ("rotational_stiffness", 1.0),
|
|
66
|
+
"N*s/m": ("linear_damping", 1.0),
|
|
67
|
+
"N*m*s/rad": ("rotational_damping", 1.0),
|
|
68
|
+
"N/m^3": ("contact_stiffness", 1.0),
|
|
69
|
+
"N*s/m^3": ("contact_damping", 1.0),
|
|
70
|
+
"1/s": ("inverse_time", 1.0),
|
|
71
|
+
# dimensionless
|
|
72
|
+
"1": ("dimensionless", 1.0),
|
|
73
|
+
"%": ("dimensionless", 0.01),
|
|
74
|
+
# thermal resistance -> C/W (temperature C/degC stay on temperature)
|
|
75
|
+
"C/W": ("thermal_resistance", 1.0),
|
|
76
|
+
# RF power level -> dBm (logarithmic; never share power with W/mW/kW —
|
|
77
|
+
# scale-factor convert or sum of dBm and watts is arithmetic nonsense)
|
|
78
|
+
"dBm": ("power_level", 1.0),
|
|
79
|
+
# RF gain / ratio -> dB (log-space only; never convertible to a linear qty)
|
|
80
|
+
"dB": ("gain", 1.0),
|
|
81
|
+
"dBi": ("gain", 1.0),
|
|
82
|
+
# data rate -> bit/s
|
|
83
|
+
"bit/s": ("data_rate", 1.0),
|
|
84
|
+
"kbit/s": ("data_rate", 1e3),
|
|
85
|
+
"Mbit/s": ("data_rate", 1e6),
|
|
86
|
+
}
|
|
87
|
+
|
|
88
|
+
|
|
89
|
+
class UnitError(ValueError):
|
|
90
|
+
"""Unknown unit or dimension mismatch."""
|
|
91
|
+
|
|
92
|
+
|
|
93
|
+
def known(unit: str) -> bool:
|
|
94
|
+
return unit in _UNITS
|
|
95
|
+
|
|
96
|
+
|
|
97
|
+
def dimension(unit: str) -> str:
|
|
98
|
+
try:
|
|
99
|
+
return _UNITS[unit][0]
|
|
100
|
+
except KeyError:
|
|
101
|
+
raise UnitError(f"unknown unit {unit!r}") from None
|
|
102
|
+
|
|
103
|
+
|
|
104
|
+
def to_base(value: float, unit: str) -> float:
|
|
105
|
+
"""Convert to the dimension's base unit (kg, W, Wh, s, Hz, ...)."""
|
|
106
|
+
try:
|
|
107
|
+
return value * _UNITS[unit][1]
|
|
108
|
+
except KeyError:
|
|
109
|
+
raise UnitError(f"unknown unit {unit!r}") from None
|
|
110
|
+
|
|
111
|
+
|
|
112
|
+
def convert(value: float, unit: str, target: str) -> float:
|
|
113
|
+
if dimension(unit) != dimension(target):
|
|
114
|
+
raise UnitError(f"cannot convert {unit!r} to {target!r}: different dimensions")
|
|
115
|
+
return to_base(value, unit) / _UNITS[target][1]
|
lockwire/urdf.py
ADDED
|
@@ -0,0 +1,348 @@
|
|
|
1
|
+
"""URDF -> Tier A extraction (`lockwire init`). Witness W2: in-repo model files.
|
|
2
|
+
|
|
3
|
+
Traps handled here (spec v3):
|
|
4
|
+
- URDF inertia is about the COM in the inertial frame -> compare principal
|
|
5
|
+
moments + axes, never raw tensor elements.
|
|
6
|
+
- Zero-mass / missing-inertial links are flagged, not silently accepted.
|
|
7
|
+
- Structure hash covers the kinematic topology (joints + links), so a moved
|
|
8
|
+
mount or re-parented joint changes it; cosmetic reordering does not.
|
|
9
|
+
- Unresolved xacro / ROS substitutions are refused — never treated as link names.
|
|
10
|
+
"""
|
|
11
|
+
|
|
12
|
+
import datetime as dt
|
|
13
|
+
import hashlib
|
|
14
|
+
import re
|
|
15
|
+
import xml.etree.ElementTree as ET
|
|
16
|
+
from dataclasses import dataclass
|
|
17
|
+
from pathlib import Path
|
|
18
|
+
|
|
19
|
+
import numpy as np
|
|
20
|
+
|
|
21
|
+
from lockwire.schema import (
|
|
22
|
+
Joint,
|
|
23
|
+
JointLimits,
|
|
24
|
+
JointOrigin,
|
|
25
|
+
Link,
|
|
26
|
+
ModelRecord,
|
|
27
|
+
PrincipalInertia,
|
|
28
|
+
Provenance,
|
|
29
|
+
Quantity,
|
|
30
|
+
_model_structure_hash,
|
|
31
|
+
)
|
|
32
|
+
|
|
33
|
+
# `${var}` in a name is xacro property substitution (TurtleBot3 ships this unexpanded).
|
|
34
|
+
# A bare `$` (e.g. link `cost_$`) is NOT matched — that would be a false positive.
|
|
35
|
+
_XACRO_PROP = re.compile(r"\$\{[^}]+\}")
|
|
36
|
+
# ROS command substitutions that survive only when xacro was never run.
|
|
37
|
+
_ROS_SUBST = re.compile(r"\$\((?:arg|find)\b")
|
|
38
|
+
_REMEDY = "Expand the xacro first, then re-run: xacro model.urdf.xacro > model.urdf"
|
|
39
|
+
|
|
40
|
+
|
|
41
|
+
class UnresolvedXacroError(ValueError):
|
|
42
|
+
"""Input still contains xacro/ROS substitutions; refuse rather than invent names."""
|
|
43
|
+
|
|
44
|
+
|
|
45
|
+
def _local_tag(tag: str) -> str:
|
|
46
|
+
"""Strip Clark notation `{ns}local` → `local`; leave un-namespaced tags alone."""
|
|
47
|
+
if tag.startswith("{") and "}" in tag:
|
|
48
|
+
return tag.split("}", 1)[1]
|
|
49
|
+
return tag
|
|
50
|
+
|
|
51
|
+
|
|
52
|
+
def _refuse_unresolved_xacro(root: ET.Element, path: Path) -> None:
|
|
53
|
+
"""Refuse unexpanded xacro rather than treat template tokens as robot links.
|
|
54
|
+
|
|
55
|
+
Detection order (most → least specific to the TurtleBot3 miss):
|
|
56
|
+
1. `${…}` surviving in a link or joint *name* (or parent/child link ref)
|
|
57
|
+
2. a `<xacro:…>` element or the xacro XML namespace still in the tree
|
|
58
|
+
3. `$(arg …)` / `$(find …)` in any attribute value
|
|
59
|
+
|
|
60
|
+
False-positive shape we deliberately do *not* match: a bare `$` in a name
|
|
61
|
+
(`link_$1`, `cost_usd`) — only braced `${…}` property syntax counts.
|
|
62
|
+
"""
|
|
63
|
+
for el in root.findall("link"):
|
|
64
|
+
name = el.get("name") or ""
|
|
65
|
+
if _XACRO_PROP.search(name):
|
|
66
|
+
raise UnresolvedXacroError(
|
|
67
|
+
f"{path}: unresolved xacro — link name {name!r} still contains "
|
|
68
|
+
f"${{…}} substitutions. This is a xacro template, not a finished URDF. "
|
|
69
|
+
f"{_REMEDY}"
|
|
70
|
+
)
|
|
71
|
+
for el in root.findall("joint"):
|
|
72
|
+
name = el.get("name") or ""
|
|
73
|
+
if _XACRO_PROP.search(name):
|
|
74
|
+
raise UnresolvedXacroError(
|
|
75
|
+
f"{path}: unresolved xacro — joint name {name!r} still contains "
|
|
76
|
+
f"${{…}} substitutions. This is a xacro template, not a finished URDF. "
|
|
77
|
+
f"{_REMEDY}"
|
|
78
|
+
)
|
|
79
|
+
for end in ("parent", "child"):
|
|
80
|
+
ref = el.find(end)
|
|
81
|
+
link_name = ref.get("link", "") if ref is not None else ""
|
|
82
|
+
if _XACRO_PROP.search(link_name):
|
|
83
|
+
raise UnresolvedXacroError(
|
|
84
|
+
f"{path}: unresolved xacro — joint {name!r} {end} link "
|
|
85
|
+
f"{link_name!r} still contains ${{…}} substitutions. "
|
|
86
|
+
f"This is a xacro template, not a finished URDF. {_REMEDY}"
|
|
87
|
+
)
|
|
88
|
+
|
|
89
|
+
for el in root.iter():
|
|
90
|
+
tag = el.tag if isinstance(el.tag, str) else ""
|
|
91
|
+
local = _local_tag(tag)
|
|
92
|
+
# Un-namespaced `xacro:foo` (common in shipped .xacro) or `{…xacro…}foo`.
|
|
93
|
+
if local.startswith("xacro:") or (
|
|
94
|
+
tag.startswith("{") and "xacro" in tag[1:].split("}", 1)[0]
|
|
95
|
+
):
|
|
96
|
+
raise UnresolvedXacroError(
|
|
97
|
+
f"{path}: unresolved xacro — <{local}> element remains in the document. "
|
|
98
|
+
f"This is a xacro template, not a finished URDF. {_REMEDY}"
|
|
99
|
+
)
|
|
100
|
+
for attr_val in el.attrib.values():
|
|
101
|
+
if _ROS_SUBST.search(attr_val):
|
|
102
|
+
raise UnresolvedXacroError(
|
|
103
|
+
f"{path}: unresolved xacro — $(arg …)/$(find …) substitutions remain "
|
|
104
|
+
f"in attribute values. This is a xacro template, not a finished URDF. "
|
|
105
|
+
f"{_REMEDY}"
|
|
106
|
+
)
|
|
107
|
+
|
|
108
|
+
|
|
109
|
+
def _principal(
|
|
110
|
+
ixx: float, ixy: float, ixz: float, iyy: float, iyz: float, izz: float
|
|
111
|
+
) -> PrincipalInertia:
|
|
112
|
+
tensor = np.array([[ixx, ixy, ixz], [ixy, iyy, iyz], [ixz, iyz, izz]])
|
|
113
|
+
moments, axes = np.linalg.eigh(tensor) # ascending; columns = eigenvectors
|
|
114
|
+
return PrincipalInertia(
|
|
115
|
+
moments_kgm2=tuple(float(m) for m in moments),
|
|
116
|
+
axes=tuple(tuple(float(v) for v in axes[:, i]) for i in range(3)),
|
|
117
|
+
)
|
|
118
|
+
|
|
119
|
+
|
|
120
|
+
def _parse_link(el: ET.Element) -> tuple[Link, bool]:
|
|
121
|
+
"""Returns (link, is_zero_mass)."""
|
|
122
|
+
name = el.get("name", "?")
|
|
123
|
+
inertial = el.find("inertial")
|
|
124
|
+
if inertial is None:
|
|
125
|
+
return Link(name=name), True
|
|
126
|
+
|
|
127
|
+
mass_el = inertial.find("mass")
|
|
128
|
+
mass_val = float(mass_el.get("value", "0")) if mass_el is not None else 0.0
|
|
129
|
+
if mass_val == 0.0:
|
|
130
|
+
return Link(name=name), True
|
|
131
|
+
|
|
132
|
+
origin = inertial.find("origin")
|
|
133
|
+
xyz = tuple(
|
|
134
|
+
float(x) for x in (origin.get("xyz", "0 0 0") if origin is not None else "0 0 0").split()
|
|
135
|
+
)
|
|
136
|
+
|
|
137
|
+
inertia_el = inertial.find("inertia")
|
|
138
|
+
inertia = None
|
|
139
|
+
if inertia_el is not None:
|
|
140
|
+
inertia = _principal(
|
|
141
|
+
*(float(inertia_el.get(k, "0")) for k in ("ixx", "ixy", "ixz", "iyy", "iyz", "izz"))
|
|
142
|
+
)
|
|
143
|
+
|
|
144
|
+
link = Link(
|
|
145
|
+
name=name,
|
|
146
|
+
mass=Quantity(value=mass_val, unit="kg"),
|
|
147
|
+
com=xyz,
|
|
148
|
+
frame=name, # URDF inertial origin is expressed in the link frame
|
|
149
|
+
inertia=inertia,
|
|
150
|
+
)
|
|
151
|
+
return link, False
|
|
152
|
+
|
|
153
|
+
|
|
154
|
+
def _xyz_rpy(
|
|
155
|
+
origin: ET.Element | None,
|
|
156
|
+
) -> tuple[tuple[float, float, float], tuple[float, float, float]]:
|
|
157
|
+
xyz = tuple(
|
|
158
|
+
float(x) for x in (origin.get("xyz", "0 0 0") if origin is not None else "0 0 0").split()
|
|
159
|
+
)
|
|
160
|
+
rpy = tuple(
|
|
161
|
+
float(x) for x in (origin.get("rpy", "0 0 0") if origin is not None else "0 0 0").split()
|
|
162
|
+
)
|
|
163
|
+
return xyz, rpy # type: ignore[return-value]
|
|
164
|
+
|
|
165
|
+
|
|
166
|
+
def _structure_hash(root: ET.Element) -> str:
|
|
167
|
+
"""Hash of the kinematic topology: sorted joints (name, type, parent, child,
|
|
168
|
+
origin) + sorted link names. Rounded so float noise doesn't churn the hash."""
|
|
169
|
+
lines: list[str] = []
|
|
170
|
+
for joint in root.findall("joint"):
|
|
171
|
+
parent = joint.find("parent")
|
|
172
|
+
child = joint.find("child")
|
|
173
|
+
xyz, rpy = _xyz_rpy(joint.find("origin"))
|
|
174
|
+
nums = " ".join(f"{v:.6f}" for v in xyz + rpy)
|
|
175
|
+
lines.append(
|
|
176
|
+
"|".join(
|
|
177
|
+
[
|
|
178
|
+
joint.get("name", "?"),
|
|
179
|
+
joint.get("type", "?"),
|
|
180
|
+
parent.get("link", "?") if parent is not None else "?",
|
|
181
|
+
child.get("link", "?") if child is not None else "?",
|
|
182
|
+
nums,
|
|
183
|
+
]
|
|
184
|
+
)
|
|
185
|
+
)
|
|
186
|
+
lines.sort()
|
|
187
|
+
lines += sorted(link.get("name", "?") for link in root.findall("link"))
|
|
188
|
+
return hashlib.sha256("\n".join(lines).encode()).hexdigest()
|
|
189
|
+
|
|
190
|
+
|
|
191
|
+
def _parse_joint(el: ET.Element) -> Joint:
|
|
192
|
+
"""URDF joint -> schema Joint; consumed by URDF drift."""
|
|
193
|
+
parent = el.find("parent")
|
|
194
|
+
child = el.find("child")
|
|
195
|
+
xyz, rpy = _xyz_rpy(el.find("origin"))
|
|
196
|
+
axis_el = el.find("axis")
|
|
197
|
+
axis = None
|
|
198
|
+
if axis_el is not None:
|
|
199
|
+
axis = tuple(float(x) for x in axis_el.get("xyz", "0 0 1").split()) # type: ignore[assignment]
|
|
200
|
+
limit_el = el.find("limit")
|
|
201
|
+
limits = None
|
|
202
|
+
if limit_el is not None:
|
|
203
|
+
lower_raw, upper_raw = limit_el.get("lower"), limit_el.get("upper")
|
|
204
|
+
effort_raw, velocity_raw = limit_el.get("effort"), limit_el.get("velocity")
|
|
205
|
+
limits = JointLimits(
|
|
206
|
+
lower=float(lower_raw) if lower_raw is not None else None,
|
|
207
|
+
upper=float(upper_raw) if upper_raw is not None else None,
|
|
208
|
+
effort=float(effort_raw) if effort_raw is not None else None,
|
|
209
|
+
velocity=float(velocity_raw) if velocity_raw is not None else None,
|
|
210
|
+
)
|
|
211
|
+
return Joint(
|
|
212
|
+
name=el.get("name", "?"),
|
|
213
|
+
type=el.get("type", "fixed"), # type: ignore[arg-type]
|
|
214
|
+
parent=parent.get("link", "?") if parent is not None else "?",
|
|
215
|
+
child=child.get("link", "?") if child is not None else "?",
|
|
216
|
+
origin=JointOrigin(xyz=xyz, rpy=rpy),
|
|
217
|
+
axis=axis,
|
|
218
|
+
limits=limits,
|
|
219
|
+
)
|
|
220
|
+
|
|
221
|
+
|
|
222
|
+
@dataclass(frozen=True)
|
|
223
|
+
class UrdfTopology:
|
|
224
|
+
"""Link/joint sets extracted for structure-hash drift comparison."""
|
|
225
|
+
|
|
226
|
+
link_names: frozenset[str]
|
|
227
|
+
joints: tuple[Joint, ...]
|
|
228
|
+
topology_hash: str # v0.1 URDF kinematic hash (links + joint poses)
|
|
229
|
+
record_hash: str # v0.2 hash over Link/Joint records (axis + limits load-bearing)
|
|
230
|
+
|
|
231
|
+
|
|
232
|
+
_DOCTYPE_ENTITY_FORBIDDEN = (
|
|
233
|
+
"URDF DOCTYPE/ENTITY declarations are not permitted (billion-laughs risk)"
|
|
234
|
+
)
|
|
235
|
+
|
|
236
|
+
|
|
237
|
+
def _refuse_doctype_entity(raw: bytes, path: Path) -> None:
|
|
238
|
+
"""Refuse a DOCTYPE/ENTITY-bearing URDF before ``ET.fromstring`` ever sees it.
|
|
239
|
+
|
|
240
|
+
stdlib ``xml.etree`` does not resolve *external* entities, but an internal
|
|
241
|
+
entity declared and referenced within the same document still expands —
|
|
242
|
+
a 336-byte billion-laughs file grows to a multi-megabyte attribute with no
|
|
243
|
+
error. Refusing on the raw bytes, the same shape ``schema.py`` uses for
|
|
244
|
+
YAML anchors/aliases, means the parser never has a chance to expand it.
|
|
245
|
+
"""
|
|
246
|
+
if b"<!DOCTYPE" in raw or b"<!ENTITY" in raw:
|
|
247
|
+
raise ValueError(f"{path}: {_DOCTYPE_ENTITY_FORBIDDEN}")
|
|
248
|
+
|
|
249
|
+
|
|
250
|
+
def read_urdf_topology(path: Path) -> UrdfTopology:
|
|
251
|
+
"""Read on-disk URDF topology for drift checks. Raises on I/O or parse errors."""
|
|
252
|
+
raw = path.read_bytes()
|
|
253
|
+
_refuse_doctype_entity(raw, path)
|
|
254
|
+
root = ET.fromstring(raw)
|
|
255
|
+
if root.tag != "robot":
|
|
256
|
+
raise ValueError(f"{path}: root element is <{root.tag}>, expected <robot>")
|
|
257
|
+
_refuse_unresolved_xacro(root, path)
|
|
258
|
+
links = [Link(name=el.get("name", "?")) for el in root.findall("link")]
|
|
259
|
+
joints = tuple(_parse_joint(el) for el in root.findall("joint"))
|
|
260
|
+
return UrdfTopology(
|
|
261
|
+
link_names=frozenset(link.name for link in links),
|
|
262
|
+
joints=joints,
|
|
263
|
+
topology_hash=_structure_hash(root),
|
|
264
|
+
record_hash=_model_structure_hash(links, list(joints)),
|
|
265
|
+
)
|
|
266
|
+
|
|
267
|
+
|
|
268
|
+
def describe_structure_drift(model: ModelRecord, topology: UrdfTopology) -> str:
|
|
269
|
+
"""Name added/removed/moved link and joint elements; not merely 'hash differs'."""
|
|
270
|
+
contract_links = {link.name for link in model.links}
|
|
271
|
+
parts: list[str] = []
|
|
272
|
+
added_links = sorted(topology.link_names - contract_links)
|
|
273
|
+
removed_links = sorted(contract_links - topology.link_names)
|
|
274
|
+
if added_links:
|
|
275
|
+
parts.append("link added: " + ", ".join(added_links))
|
|
276
|
+
if removed_links:
|
|
277
|
+
parts.append("link removed: " + ", ".join(removed_links))
|
|
278
|
+
|
|
279
|
+
# v0.1 init records omit joints[]; only diff joints when the contract locked them.
|
|
280
|
+
if model.joints:
|
|
281
|
+
disk_joints = {joint.name: joint for joint in topology.joints}
|
|
282
|
+
contract_joints = {joint.name: joint for joint in model.joints}
|
|
283
|
+
added_joints = sorted(disk_joints.keys() - contract_joints.keys())
|
|
284
|
+
removed_joints = sorted(contract_joints.keys() - disk_joints.keys())
|
|
285
|
+
if added_joints:
|
|
286
|
+
parts.append("joint added: " + ", ".join(added_joints))
|
|
287
|
+
if removed_joints:
|
|
288
|
+
parts.append("joint removed: " + ", ".join(removed_joints))
|
|
289
|
+
|
|
290
|
+
moved: list[str] = []
|
|
291
|
+
for name in sorted(disk_joints.keys() & contract_joints.keys()):
|
|
292
|
+
disk = disk_joints[name]
|
|
293
|
+
locked = contract_joints[name]
|
|
294
|
+
changes: list[str] = []
|
|
295
|
+
if (disk.parent, disk.child) != (locked.parent, locked.child):
|
|
296
|
+
changes.append(
|
|
297
|
+
f"parent/child {locked.parent}->{locked.child} -> {disk.parent}->{disk.child}"
|
|
298
|
+
)
|
|
299
|
+
if disk.type != locked.type:
|
|
300
|
+
changes.append(f"type {locked.type} -> {disk.type}")
|
|
301
|
+
if disk.origin.xyz != locked.origin.xyz or disk.origin.rpy != locked.origin.rpy:
|
|
302
|
+
changes.append("origin moved")
|
|
303
|
+
if disk.axis != locked.axis:
|
|
304
|
+
changes.append("axis changed")
|
|
305
|
+
if disk.limits != locked.limits:
|
|
306
|
+
changes.append("limits changed")
|
|
307
|
+
if changes:
|
|
308
|
+
moved.append(f"{name} ({'; '.join(changes)})")
|
|
309
|
+
if moved:
|
|
310
|
+
parts.append("joint moved: " + ", ".join(moved))
|
|
311
|
+
elif topology.joints:
|
|
312
|
+
# Contract predates this fix (or was locked from a joint-less source): the
|
|
313
|
+
# hash may still be moving on joint changes the link diff above can't see.
|
|
314
|
+
parts.append("joints not recorded in this contract — re-run init to record them")
|
|
315
|
+
|
|
316
|
+
if not parts:
|
|
317
|
+
parts.append("kinematic topology changed")
|
|
318
|
+
return "; ".join(parts)
|
|
319
|
+
|
|
320
|
+
|
|
321
|
+
def parse_urdf(path: Path) -> ModelRecord:
|
|
322
|
+
raw = path.read_bytes()
|
|
323
|
+
_refuse_doctype_entity(raw, path)
|
|
324
|
+
root = ET.fromstring(raw)
|
|
325
|
+
if root.tag != "robot":
|
|
326
|
+
raise ValueError(f"{path}: root element is <{root.tag}>, expected <robot>")
|
|
327
|
+
_refuse_unresolved_xacro(root, path)
|
|
328
|
+
|
|
329
|
+
links: list[Link] = []
|
|
330
|
+
zero_mass: list[str] = []
|
|
331
|
+
for el in root.findall("link"):
|
|
332
|
+
link, is_zero = _parse_link(el)
|
|
333
|
+
links.append(link)
|
|
334
|
+
if is_zero:
|
|
335
|
+
zero_mass.append(link.name)
|
|
336
|
+
|
|
337
|
+
joints = [_parse_joint(el) for el in root.findall("joint")]
|
|
338
|
+
|
|
339
|
+
return ModelRecord(
|
|
340
|
+
source=str(path),
|
|
341
|
+
source_sha256=hashlib.sha256(raw).hexdigest(),
|
|
342
|
+
# v0.1 hash; overridden by the schema validator once joints (below) are non-empty.
|
|
343
|
+
structure_hash=_structure_hash(root),
|
|
344
|
+
links=links,
|
|
345
|
+
joints=joints,
|
|
346
|
+
zero_mass_links=sorted(zero_mass),
|
|
347
|
+
provenance=Provenance(witness="w2", source=str(path), locked_at=dt.date.today()),
|
|
348
|
+
)
|