通用 URDF patch engine 抽取

2026-09-02 15:17:50 +08:00
parent 8a749a3687
commit d6b7bd6209
6 changed files with 563 additions and 241 deletions
@@ -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，不包含算法副本。
@@ -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",
+]
@@ -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<value>[^\"']*)\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"<joint\b(?=[^>]*\btype\s*=)[^>]*\bname\s*=\s*"
+        r"([\"'])(?P<name>[^\"']+)\1[^>]*>"
+        r".*?</joint>",
+        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"(<joint\b[^>]*\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",
+]
@@ -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"<joint\b[^>]*\bname\s*=\s*([\"'])(?P<name>[^\"']+)\1[^>]*>"
-        r".*?</joint>",
-        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"<origin\b[^>]*\brpy\s*=\s*([\"'])(?P<rpy>[^\"']*)\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"<limit\b[^>]*\bupper\s*=\s*([\"'])(?P<upper>[^\"']*)\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"<mimic\b[^>]*\boffset\s*=\s*([\"'])(?P<offset>[^\"']*)\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
@@ -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<value>[^\"']*)\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 ``<joint>`` elements.  Requiring
-    # a URDF ``type`` attribute keeps the surgical block replacement confined
-    # to the eleven kinematic joints.
-    joint_pattern = re.compile(
-        r"<joint\b(?=[^>]*\btype\s*=)[^>]*\bname\s*=\s*"
-        r"([\"'])(?P<name>[^\"']+)\1[^>]*>"
-        r".*?</joint>",
-        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"(<joint\b[^>]*\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={
@@ -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 = """<robot name="test">
+  <link name="base"/>
+  <link name="active_link"/>
+  <link name="passive_link"/>
+  <joint name="active" type="revolute">
+    <origin xyz="1 2 3" rpy="0 0 0"/>
+    <parent link="base"/>
+    <child link="active_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="-1" upper="1" effort="1" velocity="1"/>
+  </joint>
+  <joint name="passive" type="revolute">
+    <origin xyz="4 5 6" rpy="0 0 0"/>
+    <parent link="active_link"/>
+    <child link="passive_link"/>
+    <axis xyz="0 1 0"/>
+    <limit lower="0" upper="2" effort="1" velocity="1"/>
+    <mimic joint="active" multiplier="1" offset="0"/>
+  </joint>
+  <transmission name="trans_active"><joint name="active"/></transmission>
+  <mujoco><equality>
+    <joint name="couple" joint1="passive" joint2="active" polycoef="0 1 0 0 0 0"/>
+  </equality></mujoco>
+</robot>
+"""
+
+
+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 '<transmission name="trans_active"><joint name="active"/></transmission>' 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(
+        '<link name="base"/>',
+        '<link name="base"><visual><geometry>'
+        '<mesh filename="meshes/used.stl"/>'
+        '</geometry></visual></link>',
+    )
+    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"