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.
Files changed (52) hide show
  1. lockwire/__init__.py +9 -0
  2. lockwire/__main__.py +9 -0
  3. lockwire/advice.py +119 -0
  4. lockwire/api.py +1216 -0
  5. lockwire/api_payload.py +985 -0
  6. lockwire/bench.py +182 -0
  7. lockwire/blame.py +100 -0
  8. lockwire/catalog.py +100 -0
  9. lockwire/catalog_seed.yaml +545 -0
  10. lockwire/checks.py +3483 -0
  11. lockwire/cli.py +1799 -0
  12. lockwire/cli_line.py +73 -0
  13. lockwire/constraints.py +313 -0
  14. lockwire/copy.py +302 -0
  15. lockwire/describe.py +682 -0
  16. lockwire/edit.py +2098 -0
  17. lockwire/emit_ts.py +150 -0
  18. lockwire/export_mhs.py +115 -0
  19. lockwire/factdiff.py +254 -0
  20. lockwire/forces.py +334 -0
  21. lockwire/gitref.py +188 -0
  22. lockwire/hw.py +388 -0
  23. lockwire/mcp_server.py +276 -0
  24. lockwire/mechanism.py +379 -0
  25. lockwire/mesh_dae.py +555 -0
  26. lockwire/mesh_simple.py +198 -0
  27. lockwire/page.py +1485 -0
  28. lockwire/plan.py +849 -0
  29. lockwire/plan_payload.py +151 -0
  30. lockwire/plugins.py +70 -0
  31. lockwire/report.py +848 -0
  32. lockwire/report_html.py +707 -0
  33. lockwire/report_json.py +168 -0
  34. lockwire/review_draft.py +315 -0
  35. lockwire/scan.py +732 -0
  36. lockwire/scene.py +535 -0
  37. lockwire/scene_import.py +309 -0
  38. lockwire/scene_urdf.py +750 -0
  39. lockwire/scene_view.py +376 -0
  40. lockwire/schema.py +3616 -0
  41. lockwire/sdf.py +240 -0
  42. lockwire/serve.py +1256 -0
  43. lockwire/start_operation.py +92 -0
  44. lockwire/statics.py +77 -0
  45. lockwire/units.py +115 -0
  46. lockwire/urdf.py +348 -0
  47. lockwire/vcs.py +402 -0
  48. lockwire/whatif.py +161 -0
  49. lockwire-0.2.0.dist-info/METADATA +192 -0
  50. lockwire-0.2.0.dist-info/RECORD +52 -0
  51. lockwire-0.2.0.dist-info/WHEEL +4 -0
  52. 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
+ )