Skip to content

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.py
index 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: bool
message: 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: fl
want = 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.py
index 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 | None
findings.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.py
index 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 gate
assert default_motion_checks(robot(tmp_path / "none", 1), plan) is None