Official Codebase of 3DCodeVerse
Python
articulated: the motion hint states the exact axis, computed from the pivot
gaoypeng71ad2c2a34f7
diff --git a/harness/codeverse/spatial/joints_sweep.py b/harness/codeverse/spatial/joints_sweep.pyindex 7f710cb7..ba409444 100644--- a/harness/codeverse/spatial/joints_sweep.py+++ b/harness/codeverse/spatial/joints_sweep.py@@ -77,6 +77,14 @@ class MotionCheck(BaseModel):observed_dir: tuple[float, float, float]ok: boolmessage: str+ #: how aligned the observed tangent is with the expected direction (dot product)+ cos: float = 0.0+ #: an axis that WOULD send positive motion the expected way, computed from the+ #: pivot geometry (revolute: normalize(r x want); prismatic: want) — None when the+ #: lever arm is degenerate. Added 2026-08-31: af_swiss_knife failed this check on+ #: the same joints three rounds straight while the agent guessed; the geometry+ #: admits an exact answer, so the hint should state it.+ suggested_axis: tuple[float, float, float] | None = None# ------------------------------------------------------------------ sweep@@ -253,7 +261,19 @@ def motion_direction_check(robot: Robot, joint: str, expected: str, *, probe: flwant = np.asarray(_DIRS[key], dtype=float)cos = float(d @ want)ok = cos > 0.5+ suggested = None+ if not ok:+ if j.type == "prismatic":+ suggested = tuple(round(float(v), 3) for v in want)+ else:+ pivot = fk(robot, {})[j.child][:3, 3]+ r = c0 - pivot+ s = np.cross(r, want)+ n_s = float(np.linalg.norm(s))+ if n_s > 1e-6:+ suggested = tuple(round(float(v), 3) for v in s / n_s)return MotionCheck(joint=joint, expected=expected, observed_dir=tuple(round(float(v), 4) for v in d), ok=ok,+ cos=round(cos, 4), suggested_axis=suggested,message=(f"{joint}: child '{j.child}' moves {tuple(round(float(v),3) for v in d)} for q={probe:+.3g}; "f"expected {expected} ({'ok' if ok else 'WRONG — flip the axis sign or swap limits'})"))diff --git a/harness/codeverse/tracks/articulated_object.py b/harness/codeverse/tracks/articulated_object.pyindex 9c384087..29aeb372 100644--- a/harness/codeverse/tracks/articulated_object.py+++ b/harness/codeverse/tracks/articulated_object.py@@ -260,13 +260,23 @@ def default_motion_checks(ws: Workspace, plan: Plan | None) -> GateReport | Nonefindings.append(GateFinding(gate=MOTION_GATE, severity=Severity.WARN, target=j.name,message=f"motion check skipped: {e}", data={"expected": expected}))continue- data = {"expected": expected, "observed_dir": list(chk.observed_dir), "motion": j.motion}+ data = {"expected": expected, "observed_dir": list(chk.observed_dir), "motion": j.motion,+ "cos": chk.cos, "suggested_axis": list(chk.suggested_axis) if chk.suggested_axis else None}if chk.ok:findings.append(GateFinding(gate=MOTION_GATE, severity=Severity.INFO, target=j.name, message=chk.message, data=data))else:+ # anti-parallel: the sign is provably wrong — negate. Orthogonal (the+ # af_swiss_knife class: three rounds of guessing on the same joints): the+ # geometry admits an exact axis, so the hint states it verbatim.+ if chk.cos >= -0.5 and chk.suggested_axis is not None:+ sx, sy, sz = chk.suggested_axis+ hint = (f"in src/robot.urdf set joint '{j.name}' <axis xyz=\"{sx:g} {sy:g} {sz:g}\"/> "+ f"— computed from the pivot so positive motion goes {expected}")+ else:+ hint = axis_fix_hint(j.name, getattr(j, "axis", None), expected)findings.append(GateFinding(gate=MOTION_GATE, severity=Severity.ERROR, target=j.name,message=f"{chk.message}. Plan says: \"{j.motion}\"",- fix_hint=axis_fix_hint(j.name, getattr(j, "axis", None), expected), data=data))+ fix_hint=hint, data=data))return GateReport(gate=MOTION_GATE, passed=not any(f.severity == Severity.ERROR for f in findings),findings=findings, duration_ms=int((time.time() - t0) * 1000))diff --git a/harness/tests/orchestrator_tracks/test_candidates_reference_motion.py b/harness/tests/orchestrator_tracks/test_candidates_reference_motion.pyindex 1662c3c8..d8f94e0f 100644--- a/harness/tests/orchestrator_tracks/test_candidates_reference_motion.py+++ b/harness/tests/orchestrator_tracks/test_candidates_reference_motion.py@@ -380,6 +380,22 @@ def test_default_motion_checks_on_real_urdf(tmp_path):bad = default_motion_checks(robot(tmp_path / "bad", 1), plan)assert bad is not None and not bad.passed and bad.errors[0].target == "DoorHinge"assert "WRONG" in bad.errors[0].message and '<axis xyz="-0 -0 -1"/>' in bad.errors[0].fix_hint+ # orthogonal (the swiss-knife class): a prismatic joint sliding +x when the plan says+ # up — cos = 0 exactly, and the hint states the computed axis verbatim+ import copy+ ortho = robot(tmp_path / "ortho", 1)+ u = (ortho.artifacts / "robot.urdf").read_text()+ u = u.replace('type="revolute"', 'type="prismatic"').replace('<axis xyz="0 0 1"/>', '<axis xyz="1 0 0"/>')+ (ortho.artifacts / "robot.urdf").write_text(u)+ slide_plan = copy.deepcopy(plan)+ slide_plan.joints[0].type = "prismatic"+ slide_plan.joints[0].motion = "slides straight up"+ o = default_motion_checks(ortho, slide_plan)+ assert o is not None and not o.passed+ # URDF space is Z-up: 'up' is +z there (the harness converts frames at the boundary)+ assert "computed from the pivot" in o.errors[0].fix_hint and '<axis xyz="0 0 1"/>' in o.errors[0].fix_hint+ assert o.errors[0].data.get("suggested_axis") == [0.0, 0.0, 1.0]+plan.joints[0].motion = "rotates" # ambiguous → no gateassert default_motion_checks(robot(tmp_path / "none", 1), plan) is None