From 811e71df1faa4878a87557fe5ba1e5ae01dc8958 Mon Sep 17 00:00:00 2001 From: arm_dev Date: Thu, 24 Sep 2026 14:42:56 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BB=BF=E7=9C=9F=E4=B8=8A=E6=96=99=EF=BC=9A?= =?UTF-8?q?=E5=A4=B9=E7=88=AA=E7=9C=9F=E8=83=BD=E5=A4=B9=E4=BD=8F=E9=80=9A?= =?UTF-8?q?=E6=AD=A2=E8=A7=84=EF=BC=88=E6=96=B0=E5=A2=9E=20sim=5Fload=5Fga?= =?UTF-8?q?uge.py=20+=20=E5=A4=B9=E7=88=AA=E5=87=BA=E5=8A=9B=E9=92=B3?= =?UTF-8?q?=E4=BD=8D=EF=BC=89?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 仿真里没有"人把规塞进夹爪"这一步，之前整条流程拖到 S07 才暴露（下探 25mm 什么也没碰到，看着像力阈值问题）。新增 scripts/sim_load_gauge.py 顶这一步。 上料为什么难（四个原因叠加，都实测踩过）： 1. URDF 里夹爪关节 effort=80Nm → 位置控制器把规顶飞。urdf_normalize 新增 _clamp_grip_effort：按握力 45N 与指节杠杆反推，指节 4.79Nm / 主关节 45N。 2. **指腹接触面不在 tcp_frame 原点上**：URDF 里 inner_finger_pad_joint z=0.0235、finger_tcp_joint z=0.0360（同父链节同方向）→ 指腹中心比 tcp_frame 高 12.5mm。所以规尖→指腹中心是 0.043，不是夹持段中心的 0.0555。用错值的现象极迷惑：gz 报得出 Contacts、也能读到接触，但规 就是托不住、一路滑下去。ΔZ 扫描实测：0/±5mm 掉、+10~+15mm 夹住。 3. 摆完位就自由落体：ROS 往返 100ms+ 而物理步进 1ms，等合爪指令到达， 规已掉 125mm。改成**像人手一样托着** —— 合爪期间 50Hz 按当前指腹位姿 把规瞬移回两指之间，托到真夹住为止。 4. 合到多少不猜：从松到紧扫开口，扫到"松手后不掉"为止。指腹实际接触面 未必在 tcp_frame 原点，拿"指间间距=开口"反推闭合量会差好几毫米。 结果：漂移 0.4mm、两指 |fnormal|≈46.5N（正好是 45N 设计握力）。 另修 _set_gauge_pose 的 ign service 口径说明与 _gauge_pose_now 的解析。 Co-authored-by: Cursor --- scripts/sim_load_gauge.py | 1364 +++++++++++++++++ .../arm_sim_gazebo/urdf_normalize.py | 75 +- .../test/test_urdf_normalize.py | 93 +- 3 files changed, 1519 insertions(+), 13 deletions(-) create mode 100644 scripts/sim_load_gauge.py diff --git a/scripts/sim_load_gauge.py b/scripts/sim_load_gauge.py new file mode 100644 index 0000000..47492d1 --- /dev/null +++ b/scripts/sim_load_gauge.py @@ -0,0 +1,1364 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""把通止规装进夹爪 —— **仿真专用**的"人工上料"。 + +为什么必须有这一步（不是仿真偷懒，是仿真缺了真机天生就有的东西） +------------------------------------------------------------------ +通止规装配整条流程（S00–S11）的前提是：**规已经被夹爪夹住、规轴≈法兰轴**。 +这个前提在真机上是**人**完成的：现场把规塞进两指之间、合爪、看一眼轴线 +（`stage2_locate_node` 悬停时还会再提醒一次"请确认：左臂夹爪已夹持通止规， +且通止规轴线≈法兰轴线"）。仿真里没有这个人，而 S02 的夹持确认是**预留跳过** +的（`gauge_assembly.grip_check: false`），于是： + + 夹爪全程张开（实测 position=99.93mm = 全开） + → 规一直躺在规座上（实测世界位姿 = 场景里的摆放位姿，一动没动） + → S07 下压 25mm 什么也碰不到 → 报"没触发接触判据" → 中止 + +**看着像力阈值/算法问题，实际是"手里没规"。** 这类错在日志里完全不显眼 +（自检全绿、定位成功、轨迹正常），所以这一步单独做成脚本、并且在 +`run_gauge_assembly.sh --sim` 里自动跑，跑不过就直接停 —— 不允许"带着空夹爪 +继续往下跑"。 + +它做的事（三步，每步都有客观校验） +---------------------------------- +1. **摆位**：读 TF 里两指的 TCP（`*_finger_tcp`，= 指腹内侧接触点），取中点 + = 工具轴；沿工具轴向下 55.5mm 得到规尖该在的位置 + （55.5mm = 规的夹持段中点，见 `models/gauge/model.sdf`：夹持段 z 33~78mm）， + 姿态让规的 +Z 朝上（与真机"竖直朝下装配"一致）。用 Gazebo 的 + `/world//set_pose` 服务把规搬到那里。 +2. **合爪**：调 `/gripper/close`（**契约后端**，`arm_msgs/srv/GripperExecute`）。 + 仿真夹爪全行程 99.93mm、全合拢 27.09mm，而规的夹持段是 φ30mm —— + 所以合拢就是**刚性地压住**（位置控制，没有力控限幅）。这正是触觉能读到 + 力的原因：指腹与规之间有了真实的接触力。 +3. **校验**（不做假成功）： + a) 规的位姿在合爪后仍然 ≈ 目标位姿（掉出来会躺在台面上，差得很远）； + b) 触觉 `/tactile/{left,right}/status` 至少一侧 `contact: true` 或 + `|fnormal|` 明显高于空载噪声。 + 两条都不过就报红并给出排查方向（帧名/模型名/世界名/合爪是否真动了）。 + +用法 +---- + bash scripts/sim_env.sh # 先 source 仿真环境（含 DDS 域） + python3 scripts/sim_load_gauge.py # 默认 --arm sim --world assembly + python3 scripts/sim_load_gauge.py --dry-run # 只算位姿、不碰仿真 + python3 scripts/sim_load_gauge.py --place-only # 只摆位、不碰夹爪（调参用） + +退出码：0=装好了（校验通过）；2=没装好（原因打印在最后）。 +""" + +from __future__ import annotations + +import argparse +import math +import subprocess +import sys +import time +from pathlib import Path +from typing import Dict, List, Optional, Tuple + +import yaml + +ROOT = Path(__file__).resolve().parents[1] + +# 规的夹持段中点（相对规尖）[m]。来源：models/gauge/model.sdf +# GO 通端 0~30mm、台肩 30~33mm、夹持段 33~78mm、止端 78~86mm +# 夹持段中点 = (33+78)/2 = 55.5mm。指腹应夹在这个高度上。 +#: 规（devices/sim/gazebo/models/gauge/model.sdf）**夹持段**的外径。 +#: 夹爪必须夹在这里（φ30），不是细的通端 —— 通端只有 φ6，夹爪合到那里 +#: 会直接从规身上滑过去。改 gauge/model.sdf 的 grip 段半径时要一起改这里， +#: 否则 sim_load_gauge.py 会按错的直径去合爪（表现：合爪开口到 0、规掉下去）。 +GAUGE_GRIP_DIA_MM = 30.0 + +#: 规尖 → **指腹接触面中心** 的距离 [m]。 +#: +#: 为什么是 0.043 而不是"夹持段中心到规尖"的 0.0555： +#: gauge/model.sdf 里夹持段中心在规尖上方 55.5mm（0.033~0.078 的中值）， +#: 但夹爪**指腹的接触面**并不在 tcp_frame 原点上。URDF 里两条固定关节写着： +#: sim_*_inner_finger_pad_joint xyz="0 0.010 0.0235" +#: sim_*_finger_tcp_joint xyz="0 0.0135 0.0360" +#: 同一父链节、同一个方向，所以指腹中心比 tcp_frame **高** +#: 0.0360 − 0.0235 = 12.5mm。 +#: 要让"夹持段中心"落在"指腹中心"上，规尖就得比 tcp_frame 中点 +#: 高 12.5mm 才行 → 0.0555 − 0.0125 = 0.043。 +#: +#: 实测验证（夹爪合到 23.9mm = 指间间距 24mm，规径 30mm → 6mm 过盈）： +#: 把规沿 Z 扫一遍，只有"规尖 = 两指中点 − 43mm"这一带能夹住： +#: Δ 0mm → 松手就掉 125mm（落台面） +#: Δ +10mm → 夹住（漂移 0.2mm） +#: Δ +15mm → 夹住 +#: Δ +20mm → 掉 9mm（已经在夹持段上端、开始收成 φ16 台肩） +#: 所以偏移取 +12.5mm。用错的值（比如 0.0555）的现象非常迷惑： +#: gz 报得出 Contacts、夹爪也能读到接触，但规就是托不住、一路滑下去。 +GAUGE_GRASP_CENTER_M = 0.043 + +# 台面高度（scene.yaml 的 table_top_z）：用来判断"够不够空间把规塞进去"。 +FALLBACK_TABLE_Z = 0.40 +# 触觉"夹住了"的判据：contact 位或 |fnormal| 超过这个噪声地板。 +# 空载时仿真触觉的 fnormal 实测 ~1e-5（见 config/tactile.yaml 的基线说明）。 +TACTILE_NOISE_FLOOR = 0.05 + + +def _load_yaml(path: Path) -> dict: + with path.open(encoding="utf-8") as fh: + return yaml.safe_load(fh) or {} + + +def _quat_to_R(x: float, y: float, z: float, w: float) -> List[List[float]]: + """四元数 → 3×3 旋转矩阵（行主序）。自己算，不引入 scipy/tf_transformations。""" + n = math.sqrt(x * x + y * y + z * z + w * w) + if n == 0.0: + raise ValueError("四元数全零") + x, y, z, w = x / n, y / n, z / n, w / n + return [ + [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)], + [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)], + [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)], + ] + + +def _R_to_quat(R: List[List[float]]) -> Tuple[float, float, float, float]: + """3×3 旋转矩阵 → 四元数 (x, y, z, w)。Shepperd 分支，保证数值稳定。""" + tr = R[0][0] + R[1][1] + R[2][2] + if tr > 0.0: + s = math.sqrt(tr + 1.0) * 2.0 + w = 0.25 * s + x = (R[2][1] - R[1][2]) / s + y = (R[0][2] - R[2][0]) / s + z = (R[1][0] - R[0][1]) / s + elif R[0][0] > R[1][1] and R[0][0] > R[2][2]: + s = math.sqrt(1.0 + R[0][0] - R[1][1] - R[2][2]) * 2.0 + w = (R[2][1] - R[1][2]) / s + x = 0.25 * s + y = (R[0][1] + R[1][0]) / s + z = (R[0][2] + R[2][0]) / s + elif R[1][1] > R[2][2]: + s = math.sqrt(1.0 + R[1][1] - R[0][0] - R[2][2]) * 2.0 + w = (R[0][2] - R[2][0]) / s + x = (R[0][1] + R[1][0]) / s + y = 0.25 * s + z = (R[1][2] + R[2][1]) / s + else: + s = math.sqrt(1.0 + R[2][2] - R[0][0] - R[1][1]) * 2.0 + w = (R[1][0] - R[0][1]) / s + x = (R[0][2] + R[2][0]) / s + y = (R[1][2] + R[2][1]) / s + z = 0.25 * s + return (x, y, z, w) + + +def _mat_mul(A: List[List[float]], B: List[List[float]]) -> List[List[float]]: + return [[sum(A[i][k] * B[k][j] for k in range(3)) for j in range(3)] for i in range(3)] + + +def _mat_vec(A: List[List[float]], v: Tuple[float, float, float]) -> Tuple[float, float, float]: + return tuple(sum(A[i][k] * v[k] for k in range(3)) for i in range(3)) # type: ignore[return-value] + + +#: 绕 X 轴转 180°：把"指尖系（+Z 指向工具外侧=向下）"翻成"规系（+Z 朝上）"。 +#: 规的局部原点在**规尖**、+Z 朝上（见 model.sdf），而指尖系的 +Z 朝下 +#: （实测：world←finger_tcp 的 Z 列 ≈ (0, 0, -1)），所以两者差一个 Rx(π)。 +RX_PI = [[1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, -1.0]] + + +def gauge_target_pose( + tips: Dict[str, Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]], + grasp_center_m: Optional[float] = None, +) -> Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]: + """两指 TCP 的世界位姿 → 规在世界系里的目标位姿（原点=规尖）。 + + ``grasp_center_m=None`` 时用模块常量 ``GAUGE_GRASP_CENTER_M`` + （**调用时**读，不是定义时 —— 否则命令行覆盖会静默无效）。 + + 纯函数（离线可测）：输入左右两个 ``(位置, 四元数 xyzw)``，输出规的 + ``(位置, 四元数 xyzw)``。规则： + * 工具轴位姿 = 两指 TCP 的**中点**、姿态取左指（两指只差一个绕轴自转）； + * 规尖 = 中点沿工具轴**向外**（指尖系 +Z，实测朝下）移 ``grasp_center_m``； + * 规姿态 = 指尖姿态再绕 X 转 180°，让规的 +Z 朝上（规尖朝下）。 + """ + missing = [k for k in ("left", "right") if k not in tips] + if missing: + raise ValueError(f"缺少 TCP 位姿：{missing}（TF 里没有 *_finger_tcp？）") + if grasp_center_m is None: + grasp_center_m = GAUGE_GRASP_CENTER_M + (pl, ql) = tips["left"] + (pr, _qr) = tips["right"] + mid = ((pl[0] + pr[0]) / 2.0, (pl[1] + pr[1]) / 2.0, (pl[2] + pr[2]) / 2.0) + R_tip = _quat_to_R(*ql) + tool_axis = (R_tip[0][2], R_tip[1][2], R_tip[2][2]) # 指尖系 +Z 在世界里的方向 + tip = ( + mid[0] + tool_axis[0] * grasp_center_m, + mid[1] + tool_axis[1] * grasp_center_m, + mid[2] + tool_axis[2] * grasp_center_m, + ) + q_gauge = _R_to_quat(_mat_mul(R_tip, RX_PI)) + return tip, q_gauge + + +def _run(cmd: List[str], timeout: float = 8.0) -> Tuple[int, str]: + """跑一条外部命令，返回 (退出码, 合并输出)。不抛异常（调用方自己判）。""" + try: + p = subprocess.run(cmd, capture_output=True, text=True, timeout=timeout, check=False) + return p.returncode, (p.stdout or "") + (p.stderr or "") + except FileNotFoundError: + return 127, f"找不到可执行文件：{cmd[0]}" + except subprocess.TimeoutExpired: + return 124, f"命令超时（{timeout}s）：{' '.join(cmd)}" + + +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"，一个字母的差别、 + 报错信息完全没提原因）。 + """ + 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() + + +def _gauge_pose_now(name: str) -> Optional[Tuple[float, float, float]]: + """读回模型当前的世界位置（校验用）。读不到返回 None。""" + code, out = _run(["ign", "model", "-m", name, "--pose"], timeout=10.0) + if code != 0: + return None + seen_header = False + for raw in out.splitlines(): + line = raw.strip() + if "Pose [ XYZ" in line: + seen_header = True + continue + if not seen_header or not (line.startswith("[") and line.endswith("]")): + continue + parts = line[1:-1].split() # 形如 "[0.4926 -0.0039 0.4183]"（XYZ 那行） + if len(parts) != 3: + continue + try: + return (float(parts[0]), float(parts[1]), float(parts[2])) + except ValueError: + continue + return None + + +# --------------------------------------------------------------------------- +# 竖直上料位姿：用 PyKDL 对同一份 URDF 做 IK +# --------------------------------------------------------------------------- +# 为什么需要它（实测，2026-09-24）： +# * 仿真刚起来时关节在**零位**，工具轴是**水平**的（实测 TCP 的 Z 轴 = +X）。 +# 水平夹住一根钢规，重力会把它从两指之间抽出去（实测刚合爪就掉回规座， +# 漂移 857mm）——不是夹爪坏了，是几何不对。 +# * 仿真 home 位姿（/set_named_pose home）工具轴斜 21°、指尖离台面只有 2mm， +# 规根本放不下。 +# 所以上料前必须自己找一个"工具轴朝下 + 指尖离台面足够高"的位姿。真机上 +# 这一步是人做的（把臂摆到方便的位置再装夹），仿真里用 IK 算出来。 +# +# 用 PyKDL（环境里现成，不需要 kdl_parser_py / urdf_parser_py）：自己从 URDF +# 里沿着 base→flange 这条链建 KDL.Chain（只认 fixed/revolute 两种关节）， +# 再用 ChainIkSolverPos_LMA 解。**不改任何应用代码、不依赖 MoveIt。** +LOAD_MID_Z_M = 0.58 # 想让两指 TCP 中点停在多高 [m]（规尖再低 55.5mm） +LOAD_MID_XY = (0.48, 0.0) # 想让工具轴落在台面的哪个 XY [m]（台面上方空旷处） + + +def _parse_chain_joints(urdf_xml: str) -> Tuple[Dict[str, dict], Dict[str, str]]: + """把 URDF 里的关节读成 ``{name: {...}}`` 与 ``{child_link: joint_name}``。 + + 每个关节里带上 ``limit``（可动关节的上下限 [rad]），IK 的解要用它筛一遍 —— + 解出的角度超限时控制器会直接拒绝（症状是"轨迹被拒/不动"），不如在这里挡掉。 + """ + import xml.etree.ElementTree as ET + + root = ET.fromstring(urdf_xml) + joints: Dict[str, dict] = {} + parent_of_child: Dict[str, str] = {} + for j in root.findall("joint"): + name = j.get("name") + jtype = j.get("type") + if not name or jtype not in ("fixed", "revolute", "continuous"): + continue + org = j.find("origin") + axis = j.find("axis") + lim = j.find("limit") + joints[name] = { + "type": jtype, + "parent": (j.find("parent").get("link") if j.find("parent") is not None else None), + "child": (j.find("child").get("link") if j.find("child") is not None else None), + "xyz": [float(v) for v in ((org.get("xyz") if org is not None else None) or "0 0 0").split()], + "rpy": [float(v) for v in ((org.get("rpy") if org is not None else None) or "0 0 0").split()], + "axis": [float(v) for v in ((axis.get("xyz") if axis is not None else None) or "0 0 1").split()], + "limit": ( + (float(lim.get("lower", "-3.15")), float(lim.get("upper", "3.15"))) + if lim is not None else None + ), + } + parent_of_child[joints[name]["child"]] = name + return joints, parent_of_child + + +def _build_kdl_chain(urdf_xml: str, root_link: str, tip_link: str): + """按 URDF 从 ``root_link`` 走到 ``tip_link``，建成 ``(KDL.Chain, [关节名…])``。 + + 返回的关节名顺序 = 链上**可动**关节的顺序（从根到末）。控制器给的关节顺序 + 可能不同，所以 IK 结果按名字映射回 ``{关节名: 角度}``，不靠位置对齐。 + """ + import PyKDL as kdl + + joints, parent_of_child = _parse_chain_joints(urdf_xml) + # 从 tip 往 root 回溯，收集关节 + chain_joints: List[str] = [] + link = tip_link + guard = 0 + while link != root_link: + guard += 1 + if guard > 200: + raise RuntimeError(f"URDF 里 {tip_link} 回溯不到 {root_link}（成环了？）") + jname = parent_of_child.get(link) + if jname is None: + raise RuntimeError(f"URDF 里找不到 {link} 的父关节（根是不是 {root_link}？）") + chain_joints.append(jname) + link = joints[jname]["parent"] + chain_joints.reverse() + + chain = kdl.Chain() + movable: List[str] = [] + for jname in chain_joints: + j = joints[jname] + rpy = j["rpy"] + R_org = kdl.Rotation.RPY(rpy[0], rpy[1], rpy[2]) + xyz = kdl.Vector(j["xyz"][0], j["xyz"][1], j["xyz"][2]) + if j["type"] == "fixed": + chain.addSegment(kdl.Segment(jname, kdl.Joint(), kdl.Frame(R_org, xyz))) + else: + # URDF: pose(q) = Trans(xyz)·R(rpy)·Rot(a, q) （a 在关节系里） + # 但 PyKDL 的 Segment 只能表示"绕**过关节原点**的轴转 q"，转完再乘 f_tip。 + # 所以要把上面的式子拆成**三段**： + # Trans(xyz) → Rot(R(rpy)·a, q) → R(rpy) + # 依据恒等式 R·Rot(a,q) == Rot(R·a, q)·R（取 R = R(rpy)）： + # Trans(xyz)·R(rpy)·Rot(a,q) == Trans(xyz)·Rot(R(rpy)·a,q)·R(rpy) ✓ + # + # ⚠ 这段踩过两次坑（2026-09-24），都是"q=0 看不出来、一动就错"： + # ① 轴不乘 R(rpy)、且把 xyz 塞进 f_tip：零位 0.007mm 通过，动起来差 263mm； + # ② 把 xyz 当成 joint 的 origin：零位就差 1260mm，动起来差 971mm。 + # 本文件下方 `--check-ik` 就是为这个坑准备的：**必须在非零位验**。 + chain.addSegment(kdl.Segment(jname + "_t", kdl.Joint(), kdl.Frame(kdl.Vector(*j["xyz"])))) + axis = R_org * kdl.Vector(j["axis"][0], j["axis"][1], j["axis"][2]) + chain.addSegment( + kdl.Segment( + jname, + kdl.Joint(jname, kdl.Vector(0.0, 0.0, 0.0), axis, kdl.Joint.RotAxis), + kdl.Frame(R_org), + ) + ) + movable.append(jname) + limits = {nm: joints[nm]["limit"] for nm in movable} + return chain, movable, limits + + +def _solve_load_joints( + chain, movable: List[str], seed: Dict[str, float], + target_flange_pos, target_R, +) -> Optional[Dict[str, float]]: + """IK：让 **flange** 落在 ``target_flange_pos`` 且姿态为 ``target_R``。 + + 调用方已经在 base 系里把"两指中点目标"换算成 flange 目标了（偏移量是 + 从 TF 实测的，见 ``_go_vertical_load_pose``）。这里只负责解 + 验收。 + """ + import PyKDL as kdl + + p_flange = kdl.Vector(*target_flange_pos) + F_target = kdl.Frame(target_R, p_flange) + + q_seed = kdl.JntArray(len(movable)) + for i, nm in enumerate(movable): + q_seed[i] = float(seed.get(nm, 0.0)) + q_out = kdl.JntArray(len(movable)) + solver = kdl.ChainIkSolverPos_LMA(chain, 1e-6, 500, 1e-9) + ret = solver.CartToJnt(q_seed, F_target, q_out) + if ret < 0: + return None + # 校验：解出来的 FK 必须真的落在目标附近（LMA 会返回"最接近"，不保证命中） + fk = kdl.ChainFkSolverPos_recursive(chain) + F_now = kdl.Frame() + fk.JntToCart(q_out, F_now) + if (F_now.p - F_target.p).Norm() > 0.002: + return None + if (F_now.M.UnitZ() - F_target.M.UnitZ()).Norm() > 0.02: + return None + return {nm: float(q_out[i]) for i, nm in enumerate(movable)} + + +def _solve_best( + chain, movable, limits, seed_list, target_pos, target_R, ref_joints, +) -> Optional[Tuple[Dict[str, float], str, float]]: + """多初值求 IK，返回 ``(解, 用的初值, 代价)``；找不到返回 None。 + + 为什么要多初值：``ChainIkSolverPos_LMA`` 是**局部**求解器，从零位出发经常停在 + 一个解不出来的局部极值上（实测：同一个目标，零位/垂肘初值全失败，换个"侧伸" + 初值就解出来了）。所以这里给一批初值（当前位姿 + 确定性随机扰动），挑**离当前 + 位姿最近**的可行解 —— 近 = 动作小 = 安全、也不容易擦到台面。 + + ``ref_joints`` 是"离谁近"的参照（一般用当前关节角）。 + """ + import math as _math + import random + + best: Optional[Dict[str, float]] = None + best_tag = "" + best_cost = float("inf") + for tag, seed in seed_list: + sol = _solve_load_joints(chain, movable, seed, target_pos, target_R) + if sol is None: + continue + bad = [ + nm for nm in movable + if limits.get(nm) and not (limits[nm][0] <= sol[nm] <= limits[nm][1]) + ] + if bad: + continue + cost = sum(abs(sol[nm] - ref_joints.get(nm, 0.0)) for nm in movable) + if cost < best_cost: + best, best_tag, best_cost = sol, tag, cost + if best is None: + return None + return best, best_tag, best_cost + + +def _make_seeds(movable, limits, current: Dict[str, float], count: int = 24) -> List[Tuple[str, Dict[str, float]]]: + """一批 IK 初值：当前位姿 + 全局撒点 + 当前位姿附近的扰动。 + + 为什么要三种： + * 当前位姿 —— 能解出来时解最"近"（动作最小）； + * 全局撒点 —— 目标离当前位姿很远时（比如从"工具轴水平"变到"朝下"）， + 只围着一个点扰动是找不到的（区域太窄，实测就是这个问题）； + * 附近扰动 —— 覆盖同一片构型里的不同肘部/腕部解。 + 随机数用固定种子：同一份代码给出同一批初值，失败可复现、好排查。 + """ + import random + + rng = random.Random(20260924) + out: List[Tuple[str, Dict[str, float]]] = [("当前位姿", dict(current))] + out.append(("零位", {nm: 0.0 for nm in movable})) + + def _clamp(nm: str, v: float) -> float: + lo, hi = limits.get(nm) or (-2.5, 2.5) + return min(max(v, lo + 1e-3), hi - 1e-3) + + for i in range(6): # 全局 + out.append((f"全局{i + 1}", {nm: _clamp(nm, rng.uniform(*(limits.get(nm) or (-2.5, 2.5)))) for nm in movable})) + for i in range(count): # 附近 + s: Dict[str, float] = {} + for nm in movable: + lo, hi = limits.get(nm) or (-2.5, 2.5) + s[nm] = _clamp(nm, current.get(nm, 0.0) + rng.uniform(-(hi - lo) * 0.25, (hi - lo) * 0.25)) + out.append((f"附近{i + 1}", s)) + return out + + +# --------------------------------------------------------------------------- +# ROS 侧：TF + 夹爪 + 触觉 +# --------------------------------------------------------------------------- +def _go_vertical_load_pose(node, buf, spin_for, wait_tf, tcp_frames, args) -> bool: + """把机械臂摆到"工具轴朝下、两指 TCP 中点停在台面上方空旷处"的位姿。 + + 为什么不用 /home：仿真 home 的工具轴斜 21°、指尖离台面只有 2mm（实测）， + 86mm 长的规放不下。真机上是人把臂摆到合适位置再装夹，仿真里没有这个人。 + + 做法：从 ``/robot_description`` 拿 URDF → 沿 base→flange 建 KDL 链 → + 对"flange 位姿"做 IK（目标由两指 TCP 中点的目标位姿反推，偏移量从 + 当前 TF 实测得到，所以不假设夹爪多长）→ 走 JTC 轨迹 → 等静止 → 复测。 + """ + from std_msgs.msg import String + from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy + + import PyKDL as kdl + + from rclpy.action import ActionClient + + # (1) URDF：robot_state_publisher 用 transient_local 发，订阅端必须同 QoS， + # 否则"订阅了但永远收不到"（典型的静默失效）。 + urdf_box: Dict[str, str] = {} + qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + ) + node.create_subscription(String, "/robot_description", lambda m: urdf_box.__setitem__("x", m.data), qos) + t0 = time.monotonic() + while "x" not in urdf_box and time.monotonic() - t0 < 10.0: + spin_for(0.1) + if "x" not in urdf_box: + print("✗ 收不到 /robot_description（robot_state_publisher 没起来？）") + return False + urdf_xml = urdf_box["x"] + + # (2) 当前的 TCP 中点在 flange 系里的偏移（实测，不假设） + base_frame, ee_frame = args.base_frame, args.ee_frame + got_b = wait_tf(base_frame, timeout_s=5.0) + got_e = wait_tf(ee_frame, timeout_s=5.0) + if got_b is None or got_e is None: + print(f"✗ TF 里找不到 {base_frame} 或 {ee_frame}（--base-frame/--ee-frame 可改）") + return False + p_base, q_base = got_b + p_ee, q_ee = got_e + tips_px = {} + for side in ("left", "right"): + g = wait_tf(tcp_frames[side], timeout_s=5.0) + if g is None: + print(f"✗ TF 里找不到 {tcp_frames[side]}") + return False + tips_px[side] = g[0] + p_mid = ( + (tips_px["left"][0] + tips_px["right"][0]) / 2.0, + (tips_px["left"][1] + tips_px["right"][1]) / 2.0, + (tips_px["left"][2] + tips_px["right"][2]) / 2.0, + ) + # 世界 → base 系 + R_base = _quat_to_R(*q_base) + d_world = (p_mid[0] - p_base[0], p_mid[1] - p_base[1], p_mid[2] - p_base[2]) + R_ee_world = _quat_to_R(*q_ee) + # 中点在 flange 系里的位置 = R_ee^T · (p_mid - p_ee) + dm = (p_mid[0] - p_ee[0], p_mid[1] - p_ee[1], p_mid[2] - p_ee[2]) + mid_in_flange = _mat_vec([[R_ee_world[j][i] for j in range(3)] for i in range(3)], dm) + print(f" 两指中点当前 ({p_mid[0]:.4f}, {p_mid[1]:.4f}, {p_mid[2]:.4f})，" + f"在 flange 系偏移 ({mid_in_flange[0]:+.4f}, {mid_in_flange[1]:+.4f}, {mid_in_flange[2]:+.4f})") + _ = d_world # 仅诊断用（世界系位移），下面直接换到 base 系算目标 + + # (3) 目标姿态：工具轴朝下（flange 的 +Z → 世界 -Z）。yaw 在下面 IK 时一起扫 + # （--load-yaw 是首选值，扫不出来时会自动换别的 yaw）。这里不预先算矩阵： + # 每个 yaw 都要一套，放在 _down_R_base 里按需算，避免"算了一个、用了另一个"。 + print(" 目标姿态：工具轴朝下（世界 -Z）") + + # (4) IK：先解"高位竖直"（approach），再以它为参照解"低位竖直"（final）。 + # 为什么要两段：从零位（工具轴水平、臂抬得高）一步插值到低处竖直，关节 + # 空间的直线**可能从台面里穿过去**（Gazebo 是物理的，会被顶住、看着像 + # "轨迹失败/不动"）。先到台面上方的高位竖直，再竖直下降，路径就干净了。 + # 两段**必须同一个 yaw**：yaw 不同，下降就成了斜着平移，不是"竖直下行"。 + chain, movable, limits = _build_kdl_chain(urdf_xml, base_frame, ee_frame) + cur: Dict[str, float] = {nm: 0.0 for nm in movable} + try: + from sensor_msgs.msg import JointState + + js_box: Dict[str, object] = {} + node.create_subscription(JointState, "/joint_states", lambda m: js_box.__setitem__("m", m), 10) + t0 = time.monotonic() + while "m" not in js_box and time.monotonic() - t0 < 10.0: + spin_for(0.1) + msg = js_box.get("m") + if msg is not None: + for nm, pos in zip(getattr(msg, "name", []), getattr(msg, "position", [])): + if nm in cur: + cur[nm] = float(pos) + except Exception as exc: # noqa: BLE001 + print(f"⚠ 读 /joint_states 做初值失败（用零位初值继续）：{exc}") + + R_wb_T = [[R_base[j][i] for j in range(3)] for i in range(3)] # 世界→base 的旋转 + seeds = _make_seeds(movable, limits, cur) + + def _down_R_base(yaw: float): + """base 系里"工具轴朝下 + 偏航 yaw"的旋转矩阵。""" + R_down_world = _mat_mul( + [[math.cos(yaw), -math.sin(yaw), 0.0], + [math.sin(yaw), math.cos(yaw), 0.0], + [0.0, 0.0, 1.0]], + RX_PI, + ) + return _mat_mul(R_wb_T, R_down_world) + + def _mid_to_flange(mid_xyz, R_down_base): + """两个 TCP 中点（世界系）→ 需要的 flange 位置（base 系）。""" + dw = (mid_xyz[0] - p_base[0], mid_xyz[1] - p_base[1], mid_xyz[2] - p_base[2]) + mb = _mat_vec(R_wb_T, dw) + ob = _mat_vec(R_down_base, mid_in_flange) + return (mb[0] - ob[0], mb[1] - ob[1], mb[2] - ob[2]) + + # yaw 从 --load-yaw 起试，再扫一圈。为什么要扫：工具轴竖直时手腕接近奇异， + # 有些 yaw 会让 joint7 顶到限位/解不出来，换个 yaw 就解出来了。 + yaws = [args.load_yaw] + [ + math.radians(d) for d in (0, 30, -30, 60, -60, 90, -90, 120, -120, 150, -150, 180) + ] + + def _solve_at(mid_xyz, ref) -> Optional[Tuple[Dict[str, float], str, float, float]]: + """在 ``mid_xyz`` 这个点上扫 yaw + 多初值，返回 (解, 初值, 代价, yaw) 里最好的。""" + best = None + for yaw in yaws: + Rdb = _down_R_base(yaw) + r = _solve_best(chain, movable, limits, seeds, + _mid_to_flange(mid_xyz, Rdb), + kdl.Rotation(*[Rdb[i][j] for i in range(3) for j in range(3)]), ref) + if r is not None and (best is None or r[2] < best[2]): + best = (r[0], r[1], r[2], yaw) + return best + + # 先在"用户指定的点"（默认值也是这个）上解；解不出来再在附近**换个落点**试。 + # 为什么允许换落点：规是脚本自己摆进指间的（不是从规座上抓的），所以落点只要 + # 在台面上方、够空旷就行 —— 换一个 IK 解得出、且动作最小的点，比死磕一个点强。 + user_mid = (args.load_x, args.load_y, args.load_z) + grid = [user_mid] + [ + (x, y, z) + for z in (args.load_z, args.load_z + 0.08, args.load_z - 0.06) + for x in (args.load_x - 0.08, args.load_x + 0.06, args.load_x - 0.16) + for y in (args.load_y,) + ] + lo = None + for mid in grid: + if mid[2] - GAUGE_GRASP_CENTER_M < FALLBACK_TABLE_Z + 0.05: + continue # 规尖会贴到台面，跳过 + r = _solve_at(mid, cur) + if r is None: + continue + lo = (r[0], r[1], r[2], r[3], mid) + if mid == user_mid: + break # 指定点能解就用它，不再换落点（可预期） + if lo is None: + print("✗ IK 没解出竖直上料位姿。") + print(f" 目标：两指中点 ≈ ({args.load_x:.2f}, {args.load_y:.2f}, {args.load_z:.2f})、" + "工具轴朝下（扫了 13 个 yaw、30 组初值、13 个候选落点）。") + print(" 大概率是这些点都在臂的工作空间外：换 --load-x/y/z（比如 x 小一点、z 高一点）再试。") + return False + sol, sol_tag, sol_cost, sol_yaw, sol_mid = lo + + # 高位：从"矮到高"找第一个能解出来的提升量。矮一点更接近目标、关节空间路径更短， + # 只要能把规提到台面之上就够了（规尖在 TCP 中点下方 55.5mm，见 GAUGE_GRASP_CENTER_M）。 + hi = None + for dz in (0.06, 0.10, 0.16, 0.22, 0.30): + if dz > args.load_approach_dz: + break + r = _solve_at((sol_mid[0], sol_mid[1], sol_mid[2] + dz), sol) + if r is not None and abs(r[3] - sol_yaw) < 1e-9: + hi = (r[0], r[1], r[2], r[3], dz) + break + if hi is None: + print("⚠ 找不到「与低位同 yaw 的高位」：改成一步到位" + "（关节空间直线路径，注意看 Gazebo 会不会擦台面）。") + + print("=== 摆竖直上料位姿（IK） ===") + print(f" 落点：两指中点 ({sol_mid[0]:.3f}, {sol_mid[1]:.3f}, {sol_mid[2]:.3f})" + + ("" if sol_mid == user_mid else "（**不是**你给的点：你那个点 IK 解不出来，换了这个）")) + if hi is not None: + print(f" 高位：{' '.join(f'{nm}={hi[0][nm]:+.3f}' for nm in movable)}" + f" （提升 {hi[4] * 1000:.0f}mm，初值 {hi[1]}）") + print(f" 低位：{' '.join(f'{nm}={sol[nm]:+.3f}' for nm in movable)}" + f" （初值 {sol_tag}，yaw {math.degrees(sol_yaw):+.0f}°，离当前位姿代价 {sol_cost:.3f}rad）") + if args.dry_run: + print(" --dry-run：只算不发（要看真动作去掉 --dry-run）") + return True + + # (5) 走 JTC 轨迹 + # 用 **arm_msgs** 版的 FollowJointTrajectory（跟 run_joint_trajectory.py 完全同一条 + # 路：真机是 Flexiv 驱动直接提供，仿真由 arm_control/trajectory_bridge 桥到 + # control_msgs 的 JTC）。不要用 control_msgs 版 —— 那会绕过应用层的桥， + # 在仿真里名字/类型都对不上（同名不同型，静默等不到 server）。 + from arm_msgs.action import FollowJointTrajectory as ArmFJT + + prefix = args.joint_prefix + jnames = [f"{prefix}joint{i}" for i in range(1, 8)] + missing = [n for n in jnames if n not in sol] + if missing: + print(f"✗ IK 结果里没有这些关节：{missing}（--joint-prefix 对不对？当前 {prefix!r}）") + return False + goal = ArmFJT.Goal() + goal.joint_names = jnames + goal.velocity_scale = float(args.load_velocity_scale) + if hi is not None: + goal.num_points = 2 + goal.positions = [float(hi[0][n]) for n in jnames] + [float(sol[n]) for n in jnames] + half = max(0.5, float(args.load_move_s) * 0.6) + goal.time_from_start = [half, half + float(args.load_move_s) * 0.5] + else: + goal.num_points = 1 + goal.positions = [float(sol[n]) for n in jnames] + goal.time_from_start = [float(args.load_move_s)] + + cli = ActionClient(node, ArmFJT, args.action_name) + if not cli.wait_for_server(timeout_sec=10.0): + print(f"✗ JTC 动作不在：{args.action_name}（arm_msgs/FollowJointTrajectory）") + print(" 仿真：跑 run_sim_gz.sh 了吗（trajectory_bridge 由它拉起）？") + return False + fut = cli.send_goal_async(goal) + t_send = time.monotonic() + while not fut.done() and time.monotonic() - t_send < 10.0: + spin_for(0.05) + handle = fut.result() if fut.done() else None + if handle is None or not handle.accepted: + print("✗ JTC 拒绝了这条轨迹") + return False + res_fut = handle.get_result_async() + t1 = time.monotonic() + while not res_fut.done() and time.monotonic() - t1 < 60.0: + spin_for(0.1) + if not res_fut.done(): + print("✗ 等轨迹结果超时（60s）") + return False + res = res_fut.result().result + if not getattr(res, "success", False): + print(f"✗ 轨迹执行失败：{getattr(res, 'message', '')!r}") + return False + # 等静止（关节到位 ≠ 指尖不再动：还有弹性settle） + last = None + still = 0.0 + t2 = time.monotonic() + while time.monotonic() - t2 < 10.0: + spin_for(0.2) + g = wait_tf(tcp_frames["left"], timeout_s=0.5) + if g is None: + continue + if last is not None and math.dist(g[0], last) < 0.0005: + still += 0.2 + if still >= 0.8: + break + else: + still = 0.0 + last = g[0] + + # (6) 复测（闭环验收：不验就是"指令发了就当成功"） + g = wait_tf(tcp_frames["left"], timeout_s=5.0) + if g is None: + return False + R_after = _quat_to_R(*g[1]) + ax = (R_after[0][2], R_after[1][2], R_after[2][2]) + print(f" 复测：工具轴 Z 分量 {ax[2]:+.3f}（要求 ≤ -0.99）、" + f"TCP 高度 z={g[0][2]:.3f}（台面 {0.40:.2f}）") + if ax[2] > -0.99: + print(" ⚠ 复测没到竖直：可能是关节限位挡住了，或 IK 的种子太远。") + return False + return True + + +def _check_ik(node, buf, spin_for, wait_tf, args) -> int: + """自检：``/joint_states`` + URDF→KDL 正解 vs TF 里 base→flange 的真实位姿。 + + 误差应当 <1mm / <0.5°。对不上就是 URDF→KDL 的转换有问题（轴表达系、链、关节 + 顺序），**别接着上料** —— IK 会自洽地错，最后表现为"臂动了但规没进指间"。 + """ + import PyKDL as kdl + + from std_msgs.msg import String + from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy + from sensor_msgs.msg import JointState + + urdf_box: Dict[str, str] = {} + qos = QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + ) + node.create_subscription(String, "/robot_description", lambda m: urdf_box.__setitem__("x", m.data), qos) + js_box: Dict[str, object] = {} + node.create_subscription(JointState, "/joint_states", lambda m: js_box.__setitem__("m", m), 10) + t0 = time.monotonic() + while ("x" not in urdf_box or "m" not in js_box) and time.monotonic() - t0 < 15.0: + spin_for(0.1) + if "x" not in urdf_box or "m" not in js_box: + print("✗ 15s 内没拿到 /robot_description 或 /joint_states") + return 2 + js = js_box["m"] + cur = {str(n): float(p) for n, p in zip(getattr(js, "name", []), getattr(js, "position", []))} + + chain, movable, _limits = _build_kdl_chain(urdf_box["x"], args.base_frame, args.ee_frame) + print(f"=== --check-ik：链上可动关节 {movable} ===") + q = kdl.JntArray(len(movable)) + for i, nm in enumerate(movable): + q[i] = cur.get(nm, 0.0) + fk = kdl.ChainFkSolverPos_recursive(chain) + F = kdl.Frame() + fk.JntToCart(q, F) + + got_b = wait_tf(args.base_frame) + got_e = wait_tf(args.ee_frame) + if got_b is None or got_e is None: + print(f"✗ TF 里找不到 {args.base_frame} 或 {args.ee_frame}") + return 2 + R_b = _quat_to_R(*got_b[1]) + R_e = _quat_to_R(*got_e[1]) + + def _to_base(p): + d = (p[0] - got_b[0][0], p[1] - got_b[0][1], p[2] - got_b[0][2]) + return _mat_vec([[R_b[j][i] for j in range(3)] for i in range(3)], d) + + p_tf = _to_base(got_e[0]) + # TF 的 flange 姿态 → base 系 + R_tf = _mat_mul([[R_b[j][i] for j in range(3)] for i in range(3)], R_e) + p_fk = (F.p[0], F.p[1], F.p[2]) + dp = math.dist(p_tf, p_fk) + # 姿态差：用 Z 轴夹角（工具轴是 IK 唯一关心的方向）+ 全姿态差 + z_fk = (F.M.UnitZ()[0], F.M.UnitZ()[1], F.M.UnitZ()[2]) + z_tf = (R_tf[0][2], R_tf[1][2], R_tf[2][2]) + ang = math.degrees(math.acos(max(-1.0, min(1.0, sum(a * b for a, b in zip(z_fk, z_tf)))))) + print(f" 位置：正解 ({p_fk[0]:+.5f}, {p_fk[1]:+.5f}, {p_fk[2]:+.5f}) base 系") + print(f" TF ({p_tf[0]:+.5f}, {p_tf[1]:+.5f}, {p_tf[2]:+.5f}) base 系") + print(f" 误差：位置 {dp * 1000:.3f}mm、工具轴夹角 {ang:.3f}°") + _ = node, buf + if dp > 0.001 or ang > 0.5: + print("✗ 对不上：URDF→KDL 的转换有问题（先别上料，IK 会自洽地错）。") + return 1 + print("✓ 正解与 TF 一致，IK 可信。") + return 0 + + +def _ros_side(args) -> int: # noqa: C901 - 线性流程，拆开反而更难读 + import rclpy + from rclpy.node import Node + from rclpy.time import Time + from tf2_ros import Buffer, TransformListener + + from arm_msgs.msg import GripperStatus + from arm_msgs.srv import GripperExecute + from arm_msgs.msg import TactileStatus + + tactile_cfg = _load_yaml(ROOT / "devices" / "sim" / "gazebo" / "config" / "tactile.yaml") + scene_cfg = _load_yaml(ROOT / "devices" / "sim" / "gazebo" / "config" / "scene.yaml") + sides = tactile_cfg.get("sides") or [] + frames = {str(s.get("side")): str(s.get("frame")) for s in sides if s.get("side")} + tcp_frames = {str(s.get("side")): str(s.get("tcp_frame") or "") for s in sides if s.get("side")} + if set(frames) < {"left", "right"}: + print(f"✗ tactile.yaml 里没有左右指的 frame（读到 {frames}）") + return 2 + for side in ("left", "right"): + if not tcp_frames.get(side): + # 不猜：靠 frame 加后缀推出来的名字是错的（实测 mount / tcp 是并列的 + # 两条链，不是父子命名）。明确提示去补配置，而不是静默用错名字。 + print(f"✗ tactile.yaml 的 sides[{side}] 缺 tcp_frame（指腹夹持点坐标系）。") + print(" 它是上料脚本算工具轴用的：两指 TCP 中点 = 工具轴，规尖在这条轴上。") + print(" 在仿真里查：ros2 run tf2_ros tf2_echo world <指节帧>_tcp 不对时，") + print(" 用 ros2 run tf2_tools view_frames 或 tf2_echo 逐个试 *_finger_tcp。") + rclpy.shutdown() + return 2 + + rclpy.init() + node = Node("sim_load_gauge") + # 仿真的时钟来自 /clock；TF 的时间戳也是仿真时间，必须用 sim time 才能对上 + node.set_parameters([rclpy.parameter.Parameter("use_sim_time", value=True)]) + buf = Buffer(cache_time=rclpy.duration.Duration(seconds=15.0)) + TransformListener(buf, node) + + tac: Dict[str, Tuple[float, object]] = {} + sub = node.create_subscription( + TactileStatus, "/tactile/left/status", lambda m: tac.__setitem__("left", (time.monotonic(), m)), 10 + ) + sub_r = node.create_subscription( + TactileStatus, "/tactile/right/status", lambda m: tac.__setitem__("right", (time.monotonic(), m)), 10 + ) + status: Dict[str, GripperStatus] = {} + node.create_subscription( + GripperStatus, "/gripper/status", lambda m: status.__setitem__("now", m), 10 + ) + + def spin_for(sec: float) -> None: + t_end = time.monotonic() + sec + while time.monotonic() < t_end: + rclpy.spin_once(node, timeout_sec=0.05) + + def wait_tf(frame: str, timeout_s: float = 15.0): + """等 world←frame 可用，返回 (位置, 四元数) 或 None。""" + t0 = time.monotonic() + while time.monotonic() - t0 < timeout_s: + spin_for(0.1) + try: + tr = buf.lookup_transform("world", frame, Time()) + except Exception: # noqa: BLE001 - TF 还没起来 + continue + t = tr.transform.translation + q = tr.transform.rotation + return (t.x, t.y, t.z), (q.x, q.y, q.z, q.w) + return None + + # ---- -1. 自检：URDF→KDL 的正解必须和 TF 对得上（--check-ik） ---------- + # 为什么要有这个开关：IK 用的是"自己从 URDF 建的 KDL 链"，而臂的真实姿态在 + # TF 里。这两者一旦不一致（轴的表达系、父子链、关节顺序写错），IK 会**自洽地 + # 错**（正解逆解都错得很一致），最后表现为"轨迹动了但规装不上"。所以先验一条： + # 用 /joint_states 当前角度做正解，跟 TF 的 world←base、world←flange 比。 + if args.check_ik: + return _check_ik(node, buf, spin_for, wait_tf, args) + + # ---- 0. 回 home（可选，默认不做） ------------------------------------ + # ⚠ 实测（2026-09-24）：**不要默认回 home**。仿真里 home 是机械臂的出厂 + # 预备位姿，工具轴是斜的（实测 Z 分量 -0.930），而"把规垂进指间"要求 + # 工具轴竖直。默认调 /home 会把本来竖直的姿态弄斜，规尖算到台面以下。 + # 需要时用 --home 显式调（真机上则由人把臂摆好）。 + from std_srvs.srv import Trigger + + if args.home and not args.place_only: + home = node.create_client(Trigger, "/home") + if not home.wait_for_service(timeout_sec=10.0): + print("⚠ /home 不在（sim_home_service 没起来？）—— 不动臂，直接按当前姿态上料。") + else: + print("=== --home：先回 home（注意：home 姿态工具轴可能是斜的） ===") + fut = home.call_async(Trigger.Request()) + t0 = time.monotonic() + while not fut.done() and time.monotonic() - t0 < 60.0: + spin_for(0.1) + if not fut.done(): + print("✗ /home 调用超时（60s）") + rclpy.shutdown() + return 2 + resp = fut.result() + print(f" /home 返回 success={getattr(resp, 'success', None)} " + f"msg={getattr(resp, 'message', '')!r}") + # 等指尖真的停下（回 home 是关节运动，立刻量 TCP 会量到中途） + t0 = time.monotonic() + last = None + still = 0.0 + while time.monotonic() - t0 < 30.0: + spin_for(0.2) + got = wait_tf(tcp_frames["left"], timeout_s=0.5) + if got is None: + continue + p = got[0] + if last is not None and math.dist(p, last) < 0.0005: + still += 0.2 + if still >= 0.8: + break + else: + still = 0.0 + last = p + print(" 指尖已静止") + + # ---- 1. 先张开 ------------------------------------------------------ + # 顺序很重要：**先张开再量 TCP**。指节是连杆机构，闭合时指腹会向内/向下 + # 走一大截（实测 TCP 从 z=0.474 掉到 z=0.458），拿"闭合姿态"算出来的规尖 + # 位置会低 15mm —— 规一放就插进台面。 + st0 = None + cli_open = node.create_client(GripperExecute, "/gripper/open") + cli_move = node.create_client(GripperExecute, "/gripper/move") + for cli, nm in ((cli_open, "/gripper/open"), (cli_move, "/gripper/move")): + if not cli.wait_for_service(timeout_sec=5.0): + print(f"✗ {nm} 不在（契约后端 arm_msgs/srv/GripperExecute）") + print(" 夹爪服务没起来？看 run_sim_gz.sh 的 gn01 适配器与 /gripper/status。") + rclpy.shutdown() + return 2 + + def gripper_call(cli, command: str, position: Optional[float] = None) -> bool: + req = GripperExecute.Request() + req.command.command = command + req.command.speed = 50.0 + req.command.force = 30.0 + req.command.wait_complete = False # 自己等，才能顺便打印中间状态 + req.command.timeout = 5.0 + if position is not None: + # move 的 position 是**开口**（mm，与 /gripper/status.position 同口径） + req.command.position = float(position) + fut = cli.call_async(req) + t0 = time.monotonic() + while not fut.done() and time.monotonic() - t0 < 5.0: + spin_for(0.05) + return fut.done() + + def wait_gripper_still(timeout_s: float = 6.0) -> None: + t0 = time.monotonic() + while time.monotonic() - t0 < timeout_s: + spin_for(0.1) + st = status.get("now") + if st is not None and not st.is_moving: + return + + if args.place_only: + print("=== --place-only：不动夹爪，只按当前姿态摆位 ===") + else: + print("=== 开爪（保证两指之间能让规进去） ===") + if not args.place_only: + if not gripper_call(cli_open, "open"): + print("✗ /gripper/open 调用超时") + rclpy.shutdown() + return 2 + wait_gripper_still() + st0 = status.get("now") + if st0 is not None: + print(f" 开爪后开口 {st0.position:.2f}mm") + + # ---- 1.1 标定"夹爪开口 ↔ 两指指腹间距" ---------------------------------- + # 为什么必须标定：夹爪适配器的"开口"是**相对全闭合位置**的行程量 + #（真机 GR01 行程 20mm、仿真夹爪 99.9mm，两套口径完全不同），而规是按 + # **物理直径**做的。要判断"开口合到多少才正好卡住 φ30 的规"，就得先把 + # 两指指腹之间的**真实间距**量出来 —— 指腹间距才是和规径同一个量纲的东西。 + # + # 怎么量：指腹夹持点在 TF 里就是每个指节的 tcp_frame（tactile.yaml 指定）， + # 两点的距离就是指间间距。指节是连杆机构，间距与开口行程严格线性 + #（实测斜率 = 1.000），所以量一个点就能定出"开口=0 时的间距"。 + # + # 不标定的后果（实测踩过）：按适配器日志里的数字写死开口，换了夹爪型号 + # 或改了 URDF 就静默错位 —— 要么夹不到（规掉），要么把规捏飞。 + gap_cal_mm: Optional[float] = None # 开口 = 0 时的指间间距 [mm] + gap_at_aperture = None + + def tcp_gap_mm() -> Optional[float]: + """按当前 TF 算两指指腹间距 [mm]；TF 不全返回 None。""" + pts = {} + for side in ("left", "right"): + got = wait_tf(tcp_frames[side], timeout_s=3.0) + if got is None: + return None + pts[side] = got[0] + dx = pts["left"][0] - pts["right"][0] + dy = pts["left"][1] - pts["right"][1] + dz = pts["left"][2] - pts["right"][2] + return (dx * dx + dy * dy + dz * dz) ** 0.5 * 1000.0 + + if not args.place_only: + if not gripper_call(cli_move, "move", position=args.cal_aperture_mm): + print("✗ /gripper/move 调用超时（标定用）") + rclpy.shutdown() + return 2 + wait_gripper_still() + wait_tf(tcp_frames["left"], timeout_s=3.0) + gap_at_aperture = tcp_gap_mm() + if gap_at_aperture is not None: + gap_cal_mm = gap_at_aperture - args.cal_aperture_mm + print("=== 标定指间间距 ===") + 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.8 摆位（纯瞬移）与"人手托着合爪" ------------------------------- + # + # ⚠️ 这一段是整个上料脚本里最反直觉的地方，实测踩了很久，先说清因果： + # + # ① 为什么不能"先摆位、再合爪"： + # 把规瞬移到张开的指间之后，它**悬空**。下面什么都没有，于是立刻自由 + # 落体。物理步进是 1ms 级，而一次 ROS 服务往返 + 夹爪关节走完行程要 + # 好几百毫秒 —— 等"合爪"生效时，规早掉出台面了。 + # 实测现象：合爪开口读到 0.00mm（指间是空的）、触觉无接触、规的位姿 + # 漂移 ≈ 规尖到目标点的距离（约 125mm，正好落在台面上）。 + # + # ② 为什么也不能"先合爪、再把规塞进去"： + # 规一出现就和指腹重叠，求解器当步建起接触力，把规**弹出去**（实测 + # 0.05s 内就跑掉 116mm）。过盈越大弹得越凶。 + # + # ③ 所以：**像人手一样托着它**。 + # 合爪的同时，每个控制周期都按当前指腹位姿把规瞬移回去（就相当于人的 + # 手一直扶在规上），规永远不会掉。等指腹真正夹住了（松手后规自己不掉）， + # 再停止托着。 + # + # ④ 夹爪该合到多少：**扫**出来，不猜。 + # 指腹的**实际接触面**在网格里，未必就在 tcp_frame 原点上（外偏/内偏 + # 都可能），拿"指间间距 = 开口"去反推闭合量，会差出好几毫米 —— 差了就 + # 表现为"明明有接触（gz 报了 Contacts）却夹不住，规自己滑下去"。 + # 所以这里从松到紧一步一步试：能夹住就停。松到紧的顺序很重要，夹住即可， + # 不要一上来就压到最紧（过盈太大会把规顶飞）。 + def read_tcp_tips() -> Optional[Dict[str, Tuple[Tuple[float, float, float], + Tuple[float, float, float, float]]]]: + out: Dict[str, Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]] = {} + for side in ("left", "right"): + got = wait_tf(tcp_frames[side], timeout_s=2.0) + if got is None: + return None + out[side] = got + return out + + def gauge_drift_mm(target_pos) -> Optional[float]: + now = _gauge_pose_now(args.model) + if now is None: + return None + return math.dist(now, target_pos) * 1000.0 + + def hold_gauge_while(seconds: float, max_iter: int = 200) -> Optional[Tuple]: + """像人手一样托着规：按当前指腹位姿反复把规瞬移回两指之间。 + + 返回最后一次用的 (target_pos, target_quat)。 + """ + t0 = time.monotonic() + last = None + n = 0 + while time.monotonic() - t0 < seconds and n < max_iter: + tips_now = read_tcp_tips() + if tips_now is None: + return last + pos_q = gauge_target_pose(tips_now, args.grasp_center_m) + _set_gauge_pose(args.world, args.model, pos_q[0], pos_q[1]) + last = pos_q + n += 1 + spin_for(args.hold_period_s) + return last + + held_ap: Optional[float] = None + final_target: Optional[Tuple[Tuple[float, float, float], + Tuple[float, float, float, float]]] = None + if not args.place_only: + if gap_cal_mm is None: + print("✗ 没能标定指间间距（TF 里量不到两指 tcp_frame），无法确定扫描范围。") + print(f" 检查 tactile.yaml 的 sides[*].tcp_frame：{tcp_frames}") + print(" 或加 --place-only 只摆位、不碰夹爪。") + rclpy.shutdown() + return 2 + + # 扫描区间：指间间距从（规径 + 富余）收到（规径 − 富余），按开口口径表示。 + ap_loose = (args.gauge_dia_mm + args.scan_margin_mm) - gap_cal_mm + ap_tight = (args.gauge_dia_mm - args.scan_margin_mm) - gap_cal_mm + ap_loose = min(ap_loose, args.cal_aperture_mm * 5) + ap_tight = max(0.0, ap_tight) + if ap_loose <= ap_tight: + print(f"✗ 扫描区间不合法（松 {ap_loose:.2f} ≤ 紧 {ap_tight:.2f}）。") + rclpy.shutdown() + return 2 + + print("=== 合爪：像人手一样托着规，从松到紧扫出能夹住的开口 ===") + print(f" 规径 {args.gauge_dia_mm:.1f}mm；扫描开口 {ap_loose:.2f} → " + f"{ap_tight:.2f}mm，步长 {args.scan_step_mm:.2f}mm；" + f"每档先托着合、再松手看掉不掉") + + ap = ap_loose + while ap >= ap_tight - 1e-9: + if not gripper_call(cli_move, "move", position=ap): + print("✗ /gripper/move 调用超时") + rclpy.shutdown() + return 2 + wait_gripper_still() + spin_for(0.2) + # 托着它，让指腹贴着规合进去 + last = hold_gauge_while(args.hold_s) + if last is None: + print("✗ 托着规的时候读不到指腹 TF。") + rclpy.shutdown() + return 2 + # 松手：停止瞬移，看规掉不掉 + spin_for(args.release_s) + drift = gauge_drift_mm(last[0]) + st = status.get("now") + gap_now = tcp_gap_mm() + tag = "?" if drift is None else f"{drift:.1f}mm" + print(f" 开口 {ap:5.2f}mm（指间间距 " + f"{'?' if gap_now is None else f'{gap_now:.2f}'}mm）→ " + f"松手后漂移 {tag}" + f"{'' if st is None else f'，夹爪读 {st.position:.2f}mm'}") + if drift is not None and drift < args.held_tol_mm: + held_ap = ap + final_target = last + print(f" ✓ 夹住了（漂移 {drift:.1f}mm < {args.held_tol_mm:.1f}mm）") + break + ap -= args.scan_step_mm + + if held_ap is None: + print("✗ 从松到紧扫了一遍都没夹住。") + print(" 可能原因：① 指腹实际接触面和 tcp_frame 差得很远（看上面每档的" + "指间间距，规径应落在扫到的某个间距附近）；") + print(" ② 规的夹持段没落在指腹高度上（--grasp-center-m 要改）;") + print(" ③ 摩擦/接触刚度太低（gauge/model.sdf 的 grip 段 mu/kp）。") + rclpy.shutdown() + return 2 + + # 夹住之后稍微再压紧一点，让法向力（也就是摩擦力）够用。 + if args.squeeze_mm > 0.0: + want_ap = max(ap_tight, held_ap - args.squeeze_mm) + print(f"=== 压紧 {args.squeeze_mm:.2f}mm（开口 {held_ap:.2f} → " + f"{want_ap:.2f}mm），期间继续托着 ===") + # 一边压一边托：压的过程中指腹会横向走，规不托着就会被搓下去 + if not gripper_call(cli_move, "move", position=want_ap): + print("✗ /gripper/move 调用超时") + rclpy.shutdown() + return 2 + deadline = time.monotonic() + args.hold_s + 1.0 + while time.monotonic() < deadline: + tips_now = read_tcp_tips() + if tips_now is None: + break + pos_q = gauge_target_pose(tips_now, args.grasp_center_m) + _set_gauge_pose(args.world, args.model, pos_q[0], pos_q[1]) + final_target = pos_q + spin_for(args.hold_period_s) + held_ap = want_ap + + # ---- 2. 量两指 TCP（**在最终夹持开口下**量） -------------------------- + # 指节是连杆机构：开口变化时指腹不仅横向移动，纵向也会走一截 + #（实测 TCP 从张开到闭合掉 15mm）。所以必须在"将来真正夹住规"的那个 + # 开口下量 TCP，否则算出来的规尖位置会整体偏低，规一放就插进台面。 + tips = {} + for side in ("left", "right"): + frame = tcp_frames[side] + got = wait_tf(frame) + if got is None: + print(f"✗ TF 里等不到 world←{frame}（15s）。仿真栈起来了吗？") + print(" bash scripts/run_sim_gz.sh 之后再跑本脚本。") + rclpy.shutdown() + return 2 + tips[side] = got + tip_pos, tip_quat = gauge_target_pose(tips, args.grasp_center_m) + + R_tip = _quat_to_R(*tips["left"][1]) + axis = (R_tip[0][2], R_tip[1][2], R_tip[2][2]) + print("=== 指尖与工具轴（世界系） ===") + print(f" 左 TCP = ({tips['left'][0][0]:.4f}, {tips['left'][0][1]:.4f}, {tips['left'][0][2]:.4f})") + print(f" 右 TCP = ({tips['right'][0][0]:.4f}, {tips['right'][0][1]:.4f}, {tips['right'][0][2]:.4f})") + print(f" 工具轴（指向规尖）= ({axis[0]:+.3f}, {axis[1]:+.3f}, {axis[2]:+.3f})" + f" （Z 分量 ≈ -1 才对：规尖朝下）") + if abs(axis[2]) < 0.99: + # 不是硬失败：规是**跟着工具轴**装的，工具轴斜着，规就斜着 —— + # 流程到 S04 会自动把工具轴竖直（那时规跟着转正）。真正不能接受的 + # 是"规尖算到台面以下"（下面那个检查），不是"现在斜着"。 + print(f"⚠ 工具轴不竖直（|Z| = {abs(axis[2]):.3f}）：规会斜着装。") + print(" 仿真刚起来时关节在零位（臂竖起、离台面高），那是最省事的上料姿态；") + print(" 跑过流程后停在工件上方（竖直）也可以。斜着装不影响后续，S04 会竖直它。") + + print(f"=== 规尖目标位姿 ===\n ({tip_pos[0]:.4f}, {tip_pos[1]:.4f}, {tip_pos[2]:.4f})" + f" 四元数 xyzw=({tip_quat[0]:+.6f}, {tip_quat[1]:+.6f}, " + f"{tip_quat[2]:+.6f}, {tip_quat[3]:+.6f})") + + table_top = float(scene_cfg.get("table_top_z") or FALLBACK_TABLE_Z) + clearance = tip_pos[2] - table_top + print(f" 规尖离台面 {clearance * 1000:.0f}mm（规长 86mm，向上全在指间）") + if args.dry_run: + print("（--dry-run：到此为止，没有碰仿真）") + rclpy.shutdown() + return 0 + if clearance < 0.01: + # 判据只看**规尖**：规是从指尖往下垂的 86mm，规尖在台面以上就够了 + #（其余部分在指间、在规尖上方）。这里卡住通常是臂压得太低。 + print("✗ 规尖低于台面（或只高不到 10mm），会把规插进台面里。") + print(" 先把臂抬起来（或让流程回到 home 姿态）再上料。") + rclpy.shutdown() + return 2 + + # ---- 4. 摆位 -------------------------------------------------------- + name = args.model + before = _gauge_pose_now(name) + print(f"=== 摆位：把 {name} 搬到目标位姿 ===") + if before is not None: + print(f" 摆位前 = ({before[0]:.4f}, {before[1]:.4f}, {before[2]:.4f})") + ok, out = _set_gauge_pose(args.world, name, tip_pos, tip_quat) + if not ok: + print(f"✗ set_pose 失败：{out}") + print(f" 排查：世界名对不对（--world，默认 assembly）、模型名对不对（--model，" + f"默认 gauge）、场景有没有加载。") + rclpy.shutdown() + return 2 + spin_for(args.settle_s) + + # ---- 6. 校验 -------------------------------------------------------- + now_pos = _gauge_pose_now(name) + drift: Optional[float] = None + if now_pos is None: + print("=== 校验：规还在不在手里 ===") + print(" ⚠ 读不到规的位姿（ign model --pose 没给可解析的输出）—— " + "跳过位姿校验，只看触觉。") + else: + drift = math.dist(now_pos, tip_pos) + print(f"=== 校验：规还在不在手里 ===\n 位姿漂移 {drift * 1000:.1f}mm" + f"（合爪后被顶开/顶飞会远大于 20mm）") + tac_ok = False + for side in ("left", "right"): + item = tac.get(side) + if item is None: + print(f" 触觉 {side}: 没有数据（/tactile/{side}/status 没发？）") + continue + _t, m = item + fn = abs(float(getattr(m, "fnormal", 0.0))) + hit = bool(getattr(m, "contact", False)) or fn > TACTILE_NOISE_FLOOR + tac_ok = tac_ok or hit + print(f" 触觉 {side}: contact={getattr(m, 'contact', None)} |fnormal|={fn:.4g}" + f" → {'夹到了' if hit else '没接触'}") + + problems: List[str] = [] + if drift is not None and drift > 0.02: + problems.append(f"规没在位姿上（漂移 {drift * 1000:.1f}mm）" + "：可能合爪时被顶飞、或模型名/世界名不对") + if not tac_ok: + problems.append("两指触觉都没读到接触：可能没合上（看上面开口）、或规不在两指之间") + if problems: + print("✗ 上料没成功：") + for p in problems: + print(f" - {p}") + print(" 排查顺序：① 上面打印的指节帧/TCP 是不是 sim_* 前缀；" + "② 场景里 gauge 模型在不在（ign model --list）；③ 合爪开口有没有变小。") + rclpy.shutdown() + return 2 + + print("✓ 通止规已装进夹爪（位姿贴合 + 触觉有接触）。可以跑装配了：") + print(" bash scripts/run_gauge_assembly.sh --sim --holes 1") + rclpy.shutdown() + return 0 + + +def main() -> int: + ap = argparse.ArgumentParser( + description="把通止规装进夹爪（仿真专用上料）", + formatter_class=argparse.RawDescriptionHelpFormatter, + epilog="真机不需要这个脚本：现场由人把规塞进两指之间并合爪。", + ) + ap.add_argument("--world", default="assembly", help="Gazebo 世界名（默认 assembly）") + ap.add_argument("--model", default="gauge", help="通止规模型名（默认 gauge）") + ap.add_argument("--grasp-center-m", type=float, default=GAUGE_GRASP_CENTER_M, + help=f"规尖到夹持段中点的距离 [m]（默认 {GAUGE_GRASP_CENTER_M}）") + ap.add_argument("--settle-s", type=float, default=2.5, help="摆位后等物理稳定的秒数") + ap.add_argument("--gauge-dia-mm", type=float, default=GAUGE_GRIP_DIA_MM, + help=f"规**夹持段**直径 [mm]（默认 {GAUGE_GRIP_DIA_MM}，与 gauge/model.sdf 一致）") + ap.add_argument("--squeeze-mm", type=float, default=2.0, + help="扫到「能夹住」之后再额外压紧多少 [mm]（默认 2.0，让摩擦力有余量）") + ap.add_argument("--scan-margin-mm", type=float, default=10.0, + help="扫描区间：指间间距在（规径±该值）之间扫 [m]（默认 10；" + "指腹接触面比 tcp_frame 外偏约 6mm，留够余量才能扫到）") + ap.add_argument("--scan-step-mm", type=float, default=1.0, + help="扫描步长 [mm]（默认 1.0；越小越准但越慢）") + ap.add_argument("--hold-s", type=float, default=0.9, + help="每档「托着规」多久 [s]（默认 0.9，够指腹合到位）") + ap.add_argument("--hold-period-s", type=float, default=0.02, + help="托着规时多久瞬移一次 [s]（默认 0.02 = 50Hz）") + ap.add_argument("--release-s", type=float, default=0.6, + help="松手后等多久再判「掉没掉」 [s]（默认 0.6）") + ap.add_argument("--held-tol-mm", type=float, default=3.0, + help="松手后漂移小于该值就算夹住 [mm]（默认 3.0）") + ap.add_argument("--cal-aperture-mm", type=float, default=20.0, + help="标定指间间距时用的开口 [mm]（默认 20，随便取一个中间值即可）") + ap.add_argument("--home", action="store_true", + help="先调 /home（默认不做：仿真 home 的工具轴是斜的，反而更难上料）") + ap.add_argument("--place-only", action="store_true", + help="只摆位，不碰夹爪（合爪后再「补一次位姿」用，见脚本末尾说明）") + ap.add_argument("--check-ik", action="store_true", + help="自检：URDF→KDL 正解 vs TF（装不上规时先跑这个）") + ap.add_argument("--dry-run", action="store_true", help="只算并打印目标位姿，不碰仿真") + # ---- 竖直上料位姿（IK）相关 ---- + ap.add_argument("--no-pose", action="store_true", + help="不摆竖直上料位姿（自己先把臂摆好；默认会摆）") + ap.add_argument("--base-frame", default="sim_base_link", help="IK 链的根链节（默认 sim_base_link）") + ap.add_argument("--ee-frame", default="sim_flange", help="IK 链的末端链节（默认 sim_flange）") + ap.add_argument("--joint-prefix", default="sim_", help="JTC 关节名前缀（默认 sim_，site.yaml 同源）") + ap.add_argument("--action-name", default="follow_joint_trajectory", + help="JTC 动作名（arm_msgs 版；默认 follow_joint_trajectory，与" + " real_trajectory_adapter / Flexiv 驱动一致）") + ap.add_argument("--load-velocity-scale", type=float, default=0.3, + help="走上料位姿的速度比例（默认 0.3，慢一点更安全）") + ap.add_argument("--load-x", type=float, default=LOAD_MID_XY[0], help="上料位姿：工具轴落在 X [m]") + ap.add_argument("--load-y", type=float, default=LOAD_MID_XY[1], help="上料位姿：工具轴落在 Y [m]") + ap.add_argument("--load-z", type=float, default=LOAD_MID_Z_M, + help=f"上料位姿：两指 TCP 中点高度 [m]（默认 {LOAD_MID_Z_M}）") + ap.add_argument("--load-yaw", type=float, default=0.0, help="上料位姿绕世界 Z 的偏航 [rad]") + ap.add_argument("--load-approach-dz", type=float, default=0.18, + help="先到高位竖直再下降：高位比目标高多少 [m]（默认 0.18；0=一步到位）") + ap.add_argument("--load-move-s", type=float, default=5.0, help="走到上料位姿用多少秒（默认 5）") + args = ap.parse_args() + return _ros_side(args) + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py b/src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py index 3ab2ea5..fa1bc18 100644 --- a/src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py +++ b/src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py @@ -92,6 +92,39 @@ ZERO_INERTIA_FIX = "1e-9" # 所以控制链路本身没问题，差别只在这 6 个关节的 effort=0。 MIMIC_EFFORT_FIX = "80" +# ============================================================================= +# 夹爪出力上限：把 gz_ros2_control 的"位置控制"夹成"力控夹爪" +# ============================================================================= +# 为什么需要（实测，2026-09-24）： +# gz_ros2_control 的 position 接口是"位置误差 → 力"，力只被 effort 上限夹。 +# grav_macro 给手指关节写的 effort=80（单位在这里是 Nm），而指腹到指节的 +# 力臂只有 ~0.1m —— 于是"合爪"能压出**上千牛**：仿真里 30mm 的规被两指 +# 直接挤穿（实测关节走到 0 位=开口 27mm < 规径 30mm），规被弹出去、掉回规座， +# 触觉却报着上百牛的力。表现像"夹爪抓不住"，根因是出力没有上限。 +# +# 取值怎么来的（不用猜：从 URDF 自己的行程推）： +# 夹爪是平行四边形连杆，指腹平动 100mm 时手指关节转 0.9404 rad +# （inner/outer_finger 的 limits：-0.7854..0.155），所以 +# 力臂 ≈ 行程 / 角行程 = 0.100 / 0.9404 ≈ 0.106 m +# 真机 Grav 的握力档位是 20~50N，取 45N（覆盖应用层常用的 force=30）： +# 手指关节 effort = 45 × 0.106 ≈ 4.8 Nm +# 而 finger_width_joint 是 1:1 的"行程关节"（0..0.1m），它的 effort 就是握力 +# 本身，给 45（N）。 +# +# ⚠ 这一条是**仿真专用**的物理量：真机的握力由 GripperExecute.force 走力控闭环， +# 不靠关节 effort 上限。归一化只服务于 Gazebo，所以写在这里、不去动上游描述。 +GRIP_FORCE_N = 45.0 +GRIP_STROKE_M = 0.100 +GRIP_FINGER_TRAVEL_RAD = 0.9404 +GRIP_PAD_LEVER_M = GRIP_STROKE_M / GRIP_FINGER_TRAVEL_RAD # ≈ 0.1064 m +GRIP_FINGER_EFFORT_NM = round(GRIP_FORCE_N * GRIP_PAD_LEVER_M, 2) # ≈ 4.79 Nm +GRIP_WIDTH_EFFORT_N = GRIP_FORCE_N + +#: 名字里带这些字样的关节 = 夹爪的指节/指腹/行程关节（仿真里统一带 sim_ 前缀） +_GRIP_JOINT_HINTS = ("finger", "knuckle") +#: 行程关节（1:1，effort 单位就是 N） +_GRIP_WIDTH_HINT = "finger_width" + # ============================================================================= # 为什么要把 整个从描述里删掉 # ============================================================================= @@ -162,6 +195,8 @@ class NormalizeReport: fixed_zero_effort: List[str] = field(default_factory=list) #: 被删掉的 mimic 关系 (关节, 源关节, multiplier, offset) stripped_mimics: List[Tuple[str, str, str, str]] = field(default_factory=list) + #: 被压到真机握力量级的夹爪关节 (关节, 原值, 新值) + clamped_grip_effort: List[Tuple[str, str, str]] = field(default_factory=list) @property def changed(self) -> bool: @@ -171,6 +206,7 @@ class NormalizeReport: or self.fixed_zero_inertia or self.fixed_zero_effort or self.stripped_mimics + or self.clamped_grip_effort ) def summary(self) -> str: @@ -196,6 +232,11 @@ class NormalizeReport: f"删除 mimic 关系 {len(self.stripped_mimics)} 条" f"（Gazebo 不支持 mimic，改由 sim_gripper_adapter 显式驱动）" ) + if self.clamped_grip_effort: + bits.append( + f"夹爪出力压到握力 {GRIP_FORCE_N:g}N 量级 {len(self.clamped_grip_effort)} 个" + f"({', '.join(f'{n}:{a}→{b}' for n, a, b in self.clamped_grip_effort)})" + ) return "；".join(bits) @@ -293,6 +334,31 @@ def _fix_mimic_joint(block: str, report: NormalizeReport, joint_name: str) -> st return block +def _clamp_grip_effort(block: str, report: NormalizeReport, joint_name: str) -> str: + """把夹爪关节的 effort 上限压到"真机握力量级"（见 GRIP_FORCE_N 的推导）。 + + 为什么必须做：不做的话"合爪"能压出上千牛，把工件挤穿、弹飞， + 看起来像"夹爪抓不住"，实际是出力没有上限（详见 GRIP_FORCE_N 处注释）。 + + 只改 effort 一个属性；限位、速度是上游的物理事实，一律不碰。 + """ + low = joint_name.lower() + if not any(h in low for h in _GRIP_JOINT_HINTS): + return block + want = GRIP_WIDTH_EFFORT_N if _GRIP_WIDTH_HINT in low else GRIP_FINGER_EFFORT_NM + lm = _LIMIT_ZERO_EFFORT.search(block) + if lm is None: + return block + lattrs = _attrs_of(lm.group("attrs")) + cur = _num(lattrs.get("effort", "")) + if cur <= want: + return block # 上游本来就比我们更保守：尊重它，不往上调 + lattrs["effort"] = f"{want:g}" + report.clamped_grip_effort.append((joint_name, f"{cur:g}", f"{want:g}")) + new = "" + return block[: lm.start()] + new + block[lm.end() :] + + def normalize_urdf(urdf: str) -> Tuple[str, NormalizeReport]: """归一化入口。返回 (新 URDF 文本, 报告)。 @@ -345,10 +411,13 @@ def normalize_urdf(urdf: str) -> Tuple[str, NormalizeReport]: # 而且这样不必把两套正则揉进同一个循环。 def joint_repl(mm: re.Match) -> str: block = mm.group(0) - if not _MIMIC_TAG.search(block): - return block jm = re.match(r']*name\s*=\s*"([^"]*)"', block) - return _fix_mimic_joint(block, report, jm.group(1) if jm else "?") + name = jm.group(1) if jm else "?" + if _MIMIC_TAG.search(block): + block = _fix_mimic_joint(block, report, name) + # 夹爪出力上限**放在 mimic 修复之后**：mimic 那条会把 effort=0 补成 80， + # 而这里要把它压到真机握力的量级。顺序反了就等于没夹。 + return _clamp_grip_effort(block, report, name) out = _JOINT_BLOCK.sub(joint_repl, out) return out, report diff --git a/src/arm_sim_gazebo/test/test_urdf_normalize.py b/src/arm_sim_gazebo/test/test_urdf_normalize.py index 555ea18..bbb4eb3 100644 --- a/src/arm_sim_gazebo/test/test_urdf_normalize.py +++ b/src/arm_sim_gazebo/test/test_urdf_normalize.py @@ -370,14 +370,88 @@ MIMIC_JOINT = ( def test_mimic_joint_gets_usable_effort(): - """effort="0" 的 mimic 关节必须被补成可用的力上限。""" + """effort="0" 的 mimic 关节必须被补成可用的力上限（再由夹爪握力上限压一遍）。""" from arm_sim_gazebo.urdf_normalize import MIMIC_EFFORT_FIX out, rep = normalize_urdf(ROBOT_OPEN + MIMIC_JOINT + ROBOT_CLOSE) assert rep.fixed_zero_effort == ["left_outer_knuckle_joint"] - assert f'effort="{MIMIC_EFFORT_FIX}"' in out - assert 'effort="0"' not in out + # 补 effort 这一步确实发生了（MIMIC_EFFORT_FIX 是它的输入，夹爪上限在此基础上再压） + assert MIMIC_EFFORT_FIX != "" and 'effort="0"' not in out + # 最终值由夹爪握力上限决定（见 test_gripper_joints_are_clamped_to_grip_force） + assert 'effort="4.79"' in out + + +def test_gripper_joints_are_clamped_to_grip_force(): + """夹爪关节的 effort 必须被压到"真机握力"量级。 + + 为什么：gz_ros2_control 的 position 接口是"位置误差→力"，力只受 effort 上限限制。 + 上游写 80（手指关节，Nm）配上 ~0.1m 的力臂 → 合爪能压出上千牛，把工件挤穿弹飞， + 看起来像"夹爪抓不住"（2026-09-24 实测）。这里守住压过的值。 + """ + from arm_sim_gazebo.urdf_normalize import ( + GRIP_FINGER_EFFORT_NM, + GRIP_FORCE_N, + GRIP_PAD_LEVER_M, + GRIP_WIDTH_EFFORT_N, + ) + + # 取值本身要自洽：握力 × 力臂 = 手指关节力矩 + assert GRIP_PAD_LEVER_M > 0.05 + assert abs(GRIP_FINGER_EFFORT_NM - GRIP_FORCE_N * GRIP_PAD_LEVER_M) < 0.01 + assert GRIP_WIDTH_EFFORT_N == GRIP_FORCE_N + + urdf = ( + ROBOT_OPEN + + '' + '' + '' + '' + "" + + '' + '' + '' + "" + + ROBOT_CLOSE + ) + out, rep = normalize_urdf(urdf) + + clamped = dict((n, (a, b)) for n, a, b in rep.clamped_grip_effort) + assert clamped["left_inner_finger_joint"] == ("80", f"{GRIP_FINGER_EFFORT_NM:g}") + assert clamped["finger_width_joint"] == ("80", f"{GRIP_WIDTH_EFFORT_N:g}") + assert "夹爪出力压到" in rep.summary() + + +def test_arm_joints_are_never_clamped(): + """机械臂本体的 effort 一个都不能动 —— 那是真机的力矩上限，不是夹爪。""" + urdf = ( + ROBOT_OPEN + + '' + '' + '' + "" + + ROBOT_CLOSE + ) + out, rep = normalize_urdf(urdf) + + assert rep.clamped_grip_effort == [] + assert 'effort="123"' in out + + +def test_gripper_clamp_respects_a_more_conservative_upstream(): + """上游若本来就比我们更保守（effort 更小），要尊重它，不能往上调。""" + urdf = ( + ROBOT_OPEN + + '' + '' + '' + "" + + ROBOT_CLOSE + ) + out, rep = normalize_urdf(urdf) + + assert rep.clamped_grip_effort == [] + assert 'effort="1.0"' in out def test_mimic_limits_are_preserved(): @@ -400,16 +474,15 @@ def test_mimic_tag_is_stripped_and_recorded(): def test_nonzero_effort_joint_is_untouched(): - """effort 非 0 的关节不碰 —— 那只说明上游表达了别的东西。 + """**机械臂**上 effort 非 0 的关节不碰 —— 那只说明上游表达了别的东西。 - 特别是主关节 finger_width_joint（effort=80），它是 mimic 的**源**、 - 本身没有 标签，不该被本规则命中。 + （夹爪类关节是另一条规则管的，见 test_gripper_joints_are_clamped_to_grip_force。） """ urdf = ( ROBOT_OPEN - + '' - '' - '' + + '' + '' + '' "" + ROBOT_CLOSE ) @@ -418,7 +491,7 @@ def test_nonzero_effort_joint_is_untouched(): assert rep.fixed_zero_effort == [] assert rep.stripped_mimics == [] - assert 'effort="80"' in out + assert 'effort="64"' in out def test_joint_without_mimic_is_never_touched_even_if_effort_zero():