仿真夹爪适配器：服务接口对齐真机，开口按正运动学换算成真毫米
为什么
------
应用层只认 arm_msgs/srv/GripperExecute（开口毫米），真机与仿真必须同一套接口。
但 GN01 的 URDF 里 <mimic> 是**线性近似**：直接按线性系数下发 finger_width_joint，
"命令开 20 mm"在 Gazebo 里量出来是 21.75 mm —— 抓不住工件，而且看起来像算法问题。
实测（2026-09-24，Gazebo 内量两指尖间距）：
线性下发 20 mm → 实际 21.74 mm（偏差 1.74 mm，超 2 mm 抓取容差的前半步）
走 FK 反解 → 实际 20.00 mm（≤0.5 mm）
怎么改
------
- 新增 sim_gripper_kin.py：解析 URDF 正运动学，算出"主关节值 ↔ 真实开口 mm"的
单调曲线，反解用二分。曲线非单调/零行程时**主动报错**，不静默退化成线性。
- sim_gripper.py：开口换算全部走该曲线；容差统一改成毫米。
- sim_gripper_adapter.py：反馈改取**从动关节**（见下）；is_gripped 按毫米判定。
- 新增 check_sim_gripper.py：三层验收（服务/开口一致/指尖真实几何行程 + 对称性）。
踩到的坑
--------
- finger_width_joint 挂在无质量的 unused_finger_width_link 上，与手指**没有机械
连接**：命令到 0.100，它的 /joint_states 恒为 1.4e-08，6 个从动关节才准到
0.7854。反馈若取它，/gripper/status 会永远报"开口 0 mm"。
- 残留进程会伪造出"代码写错了"的现场：旧 sim_gripper_adapter 与新实例抢话题，
验收读到"指尖间距 0.03 mm"（真实 127 mm）；孤儿 static_transform_publisher
用 latched /tf_static 把旧相机外参按在新仿真上。
→ 新增 scripts/lib/sim_procs.sh；run_sim_gz.sh 用 setsid 独立进程组，
PGID 记进 log_sim/sim.pgid，stop 时整组 kill；启动前预检拒绝带病启动。
- 验收脚本自己也错过两次（把 TF 的米当毫米比 → 127 mm 显示成 0.13 mm；
用会转动指垫的中点漂移判对称 → 必然位移 16 mm）。判据改成"两侧各走多远"，
单位只在 _dist_mm 里换算一次。
验证
----
python3 -m pytest src/arm_sim_gazebo/test/ -q → 307 passed
python3 scripts/check_sim_gripper.py → 三层全过
第 2 层：请求/反馈开口最大偏差 < 0.5 mm
第 3 层：期望行程 vs 实际行程、左右行程差，均 < 1.5 mm
bash scripts/stop_sim_gz.sh --list → 整组列出（含 static tf）
文档
----
- 仿真手册：新增 §4.4（开口 mm 怎么换算）、坑 12（宽度关节不动、
反馈必须取从动关节）；停止一节改写成进程组语义。
- FRAMEWORK_RULES：新增 R4.11（残留进程会伪造现场）、R4.12（验收判据要挑
与实现无关的物理量）；R4.9 表补夹爪；清单补 13/14 两条。
- HANDBOOK：新增 §3.4「在仿真里开合夹爪」，补 §4.4 的两条新教训。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -0,0 +1,411 @@
|
||||
#!/usr/bin/env python3
|
||||
"""仿真夹爪的验收：**几何**行程对不对，不是"服务有返回"。
|
||||
|
||||
为什么需要它
|
||||
------------
|
||||
夹爪出问题的方式几乎全是静默的：
|
||||
|
||||
* 耦合链只套了一层 multiplier —— 夹爪会动、角度也是那个量级，就是夹不准；
|
||||
* 命令数组顺序与控制器不一致 —— 角度发给了别的关节，夹爪"动得很奇怪"；
|
||||
* 毫米与米混用（差 1000 倍） —— 夹爪开到别的地方；
|
||||
* 夹爪适配器根本没起来 —— 服务不存在，但**话题照旧**、控制器照旧 active。
|
||||
|
||||
前三条都能通过"调服务有 success=true、status 里有数"这种验收。
|
||||
所以这里分三层，只有第二、三层算数：
|
||||
|
||||
第 1 层 /gripper/* 服务在、/gripper/status 有数据 （不算验收）
|
||||
第 2 层 请求开口 mm == 关节反馈换回来的开口 mm （证明下发链路）
|
||||
第 3 层 两个指尖在 world 系里的**实际间距**随请求变化，
|
||||
且"合拢→张开"的行程 == 请求的开口 （证明耦合与几何）
|
||||
|
||||
第 3 层是关键：第 2 层只说明"关节值到位了"。如果 mimic 的 multiplier 写错，
|
||||
手指根本不会按预期张开，而第 2 层完全看不出来。也正因为这一层是**独立**的，
|
||||
它才逼出了下面两个真实故障 —— 都是"验收脚本自己错"，比被测对象错更常见：
|
||||
|
||||
⚠️ 三个踩过的坑（都让本脚本先给出过错误结论）
|
||||
--------------------------------------------
|
||||
1. **米 / 毫米**。TF 查出来的坐标是米，而本脚本的阈值与期望值全是毫米。
|
||||
漏掉 ×1000 时，127 mm 会显示成 0.13 mm —— 恰好"看起来像夹爪没张开"，
|
||||
于是跑去查耦合公式，而真正的问题就在本文件里。（判据全部走 _dist_mm。）
|
||||
|
||||
2. **残留进程**。旧会话没停干净时，会有两个 sim_gripper_adapter 同时订阅
|
||||
/joint_states、同时发布 /gripper/status，服务调用还会被随机分给其中一个；
|
||||
残留的 static_transform_publisher 更阴 —— 它发 latched 的 /tf_static，
|
||||
会把**旧相机外参**按在新仿真的 TF 树上。症状一样是"读数全乱"。
|
||||
→ 所以本脚本第一步查重名节点（`:meth:`GripperCheck._duplicate_nodes`），
|
||||
启动侧也加了闸门（scripts/run_sim_gz.sh 的预检 + setsid 进程组）。
|
||||
|
||||
3. **判据要与机构无关**。两侧"对称"不能用"两指中点漂移"来判：tip frame 是
|
||||
刚性固连在**会转动**的指垫上的（指垫既平移又转，指面才保持平行），所以
|
||||
frame 走的是弧线，中点必然位移 16 mm 左右。真正与方向无关的判据是
|
||||
"两侧各自走了多远"（实测 52.56 / 52.56 mm）。
|
||||
|
||||
⚠️ 指尖坐标系原点的固有间距
|
||||
实测两个 `*_finger_tip` frame 在完全合拢时仍然相距 27.09 mm（tip frame 不在
|
||||
夹持面上，而且会随手指转动）。所以**不要**拿"指尖间距"直接当开口看；
|
||||
本脚本用它的**变化量**（行程）判据 —— 那个变化量与开口是 1:1 的
|
||||
（实测：请求 8 / 20 / 50 mm → 间距增加 8.00 / 20.00 / 50.00 mm）。
|
||||
|
||||
用法
|
||||
----
|
||||
source scripts/sim_env.sh
|
||||
python3 scripts/check_sim_gripper.py # 需要 Gazebo 仿真已在跑
|
||||
python3 scripts/check_sim_gripper.py --verbose # 另打印每档的指尖原始坐标
|
||||
|
||||
退出码：0 = 通过；1 = 未通过；2 = 环境不可用（服务不在 / 有重名节点）。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import math
|
||||
import sys
|
||||
import time
|
||||
from typing import Dict, List, Optional, Tuple
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from tf2_ros import Buffer, TransformListener
|
||||
|
||||
try:
|
||||
from arm_msgs.msg import GripperStatus
|
||||
from arm_msgs.srv import GripperExecute
|
||||
except ModuleNotFoundError as exc: # pragma: no cover - 只在环境没 source 时触发
|
||||
# 直接让 Python 抛"找不到模块"很难定位：真正的原因通常是**忘了 source 环境**，
|
||||
# 而不是 arm_msgs 没编译。所以这里把原因和修法一次说清。
|
||||
sys.exit(
|
||||
f"找不到 {exc.name}：请先 source 仿真环境（它把 install_sim/ 加进 PYTHONPATH）：\n"
|
||||
f" source scripts/sim_env.sh\n"
|
||||
f"再执行本脚本。若仍失败，说明 install_sim/ 还没构建：\n"
|
||||
f" bash scripts/build_sim.sh"
|
||||
)
|
||||
|
||||
#: 指尖坐标系的**固有**间距不影响行程判据，但打印出来省得有人误读。
|
||||
TIP_LEFT = "sim_left_finger_tip"
|
||||
TIP_RIGHT = "sim_right_finger_tip"
|
||||
WORLD = "world"
|
||||
|
||||
#: 每个开口请求之后等多久再量（位置控制器 + 物理稳定）。
|
||||
SETTLE_S = 2.5
|
||||
|
||||
#: 第 2 层容差（毫米）：请求开口 vs 关节反馈换回的开口。
|
||||
POSITION_TOL_MM = 0.5
|
||||
#: 第 3 层容差（毫米）：指尖行程 vs 期望行程。
|
||||
TRAVEL_TOL_MM = 1.5
|
||||
#: 左右对称性容差（毫米）：两侧位移量之差。
|
||||
SYMMETRY_TOL_MM = 1.5
|
||||
|
||||
|
||||
def _spin(node: Node, seconds: float) -> None:
|
||||
deadline = time.monotonic() + seconds
|
||||
while time.monotonic() < deadline:
|
||||
rclpy.spin_once(node, timeout_sec=0.05)
|
||||
|
||||
|
||||
def _tip_positions(buf: Buffer, node: Node, attempts: int = 40):
|
||||
"""取两个指尖在 world 系里的位置；查不到返回 None。"""
|
||||
for _ in range(attempts):
|
||||
try:
|
||||
left = buf.lookup_transform(WORLD, TIP_LEFT, rclpy.time.Time())
|
||||
right = buf.lookup_transform(WORLD, TIP_RIGHT, rclpy.time.Time())
|
||||
return (
|
||||
(left.transform.translation.x, left.transform.translation.y, left.transform.translation.z),
|
||||
(right.transform.translation.x, right.transform.translation.y, right.transform.translation.z),
|
||||
)
|
||||
except Exception:
|
||||
_spin(node, 0.2)
|
||||
return None
|
||||
|
||||
|
||||
def _dist_pts(a, b) -> float:
|
||||
"""两点距离，**米**（原始量纲，调用方自己决定怎么用）。"""
|
||||
return math.sqrt(sum((x - y) ** 2 for x, y in zip(a, b)))
|
||||
|
||||
|
||||
def _dist_mm(a, b) -> float:
|
||||
"""两个位置之间的距离，**毫米**。
|
||||
|
||||
⚠️ 这个换算漏过一次，症状很典型：TF 查出来的坐标是**米**，而本脚本里所有
|
||||
阈值/期望值都是**毫米**（POSITION_TOL_MM、TRAVEL_TOL_MM、请求开口…）。
|
||||
漏掉 *1000 的话，127 mm 会显示成 0.13 —— 恰好"看起来像夹爪没张开"，
|
||||
于是跑去看耦合公式，而真正的问题只在本文件这一行。
|
||||
（实测：同一份数据下，指尖坐标是对的，只是判据全被缩小了 1000 倍。）
|
||||
"""
|
||||
return 1000.0 * _dist_pts(a, b)
|
||||
|
||||
|
||||
# ⚠️ 属性名都要避开 rclpy.Node 自己的内部属性：`self._clients` 是 Node 的私有
|
||||
# 成员（一个 list），覆盖它会直接在 create_client 里崩（实测踩过）。
|
||||
# 所以这里统一用 `_grip_clients` / `_last_status` / `_tf_buf`。
|
||||
class GripperCheck(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim_gripper_check")
|
||||
self._tf_buf = Buffer()
|
||||
TransformListener(self._tf_buf, self)
|
||||
self._last_status: Optional[GripperStatus] = None
|
||||
self.create_subscription(GripperStatus, "/gripper/status", self._on_status, 10)
|
||||
self._grip_clients: Dict[str, object] = {}
|
||||
for verb in ("open", "close", "move"):
|
||||
self._grip_clients[verb] = self.create_client(GripperExecute, f"/gripper/{verb}")
|
||||
|
||||
def _on_status(self, msg: GripperStatus) -> None:
|
||||
self._last_status = msg
|
||||
|
||||
# --- 基础设施 ---------------------------------------------------------
|
||||
def wait_ready(self, timeout: float = 30.0) -> List[str]:
|
||||
"""等服务和状态都就绪。返回缺失项（空列表=就绪）。"""
|
||||
missing = []
|
||||
for verb, cli in self._grip_clients.items():
|
||||
if not cli.wait_for_service(timeout_sec=timeout): # type: ignore[attr-defined]
|
||||
missing.append(f"/gripper/{verb}")
|
||||
if self._last_status is None:
|
||||
_spin(self, 3.0)
|
||||
if self._last_status is None:
|
||||
missing.append("/gripper/status（话题无数据）")
|
||||
return missing
|
||||
|
||||
def call(self, verb: str, position: float = 0.0) -> Tuple[bool, str]:
|
||||
cli = self._grip_clients[verb]
|
||||
req = GripperExecute.Request()
|
||||
req.command.command = verb
|
||||
req.command.position = float(position)
|
||||
fut = cli.call_async(req) # type: ignore[attr-defined]
|
||||
while not fut.done():
|
||||
_spin(self, 0.05)
|
||||
res = fut.result()
|
||||
if res is None:
|
||||
return False, "服务没有返回"
|
||||
return bool(res.success), str(res.message)
|
||||
|
||||
def movement(self, verb: str, position: float = 0.0):
|
||||
"""下发一条命令，等稳定，返回 (请求说明, 状态开口, 两指尖, 是否成功)。"""
|
||||
ok, msg = self.call(verb, position)
|
||||
_spin(self, SETTLE_S)
|
||||
tips = _tip_positions(self._tf_buf, self)
|
||||
aperture = float(self._last_status.position) if self._last_status else None
|
||||
return ok, msg, aperture, tips
|
||||
|
||||
# --- 三层验收 ---------------------------------------------------------
|
||||
def _duplicate_nodes(self) -> List[str]:
|
||||
"""查出"同名节点有两个"的情况。
|
||||
|
||||
⚠️ 这是踩出来的：残留的旧会话与新会话同时活着时，两个
|
||||
``sim_gripper_adapter`` 会同时订阅 /joint_states、同时发布
|
||||
/gripper/status，服务调用还会被随机分给其中一个 ——
|
||||
验收脚本读到的开口恒为 0 mm、指尖间距 0.03 mm（真实值 127 mm），
|
||||
看起来完全像夹爪换算写错了。
|
||||
|
||||
所以验收第一步就是确认"图里只有一个我"。启动侧的那道闸门在
|
||||
scripts/run_sim_gz.sh 的预检里（拒绝起第二个仿真），这里是第二道。
|
||||
"""
|
||||
names = self.get_node_names()
|
||||
dupes = []
|
||||
for known in (
|
||||
"sim_gripper_adapter",
|
||||
"sim_camera_adapter",
|
||||
"sim_tactile_adapter",
|
||||
"robot_state_publisher",
|
||||
"joint_state_broadcaster",
|
||||
"gripper_controller",
|
||||
):
|
||||
n = sum(1 for name in names if name.strip("/") == known)
|
||||
if n > 1:
|
||||
dupes.append(f"{known} × {n}")
|
||||
return dupes
|
||||
|
||||
def check(self, verbose: bool) -> int:
|
||||
dupes = self._duplicate_nodes()
|
||||
if dupes:
|
||||
print("环境不干净：仿真节点有重名（说明有残留的旧会话在跑）：", file=sys.stderr)
|
||||
for d in dupes:
|
||||
print(f" - {d}", file=sys.stderr)
|
||||
print(
|
||||
"\n两个同名节点会抢话题/服务/TF，读数全是乱的。先停干净：\n"
|
||||
" bash scripts/stop_sim_gz.sh\n"
|
||||
"再重启（run_sim_gz.sh 自己也会拒绝在残留状态下启动）。",
|
||||
file=sys.stderr,
|
||||
)
|
||||
return 2
|
||||
|
||||
missing = self.wait_ready()
|
||||
if missing:
|
||||
print("环境不可用，缺：", file=sys.stderr)
|
||||
for m in missing:
|
||||
print(f" - {m}", file=sys.stderr)
|
||||
print(
|
||||
"\n先确认仿真在跑（bash scripts/run_sim_gz.sh），"
|
||||
"且夹爪适配器起来了（ros2 node list | grep sim_gripper_adapter）。",
|
||||
file=sys.stderr,
|
||||
)
|
||||
return 2
|
||||
|
||||
print("第 1 层：服务与状态")
|
||||
print(f" /gripper/{{open,close,move}} 可用；/gripper/status 有数据（开口 {self._last_status.position:.2f} mm）")
|
||||
|
||||
# --- 第 2、3 层：走一遍开口序列 -----------------------------------
|
||||
requests: List[Tuple[str, float]] = [
|
||||
("close", 0.0),
|
||||
("move", 8.0),
|
||||
("move", 20.0),
|
||||
("move", 50.0),
|
||||
("open", 0.0),
|
||||
]
|
||||
rows = []
|
||||
for verb, pos in requests:
|
||||
ok, msg, aperture, tips = self.movement(verb, pos)
|
||||
label = "全开" if verb == "open" else ("全闭合" if verb == "close" else f"{pos:.0f} mm")
|
||||
rows.append((label, float(pos) if verb == "move" else None, ok, msg, aperture, tips))
|
||||
|
||||
print("\n第 2 层：请求开口 vs 关节反馈换回的开口（下发链路）")
|
||||
print(f" {'命令':<10}{'期望开口':>10}{'状态开口':>10}{'差(mm)':>10} ")
|
||||
l2_fail = []
|
||||
# "期望开口"对 open 来说是仿真行程，从状态里推出来（open 时的读数）。
|
||||
open_row = rows[-1]
|
||||
stroke = open_row[4]
|
||||
for label, want, ok, msg, aperture, tips in rows:
|
||||
expect = stroke if label == "全开" else (0.0 if label == "全闭合" else want)
|
||||
if aperture is None:
|
||||
l2_fail.append(f"{label}: 拿不到状态开口")
|
||||
print(f" {label:<10}{expect:>10.2f}{'--':>10}{'--':>10} 状态缺失")
|
||||
continue
|
||||
diff = aperture - (expect or 0.0)
|
||||
bad = abs(diff) > POSITION_TOL_MM
|
||||
if bad:
|
||||
l2_fail.append(f"{label}: 状态开口 {aperture:.2f} != 期望 {expect:.2f}")
|
||||
if not ok:
|
||||
l2_fail.append(f"{label}: 服务返回失败（{msg}）")
|
||||
print(f" {label:<10}{expect:>10.2f}{aperture:>10.2f}{diff:>10.2f} {'✗' if bad else '✓'} {msg}")
|
||||
|
||||
print("\n第 3 层：指尖的**实际**位移（真几何，这一层才证明耦合是对的）")
|
||||
tips_ok = all(r[5] is not None for r in rows)
|
||||
if not tips_ok:
|
||||
print(" 查不到指尖 TF，无法验证几何 —— 这不算通过。", file=sys.stderr)
|
||||
return 1
|
||||
|
||||
# ⚠️ 这一层**故意不用 status 的开口读数**当基准：开口反馈取自从动关节，
|
||||
# 而第 2 层已经在验那条链路。这里只认"我请求了多少毫米"，
|
||||
# 几何摆在那里，跟任何软件读数无关 —— 这才叫独立验证。
|
||||
closed_tips = rows[0][5]
|
||||
open_tips = rows[-1][5]
|
||||
span_closed = _dist_mm(*closed_tips)
|
||||
span_open = _dist_mm(*open_tips)
|
||||
spans = [_dist_mm(*r[5]) for r in rows]
|
||||
|
||||
print(f" 合拢：指尖间距 {span_closed:7.2f} mm（tip frame 固有间距，不是开口）")
|
||||
print(f" 全开：指尖间距 {span_open:7.2f} mm")
|
||||
|
||||
# 逐档对比"请求开口"与"实测间距增量"：仿真里 1 mm 开口 ≙ 1 mm 间距，
|
||||
# 所以两者必须相等（探针实测 27.02→127.02 mm，正好 1.000）。
|
||||
travel_bad = []
|
||||
for label, want, ok, msg, aperture, tips in rows:
|
||||
if want is None:
|
||||
continue # close / open 的 request 不是毫米数，单独判
|
||||
actual = _dist_mm(*tips) - span_closed
|
||||
err = actual - want
|
||||
bad = abs(err) > TRAVEL_TOL_MM
|
||||
if bad:
|
||||
travel_bad.append(
|
||||
f"请求 {want:.0f} mm，指尖实际张开 {actual:.2f} mm（差 {err:+.2f}）"
|
||||
)
|
||||
print(
|
||||
f" 请求 {want:5.1f} mm：指尖张开 {actual:7.2f} mm "
|
||||
f"差 {err:+6.2f} mm {'✗' if bad else '✓'}"
|
||||
)
|
||||
|
||||
# 全开那一档用**几何自洽**来判：必须比 50 mm 那档还大，且与第 2 层的
|
||||
# 开口读数一致（两者都错成一样是不可能的）。
|
||||
if stroke:
|
||||
err = (span_open - span_closed) - stroke
|
||||
if abs(err) > TRAVEL_TOL_MM:
|
||||
travel_bad.append(
|
||||
f"全开行程 {span_open - span_closed:.2f} mm != 开口读数 {stroke:.2f} mm"
|
||||
)
|
||||
print(
|
||||
f" 全开行程 {span_open - span_closed:7.2f} mm 期望 {stroke:.2f} mm "
|
||||
f"差 {err:+.2f} mm {'✓' if abs(err) <= TRAVEL_TOL_MM else '✗'}"
|
||||
)
|
||||
|
||||
# 单调性：请求越大，间距必须越大（方向错了会在这里露出来）。
|
||||
monotonic = all(b > a for a, b in zip(spans, spans[1:]))
|
||||
print(f" 单调递增：{'✓' if monotonic else '✗ ' + ' → '.join(f'{s:.2f}' for s in spans)}")
|
||||
|
||||
# 对称性：两侧**行程相等**才算这条耦合链是对的。
|
||||
#
|
||||
# ⚠️ 这里换过两次判据，两次都是"判据本身错了"，值得记下来：
|
||||
# 1) 最初把两侧位移投影到"左指的张开方向"再比大小 → 一个完全
|
||||
# 对称的夹爪被判成不对称（左 +52.6 / 右 -42.5）。因为左指的
|
||||
# 位移方向自己带着 x 分量，拿它当基准去投影右侧必然偏。
|
||||
# 2) 改成"两指中点漂移" → 又误判（漂移 16.29 mm，而两侧各走
|
||||
# 52.56 / 52.55 mm，好得不能再好）。原因：tip frame 是刚性固连
|
||||
# 在**会旋转**的指垫上的（指垫既平移又转动，指面才保持平行），
|
||||
# 所以 frame 走的是弧线 —— 两侧弧线的"共同分量"让中点发生位移，
|
||||
# 这跟"左右不对称"是两回事。
|
||||
# 真正与方向无关、又能反映耦合好坏的量只有一个：**两侧各自走了多远**。
|
||||
left_move = 1000.0 * _dist_pts(closed_tips[0], open_tips[0])
|
||||
right_move = 1000.0 * _dist_pts(closed_tips[1], open_tips[1])
|
||||
asym = abs(left_move - right_move)
|
||||
asym_bad = asym > SYMMETRY_TOL_MM
|
||||
mid_closed = tuple((a + b) / 2.0 for a, b in zip(*closed_tips))
|
||||
mid_open = tuple((a + b) / 2.0 for a, b in zip(*open_tips))
|
||||
mid_drift = 1000.0 * _dist_pts(mid_closed, mid_open)
|
||||
print(
|
||||
f" 对称性：左右各走 {left_move:.2f} / {right_move:.2f} mm，"
|
||||
f"差 {asym:.2f} mm {'✗' if asym_bad else '✓'}"
|
||||
)
|
||||
print(
|
||||
f" （参考：两指中点整体位移 {mid_drift:.2f} mm。tip frame 固连在"
|
||||
f"会转动的指垫上，会画弧，所以这个数不为 0 是正常的，不参与判据。）"
|
||||
)
|
||||
|
||||
if verbose:
|
||||
print("\n明细：")
|
||||
for label, _want, ok, msg, aperture, tips in rows:
|
||||
d = "--" if tips is None else f"{_dist_mm(*tips):.2f}"
|
||||
print(f" {label:<10} ok={ok} 开口={aperture} 指尖间距={d} {msg}")
|
||||
# 原始坐标也打出来：上面那一列数字是"间距"，看着对/错时都需要
|
||||
# 回头核对它到底取到了哪两个点（历史上这里出过取错帧的假象）。
|
||||
print("\n 指尖坐标（world 系，米）：")
|
||||
for label, _want, _ok, _msg, _ap, tips in rows:
|
||||
if tips is None:
|
||||
print(f" {label:<10} --")
|
||||
continue
|
||||
lx, rx = tips
|
||||
print(
|
||||
f" {label:<10} 左=({lx[0]:+.4f},{lx[1]:+.4f},{lx[2]:+.4f}) "
|
||||
f"右=({rx[0]:+.4f},{rx[1]:+.4f},{rx[2]:+.4f})"
|
||||
)
|
||||
|
||||
problems = list(l2_fail)
|
||||
problems.extend(travel_bad)
|
||||
if not monotonic:
|
||||
problems.append("开口不是单调递增 —— 方向或顺序反了")
|
||||
if asym_bad:
|
||||
problems.append(
|
||||
f"左右不对称（左右行程差 {asym:.2f} mm）—— 某一侧耦合没生效"
|
||||
)
|
||||
|
||||
print()
|
||||
if problems:
|
||||
print("结论：不通过", file=sys.stderr)
|
||||
for p in problems:
|
||||
print(f" - {p}", file=sys.stderr)
|
||||
return 1
|
||||
print("结论：通过（下发链路 + 耦合几何 + 方向 + 对称性）")
|
||||
return 0
|
||||
|
||||
|
||||
def main() -> int:
|
||||
ap = argparse.ArgumentParser(description=__doc__.splitlines()[0])
|
||||
ap.add_argument("--verbose", action="store_true", help="打印每一档的明细")
|
||||
args = ap.parse_args()
|
||||
|
||||
rclpy.init()
|
||||
node = GripperCheck()
|
||||
try:
|
||||
return node.check(args.verbose)
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
Reference in New Issue
Block a user