Skip to content

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.py
index 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 annotations
import logging
+import os
import time
+import xml.etree.ElementTree as ET
from collections.abc import Sequence
from 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"] = views
out.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
+
@staticmethod
def _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 motion
direction. ``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.py
new file mode 100644
index 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()