真机落地前的仿真补齐：夹持/腕力/真值位姿，让仿真能验证 S07–S09
这批是 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。
This commit is contained in:
arm_dev
+340
-41
@@ -91,12 +91,78 @@ GAUGE_GRIP_DIA_MM = 30.0
|
||||
#: gz 报得出 Contacts、夹爪也能读到接触，但规就是托不住、一路滑下去。
|
||||
GAUGE_GRASP_CENTER_M = 0.043
|
||||
|
||||
#: 标定结果的合理性上限 [mm]：|开口 0 时的指间间距| 超过它就判标定无效。
|
||||
#: 指节是连杆机构、间距与开口斜率实测 1.000，所以这个量应是个小偏置；
|
||||
#: 大得离谱只有一个原因 —— 合爪时手指被台面/工件挡住了（量到的是被挡的值）。
|
||||
#: 实测（2026-09-24）：home 位姿下量出 −10.15mm，扫描区间整体偏 10mm，
|
||||
#: 17 档全掉，白扫 80 秒。
|
||||
CAL_SANITY_MM = 6.0
|
||||
|
||||
# 台面高度（scene.yaml 的 table_top_z）：用来判断"够不够空间把规塞进去"。
|
||||
FALLBACK_TABLE_Z = 0.40
|
||||
# 触觉"夹住了"的判据：contact 位或 |fnormal| 超过这个噪声地板。
|
||||
# 空载时仿真触觉的 fnormal 实测 ~1e-5（见 config/tactile.yaml 的基线说明）。
|
||||
TACTILE_NOISE_FLOOR = 0.05
|
||||
|
||||
#: 合爪后规在指间**允许**的下滑量 [m]。超过就判"这次装夹和上次不一样"。
|
||||
#:
|
||||
#: 为什么是 3mm：下滑量会直接加在工具长上（site.yaml 的
|
||||
#: ``gauge_assembly.tool_tip_below_flange_m``）。而流程现在**读实测值**
|
||||
#:（见 :func:`_write_measured_geometry`），所以下滑本身不再是错误 ——
|
||||
#: 它只是"这次的工具长和上次不同"，改由实测值带着走。
|
||||
#: 超过这个量仍然要**说出来**：说明装夹方式不稳定，值得人看一眼。
|
||||
#: 实测（2026-09-24）：同一个仿真台先后测到 254.4mm 与 243mm（差 11mm）。
|
||||
DRIFT_SLIP_MAX_M = 0.003
|
||||
|
||||
|
||||
def _write_measured_geometry(
|
||||
path: Path,
|
||||
*,
|
||||
below: float,
|
||||
flange_z: float,
|
||||
tip_z: float,
|
||||
world: str,
|
||||
model: str,
|
||||
) -> Optional[Path]:
|
||||
"""把本次实测的工具几何落盘（供 ``run_gauge_assembly.sh --sim`` 读）。
|
||||
|
||||
**为什么要有这个文件**（2026-09-24 一整轮排查换来的）：
|
||||
|
||||
仿真里规在指间会下滑到落座位置（合爪时规被两指挤住，会一直滑到
|
||||
它的台阶卡在指腹上为止）。下滑量**随装夹变化** —— 实测见过 243mm
|
||||
与 254mm 两种，差 11mm。而 site.yaml 里那个值决定两件事：
|
||||
· 二阶段悬停高度（写小了 → 悬停时规尖已经插进工件 → S05 的横向
|
||||
对准变成拿规尖拨工件，工件被推走、残差恒不收敛）；
|
||||
· S07 的下探预算（写小了 → 台肩还没落面就用完预算）。
|
||||
两个症状都离真因（几何）很远，所以**不许**让它靠"人手抄一个数字"
|
||||
来保持一致：上料脚本测出来就落盘，流程读它。
|
||||
|
||||
写入失败**不抛异常**（返回 ``None``）：上料这件事本身已经成功了，
|
||||
调用方只需要把"退回标称值"这件事喊出来。
|
||||
"""
|
||||
try:
|
||||
data = {
|
||||
"tool_tip_below_flange_m": round(float(below), 4),
|
||||
"measured_at": time.strftime("%Y-%m-%dT%H:%M:%S"),
|
||||
"world": world,
|
||||
"model": model,
|
||||
"flange_z_m": round(float(flange_z), 4),
|
||||
"tip_z_m": round(float(tip_z), 4),
|
||||
}
|
||||
header = (
|
||||
"# ⚠ 本文件由 scripts/sim_load_gauge.py 自动生成（每次仿真上料都覆盖）。\n"
|
||||
"# 它是**这次装夹**实测到的工具几何，run_gauge_assembly.sh --sim 会读它\n"
|
||||
"# 并作为启动参数覆盖 site.yaml 的 gauge_assembly.tool_tip_below_flange_m。\n"
|
||||
"# 为什么必须这样：仿真里规会在指间下滑到落座位置，下滑量随装夹变化\n"
|
||||
"# （实测见过 243mm / 254mm 两种）。写成固定值迟早会算错悬停高度。\n"
|
||||
"# 真机没有这个文件（现场装夹后把实测值写进 site.yaml）。\n"
|
||||
)
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
path.write_text(header + yaml.safe_dump(data, allow_unicode=True), encoding="utf-8")
|
||||
return path
|
||||
except Exception: # noqa: BLE001 - 见 docstring：失败只退回标称值
|
||||
return None
|
||||
|
||||
|
||||
def _load_yaml(path: Path) -> dict:
|
||||
with path.open(encoding="utf-8") as fh:
|
||||
@@ -194,6 +260,59 @@ def gauge_target_pose(
|
||||
return tip, q_gauge
|
||||
|
||||
|
||||
def _reset_scene_objects(scene_cfg, world: str, spin_for) -> None:
|
||||
"""把 ``scene.yaml`` 里**所有**物体放回名义位姿（幂等）。
|
||||
|
||||
为什么不止复位通止规（2026-09-24 实测踩到，代价是白跑一轮）：
|
||||
上一轮 S08 带着 ~70N 的轴向力转了一圈，把**工件**推走了 ——
|
||||
实测 |Δp|=76.9mm、yaw 偏 8.3°。而真值感知是**纯几何**的：它按
|
||||
``scene.yaml`` 的名义位姿算孔位/目标点，物体一挪，算出来的就是假测量。
|
||||
幸好有漂移看门狗（``sim_truth_perception``）当场停下不发点，否则现象会是
|
||||
「S05 两孔间距 169mm」或「S07 一路下压不接触」，看起来像检测坏了。
|
||||
|
||||
但看门狗只会"停下来"，它**不会把工件搬回去** —— 于是下一轮跑不了，
|
||||
报的还是那句「一个视觉目标点都没收到」，看起来像感知节点根本没起来。
|
||||
复位这一下，让"重跑"真的从干净状态开始，不必重启整个仿真。
|
||||
|
||||
顺序：先全部搬回名义位姿，再等物理稳一下。逐条打印结果 ——
|
||||
哪一件没复位成功必须看得见（它会以"真值不发点"的形式在下游爆出来）。
|
||||
"""
|
||||
objs = [o for o in (scene_cfg.get("objects") or []) if o.get("pose")]
|
||||
if not objs:
|
||||
print("⚠ scene.yaml 里没有带 pose 的物体，跳过场景复位。")
|
||||
return
|
||||
failed: List[str] = []
|
||||
for obj in objs:
|
||||
name = str(obj.get("name") or "")
|
||||
if not name:
|
||||
continue
|
||||
p = obj.get("pose") or {}
|
||||
try:
|
||||
x = float(p.get("x", 0.0))
|
||||
y = float(p.get("y", 0.0))
|
||||
z = float(p.get("z", 0.0))
|
||||
yaw = float(p.get("yaw", 0.0))
|
||||
except (TypeError, ValueError) as exc:
|
||||
print(f"⚠ {name}: pose 字段不是数字（{exc}），跳过。")
|
||||
failed.append(name)
|
||||
continue
|
||||
quat = (0.0, 0.0, math.sin(yaw / 2.0), math.cos(yaw / 2.0))
|
||||
ok, out = _set_gauge_pose(world, name, (x, y, z), quat)
|
||||
if ok:
|
||||
print(f"=== 复位: {name} → ({x:.3f}, {y:.3f}, {z:.3f}) yaw={math.degrees(yaw):.1f}° ===")
|
||||
else:
|
||||
print(f"⚠ 复位失败: {name}：{out[:120]}")
|
||||
failed.append(name)
|
||||
if failed:
|
||||
print(f"⚠ 以下物体没能复位：{', '.join(failed)}\n"
|
||||
f" 后果：真值感知会（正确地）拒绝发点，S04 收不到目标点、报"
|
||||
f"「一个视觉目标点都没收到」。\n"
|
||||
f" 真修法：重启仿真（bash scripts/run_sim_gz.sh）让它按场景重新 spawn。")
|
||||
# 让物理静一下再往下走：连着瞬移多个物体会让接触求解器抖几下。
|
||||
if objs:
|
||||
spin_for(1.5)
|
||||
|
||||
|
||||
def _run(cmd: List[str], timeout: float = 8.0) -> Tuple[int, str]:
|
||||
"""跑一条外部命令，返回 (退出码, 合并输出)。不抛异常（调用方自己判）。"""
|
||||
try:
|
||||
@@ -208,34 +327,13 @@ def _run(cmd: List[str], timeout: float = 8.0) -> Tuple[int, str]:
|
||||
def _set_gauge_pose(world: str, name: str, pos, quat) -> Tuple[bool, str]:
|
||||
"""用 Gazebo 的世界服务把模型搬到指定位姿。
|
||||
|
||||
这里用 ``ign service`` 而不是另起一个 ROS 桥：仿真的场景加载本来就是
|
||||
用命令行 `ros_gz_sim create` 做的（见 arm_sim_gazebo/sim_scene.py），
|
||||
口径一致、少一个需要维护的话题/服务桥。
|
||||
|
||||
注意 ``--reqtype`` 必须写 ``ignition.msgs.Pose``（本环境实测：写
|
||||
``gz.msgs.Pose`` 会报 "Unable to create request"，一个字母的差别、
|
||||
报错信息完全没提原因）。
|
||||
实现**只有一份**：``arm_sim_gazebo.sim_pose_service.set_model_pose``
|
||||
（那个 `--reqtype` 必须写 ``ignition.msgs.Pose`` 的坑也写在那边）。
|
||||
真值感知的启动复位走的是同一个函数。
|
||||
"""
|
||||
req = (
|
||||
f'name: "{name}", '
|
||||
f"position: {{x: {pos[0]:.6f}, y: {pos[1]:.6f}, z: {pos[2]:.6f}}}, "
|
||||
"orientation: {"
|
||||
f"x: {quat[0]:.8f}, y: {quat[1]:.8f}, z: {quat[2]:.8f}, w: {quat[3]:.8f}"
|
||||
"}"
|
||||
)
|
||||
code, out = _run(
|
||||
[
|
||||
"ign", "service",
|
||||
"-s", f"/world/{world}/set_pose",
|
||||
"--reqtype", "ignition.msgs.Pose",
|
||||
"--reptype", "ignition.msgs.Boolean",
|
||||
"--timeout", "3000",
|
||||
"--req", req,
|
||||
],
|
||||
timeout=10.0,
|
||||
)
|
||||
ok = code == 0 and "data: true" in out
|
||||
return ok, out.strip()
|
||||
from arm_sim_gazebo.sim_pose_service import set_model_pose
|
||||
|
||||
return set_model_pose(world, name, pos, quat)
|
||||
|
||||
|
||||
def _gauge_pose_now(name: str) -> Optional[Tuple[float, float, float]]:
|
||||
@@ -977,6 +1075,64 @@ def _ros_side(args) -> int: # noqa: C901 - 线性流程，拆开反而更难读
|
||||
print("=== --place-only：不动夹爪，只按当前姿态摆位 ===")
|
||||
else:
|
||||
print("=== 开爪（保证两指之间能让规进去） ===")
|
||||
|
||||
# ---- 0. 归位：先把规放回规座（幂等，避免上一次的残留污染这一次）--------
|
||||
# 为什么必须做：本脚本是要**反复重跑**的（调试时一分钟一次）。上一次跑完规
|
||||
# 可能还卡在指间/落在台面槽里，而后面两步都会被"指间有东西"毁掉：
|
||||
# · 标定指间间距：规卡在指间时量到的是**规的外径**（实测 28.29mm 而不是
|
||||
# 20mm），于是 gap_cal 凭空多出 8mm，整个扫描区间偏移 → 一处都夹不到；
|
||||
# · 摆位：规已经在指间时再瞬移一次，落点是在旧位姿上叠的。
|
||||
# 先把它放回 scene.yaml 写的位置再开始，行为就与"刚启动仿真"完全一致。
|
||||
if not args.place_only:
|
||||
# 先解开可能残留的抓取锁：上锁之后规是被**固定关节**钉在手指上的，
|
||||
# 这时瞬移回规座会被关节立刻拉回来（实测：瞬移瞬间 z 变 0.36 又弹回），
|
||||
# 而手指合到底也只能到"规的外径"（实测 28.29mm 而不是 20mm）——
|
||||
# 于是标定读到的正是规径，gap_cal 凭空多 8mm。
|
||||
try:
|
||||
from arm_sim_gazebo.sim_grasp_lock import load_grasp_lock, publish_lock_event
|
||||
|
||||
_spec = load_grasp_lock(root=ROOT)
|
||||
if _spec.enabled and publish_lock_event(_spec, False):
|
||||
print("=== 归位前先解锁（清掉上一次可能残留的抓取锁） ===")
|
||||
spin_for(0.5)
|
||||
except Exception: # noqa: BLE001 - 锁是可选补救措施，读不到就跳过
|
||||
pass
|
||||
|
||||
nom = None
|
||||
for obj in (scene_cfg.get("objects") or []):
|
||||
if str(obj.get("name")) == args.model:
|
||||
p = obj.get("pose") or {}
|
||||
try:
|
||||
nom = (
|
||||
float(p.get("x", 0.0)),
|
||||
float(p.get("y", 0.0)),
|
||||
float(p.get("z", 0.0)),
|
||||
float(p.get("yaw", 0.0)),
|
||||
)
|
||||
except (TypeError, ValueError):
|
||||
nom = None
|
||||
break
|
||||
if nom is None:
|
||||
print(f"⚠ scene.yaml 里没有 {args.model} 的名义位姿，跳过「归位」这一步。")
|
||||
else:
|
||||
yaw = nom[3]
|
||||
quat = (0.0, 0.0, math.sin(yaw / 2.0), math.cos(yaw / 2.0))
|
||||
ok, out = _set_gauge_pose(args.world, args.model, nom[:3], quat)
|
||||
if ok:
|
||||
print(f"=== 归位：把 {args.model} 放回规座 "
|
||||
f"({nom[0]:.3f}, {nom[1]:.3f}, {nom[2]:.3f}) ===")
|
||||
wait_tf(tcp_frames["left"], timeout_s=1.0) # 顺便让物理先稳一下
|
||||
spin_for(1.5)
|
||||
else:
|
||||
print(f"⚠ 归位失败（{args.model} 可能还在指间）：{out[:120]}")
|
||||
print(" 后果：下面的标定会量到规的外径，扫描区间整体偏移。")
|
||||
# 顺带把**其它**场景物体也复位（最要紧的是工件）。
|
||||
# 为什么：上一轮 S08 若带着大轴向力转了一圈，工件会被推走，而真值感知
|
||||
# 按名义位姿算孔位 —— 看门狗会（正确地）停发点，于是下一轮报
|
||||
# "一个视觉目标点都没收到"，看起来像感知节点没起来。
|
||||
# 详见 _reset_scene_objects 的说明。
|
||||
_reset_scene_objects(scene_cfg, args.world, spin_for)
|
||||
|
||||
if not args.place_only:
|
||||
if not gripper_call(cli_open, "open"):
|
||||
print("✗ /gripper/open 调用超时")
|
||||
@@ -1015,6 +1171,24 @@ def _ros_side(args) -> int: # noqa: C901 - 线性流程，拆开反而更难读
|
||||
dz = pts["left"][2] - pts["right"][2]
|
||||
return (dx * dx + dy * dy + dz * dz) ** 0.5 * 1000.0
|
||||
|
||||
# ---- 1.5 摆到"竖直上料位姿"（工具轴朝下 + 离台面够高） ---------------
|
||||
# ⚠️ 这一步必须在**标定之前**（见下面对 gap_cal 的合理性检查）：
|
||||
# 标定要在"手指前方是空的"前提下量，而零位/ home 位姿又低又斜，
|
||||
# 合爪时手指很容易顶在台面或工件上 —— 那时量到的间距是**被挡住的值**，
|
||||
# 于是 gap_cal 凭空差出十几毫米，整个扫描区间都跟着错，一处都夹不到。
|
||||
# 实测（2026-09-24）：home 位姿下标定出"开口 20mm 时间距 9.85mm"，
|
||||
# 扫了 17 档全掉，白跑 80 秒。
|
||||
# 不摆的话：仿真零位工具轴是**水平**的，水平夹一根钢规会被重力抽出去
|
||||
#（实测合爪后漂移 857mm，直接掉回规座）；home 位姿又斜又低，放不下。
|
||||
# 这里用 IK 把两指 TCP 中点送到台面上方的空旷处、工具轴朝下。
|
||||
if not args.place_only and not args.no_pose:
|
||||
if not _go_vertical_load_pose(
|
||||
node, buf, spin_for, wait_tf, tcp_frames, args,
|
||||
):
|
||||
print("✗ 摆竖直上料位姿失败。可以用 --no-pose 跳过（自己先把臂摆好）。")
|
||||
rclpy.shutdown()
|
||||
return 2
|
||||
|
||||
if not args.place_only:
|
||||
if not gripper_call(cli_move, "move", position=args.cal_aperture_mm):
|
||||
print("✗ /gripper/move 调用超时（标定用）")
|
||||
@@ -1029,18 +1203,19 @@ def _ros_side(args) -> int: # noqa: C901 - 线性流程，拆开反而更难读
|
||||
print(f" 开口 {args.cal_aperture_mm:.2f}mm → 指间间距 {gap_at_aperture:.2f}mm")
|
||||
print(f" ⇒ 开口 0（全闭合）时间距 {gap_cal_mm:.2f}mm，"
|
||||
f"间距 = 开口 + {gap_cal_mm:.2f}mm")
|
||||
|
||||
# ---- 1.5 摆到"竖直上料位姿"（工具轴朝下 + 离台面够高） ---------------
|
||||
# 不摆的话：仿真零位工具轴是**水平**的，水平夹一根钢规会被重力抽出去
|
||||
#（实测合爪后漂移 857mm，直接掉回规座）；home 位姿又斜又低，放不下。
|
||||
# 这里用 IK 把两指 TCP 中点送到台面上方的空旷处、工具轴朝下。
|
||||
if not args.place_only and not args.no_pose:
|
||||
if not _go_vertical_load_pose(
|
||||
node, buf, spin_for, wait_tf, tcp_frames, args,
|
||||
):
|
||||
print("✗ 摆竖直上料位姿失败。可以用 --no-pose 跳过（自己先把臂摆好）。")
|
||||
rclpy.shutdown()
|
||||
return 2
|
||||
# 合理性检查：指节是连杆机构，间距与开口严格线性、斜率实测 = 1.000，
|
||||
# 所以 gap_cal（开口=0 时的间距）应该是个**小量**（几毫米量级）。
|
||||
# 它大得离谱时唯一的原因是"手指被挡住了"：量到的是被挡的位置。
|
||||
# 这个检查为什么值得写：不查的话后面会白扫十几档、最后只说
|
||||
# "都没夹住"，还列一堆可能原因 —— 而真因是标定这一步就错了。
|
||||
if abs(gap_cal_mm) > CAL_SANITY_MM:
|
||||
print(f"✗ 标定结果不可信：开口 0 时间距 {gap_cal_mm:.2f}mm"
|
||||
f"（正常应在 ±{CAL_SANITY_MM:.0f}mm 内）。")
|
||||
print(" 最可能：合爪时手指顶在台面/工件上（手指前方不是空的）。")
|
||||
print(" 修法：先把臂摆到空旷处再上料 ——")
|
||||
print(" bash scripts/go_home.sh 或用 --no-pose 自行摆位。")
|
||||
rclpy.shutdown()
|
||||
return 2
|
||||
|
||||
# ---- 1.8 摆位（纯瞬移）与"人手托着合爪" -------------------------------
|
||||
#
|
||||
@@ -1264,8 +1439,14 @@ def _ros_side(args) -> int: # noqa: C901 - 线性流程，拆开反而更难读
|
||||
"跳过位姿校验，只看触觉。")
|
||||
else:
|
||||
drift = math.dist(now_pos, tip_pos)
|
||||
print(f"=== 校验：规还在不在手里 ===\n 位姿漂移 {drift * 1000:.1f}mm"
|
||||
f"（合爪后被顶开/顶飞会远大于 20mm）")
|
||||
print(f"=== 校验：规还在不在手里 ===")
|
||||
print(f" 位姿漂移 {drift * 1000:.1f}mm。两种漂移**性质不同**，别混：")
|
||||
print(" · > 20mm → 合爪时被顶飞 / 模型名或世界名不对（上料失败）")
|
||||
print(" · 3~20mm → 规在指间**下滑**到了它的落座位置。这不是上料失败，"
|
||||
"而是")
|
||||
print(" site.yaml 的 tool_tip_below_flange_m 与实际装夹不符 ——")
|
||||
print(" 下滑量会**直接加在工具长上**，写小了二阶段悬停就会把")
|
||||
print(" 规尖停在工件里面（S05 横向对准于是拿规尖拨工件）")
|
||||
tac_ok = False
|
||||
for side in ("left", "right"):
|
||||
item = tac.get(side)
|
||||
@@ -1283,6 +1464,18 @@ def _ros_side(args) -> int: # noqa: C901 - 线性流程，拆开反而更难读
|
||||
if drift is not None and drift > 0.02:
|
||||
problems.append(f"规没在位姿上（漂移 {drift * 1000:.1f}mm）"
|
||||
"：可能合爪时被顶飞、或模型名/世界名不对")
|
||||
elif drift is not None and drift > DRIFT_SLIP_MAX_M:
|
||||
# 3~20mm：规落到了指间的落座位置。**不是**上料失败 —— 它是"这次的
|
||||
# 工具长和 site.yaml 里那个值不一样"，而工具长稍后会实测并落盘，
|
||||
# 流程读的是实测值（见 _write_measured_geometry）。所以这里只"说出来"：
|
||||
# 下滑量偏大说明装夹不稳定，值得人看一眼。
|
||||
print(f" ⚠ 规在指间下滑了 {drift * 1000:.1f}mm 才落座 —— 这次的工具长与"
|
||||
f"site.yaml 的标称值差了这么多（不是上料失败）：")
|
||||
print(" 下滑量会直接加在工具长上。稍后的「实测工具几何」会把它测出来并"
|
||||
"落盘成")
|
||||
print(" sim_tool_geometry.yaml，流程（--sim）会读它覆盖标称值 —— 所以"
|
||||
"能继续跑。")
|
||||
print(" 但下滑量偏大说明装夹不稳，值得确认一下规是不是卡在指腹上了。")
|
||||
if not tac_ok:
|
||||
problems.append("两指触觉都没读到接触：可能没合上（看上面开口）、或规不在两指之间")
|
||||
if problems:
|
||||
@@ -1296,6 +1489,112 @@ def _ros_side(args) -> int: # noqa: C901 - 线性流程，拆开反而更难读
|
||||
|
||||
print("✓ 通止规已装进夹爪（位姿贴合 + 触觉有接触）。可以跑装配了：")
|
||||
print(" bash scripts/run_gauge_assembly.sh --sim --holes 1")
|
||||
|
||||
# ---- 4. 上"抓取锁"（仿真专用）------------------------------------------
|
||||
# 为什么由**这里**上锁，而不是等夹爪适配器自己判：
|
||||
# 适配器的 is_gripped 判据是"指令到全闭合、实际被工件顶住"（真机 DH 没有
|
||||
# 夹持状态字，只能这么推）。而本脚本是**按直径精确合到某个开口**的
|
||||
# （例如 24.9mm 卡住 φ30 的规），指令与实际一致 —— 于是适配器**判不出**
|
||||
# "夹住了"，锁也就不会触发。
|
||||
# 本脚本是"装夹"这件事的执行者，它自己知道刚刚校验过（位姿贴合 + 触觉
|
||||
# 有接触），所以由它上锁最准，也最好解释。
|
||||
# 解锁不需要在这里做：夹爪适配器在收到 open / 开口变大的 move 时会解锁
|
||||
#（见 sim_gripper_adapter._release_grasp_lock）。
|
||||
try:
|
||||
from arm_sim_gazebo.sim_grasp_lock import load_grasp_lock, publish_lock_event
|
||||
|
||||
_spec = load_grasp_lock(root=ROOT)
|
||||
if _spec.enabled and _spec.object == args.model:
|
||||
if publish_lock_event(_spec, True):
|
||||
print(f"✓ 抓取锁已上：{_spec.object}.{_spec.object_link} 固定到 "
|
||||
f"{_spec.robot_model}.{_spec.robot_link}")
|
||||
print(" （仿真专用补丁：Gazebo 的刚性接触给不出真实指垫的静摩擦，"
|
||||
"不上锁时规会在连续轨迹里打滑 —— 真机 46N 握力 + µ≈0.8 本来就不掉）")
|
||||
else:
|
||||
print("⚠ 抓取锁没上成（详见日志）——机械臂连续运动时规可能打滑。")
|
||||
elif _spec.enabled:
|
||||
print(f"⚠ 抓取锁配的对象是 {_spec.object}，与 --model {args.model} 不一致，跳过上锁。")
|
||||
except Exception as exc: # noqa: BLE001 - 锁是补救措施，读不到配置不该拦住上料
|
||||
print(f"⚠ 读抓取锁配置失败，跳过上锁：{exc}")
|
||||
|
||||
# ---- 7. 实测工具几何（给 site.yaml 的 tool_tip_below_flange_m 用）----
|
||||
# 为什么必须**实测**而不是推算：这个值决定 S07 的悬停/下探预算（见
|
||||
# gauge_fsm.descend_needed_m），差 10mm 就会让"台肩落面"永远到不了。
|
||||
# 推算要凑三层偏移（TCP 在法兰下方多少 + 规尖在 TCP 下方多少），而
|
||||
# 中间任何一层的定义（比如 tcp 帧是零偏移还是带偏置）都可能与假设不同。
|
||||
# 实测只有一次减法：法兰 z − 规尖 z。真机同法（夹住规后直接读 TF）。
|
||||
got_e2 = wait_tf(args.ee_frame, timeout_s=3.0)
|
||||
tip_now = _gauge_pose_now(name)
|
||||
if got_e2 is not None and tip_now is not None:
|
||||
p_ee = got_e2[0]
|
||||
below = float(p_ee[2]) - float(tip_now[2])
|
||||
print("\n=== 实测工具几何（抄进 config/site.yaml 的 gauge_assembly）===")
|
||||
print(f" 法兰 z={p_ee[2]:.4f} 规尖 z={tip_now[2]:.4f}"
|
||||
f" → **tool_tip_below_flange_m: {below:.3f}**")
|
||||
if below <= 0.0:
|
||||
print(" ⚠ 规尖不在法兰下方 —— 多半是规掉了或者没夹在指间，"
|
||||
"别把这个值抄进配置。")
|
||||
else:
|
||||
# ⚠️ 这里以前算的是「当前法兰 z − 工具长」，也就是**上料时臂停在哪**，
|
||||
# 与悬停高度毫无关系 —— 实测它印出"工件面 z=0.412 → 尖端 z=0.5201"，
|
||||
# 看着很安全，而真正决定"会不会把规捅进工件"的是 site.yaml 里
|
||||
# servo.hole_hover_z_m（当时 0.632 ⇒ 尖端 0.387 = 面**下** 25mm）。
|
||||
# 现在按配置的悬停高度算，并且复用 gauge_fsm 的判据（一份实现）。
|
||||
try:
|
||||
from arm_application.gauge_fsm import hover_z_guard
|
||||
|
||||
site = _load_yaml(ROOT / "config" / "site.yaml")
|
||||
rob = next(
|
||||
(r for r in (site.get("robot") or {}).get("robots") or []
|
||||
if str(r.get("sn")) == str(getattr(args, "arm", "sim"))),
|
||||
{},
|
||||
)
|
||||
ga = dict(site.get("gauge_assembly") or {})
|
||||
ga.update(rob.get("gauge_assembly") or {})
|
||||
servo = dict(site.get("servo") or {})
|
||||
servo.update(rob.get("servo") or {})
|
||||
surface = float(ga.get("workpiece_surface_z_m") or 0.0)
|
||||
hover = float(servo.get("hole_hover_z_m") or 0.0)
|
||||
# 目标间隙与二阶段"最后轻降"用的是**同一个**配置（hole_detect.stage2
|
||||
# .final_gap_mm）：都是"规尖悬在工件面上方多少"。两处各读一份迟早
|
||||
# 会打架，而症状是"上料脚本说安全、stage2 说危险"。
|
||||
st2 = dict((site.get("hole_detect") or {}).get("stage2") or {})
|
||||
gap = float(st2.get("final_gap_mm") or 2.0) / 1000.0
|
||||
fixed, why = hover_z_guard(hover, surface, below, gap)
|
||||
print(f" 用它核二阶段悬停高：servo.hole_hover_z_m={hover:.3f}、"
|
||||
f"工件上表面 z={surface:.3f}")
|
||||
if why:
|
||||
print(" ⚠ **悬停时规尖会捅进工件**（S05 的横向对准会拖着工件走）：")
|
||||
print(f" {why}")
|
||||
else:
|
||||
print(f" → 悬停时规尖离工件面 "
|
||||
f"{(hover - below - surface) * 1000:+.1f}mm（安全）")
|
||||
except Exception as exc: # noqa: BLE001 - 这一项只是提示，别拦住上料
|
||||
print(f" （悬停高度自检跳过：{exc}）")
|
||||
|
||||
# ---- 把实测值落盘，交给流程当启动参数 --------------------------
|
||||
# 为什么不能只"打印出来让人抄进 site.yaml"（2026-09-24 实测的教训）：
|
||||
# 仿真里规会在指间下滑到落座位置，而**下滑量随装夹变化** ——
|
||||
# 同一个仿真台先后测到过 0.243 和 0.254（差 11mm）。写死哪个值，
|
||||
# 下一次都可能算错悬停高度，而错误现象是"S05 残差恒不收敛"
|
||||
# （看起来像手眼坏了）。所以：**测量 → 落盘 → 流程读它**。
|
||||
# 真机走不到这里（真机由人装夹、同法读 TF，但由现场把值写进 site.yaml）。
|
||||
geom_path = _write_measured_geometry(
|
||||
ROOT / "devices" / "sim" / "gazebo" / "config" / "sim_tool_geometry.yaml",
|
||||
below=below,
|
||||
flange_z=float(p_ee[2]),
|
||||
tip_z=float(tip_now[2]),
|
||||
world=args.world,
|
||||
model=name,
|
||||
)
|
||||
if geom_path is None:
|
||||
print(" ⚠ 实测值没能落盘 —— 流程会退回 site.yaml 的标称值，"
|
||||
"悬停/下探预算可能差几毫米到十几毫米。")
|
||||
else:
|
||||
print(f" 已落盘：{geom_path.relative_to(ROOT)}"
|
||||
f"（run_gauge_assembly.sh --sim 会把它当启动参数覆盖 site.yaml）")
|
||||
else:
|
||||
print("\n（读不到法兰/规尖的位姿，跳过工具几何实测）")
|
||||
rclpy.shutdown()
|
||||
return 0
|
||||
|
||||
|
||||
Reference in New Issue
Block a user