arm_dev
c1cd862eea
这批是 09-24 那轮仿真工作的收尾，一直没提交。内容按目的分四块： 1) 仿真真能「夹住」工件 sim_grasp_lock：给规与夹爪之间加接触锁（默认关，run_gauge_assembly --sim 才开）。 没有它时仿真里规只靠摩擦，S08 棘轮一转就滑出指间，于是把「仿真不真」误读成 「力判据不对」。新增 test_sim_grasp_lock.py。 2) 腕部力传感器进仿真 sim_wrist_ft（+ config/wrist_ft.yaml）：S07 的首选判据是腕部 FT 的轴向突变， 而仿真里以前根本没有腕部力，S07 只能退到触觉兜底，两套判据从没在同一场景下比过。 现在两条通道都在，接触判定可对照。 3) 真值位姿可查（不再靠猜） sim_live_pose / sim_pose_service：把 Gazebo 里刚体的实测位姿发布出来， stage2 的真值路径按**实测**位姿算孔位，而不是按场景文件里的标称位姿。 顺带把真值孔位的推导（truth_holes）独立出来并补测。 4) 工具几何成为可配置项 config/sim_tool_geometry.yaml + stage2 的 tool_tip_below_flange_m 启动参数： 仿真里规会在指间下滑，工具长每次装夹都不同（实测见过 243/254mm）， 悬停高度与 S07 下探预算必须按实测值算，不能再写死。 应用层同时跟进：gauge_fsm / gauge_config 把「悬停时工具尖端入没入工件」的判据 （hover_z_guard）与拖件判定（drag_verdict）收成纯函数并补测；stage3 搜孔、 site 导出、run_gauge_assembly 与若干文档同步。 新增 test_gauge_exit_lift / test_gauge_gripper_client / test_node_handler_collisions。 测试：全仓库 1085 passed。
611 lines
26 KiB
Python
611 lines
26 KiB
Python
#!/usr/bin/env python3
|
||
"""仿真场景验收：摆放对不对、孔检测的成像几何够不够、粗定位看不看得见工件。
|
||
|
||
=============================================================================
|
||
为什么需要这个工具
|
||
=============================================================================
|
||
场景里的错误和相机一样，几乎都是**静默**的 —— 跑得起来、报不出错，
|
||
只是"结果不对"。这个脚本把四类问题在启动阶段就变成可打印的结论：
|
||
|
||
1. 摆放讲不通：悬空 / 穿进台面 / 超出台面 / 两个物体互相嵌入。
|
||
表现是"工件自己会动、会飞"，而人会怀疑控制或标定。
|
||
|
||
2. 场景文件里的像素门槛与真机配置**漂移**。
|
||
scene.yaml 为了能在启动时就算出"能不能看见"，抄了一份门槛值；
|
||
抄一份就有漂移风险。这里读 hole_detect.yaml + site.yaml 的生效值做核对，
|
||
不一致直接判失败 —— 不允许"仿真按旧门槛通过、真机按新门槛不认"。
|
||
|
||
3. 成像几何不够用：亮环太小 / 两孔间距不够像素门槛。
|
||
这些门槛是**像素**量，只有在仿真侧用几何算出来才能提前发现；
|
||
等上真机再发现要贵得多（要重新调悬停高度、甚至改工件工装）。
|
||
|
||
4. 二阶段的悬停高度**超出机械臂工作空间**。
|
||
这个坑很实在：相机到工件的距离决定了像素尺度，而"距离够近"与
|
||
"法兰别抬太高"是一对矛盾。脚本会把两者都算出来并核对工作空间上下界。
|
||
|
||
5. 粗定位（D435）看不看得见工件。
|
||
二阶段粗居中靠 D435 出工件的 bbox；工件被裁掉一角时检测仍会出结果
|
||
（框不全而已），于是算出的中心偏掉 —— 这属于"能出结果但结果是错的"。
|
||
|
||
用法（需要 ROS 环境以读 cameras.yaml 内参；查 Gazebo 位姿需要仿真在跑）：
|
||
source scripts/sim_env.sh
|
||
python3 scripts/check_sim_scene.py
|
||
python3 scripts/check_sim_scene.py --no-poses # 跳过"查 Gazebo 实际位姿"
|
||
|
||
退出码 0 = 通过；非 0 = 有问题（可直接用于启动自检 / CI）。
|
||
=============================================================================
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import argparse
|
||
import math
|
||
import subprocess
|
||
import sys
|
||
from pathlib import Path
|
||
from typing import Optional
|
||
|
||
REPO_ROOT = Path(__file__).resolve().parents[1]
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# 小工具
|
||
# ---------------------------------------------------------------------------
|
||
def rpy_to_R(r: float, p: float, y: float):
|
||
"""rpy（ZYX 复合顺序：R = Rz·Ry·Rx）→ 3x3 旋转矩阵。
|
||
|
||
顺序不能想当然：URDF/SDF 的 rpy 都是 Rz·Ry·Rx。
|
||
"先俯 50° 再偏航 180°" 与 "先偏航再俯" 的结果**不同**，
|
||
这个项目就为此把 D435 的视线翻到背后过（见 assembly.world 的注释）。
|
||
"""
|
||
cr, sr = math.cos(r), math.sin(r)
|
||
cp, sp = math.cos(p), math.sin(p)
|
||
cy, sy = math.cos(y), math.sin(y)
|
||
return [
|
||
[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
|
||
[sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
|
||
[-sp, cp * sr, cp * cr],
|
||
]
|
||
|
||
|
||
def matmul3(a, b):
|
||
return [[sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3)] for i in range(3)]
|
||
|
||
|
||
def matvec3(a, v):
|
||
return [sum(a[i][k] * v[k] for k in range(3)) for i in range(3)]
|
||
|
||
|
||
def transpose3(a):
|
||
return [[a[j][i] for j in range(3)] for i in range(3)]
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# 门槛核对：scene.yaml 抄的那份 vs 真机真正生效的那份
|
||
# ---------------------------------------------------------------------------
|
||
def effective_gates(root: Optional[Path] = None) -> dict:
|
||
"""读出真机**实际生效**的孔检测像素门槛。
|
||
|
||
生效规则（见 arm_application/hole_detector.config_from_site）：
|
||
包内 config/hole_detect.yaml 给默认值，config/site.yaml 的 hole_detect.* 覆盖它。
|
||
|
||
半径取两个区间的**交集**：HoughCircles 的搜索范围（hough_min/max_radius）
|
||
与工业校验的接受范围（min_radius/max_radius）必须同时满足才叫"能被用上"。
|
||
只按其中一个判，会得出"能检测到"的乐观结论，而实际在另一个环节被滤掉。
|
||
"""
|
||
import yaml
|
||
|
||
base_root = Path(root) if root is not None else REPO_ROOT
|
||
pkg = base_root / "src/arm_application/config/hole_detect.yaml"
|
||
site = base_root / "config/site.yaml"
|
||
if not site.is_file():
|
||
site = base_root / "config/site.example.yaml"
|
||
|
||
base = {}
|
||
if pkg.is_file():
|
||
base = yaml.safe_load(pkg.read_text(encoding="utf-8")) or {}
|
||
over = {}
|
||
if site.is_file():
|
||
doc = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
|
||
over = (doc.get("hole_detect") or {}) if isinstance(doc, dict) else {}
|
||
|
||
def val(key, default):
|
||
v = over.get(key, base.get(key, default))
|
||
try:
|
||
return float(v)
|
||
except (TypeError, ValueError):
|
||
return float(default)
|
||
|
||
lo = max(val("hough_min_radius", 10), val("min_radius", 10))
|
||
hi = min(val("hough_max_radius", 30), val("max_radius", 30))
|
||
return {
|
||
"insert_ring_radius": (lo, hi),
|
||
"hole_spacing": (val("hole_distance_min", 150), val("hole_distance_max", 700)),
|
||
"_source": (str(site), str(pkg)),
|
||
}
|
||
|
||
|
||
def sim_robot_servo(doc: dict) -> dict:
|
||
"""仿真那台臂的 servo 段（顶层 servo 被 robots[] 里 sim 那台覆盖后的结果）。
|
||
|
||
⚠️ 必须合并，不能只读顶层：顶层那套是**真机**的（台面与基座同高，所以
|
||
工作盒 z 是 0.18~0.65），而仿真里机械臂站在地面、台面 0.40 高，一切 Z 都
|
||
大 0.412。只读顶层会拿真机的界去核对仿真的悬停高 —— 看起来"过了"，
|
||
而真正生效的配置可能根本不对（或者反过来，明明配对了却报越界）。
|
||
"""
|
||
servo = dict(doc.get("servo") or {})
|
||
for robot in (doc.get("robot") or {}).get("robots") or []:
|
||
if str(robot.get("arm") or "") == "sim" or str(robot.get("sn") or "") == "sim":
|
||
servo.update(robot.get("servo") or {})
|
||
return servo
|
||
|
||
|
||
def workspace_z_limits(root: Optional[Path] = None):
|
||
"""机械臂法兰的工作空间 Z 上下界（按**仿真那台臂**的生效值）。"""
|
||
import yaml
|
||
|
||
base_root = Path(root) if root is not None else REPO_ROOT
|
||
site = base_root / "config/site.yaml"
|
||
if not site.is_file():
|
||
site = base_root / "config/site.example.yaml"
|
||
if not site.is_file():
|
||
return None
|
||
doc = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
|
||
servo = sim_robot_servo(doc)
|
||
try:
|
||
return float(servo["workspace_xyz_min"][2]), float(servo["workspace_xyz_max"][2])
|
||
except (KeyError, IndexError, TypeError, ValueError):
|
||
return None
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# 检查项
|
||
# ---------------------------------------------------------------------------
|
||
def check_placement(scene, root) -> list[str]:
|
||
from arm_sim_gazebo.sim_scene import validate_scene
|
||
|
||
return validate_scene(scene, root)
|
||
|
||
|
||
def check_gate_drift(scene, root=None) -> list[str]:
|
||
"""scene.yaml 的 gates_px 是否与真机生效值一致。"""
|
||
eff = effective_gates(root)
|
||
out = []
|
||
for key, gate in scene.gates_px.items():
|
||
want = eff.get(key)
|
||
if want is None:
|
||
continue
|
||
if abs(gate[0] - want[0]) > 1e-6 or abs(gate[1] - want[1]) > 1e-6:
|
||
out.append(
|
||
f"gates_px.{key} 写的是 [{gate[0]:g}, {gate[1]:g}]，"
|
||
f"而真机生效值是 [{want[0]:g}, {want[1]:g}]。\n"
|
||
f" 真机值来自 {eff['_source'][0]}（覆盖 {eff['_source'][1]}）。\n"
|
||
f" 这不是小事：两边不一致时，仿真会按旧门槛判'能检测到'，"
|
||
f"而真机按新门槛不认 —— 属于最费时间的一类问题。\n"
|
||
f" 修法：把 devices/sim/gazebo/config/scene.yaml 的 gates_px 改成真机值。"
|
||
)
|
||
missing = set(eff) - {"_source"} - set(scene.gates_px)
|
||
for key in sorted(missing):
|
||
out.append(
|
||
f"gates_px 里缺 {key}（真机有这个门槛 {eff[key]}）—— "
|
||
f"缺了就不会校验它，属于「静默放行」。补上它。"
|
||
)
|
||
return out
|
||
|
||
|
||
def check_geometry(scene, root) -> tuple[list[str], dict]:
|
||
"""孔检测的像素几何：算出实际像素值，与门槛比。"""
|
||
from arm_sim_gazebo.sim_cameras import intrinsics, load_cameras
|
||
from arm_sim_gazebo.sim_scene import check_pixel_gates, operating_pixel_report
|
||
|
||
specs = load_cameras(root=root)
|
||
cam = next((s for s in specs if s.ros_ns == scene.operating.camera), None)
|
||
if cam is None:
|
||
return [f"cameras.yaml 里没有相机 {scene.operating.camera}"], {}
|
||
k, _ = intrinsics(cam, cam.width or 640, cam.height or 480)
|
||
px = operating_pixel_report(scene, k[0], root)
|
||
if not px:
|
||
return ["读不出工件模型里的检测特征（visual 名字约定变了？见 sim_scene.features_from_workpiece）"], {}
|
||
return check_pixel_gates(px, scene.gates_px), px
|
||
|
||
|
||
def check_hover_height(scene, root) -> list[str]:
|
||
"""悬停高度是否落在机械臂工作空间的 Z 区间内。"""
|
||
from arm_sim_gazebo.sim_scene import operating_flange_z
|
||
|
||
fz = operating_flange_z(scene, root)
|
||
if fz is None:
|
||
return ["读不出工件高度，算不出建议悬停高度"]
|
||
lim = workspace_z_limits(root)
|
||
if lim is None:
|
||
return []
|
||
lo, hi = lim
|
||
out = []
|
||
if fz > hi:
|
||
out.append(
|
||
f"按当前 camera_to_part_m={scene.operating.camera_to_part_m}m 算出的悬停法兰高度 "
|
||
f"z≈{fz:.3f}m **超出工作空间上界 {hi}m**。\n"
|
||
f" 二阶段的位姿检查会直接拒绝执行。\n"
|
||
f" 两个旋钮：调小 camera_to_part_m（相机更贴近工件 → 像素尺度更大、"
|
||
f"更容易过像素门槛），或把工件/台面改矮。\n"
|
||
f" 注意这两者是对立的：离得越近像素越大（检测更好），但法兰越高（越够不着）。"
|
||
)
|
||
elif fz < lo:
|
||
out.append(f"算出的悬停法兰高度 z≈{fz:.3f}m 低于工作空间下界 {lo}m。")
|
||
|
||
# 一阶段跟踪高还要再抬 TRACK_CLEARANCE_M（见 pipeline_node 的常量）。
|
||
# 单独查这一条的原因：二阶段能过、S04 一跟踪就被"超出工作盒"截断，
|
||
# 现象是"跟踪位置总差一截/越跟踪越偏"，而配置看着完全合理。
|
||
try:
|
||
from arm_application.pipeline_node import TRACK_CLEARANCE_M
|
||
except Exception: # noqa: BLE001 —— 拿不到常量就不查，不因此拦人
|
||
return out
|
||
track = fz + float(TRACK_CLEARANCE_M)
|
||
if track > hi:
|
||
out.append(
|
||
f"一阶段跟踪高 = 悬停 {fz:.3f} + {TRACK_CLEARANCE_M:.2f} = {track:.3f}m "
|
||
f"**超出工作空间上界 {hi}m**。\n"
|
||
f" 二阶段的位姿检查能过、S04 一跟踪却会被「超出工作盒」截断 —— "
|
||
f"现象是「越跟踪越偏」，很难往配置上想。\n"
|
||
f" 修法（仿真里都自由）：把 site.yaml 里仿真那台臂的 "
|
||
f"servo.workspace_xyz_max.z 抬到 ≥ {track:.3f}，"
|
||
f"或调小 scene.yaml 的 camera_to_part_m（法兰随之降低）。"
|
||
)
|
||
return out
|
||
|
||
|
||
def check_d435_visibility(scene, root) -> tuple[list[str], str]:
|
||
"""把工件顶面的四角投影到 D435 画面，看是否完整可见。
|
||
|
||
这一步对应二阶段的**粗定位**：D435 出工件的 bbox，取其中心作为粗目标。
|
||
工件被裁掉一角时检测照样出结果（框小了而已），算出的中心就偏了 ——
|
||
"能出结果但结果是错的"，正是这个项目反复强调的那一类。
|
||
"""
|
||
from arm_sim_gazebo.sim_cameras import intrinsics, load_cameras
|
||
from arm_sim_gazebo.sim_scene import world_boxes
|
||
from arm_sim_gazebo.robot_description import model_link_pose
|
||
from arm_sim_gazebo.sim_site import resolve_world
|
||
|
||
specs = load_cameras(root=root)
|
||
cam = next((s for s in specs if s.is_world_mounted), None)
|
||
if cam is None:
|
||
return [], "（cameras.yaml 里没有世界相机，跳过）"
|
||
|
||
part = next((o for o in scene.objects if o.name == scene.operating.part_object), None)
|
||
if part is None:
|
||
return [f"场景里没有 {scene.operating.part_object!r}，无法核对粗定位可见性"], ""
|
||
|
||
world_path, _ = resolve_world("", root)
|
||
try:
|
||
(px, py, pz), (rr, rp, ry) = model_link_pose(Path(world_path), cam.world_model, cam.world_link)
|
||
except Exception as exc:
|
||
return [f"读不出 {cam.world_model} 在世界里的位姿：{exc}"], ""
|
||
|
||
boxes = world_boxes(scene, part, root)
|
||
if not boxes:
|
||
return ["读不出工件的碰撞体，无法投影"], ""
|
||
top = max(b[5] for b in boxes)
|
||
x0 = min(b[0] for b in boxes)
|
||
x1 = max(b[1] for b in boxes)
|
||
y0 = min(b[2] for b in boxes)
|
||
y1 = max(b[3] for b in boxes)
|
||
corners = [(x0, y0, top), (x0, y1, top), (x1, y0, top), (x1, y1, top)]
|
||
|
||
w, h = cam.width or 640, cam.height or 480
|
||
k, _ = intrinsics(cam, w, h)
|
||
fx, fy, cx, cy = k[0], k[4], k[2], k[5]
|
||
|
||
R_wb = rpy_to_R(rr, rp, ry)
|
||
# 本体 → 光学：相机本体是"X 朝前/Y 左/Z 上"，光学系是"Z 前/X 右/Y 下"
|
||
R_bo = rpy_to_R(-math.pi / 2, 0.0, -math.pi / 2)
|
||
R_wo = matmul3(R_wb, R_bo)
|
||
R_ow = transpose3(R_wo)
|
||
t = [px, py, pz]
|
||
|
||
probs: list[str] = []
|
||
us, vs = [], []
|
||
for c in corners:
|
||
d = [c[i] - t[i] for i in range(3)]
|
||
o = matvec3(R_ow, d)
|
||
if o[2] <= 1e-6:
|
||
probs.append(
|
||
f"工件有一个角在 {cam.ros_ns} 的**背后**（光学系 z={o[2]:.3f}）——"
|
||
f"相机朝向反了或工件跑到相机后面了。"
|
||
)
|
||
return probs, ""
|
||
us.append(cx + fx * o[0] / o[2])
|
||
vs.append(cy + fy * o[1] / o[2])
|
||
|
||
u0, u1, v0, v1 = min(us), max(us), min(vs), max(vs)
|
||
info = (f"{cam.ros_ns}: 工件顶面四角投影 u∈[{u0:.0f},{u1:.0f}] v∈[{v0:.0f},{v1:.0f}] "
|
||
f"（画面 {w}x{h}）")
|
||
if u0 < 0 or u1 > w or v0 < 0 or v1 > h:
|
||
probs.append(
|
||
f"工件没有完整落在 {cam.ros_ns} 画面内（{info}）。\n"
|
||
f" 粗定位会框不全工件 → bbox 中心偏 → **照样出结果但结果是错的**。\n"
|
||
f" 修法：挪工件（scene.yaml 的 pose）、挪相机（world 里的 pose），"
|
||
f"或确认这台相机的视野本来就该覆盖整个台面。"
|
||
)
|
||
return probs, info
|
||
|
||
|
||
def check_table_slot(scene, root) -> tuple[list[str], str]:
|
||
"""台面通槽 vs 工件：既要"规的通端过得去"，又要"工件掉不下去"。
|
||
|
||
两个方向都会出事，而且都是**静默**的（2026-09-24 各栽过一次）：
|
||
|
||
a) 槽开得不够 **窄**（< 规的通端）：规下压到 12mm 就顶在台面上，
|
||
S07 一路压到上限都等不到台肩落面 → 报"没触发接触判据"，
|
||
看起来像传感器/阈值坏了。
|
||
b) 槽开得**太宽**：工件跨在槽上时承压面太小，一有扰动就翻进槽里、
|
||
落到地面 —— 于是 `perception.source=truth` 开始撒谎：孔位按名义位姿
|
||
投到像素上，而孔位正下方已经不是工件（是槽底/地面），
|
||
深度反投影把 58mm 孔距算成 169mm，S05 连丢 9 拍超时。
|
||
|
||
所以这里同时查两件事：孔位必须落在**开口**内，承压宽度必须**够**。
|
||
"""
|
||
import xml.etree.ElementTree as ET
|
||
|
||
from arm_sim_gazebo.sim_scene import rings_local, world_boxes
|
||
from arm_sim_gazebo.sim_site import resolve_world
|
||
|
||
world_path, _ = resolve_world("", root)
|
||
try:
|
||
tree = ET.parse(world_path)
|
||
except (OSError, ET.ParseError) as exc:
|
||
return [f"读不出 world（{world_path}）：{exc}"], ""
|
||
table = None
|
||
for m in tree.getroot().iter("model"):
|
||
if m.get("name") == "work_table":
|
||
table = m
|
||
break
|
||
if table is None:
|
||
return [f"{world_path} 里没有 <model name=\"work_table\"> —— 台面被改名了？"], ""
|
||
|
||
def _pose_xyz(node) -> tuple[float, float, float]:
|
||
p = node.find("pose")
|
||
if p is None or not (p.text or "").strip():
|
||
return (0.0, 0.0, 0.0)
|
||
v = [float(x) for x in p.text.split()[:3]]
|
||
while len(v) < 3:
|
||
v.append(0.0)
|
||
return (v[0], v[1], v[2])
|
||
|
||
my = _pose_xyz(table)[1]
|
||
spans: list[tuple[float, float]] = []
|
||
for link in table.findall("link"):
|
||
ly = my + _pose_xyz(link)[1]
|
||
for col in link.findall("collision"):
|
||
box = col.find("geometry/box/size")
|
||
if box is None or not (box.text or "").strip():
|
||
continue
|
||
cy = ly + _pose_xyz(col)[1]
|
||
half = float(box.text.split()[1]) / 2.0
|
||
spans.append((cy - half, cy + half))
|
||
if not spans:
|
||
return ["work_table 里读不到任何 box 碰撞体 —— 台面几何解析不出来"], ""
|
||
spans.sort()
|
||
|
||
b = scene.table_bounds
|
||
# 开口 = 台面范围内的"没被碰撞体盖住"的 y 区间。
|
||
gaps: list[tuple[float, float]] = []
|
||
cur = b.y_min
|
||
for lo, hi in spans:
|
||
if lo > cur + 1e-9:
|
||
gaps.append((cur, min(lo, b.y_max)))
|
||
cur = max(cur, hi)
|
||
if cur < b.y_max - 1e-9:
|
||
gaps.append((cur, b.y_max))
|
||
gaps = [(lo, hi) for lo, hi in gaps if hi - lo > 1e-6]
|
||
|
||
part = next((o for o in scene.objects if o.name == scene.operating.part_object), None)
|
||
if part is None:
|
||
return [f"场景里没有 {scene.operating.part_object!r}"], ""
|
||
boxes = world_boxes(scene, part, root)
|
||
if not boxes:
|
||
return ["读不出工件的碰撞体"], ""
|
||
wy0 = min(float(v) for bx in boxes for v in (bx[2], bx[3]))
|
||
wy1 = max(float(v) for bx in boxes for v in (bx[2], bx[3]))
|
||
|
||
# 承压宽度 = 工件在 y 方向上被台面碰撞体盖住的长度（区间求交后累加）。
|
||
supported = 0.0
|
||
for lo, hi in spans:
|
||
supported += max(0.0, min(hi, wy1) - max(lo, wy0))
|
||
|
||
rings = rings_local(scene, part, root)
|
||
hole_ys: list[float] = []
|
||
for r in rings:
|
||
c, s = math.cos(float(part.yaw)), math.sin(float(part.yaw))
|
||
hole_ys.append(float(part.y) + s * float(r.x) + c * float(r.y))
|
||
|
||
probs: list[str] = []
|
||
info = (
|
||
f"台面沿 y 的支撑区间 {[(round(lo, 4), round(hi, 4)) for lo, hi in spans]}；"
|
||
f"开口 {[(round(lo, 4), round(hi, 4)) for lo, hi in gaps]}；"
|
||
f"工件 y∈[{wy0:.4f},{wy1:.4f}] 承压 {supported * 1000:.1f}mm"
|
||
)
|
||
|
||
# (a) 孔位必须落在开口里 —— 否则规的通端顶在台面上，S07 永远等不到台肩落面。
|
||
for i, hy in enumerate(hole_ys):
|
||
ok = any(lo < hy < hi for lo, hi in gaps)
|
||
if not ok:
|
||
probs.append(
|
||
f"第 {i + 1} 个孔（世界 y={hy:+.4f}）**不在台面开口内** —— "
|
||
f"规的通端会顶在台面上。\n"
|
||
f" 后果：S07 下压 12mm 就到底，一路到上限都等不到台肩落面，"
|
||
f"报「没触发接触判据」，看起来像传感器/阈值坏了。\n"
|
||
f" 修法：把开口挪到孔位下方（worlds/assembly.world 的 work_table "
|
||
f"碰撞体 y 区间），或改工件摆放。\n"
|
||
f" 当前开口：{[(round(lo, 4), round(hi, 4)) for lo, hi in gaps]}"
|
||
)
|
||
|
||
# (b) 承压宽度必须够 —— 否则工件会翻进槽里掉下去，而真值感知会继续按
|
||
# 名义位姿算，把 58mm 孔距算成 169mm（见 truth_holes.HoleTruth 的说明）。
|
||
min_support = 0.020
|
||
if supported < min_support:
|
||
probs.append(
|
||
f"工件跨在台面开口上的**承压宽度只有 {supported * 1000:.1f}mm**"
|
||
f"（要求 ≥ {min_support * 1000:.0f}mm）。\n"
|
||
f" 后果：板一有扰动就翻进槽里、落到地面；而 "
|
||
f"perception.source=truth 仍按 scene.yaml 的**名义位姿**投孔 → "
|
||
f"孔位下方已经不是工件而是槽底/地面 →\n"
|
||
f" 深度反投影把 58mm 孔距算成 169mm → S05 每拍都丢、最后超时。"
|
||
f"现象看起来是「检测坏了」，根因却是**工件掉了**。\n"
|
||
f" 修法：把开口收窄（改 worlds/assembly.world 里 work_table 碰撞体的 "
|
||
f"y 尺寸/位姿，开口只要够规的通端过就行，不需要覆盖两个孔所在的整个 y 带）。"
|
||
)
|
||
return probs, info
|
||
|
||
|
||
def check_gazebo_poses(scene, root) -> tuple[list[str], str]:
|
||
"""查 Gazebo 里物体的实际位姿，与 scene.yaml 对照。
|
||
|
||
这一步能抓到"spawn 了但掉下去/被顶飞"这类只在运行后暴露的问题。
|
||
需要仿真在跑；不在跑就跳过（不算失败）。
|
||
"""
|
||
from arm_sim_gazebo.sim_scene import parse_spawned_poses, pose_mismatches
|
||
|
||
try:
|
||
r = subprocess.run(
|
||
["ign", "model", "--list"],
|
||
capture_output=True,
|
||
text=True,
|
||
timeout=20,
|
||
)
|
||
except (OSError, subprocess.TimeoutExpired) as exc:
|
||
return [], f"（跳过：调不动 ign（{exc}））"
|
||
if r.returncode != 0 or "Requesting state" not in (r.stdout + r.stderr):
|
||
return [], "（跳过：Gazebo 没在跑）"
|
||
|
||
actual = {}
|
||
for obj in scene.objects:
|
||
try:
|
||
rr = subprocess.run(
|
||
["ign", "model", "-m", obj.name, "-p"],
|
||
capture_output=True,
|
||
text=True,
|
||
timeout=20,
|
||
)
|
||
except (OSError, subprocess.TimeoutExpired):
|
||
continue
|
||
got = parse_spawned_poses(rr.stdout + rr.stderr)
|
||
if obj.name in got:
|
||
actual[obj.name] = got[obj.name]
|
||
if not actual:
|
||
return [], "（跳过：解析不出 Gazebo 的位姿输出）"
|
||
return pose_mismatches(scene, actual), f"（已在 Gazebo 里核对 {len(actual)} 个物体）"
|
||
|
||
|
||
# ---------------------------------------------------------------------------
|
||
def main() -> int:
|
||
ap = argparse.ArgumentParser(
|
||
description="仿真场景验收：摆放 + 孔检测成像几何 + 粗定位可见性。",
|
||
formatter_class=argparse.RawDescriptionHelpFormatter,
|
||
)
|
||
ap.add_argument("--scene", default="", help="场景文件路径（留空=仓库默认）")
|
||
ap.add_argument("--no-poses", action="store_true", help="跳过「查 Gazebo 实际位姿」")
|
||
args = ap.parse_args()
|
||
|
||
try:
|
||
from arm_sim_gazebo.sim_scene import SceneError, load_scene
|
||
except ImportError as exc:
|
||
print(
|
||
f"✘ 导入 arm_sim_gazebo 失败：{exc}\n"
|
||
f" 先执行： source scripts/sim_env.sh && . install_sim/setup.bash",
|
||
file=sys.stderr,
|
||
)
|
||
return 2
|
||
|
||
root = REPO_ROOT
|
||
try:
|
||
scene = load_scene(Path(args.scene) if args.scene else None, root=root)
|
||
except SceneError as exc:
|
||
print(f"✘ 场景配置读不了：\n{exc}", file=sys.stderr)
|
||
return 2
|
||
|
||
print(f"场景文件: {scene.source}")
|
||
print(f"模型目录: {scene.models_dir} 台面顶面 z={scene.table_top_z}")
|
||
print("物体:")
|
||
for o in scene.objects:
|
||
print(f" {o.name:<14} model={o.model:<14} pose=({o.x:+.3f}, {o.y:+.3f}, {o.z:+.3f}) yaw={o.yaw:+.3f}")
|
||
|
||
problems: list[str] = []
|
||
|
||
print("\n[1] 摆放校验（悬空/穿模/出界/嵌入）")
|
||
p = check_placement(scene, root)
|
||
problems += p
|
||
print(" ✓ 通过" if not p else "\n".join(f" ✘ {x}" for x in p))
|
||
|
||
print("\n[2] 像素门槛与真机配置核对")
|
||
p = check_gate_drift(scene, root)
|
||
problems += p
|
||
print(" ✓ 与真机生效值一致" if not p else "\n".join(f" ✘ {x}" for x in p))
|
||
|
||
print("\n[3] 孔检测成像几何")
|
||
p, px = check_geometry(scene, root)
|
||
problems += p
|
||
for k, v in sorted(px.items()):
|
||
gate = scene.gates_px.get(k)
|
||
tag = f"门槛 [{gate[0]:g}, {gate[1]:g}]" if gate else "无门槛"
|
||
ok = "" if (gate is None or gate[0] <= v <= gate[1]) else " ← 不合格"
|
||
print(f" {k:<22} = {v:7.1f}px {tag}{ok}")
|
||
if p:
|
||
print("\n".join(f" ✘ {x}" for x in p))
|
||
|
||
print("\n[4] 二阶段悬停高度 vs 工作空间")
|
||
p = check_hover_height(scene, root)
|
||
problems += p
|
||
from arm_sim_gazebo.sim_scene import operating_flange_z
|
||
|
||
fz = operating_flange_z(scene, root)
|
||
lim = workspace_z_limits(root)
|
||
if fz is not None:
|
||
print(f" 建议悬停法兰 z≈{fz:.3f}m"
|
||
+ (f" 工作空间 z∈[{lim[0]}, {lim[1]}]" if lim else ""))
|
||
print(
|
||
" （真机 site.yaml 写的是 0.23 —— 两者不同是已知的："
|
||
"决定成像的是相机到工件的距离，不是绝对高度）"
|
||
)
|
||
if p:
|
||
print("\n".join(f" ✘ {x}" for x in p))
|
||
|
||
print("\n[5] 台面通槽 vs 工件（规过得去 + 工件掉不下去）")
|
||
p, info = check_table_slot(scene, root)
|
||
problems += p
|
||
if info:
|
||
print(f" {info}")
|
||
if p:
|
||
print("\n".join(f" ✘ {x}" for x in p))
|
||
else:
|
||
print(" ✓ 孔位都在开口内，且承压宽度够")
|
||
|
||
print("\n[6] 粗定位可见性（工件是否完整落在手外相机画面内）")
|
||
p, info = check_d435_visibility(scene, root)
|
||
problems += p
|
||
if info:
|
||
print(f" {info}")
|
||
if p:
|
||
print("\n".join(f" ✘ {x}" for x in p))
|
||
elif info and not info.startswith("（"):
|
||
print(" ✓ 完整可见")
|
||
|
||
if not args.no_poses:
|
||
print("\n[7] Gazebo 实际位姿核对")
|
||
p, info = check_gazebo_poses(scene, root)
|
||
problems += p
|
||
if info:
|
||
print(f" {info}")
|
||
print(" ✓ 一致" if not p else "\n".join(f" ✘ {x}" for x in p))
|
||
|
||
print()
|
||
if problems:
|
||
print(f"=== ✘ 不通过：{len(problems)} 个问题 ===")
|
||
print(" 场景与成像的问题大多是静默的：跑得起来、报不出错，只是结果不对。")
|
||
return 1
|
||
print("=== ✓ 全部通过 ===")
|
||
return 0
|
||
|
||
|
||
if __name__ == "__main__":
|
||
sys.exit(main())
|