From d6b7bd6209a2dd8d9ac1256e0c71ac9d46f933a9 Mon Sep 17 00:00:00 2001 From: lxp <2770281812@qq.com> Date: Wed, 2 Sep 2026 15:17:50 +0800 Subject: [PATCH] =?UTF-8?q?=E9=80=9A=E7=94=A8=20URDF=20patch=20engine=20?= =?UTF-8?q?=E6=8A=BD=E5=8F=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/linkerhand_calibration/README.md | 8 +- .../core/urdf/__init__.py | 19 +- .../linkerhand_calibration/core/urdf/patch.py | 312 ++++++++++++++++++ .../models/g20/zero_solver.py | 186 ++--------- .../linkerhand_calibration/models/l6/urdf.py | 115 ++----- .../test/test_urdf_patch_engine.py | 164 +++++++++ 6 files changed, 563 insertions(+), 241 deletions(-) create mode 100644 src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py create mode 100644 src/linkerhand_calibration/test/test_urdf_patch_engine.py diff --git a/src/linkerhand_calibration/README.md b/src/linkerhand_calibration/README.md index b69d81b..2bfba48 100644 --- a/src/linkerhand_calibration/README.md +++ b/src/linkerhand_calibration/README.md @@ -102,10 +102,14 @@ ros2 run linkerhand_calibration calibrate_hand --config <产品配置.yaml> ## 代码边界 - `core/`：无 ROS、无具体型号，包含领域类型、PnP/旋转数学、拟合接口、统一样本 - 契约、`TaskEvaluator/SessionSolver` 协议和 `UrdfCorrectionPlan`。 + 契约、`TaskEvaluator/SessionSolver` 协议和 `UrdfCorrectionPlan`。其中 + `core/urdf/patch.py` 是所有型号共用的声明式 URDF patch engine，统一负责属性级 + 文本修改、MuJoCo equality、mesh 安全复制、禁止覆盖和原子发布。 - `runtime/`：通用会话状态机与注册 Profile 分发；ROS 消息和硬件适配只能位于 `runtime/nodes`、`runtime/adapters`。 -- `models/g20/`：G20 right-19、legacy-11、运动、零位、产物和中文诊断策略。 +- `models/g20/`、`models/l6/`：只保留型号 Profile、拟合/零位策略以及把拟合结果 + 转成 `UrdfPatchSet` 的薄适配层，不再各自实现 XML/mesh 文件写入器。新增 O6 时 + 优先新增 Profile；只有测量链或传动模型不同的部分才增加小型拟合插件。 后续型号或左右手作为新的独立 Profile 加入 `models/`，不在通用层增加分支。 - `compat/`：v1 配置、旧路径、旧会话与旧单相机逻辑。旧 Python 包名仅保留 一版最小转发 shim，不包含算法副本。 diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/__init__.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/__init__.py index 93bf5f2..19aabbb 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/__init__.py +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/__init__.py @@ -1,5 +1,22 @@ """URDF correction authorization and validation types.""" from .plan import UrdfCorrectionPlan, build_correction_plan +from .patch import ( + MujocoEqualityPatch, + UrdfJointPatch, + UrdfPatchSet, + apply_urdf_patch_text, + materialize_relative_mesh_assets, + write_urdf_patches, +) -__all__ = ["UrdfCorrectionPlan", "build_correction_plan"] +__all__ = [ + "MujocoEqualityPatch", + "UrdfCorrectionPlan", + "UrdfJointPatch", + "UrdfPatchSet", + "apply_urdf_patch_text", + "build_correction_plan", + "materialize_relative_mesh_assets", + "write_urdf_patches", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py new file mode 100644 index 0000000..8c66ef0 --- /dev/null +++ b/src/linkerhand_calibration/linkerhand_calibration/core/urdf/patch.py @@ -0,0 +1,312 @@ +"""Byte-preserving, declarative URDF patch application. + +Model profiles decide *what* values are authorized. This module owns the +shared mechanics of locating those fields in the original XML text, changing +only the declared attributes, materializing mesh resources and atomically +publishing a new file. +""" + +from __future__ import annotations + +from dataclasses import dataclass, field +import os +from pathlib import Path +import re +import shutil +from typing import Mapping, Sequence +import xml.etree.ElementTree as ET + + +@dataclass(frozen=True) +class UrdfJointPatch: + """Authorized attribute replacements inside one top-level URDF joint.""" + + origin_rpy: str | None = None + limit_lower: str | None = None + limit_upper: str | None = None + mimic_multiplier: str | None = None + mimic_offset: str | None = None + + def replacements(self) -> tuple[tuple[str, str, str], ...]: + values = ( + ("origin", "rpy", self.origin_rpy), + ("limit", "lower", self.limit_lower), + ("limit", "upper", self.limit_upper), + ("mimic", "multiplier", self.mimic_multiplier), + ("mimic", "offset", self.mimic_offset), + ) + return tuple( + (element, attribute, str(value)) + for element, attribute, value in values + if value is not None + ) + + +@dataclass(frozen=True) +class MujocoEqualityPatch: + """Replacement and optional topology assertion for one equality joint.""" + + polycoef: str + expected_joint1: str | None = None + expected_joint2: str | None = None + + +@dataclass(frozen=True) +class UrdfPatchSet: + """Complete declarative edit set for one generated URDF.""" + + joints: Mapping[str, UrdfJointPatch] + mujoco_equalities: Mapping[str, MujocoEqualityPatch] = field( + default_factory=dict + ) + + def __post_init__(self) -> None: + empty = [name for name, patch in self.joints.items() if not patch.replacements()] + if empty: + raise ValueError( + "URDF joint patch contains no replacements: " + + ",".join(sorted(empty)) + ) + + +def _replace_attribute( + block: str, element: str, attribute: str, value: str +) -> str: + pattern = re.compile( + rf"(<{element}\b[^>]*\b{attribute}\s*=\s*)([\"'])" + rf"(?P[^\"']*)\2", + re.DOTALL, + ) + match = pattern.search(block) + if match is None: + raise ValueError(f"{element} has no {attribute} attribute") + start, end = match.span("value") + return block[:start] + str(value) + block[end:] + + +def apply_urdf_patch_text( + original_text: str, + root: ET.Element, + patches: UrdfPatchSet, +) -> str: + """Apply declared patches without serializing unaffected XML.""" + top_level_joints = { + str(joint.get("name")): joint for joint in root.findall("joint") + } + missing_joints = set(patches.joints) - set(top_level_joints) + if missing_joints: + raise ValueError( + "source URDF is missing target joints: " + + ",".join(sorted(missing_joints)) + ) + + equality_nodes = { + str(joint.get("name")): joint + for joint in root.findall("./mujoco/equality/joint") + } + missing_equalities = set(patches.mujoco_equalities) - set(equality_nodes) + if missing_equalities: + raise ValueError( + "source URDF is missing MuJoCo equalities: " + + ",".join(sorted(missing_equalities)) + ) + for name, patch in patches.mujoco_equalities.items(): + node = equality_nodes[name] + if ( + patch.expected_joint1 is not None + and node.get("joint1") != patch.expected_joint1 + ): + raise ValueError(f"MuJoCo equality joint1 differs for {name}") + if ( + patch.expected_joint2 is not None + and node.get("joint2") != patch.expected_joint2 + ): + raise ValueError(f"MuJoCo equality joint2 differs for {name}") + + # Requiring the URDF ``type`` attribute excludes transmission and MuJoCo + # elements which also use the tag name ``joint``. + joint_pattern = re.compile( + r"]*\btype\s*=)[^>]*\bname\s*=\s*" + r"([\"'])(?P[^\"']+)\1[^>]*>" + r".*?", + re.DOTALL, + ) + applied_joints: set[str] = set() + + def replace_joint(match: re.Match[str]) -> str: + name = match.group("name") + patch = patches.joints.get(name) + if patch is None: + return match.group(0) + if name in applied_joints: + raise ValueError(f"duplicate top-level URDF joint text: {name}") + block = match.group(0) + for element, attribute, value in patch.replacements(): + block = _replace_attribute(block, element, attribute, value) + applied_joints.add(name) + return block + + corrected = joint_pattern.sub(replace_joint, original_text) + if applied_joints != set(patches.joints): + missing = set(patches.joints) - applied_joints + raise ValueError( + "could not locate every target joint in source URDF text: " + + ",".join(sorted(missing)) + ) + + applied_equalities: set[str] = set() + for name, patch in patches.mujoco_equalities.items(): + equality_pattern = re.compile( + rf"(]*\bname\s*=\s*([\"']))" + rf"{re.escape(name)}\2[^>]*>", + re.DOTALL, + ) + matches = list(equality_pattern.finditer(corrected)) + if len(matches) != 1: + raise ValueError(f"could not uniquely locate MuJoCo equality {name}") + match = matches[0] + replacement = _replace_attribute( + match.group(0), "joint", "polycoef", patch.polycoef + ) + corrected = corrected[: match.start()] + replacement + corrected[match.end() :] + applied_equalities.add(name) + if applied_equalities != set(patches.mujoco_equalities): + raise ValueError("could not apply every MuJoCo equality patch") + return corrected + + +def _files_have_identical_contents(left: Path, right: Path) -> bool: + if left.stat().st_size != right.stat().st_size: + return False + with left.open("rb") as left_stream, right.open("rb") as right_stream: + while True: + left_chunk = left_stream.read(1024 * 1024) + right_chunk = right_stream.read(1024 * 1024) + if left_chunk != right_chunk: + return False + if not left_chunk: + return True + + +def materialize_relative_mesh_assets( + *, source: Path, output: Path, urdf_root: ET.Element +) -> tuple[Path, ...]: + """Copy safe relative mesh resources beside the generated URDF.""" + filenames = sorted( + { + str(mesh.get("filename", "")).strip() + for mesh in urdf_root.findall(".//mesh") + if str(mesh.get("filename", "")).strip() + } + ) + materialized: list[Path] = [] + for filename in filenames: + if "://" in filename or filename.startswith("package:"): + continue + relative = Path(filename) + if relative.is_absolute() or ".." in relative.parts: + raise ValueError( + f"URDF mesh path must be a safe relative path or URI: {filename}" + ) + source_asset = (source.parent / relative).resolve() + if not source_asset.is_file(): + raise ValueError(f"URDF mesh resource does not exist: {source_asset}") + destination_asset = (output / relative).resolve() + try: + destination_asset.relative_to(output) + except ValueError as error: + raise ValueError( + f"URDF mesh destination escapes output directory: {filename}" + ) from error + if destination_asset == source_asset: + materialized.append(destination_asset) + continue + destination_asset.parent.mkdir(parents=True, exist_ok=True) + if destination_asset.exists(): + if not destination_asset.is_file() or not _files_have_identical_contents( + source_asset, destination_asset + ): + raise ValueError( + "refusing to overwrite a different mesh resource: " + f"{destination_asset}" + ) + materialized.append(destination_asset) + continue + temporary_asset = destination_asset.with_name( + f".{destination_asset.name}.{os.getpid()}.tmp" + ) + if temporary_asset.exists(): + raise ValueError(f"temporary mesh path is occupied: {temporary_asset}") + try: + shutil.copy2(source_asset, temporary_asset) + os.replace(temporary_asset, destination_asset) + finally: + if temporary_asset.exists(): + temporary_asset.unlink() + materialized.append(destination_asset) + return tuple(materialized) + + +def _copy_complete_mesh_directory(source: Path, output: Path) -> None: + source_meshes = source.parent / "meshes" + if not source_meshes.is_dir(): + return + destination_meshes = output / "meshes" + destination_meshes.mkdir(parents=True, exist_ok=True) + for mesh in source_meshes.iterdir(): + if mesh.is_file(): + shutil.copy2(mesh, destination_meshes / mesh.name) + + +def write_urdf_patches( + *, + source_urdf: str | Path, + destination_urdf: str | Path, + patches: UrdfPatchSet, + forbidden_source_stem_patterns: Sequence[str] = (), + copy_complete_mesh_directory: bool = False, +) -> Path: + """Validate and atomically materialize one patched URDF.""" + source = Path(source_urdf).expanduser().resolve() + destination = Path(destination_urdf).expanduser().resolve() + if not source.is_file(): + raise ValueError(f"source URDF does not exist: {source}") + for pattern in forbidden_source_stem_patterns: + if re.search(str(pattern), source.stem, re.IGNORECASE): + raise ValueError( + "source URDF must be the immutable original CAD URDF" + ) + if destination == source or destination.exists(): + raise ValueError(f"refusing to overwrite URDF: {destination}") + destination.parent.mkdir(parents=True, exist_ok=True) + tree = ET.parse(source) + root = tree.getroot() + corrected = apply_urdf_patch_text( + source.read_text(encoding="utf-8"), root, patches + ) + materialize_relative_mesh_assets( + source=source, output=destination.parent, urdf_root=root + ) + if copy_complete_mesh_directory: + _copy_complete_mesh_directory(source, destination.parent) + temporary = destination.with_suffix(destination.suffix + ".tmp") + try: + with temporary.open("w", encoding="utf-8") as stream: + stream.write(corrected) + stream.flush() + os.fsync(stream.fileno()) + os.replace(temporary, destination) + finally: + if temporary.exists(): + temporary.unlink() + return destination + + +__all__ = [ + "MujocoEqualityPatch", + "UrdfJointPatch", + "UrdfPatchSet", + "apply_urdf_patch_text", + "materialize_relative_mesh_assets", + "write_urdf_patches", +] diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py b/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py index e4f1b5a..0fe09a3 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/g20/zero_solver.py @@ -5,10 +5,8 @@ from __future__ import annotations from dataclasses import dataclass, replace from datetime import datetime import math -import os from pathlib import Path import re -import shutil from typing import Any, Mapping, Sequence import xml.etree.ElementTree as ET @@ -18,7 +16,13 @@ from scipy.spatial.transform import Rotation from scipy.stats import t as student_t from ...core import fit_rotation_axis, robust_rotation_summary -from ...core.urdf import UrdfCorrectionPlan +from ...core.urdf import ( + UrdfCorrectionPlan, + UrdfJointPatch, + UrdfPatchSet, + materialize_relative_mesh_assets as _materialize_relative_mesh_assets, + write_urdf_patches, +) from .profile import ( G20_RIGHT_19_LAYOUT, IMAGE_TRAJECTORY_JOINTS, @@ -4378,80 +4382,6 @@ def solve_urdf_zero_offsets( ) -def _files_have_identical_contents(left: Path, right: Path) -> bool: - if left.stat().st_size != right.stat().st_size: - return False - with left.open("rb") as left_stream, right.open("rb") as right_stream: - while True: - left_chunk = left_stream.read(1024 * 1024) - right_chunk = right_stream.read(1024 * 1024) - if left_chunk != right_chunk: - return False - if not left_chunk: - return True - - -def _materialize_relative_mesh_assets( - *, source: Path, output: Path, urdf_root: ET.Element -) -> tuple[Path, ...]: - """Copy relative mesh resources so a session-local URDF remains loadable.""" - filenames = sorted( - { - str(mesh.get("filename", "")).strip() - for mesh in urdf_root.findall(".//mesh") - if str(mesh.get("filename", "")).strip() - } - ) - materialized: list[Path] = [] - for filename in filenames: - # URI-backed resources are resolved by the URDF consumer. Only local - # relative resources need to follow a URDF copied to a session folder. - if "://" in filename or filename.startswith("package:"): - continue - relative = Path(filename) - if relative.is_absolute() or ".." in relative.parts: - raise ValueError( - f"URDF mesh path must be a safe relative path or URI: {filename}" - ) - source_asset = (source.parent / relative).resolve() - if not source_asset.is_file(): - raise ValueError(f"URDF mesh resource does not exist: {source_asset}") - destination_asset = (output / relative).resolve() - try: - destination_asset.relative_to(output) - except ValueError as error: - raise ValueError( - f"URDF mesh destination escapes output directory: {filename}" - ) from error - if destination_asset == source_asset: - materialized.append(destination_asset) - continue - destination_asset.parent.mkdir(parents=True, exist_ok=True) - if destination_asset.exists(): - if not destination_asset.is_file() or not _files_have_identical_contents( - source_asset, destination_asset - ): - raise ValueError( - f"refusing to overwrite a different mesh resource: " - f"{destination_asset}" - ) - materialized.append(destination_asset) - continue - temporary_asset = destination_asset.with_name( - f".{destination_asset.name}.{os.getpid()}.tmp" - ) - if temporary_asset.exists(): - raise ValueError(f"temporary mesh path is occupied: {temporary_asset}") - try: - shutil.copy2(source_asset, temporary_asset) - os.replace(temporary_asset, destination_asset) - finally: - if temporary_asset.exists(): - temporary_asset.unlink() - materialized.append(destination_asset) - return tuple(materialized) - - def write_zero_corrected_urdf( *, source_urdf: str | Path, @@ -4466,13 +4396,6 @@ def write_zero_corrected_urdf( output = Path(output_directory).expanduser().resolve() if not source.is_file(): raise ValueError(f"source URDF does not exist: {source}") - if ( - "zero_calibrated" in source.stem.lower() - or re.search(r"calibrated_20\d{6}", source.stem.lower()) - ): - raise ValueError( - "source_urdf must be the original CAD URDF, not a calibrated URDF" - ) if not offsets_rad: raise ValueError("offsets_rad must contain at least one joint") offsets = {str(name): float(value) for name, value in offsets_rad.items()} @@ -4496,7 +4419,6 @@ def write_zero_corrected_urdf( for value in offsets.values() ): raise ValueError("URDF zero offsets must be finite and within +/-90deg") - output.mkdir(parents=True, exist_ok=True) stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S") if re.fullmatch(r"\d{8}_\d{6}", stamp) is None: raise ValueError("URDF zero timestamp must use YYYYMMDD_HHMMSS") @@ -4506,12 +4428,8 @@ def write_zero_corrected_urdf( ) if not safe_serial: raise ValueError("serial_number must not be empty") - destination = output / f"{source.stem}_zero_calibrated_{safe_serial}_{stamp}.urdf" - if destination == source or destination.exists(): - raise ValueError(f"refusing to overwrite URDF: {destination}") tree = ET.parse(source) root = tree.getroot() - original_text = source.read_text(encoding="utf-8") replacement_rpy: dict[str, str] = {} replacement_upper: dict[str, str] = {} replacement_mimic_offset: dict[str, str] = {} @@ -4569,72 +4487,28 @@ def write_zero_corrected_urdf( missing = sorted(set(offsets) - found) if missing: raise ValueError("source URDF is missing target joints: " + ",".join(missing)) - joint_pattern = re.compile( - r"]*\bname\s*=\s*([\"'])(?P[^\"']+)\1[^>]*>" - r".*?", - re.DOTALL, + patch_names = ( + set(replacement_rpy) + | set(replacement_upper) + | set(replacement_mimic_offset) ) - edits: list[tuple[int, int, str]] = [] - for match in joint_pattern.finditer(original_text): - name = match.group("name") - if name not in replacement_rpy: - if name not in replacement_upper and name not in replacement_mimic_offset: - continue - block = match.group(0) - if name in replacement_rpy: - origin_match = re.search( - r"]*\brpy\s*=\s*([\"'])(?P[^\"']*)\1", - block, - re.DOTALL, - ) - if origin_match is None: - raise ValueError(f"joint {name} origin has no rpy attribute") - edits.append(( - match.start() + origin_match.start("rpy"), - match.start() + origin_match.end("rpy"), - replacement_rpy[name], - )) - if name in replacement_upper: - limit_match = re.search( - r"]*\bupper\s*=\s*([\"'])(?P[^\"']*)\1", - block, - re.DOTALL, - ) - if limit_match is None: - raise ValueError(f"joint {name} limit has no upper attribute") - edits.append(( - match.start() + limit_match.start("upper"), - match.start() + limit_match.end("upper"), - replacement_upper[name], - )) - if name in replacement_mimic_offset: - mimic_match = re.search( - r"]*\boffset\s*=\s*([\"'])(?P[^\"']*)\1", - block, - re.DOTALL, - ) - if mimic_match is None: - raise ValueError(f"joint {name} mimic has no offset attribute") - edits.append(( - match.start() + mimic_match.start("offset"), - match.start() + mimic_match.end("offset"), - replacement_mimic_offset[name], - )) - expected_edit_count = ( - len(replacement_rpy) - + len(replacement_upper) - + len(replacement_mimic_offset) + joint_patches = { + name: UrdfJointPatch( + origin_rpy=replacement_rpy.get(name), + limit_upper=replacement_upper.get(name), + mimic_offset=replacement_mimic_offset.get(name), + ) + for name in sorted(patch_names) + } + destination = output / ( + f"{source.stem}_zero_calibrated_{safe_serial}_{stamp}.urdf" + ) + return write_urdf_patches( + source_urdf=source, + destination_urdf=destination, + patches=UrdfPatchSet(joints=joint_patches), + forbidden_source_stem_patterns=( + r"zero_calibrated", + r"calibrated_20\d{6}", + ), ) - if len(edits) != expected_edit_count: - raise ValueError("could not locate every target joint field in source URDF text") - corrected_text = original_text - for start, end, value in reversed(edits): - corrected_text = corrected_text[:start] + value + corrected_text[end:] - _materialize_relative_mesh_assets(source=source, output=output, urdf_root=root) - temporary = destination.with_suffix(".urdf.tmp") - with temporary.open("w", encoding="utf-8") as stream: - stream.write(corrected_text) - stream.flush() - os.fsync(stream.fileno()) - os.replace(temporary, destination) - return destination diff --git a/src/linkerhand_calibration/linkerhand_calibration/models/l6/urdf.py b/src/linkerhand_calibration/linkerhand_calibration/models/l6/urdf.py index d505f23..8bbf226 100644 --- a/src/linkerhand_calibration/linkerhand_calibration/models/l6/urdf.py +++ b/src/linkerhand_calibration/linkerhand_calibration/models/l6/urdf.py @@ -5,17 +5,20 @@ from __future__ import annotations from dataclasses import dataclass from datetime import datetime import math -import os from pathlib import Path import re -import shutil from typing import Mapping import xml.etree.ElementTree as ET import numpy as np from scipy.spatial.transform import Rotation -from ..g20.zero_solver import _materialize_relative_mesh_assets +from ...core.urdf import ( + MujocoEqualityPatch, + UrdfJointPatch, + UrdfPatchSet, + write_urdf_patches, +) from .fitting import L6FitResult from .profile import ( CALIBRATED_ACTIVE_JOINTS, @@ -45,21 +48,6 @@ def _triplet(value: str) -> np.ndarray: return result -def _replace_attribute( - block: str, element: str, attribute: str, value: str -) -> str: - pattern = re.compile( - rf"(<{element}\b[^>]*\b{attribute}\s*=\s*)([\"'])" - rf"(?P[^\"']*)\2", - re.DOTALL, - ) - match = pattern.search(block) - if match is None: - raise ValueError(f"{element} has no {attribute} attribute") - start, end = match.span("value") - return block[:start] + value + block[end:] - - def _corrected_origin_rpy(joint: ET.Element, offset: float) -> str: origin = joint.find("origin") if origin is None or origin.get("rpy") is None: @@ -205,7 +193,7 @@ def write_l6_corrected_urdf( ) for name in sorted(CORRECTED_PASSIVE_JOINTS) } - equality_replacements: dict[str, str] = {} + equality_patches: dict[str, MujocoEqualityPatch] = {} for equality in root.findall("./mujoco/equality/joint"): target = str(equality.get("joint1", "")) source_name = str(equality.get("joint2", "")) @@ -224,51 +212,24 @@ def write_l6_corrected_urdf( equality_name = str(equality.get("name", "")) if not equality_name: raise ValueError(f"MuJoCo equality has no name for {target}") - equality_replacements[equality_name] = " ".join( - f"{value:.15g}" for value in coefficients + equality_patches[equality_name] = MujocoEqualityPatch( + polycoef=" ".join(f"{value:.15g}" for value in coefficients), + expected_joint1=target, + expected_joint2=source_name, ) - if len(equality_replacements) != len(coupling_polycoef): + if len(equality_patches) != len(coupling_polycoef): raise ValueError("source URDF lacks a MuJoCo equality for fitted coupling") - original = source.read_text(encoding="utf-8") - # The file also contains MuJoCo/transmission ```` elements. Requiring - # a URDF ``type`` attribute keeps the surgical block replacement confined - # to the eleven kinematic joints. - joint_pattern = re.compile( - r"]*\btype\s*=)[^>]*\bname\s*=\s*" - r"([\"'])(?P[^\"']+)\1[^>]*>" - r".*?", - re.DOTALL, - ) - - def replace_joint(match: re.Match[str]) -> str: - name = match.group("name") - block = match.group(0) - if name in active_replacements: - rpy, lower, upper = active_replacements[name] - block = _replace_attribute(block, "origin", "rpy", rpy) - block = _replace_attribute(block, "limit", "lower", lower) - block = _replace_attribute(block, "limit", "upper", upper) - if name in mimic_replacements: - block = _replace_attribute( - block, "mimic", "multiplier", mimic_replacements[name] - ) - return block - - corrected = joint_pattern.sub(replace_joint, original) - for equality_name, polycoef in equality_replacements.items(): - equality_pattern = re.compile( - rf"(]*\bname\s*=\s*([\"'])" - rf"{re.escape(equality_name)}\2[^>]*>)", - re.DOTALL, + patch_names = set(active_replacements) | set(mimic_replacements) + joint_patches = {} + for name in sorted(patch_names): + active = active_replacements.get(name) + joint_patches[name] = UrdfJointPatch( + origin_rpy=None if active is None else active[0], + limit_lower=None if active is None else active[1], + limit_upper=None if active is None else active[2], + mimic_multiplier=mimic_replacements.get(name), ) - match = equality_pattern.search(corrected) - if match is None: - raise ValueError(f"could not locate MuJoCo equality {equality_name}") - replacement = _replace_attribute( - match.group(0), "joint", "polycoef", polycoef - ) - corrected = corrected[: match.start()] + replacement + corrected[match.end() :] stamp = timestamp or datetime.now().strftime("%Y%m%d_%H%M%S") if re.fullmatch(r"\d{8}_\d{6}", stamp) is None: @@ -283,28 +244,18 @@ def write_l6_corrected_urdf( destination = output / ( f"{source.stem}_partial_zero_calibrated_{safe_serial}_{stamp}.urdf" ) - if destination.exists() or destination == source: - raise ValueError(f"refusing to overwrite URDF: {destination}") - _materialize_relative_mesh_assets(source=source, output=output, urdf_root=root) - # Keep the complete vendor mesh bundle beside the generated URDF, including - # auxiliary meshes not referenced by this particular XML revision. - source_meshes = source.parent / "meshes" - if source_meshes.is_dir(): - destination_meshes = output / "meshes" - destination_meshes.mkdir(parents=True, exist_ok=True) - for mesh in source_meshes.iterdir(): - if mesh.is_file(): - shutil.copy2(mesh, destination_meshes / mesh.name) - temporary = destination.with_suffix(".urdf.tmp") - try: - with temporary.open("w", encoding="utf-8") as stream: - stream.write(corrected) - stream.flush() - os.fsync(stream.fileno()) - os.replace(temporary, destination) - finally: - if temporary.exists(): - temporary.unlink() + write_urdf_patches( + source_urdf=source, + destination_urdf=destination, + patches=UrdfPatchSet( + joints=joint_patches, + mujoco_equalities=equality_patches, + ), + forbidden_source_stem_patterns=(r"calibrated",), + # Keep the complete vendor mesh bundle, including auxiliary meshes not + # referenced by this XML revision. + copy_complete_mesh_directory=True, + ) return L6UrdfCorrection( path=destination, origin_offsets_rad={ diff --git a/src/linkerhand_calibration/test/test_urdf_patch_engine.py b/src/linkerhand_calibration/test/test_urdf_patch_engine.py new file mode 100644 index 0000000..41d5260 --- /dev/null +++ b/src/linkerhand_calibration/test/test_urdf_patch_engine.py @@ -0,0 +1,164 @@ +from __future__ import annotations + +from pathlib import Path +import xml.etree.ElementTree as ET + +import pytest + +from linkerhand_calibration.core.urdf import ( + MujocoEqualityPatch, + UrdfJointPatch, + UrdfPatchSet, + apply_urdf_patch_text, + write_urdf_patches, +) + + +SOURCE_TEXT = """ + + + + + + + + + + + + + + + + + + + + + + + +""" + + +def _patches() -> UrdfPatchSet: + return UrdfPatchSet( + joints={ + "active": UrdfJointPatch( + origin_rpy="0 0.2 0", + limit_lower="0", + limit_upper="1.2", + ), + "passive": UrdfJointPatch( + mimic_multiplier="0.9", + mimic_offset="0.01", + ), + }, + mujoco_equalities={ + "couple": MujocoEqualityPatch( + polycoef="0.01 1.1 -0.2 0 0 0", + expected_joint1="passive", + expected_joint2="active", + ) + }, + ) + + +def test_patch_engine_changes_only_declared_attributes() -> None: + corrected = apply_urdf_patch_text( + SOURCE_TEXT, ET.fromstring(SOURCE_TEXT), _patches() + ) + + expected = SOURCE_TEXT + expected = expected.replace('rpy="0 0 0"', 'rpy="0 0.2 0"', 1) + expected = expected.replace('lower="-1"', 'lower="0"', 1) + expected = expected.replace('upper="1"', 'upper="1.2"', 1) + expected = expected.replace('multiplier="1"', 'multiplier="0.9"', 1) + expected = expected.replace('offset="0"', 'offset="0.01"', 1) + expected = expected.replace( + 'polycoef="0 1 0 0 0 0"', + 'polycoef="0.01 1.1 -0.2 0 0 0"', + 1, + ) + assert corrected == expected + assert '' in corrected + + +def test_patch_engine_rejects_missing_fields_and_wrong_topology() -> None: + root = ET.fromstring(SOURCE_TEXT) + with pytest.raises(ValueError, match="has no multiplier"): + apply_urdf_patch_text( + SOURCE_TEXT, + root, + UrdfPatchSet( + joints={"active": UrdfJointPatch(mimic_multiplier="1.1")} + ), + ) + with pytest.raises(ValueError, match="joint2 differs"): + apply_urdf_patch_text( + SOURCE_TEXT, + root, + UrdfPatchSet( + joints={}, + mujoco_equalities={ + "couple": MujocoEqualityPatch( + polycoef="0 1 0 0 0 0", + expected_joint2="wrong", + ) + }, + ), + ) + + +def test_patch_engine_atomically_writes_and_refuses_overwrite( + tmp_path: Path, +) -> None: + source = tmp_path / "source.urdf" + source.write_text(SOURCE_TEXT, encoding="utf-8") + destination = tmp_path / "output/corrected.urdf" + + result = write_urdf_patches( + source_urdf=source, + destination_urdf=destination, + patches=_patches(), + ) + + assert result == destination.resolve() + assert result.is_file() + with pytest.raises(ValueError, match="refusing to overwrite"): + write_urdf_patches( + source_urdf=source, + destination_urdf=destination, + patches=_patches(), + ) + + +def test_patch_engine_copies_referenced_and_complete_mesh_bundle( + tmp_path: Path, +) -> None: + source_dir = tmp_path / "source" + meshes = source_dir / "meshes" + meshes.mkdir(parents=True) + (meshes / "used.stl").write_bytes(b"used") + (meshes / "auxiliary.stl").write_bytes(b"auxiliary") + text = SOURCE_TEXT.replace( + '', + '' + '' + '', + ) + source = source_dir / "source.urdf" + source.write_text(text, encoding="utf-8") + destination = tmp_path / "output/corrected.urdf" + + write_urdf_patches( + source_urdf=source, + destination_urdf=destination, + patches=_patches(), + copy_complete_mesh_directory=True, + ) + + assert (destination.parent / "meshes/used.stl").read_bytes() == b"used" + assert ( + destination.parent / "meshes/auxiliary.stl" + ).read_bytes() == b"auxiliary"