Official Codebase of 3DCodeVerse
Python
articulated: deterministic axis repair before the sweep (CV3D_AXIS_REPAIR)
gaoypeng6c7ea395d24a
diff --git a/harness/codeverse/tracks/articulated_object.py b/harness/codeverse/tracks/articulated_object.pyindex 29aeb372..e2877785 100644--- a/harness/codeverse/tracks/articulated_object.py+++ b/harness/codeverse/tracks/articulated_object.py@@ -9,7 +9,9 @@ renders a pose sheet which is appended to the judge's views.from __future__ import annotationsimport logging+import osimport time+import xml.etree.ElementTree as ETfrom collections.abc import Sequencefrom pathlib import Path@@ -40,15 +42,37 @@ class ArticulatedPipeline(ObjectPipeline):def gates(self, ctx: RunContext, round_index: int, build: BuildResult, measurement: Measurement | None) -> list[GateReport]:out = super().gates(ctx, round_index, build, measurement)+ repaired = self._axis_repair(ctx)out_dir = ctx.ws.renders_dir(round_index) / "poses"report, views = ctx.services.joint_sweep(ctx.ws, ctx.plan, out_dir)ctx.extra["pose_views"] = viewsout.append(report)motion = self._motion_gate(ctx)if motion is not None:+ if repaired:+ motion.findings.append(GateFinding(+ gate=MOTION_GATE, severity=Severity.INFO, target=", ".join(repaired),+ message=(f"harness axis repair rewrote <axis xyz> of {', '.join(repaired)} in src/robot.urdf "+ "(measured motion was provably wrong) — build on it, do not undo it")))out.append(motion)return out+ @staticmethod+ def _axis_repair(ctx: RunContext) -> list[str]:+ """Measure → deterministically fix wrong joint axes BEFORE the sweep renders,+ so this round's poses, renders and judge all see the corrected motion."""+ if os.environ.get(AXIS_REPAIR_ENV) == "0":+ return []+ try:+ pre = ctx.services.motion_checks(ctx.ws, ctx.plan)+ repaired = repair_motion_axes(ctx.ws, pre)+ except Exception as e: # noqa: BLE001 — advisory repair; the gate still reports truth+ log.warning("axis repair skipped: %s", e)+ return []+ if repaired:+ ctx.events.emit("gate.axis_repaired", joints=repaired)+ return repaired+@staticmethoddef _motion_gate(ctx: RunContext) -> GateReport | None:"""Planned motion text ("pulls out to the front") vs the URDF's real direction."""@@ -230,6 +254,111 @@ def axis_fix_hint(joint_name: str, axis: Sequence[float] | None, expected: str)return f"negate the <axis> of joint '{joint_name}' in src/robot.urdf (or swap lower/upper) so motion goes {expected}"+#: kill-switch for the deterministic axis repair (default ON, mirrors CV3D_CAMERA_REPAIR)+AXIS_REPAIR_ENV = "CV3D_AXIS_REPAIR"+++def _set_axis_in_urdf_text(text: str, urdf_joint: str, axis: tuple[float, float, float]) -> str | None:+ """``text`` with joint ``urdf_joint``'s ``<axis xyz>`` replaced (inserted when+ missing). String surgery instead of an XML round-trip so authored comments and+ formatting survive; ``None`` when the joint block cannot be located safely."""+ xyz = f"{axis[0]:g} {axis[1]:g} {axis[2]:g}"+ start = -1+ pos = 0+ while (jpos := text.find("<joint", pos)) != -1:+ head_end = text.find(">", jpos)+ if head_end == -1:+ return None+ head = text[jpos:head_end]+ if f'name="{urdf_joint}"' in head or f"name='{urdf_joint}'" in head:+ start = jpos+ break+ pos = jpos + 6+ if start == -1:+ return None+ end = text.find("</joint>", start)+ if end == -1:+ return None+ block = text[start:end]+ a0 = block.find("<axis")+ if a0 != -1:+ close = block.find("/>", a0)+ if close == -1:+ return None+ new_block = block[:a0] + f'<axis xyz="{xyz}"/>' + block[close + 2:]+ else:+ head_end = block.find(">")+ if head_end == -1:+ return None+ new_block = block[: head_end + 1] + f'\n <axis xyz="{xyz}"/>' + block[head_end + 1:]+ return text[:start] + new_block + text[start + len(block):]+++def repair_motion_axes(ws: Workspace, report: GateReport | None) -> list[str]:+ """Deterministically rewrite provably-wrong ``<axis xyz>`` in ``src/robot.urdf``.++ For every motion_direction ERROR whose measured data admits an exact fix:+ anti-parallel motion (``cos <= -0.5``) negates the authored axis; otherwise a+ computed ``suggested_axis`` is written verbatim. The same fixes rode along as+ fix_hints for a whole battery and the agents applied none of them+ (ax_metronome / ax_swiss_knife, 2026-08-31): the measurement exists, so the+ execution goes deterministic. ``CV3D_AXIS_REPAIR=0`` disables. Both the+ authored file and the built ``artifacts/robot.urdf`` copy are updated so the+ re-run gate and the pose sweep see the repair. Returns repaired plan-joint+ names; a finding without measured data (``cos``) is never touched."""+ if report is None or os.environ.get(AXIS_REPAIR_ENV) == "0":+ return []+ src = ws.src / "robot.urdf"+ if not src.is_file():+ return []+ text = src.read_text()+ try:+ root = ET.fromstring(text)+ except ET.ParseError:+ return []+ joints: dict[str, ET.Element] = {j.get("name", ""): j for j in root.findall("joint")}+ by_snake = {to_snake(n): n for n in joints}+ repaired: list[str] = []+ for f in report.findings:+ if f.severity != Severity.ERROR:+ continue+ data = f.data or {}+ cos = data.get("cos")+ if cos is None:+ continue+ urdf_name = by_snake.get(to_snake(f.target or ""))+ if urdf_name is None:+ continue+ if cos <= -0.5:+ ax_el = joints[urdf_name].find("axis")+ cur = [float(v) for v in (ax_el.get("xyz") or "1 0 0").split()] if ax_el is not None else [1.0, 0.0, 0.0]+ target_axis = (-cur[0], -cur[1], -cur[2])+ elif data.get("suggested_axis"):+ sx, sy, sz = data["suggested_axis"]+ target_axis = (float(sx), float(sy), float(sz))+ else:+ continue+ new_text = _set_axis_in_urdf_text(text, urdf_name, target_axis)+ if new_text is None:+ continue+ try: # the repair must never ship a file it cannot prove well-formed and applied+ got = ET.fromstring(new_text).findall("joint")+ el = next(j for j in got if j.get("name") == urdf_name).find("axis")+ assert el is not None+ vals = [float(v) for v in el.get("xyz").split()]+ assert all(abs(a - b) < 1e-9 for a, b in zip(vals, target_axis, strict=True))+ except Exception:+ continue+ text = new_text+ repaired.append(f.target or urdf_name)+ if repaired:+ src.write_text(text)+ art = ws.artifacts / "robot.urdf"+ if art.is_file():+ art.write_text(text)+ return repaired++def default_motion_checks(ws: Workspace, plan: Plan | None) -> GateReport | None:"""Run ``motion_direction_check`` for every plan joint with an obvious motiondirection. ``None`` when the plan has no such joints; unknown joints /diff --git a/harness/tests/urdf_joints/test_axis_repair.py b/harness/tests/urdf_joints/test_axis_repair.pynew file mode 100644index 00000000..ea3980f8--- /dev/null+++ b/harness/tests/urdf_joints/test_axis_repair.py@@ -0,0 +1,95 @@+"""Deterministic axis repair: measured-wrong ``<axis xyz>`` rewritten in place."""+from __future__ import annotations++import xml.etree.ElementTree as ET++from codeverse.contracts.artifacts import GateFinding, GateReport, Severity+from codeverse.tracks.articulated_object import MOTION_GATE, repair_motion_axes+from codeverse.workspace import Workspace++URDF = """<?xml version="1.0"?>+<robot name="cab">+ <!-- authored comment must survive the repair -->+ <link name="body"/>+ <link name="door"/>+ <link name="tray"/>+ <joint name="Hinge" type="revolute">+ <parent link="body"/><child link="door"/>+ <origin xyz="-0.29 -0.2 0" rpy="0 0 0"/><axis xyz="0 0 -1"/>+ <limit lower="0" upper="1.57" effort="10" velocity="1"/>+ </joint>+ <joint name="Slide" type="prismatic">+ <parent link="body"/><child link="tray"/>+ <origin xyz="0 0 0" rpy="0 0 0"/>+ <limit lower="0" upper="0.3" effort="10" velocity="1"/>+ </joint>+</robot>+"""+++def _ws(tmp_path) -> Workspace:+ ws = Workspace(tmp_path / "run")+ ws.create()+ (ws.src / "robot.urdf").write_text(URDF)+ return ws+++def _finding(target: str, *, cos=None, suggested=None, sev=Severity.ERROR) -> GateFinding:+ data = {} if cos is None else {"cos": cos, "suggested_axis": suggested, "expected": "up"}+ return GateFinding(gate=MOTION_GATE, severity=sev, target=target, message="wrong direction", data=data)+++def _report(*findings: GateFinding) -> GateReport:+ return GateReport(gate=MOTION_GATE, passed=False, findings=list(findings))+++def _axis_of(ws: Workspace, joint: str):+ root = ET.fromstring((ws.src / "robot.urdf").read_text())+ el = next(j for j in root.findall("joint") if j.get("name") == joint).find("axis")+ return None if el is None else tuple(float(v) for v in el.get("xyz").split())+++def test_anti_parallel_negates_the_authored_axis(tmp_path):+ ws = _ws(tmp_path)+ assert repair_motion_axes(ws, _report(_finding("Hinge", cos=-0.98))) == ["Hinge"]+ assert _axis_of(ws, "Hinge") == (0.0, 0.0, 1.0)+ assert "authored comment must survive" in (ws.src / "robot.urdf").read_text()+++def test_orthogonal_writes_suggested_axis_and_inserts_missing_element(tmp_path):+ ws = _ws(tmp_path)+ assert repair_motion_axes(ws, _report(_finding("Slide", cos=0.0, suggested=(0, 0, 1)))) == ["Slide"]+ assert _axis_of(ws, "Slide") == (0.0, 0.0, 1.0)+++def test_plan_casing_maps_to_urdf_joint(tmp_path):+ ws = _ws(tmp_path)+ assert repair_motion_axes(ws, _report(_finding("hinge", cos=-1.0))) == ["hinge"]+ assert _axis_of(ws, "Hinge") == (0.0, 0.0, 1.0)+++def test_findings_without_measured_data_are_never_touched(tmp_path):+ ws = _ws(tmp_path) # the FakeServices shape: ERROR finding, empty data+ assert repair_motion_axes(ws, _report(_finding("Hinge"))) == []+ assert _axis_of(ws, "Hinge") == (0.0, 0.0, -1.0)+++def test_warn_and_unknown_joints_are_skipped(tmp_path):+ ws = _ws(tmp_path)+ rep = _report(_finding("Hinge", cos=-1.0, sev=Severity.WARN), _finding("Ghost", cos=-1.0))+ assert repair_motion_axes(ws, rep) == []+ assert _axis_of(ws, "Hinge") == (0.0, 0.0, -1.0)+++def test_kill_switch_disables(tmp_path, monkeypatch):+ monkeypatch.setenv("CV3D_AXIS_REPAIR", "0")+ ws = _ws(tmp_path)+ assert repair_motion_axes(ws, _report(_finding("Hinge", cos=-1.0))) == []+ assert _axis_of(ws, "Hinge") == (0.0, 0.0, -1.0)+++def test_artifacts_copy_is_kept_in_step(tmp_path):+ ws = _ws(tmp_path)+ (ws.artifacts / "robot.urdf").write_text(URDF)+ assert repair_motion_axes(ws, _report(_finding("Hinge", cos=-1.0))) == ["Hinge"]+ assert (ws.artifacts / "robot.urdf").read_text() == (ws.src / "robot.urdf").read_text()