From d8942defd18fbddec541f687f8786db868475cb7 Mon Sep 17 00:00:00 2001 From: arm_dev Date: Mon, 19 Oct 2026 15:44:09 +0800 Subject: [PATCH] =?UTF-8?q?=E5=88=9D=E6=AD=A5=E5=AE=8C=E6=88=90=E5=B9=B3?= =?UTF-8?q?=E8=A1=8C=E5=A4=B9=E7=88=AA=E6=97=8B=E8=BD=AC=E5=85=A5=E5=AD=94?= =?UTF-8?q?-=E4=B8=8D=E6=96=AD=E7=9A=84=E5=BC=A0=E5=BC=80=E8=BD=AC?= =?UTF-8?q?=E5=88=B0=E9=80=86=E6=97=B6=E9=92=88=E6=9E=81=E9=99=90=E5=86=8D?= =?UTF-8?q?=E9=97=AD=E5=90=88=E8=BD=AC=E5=88=B0=E9=A1=BA=E6=97=B6=E9=92=88?= =?UTF-8?q?=E6=9E=81=E9=99=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Cursor --- config/site.example.yaml | 34 ++- config/site.yaml | 13 +- docs/HANDBOOK.md | 7 +- .../arm_application/gauge_kin.py | 40 +++ .../arm_application/overhead_oneshot.py | 52 +++- .../arm_application/pipeline_node.py | 267 +++++++++++++++++- .../arm_application/stage2_locate_node.py | 46 ++- src/arm_application/test/test_gauge_kin.py | 16 ++ .../test/test_overhead_oneshot.py | 25 ++ .../test/test_pipeline_stage1_diag.py | 23 +- .../src/flexiv_hardware_interface.cpp | 17 ++ 11 files changed, 506 insertions(+), 34 deletions(-) diff --git a/config/site.example.yaml b/config/site.example.yaml index 23a87d0..5f2bdb8 100644 --- a/config/site.example.yaml +++ b/config/site.example.yaml @@ -39,7 +39,7 @@ robot: # 本臂装了 Orisys 视触觉（只有左臂有）。脚本靠这一项决定要不要拉触觉服务， # 而不是靠"SN 等于某个具体值"这种写死的判断（换台臂就会失效）。 tactile: true - # 2026-10-12 示教的工具 TCP（tool + 1012 + 0938）。眼在手上 AX=XB 用这份 TCP。 + # 2026-10-16 下午 RDK 重读 Studio tool10120938（用户再次更新 TCP）。 tool_name: "tool10120938" camera: name: d405_left # 话题 /d405_left/... 与 91 分开 @@ -68,6 +68,8 @@ robot: handeye_workdir: "/home/USER/ros2/handeye/tmp/89" handeye: # Studio tcp_location（法兰系，米 + qwxyz）。与 rdk Tool.params 必须一致。 + # 眼在手上：AX=XB 的手是这份 TCP，再折成 cam→法兰写 T_4x4。 + # 眼在手外：AX=XB 的手同样是这份 TCP（板夹在工具上）；T_4x4 仍是 cam→基座，不折。 gripper_tool: "tool10120938" gripper_tcp_xyz: [0.000537, -0.000264, 0.226587] gripper_tcp_qwxyz: [1.0, 0.0, 0.0, 0.0] @@ -177,7 +179,10 @@ robot: # 落在本文件下方注释记录的合理区间 0.204~0.233 内。 # 复核：python3 scripts/sense_tool_length.py --sn Rizon4-XXXXXX # python3 scripts/lib/real_geometry_check.py --sn Rizon4-XXXXXX - # 下臂安全距离按 tool10120938 的 TCP +Z，不再用旧 tool1 的 0.223。 + # 2026-10-16 下午：RDK Tool.params("tool10120938").tcp_location + # [x,y,z,qw,qx,qy,qz] = [0.000537, -0.000264, 0.226587, 1, 0, 0, 0] + # 示教器拟合误差：平均 0.58 / 最大 0.99 / 最小 0.29 mm。 + # 下臂安全距离用 TCP +Z。上午 0.226274；再往前 0.2245 是 10-12；0.223 是 tool1。 tool_tip_below_flange_m: 0.226587 # >>> 仿真那台臂（Gazebo）也登记成 robots[] 里的一台 <<< # 为什么必须登记：应用是按"当前臂"的名字去查 frame / joint / 相机话题的 @@ -769,17 +774,25 @@ hole_detect: # —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 —— align_speed: 0.014 # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲） move_speed: 0.022 # 升降/粗居中段速度 [m/s]（原0.05，放缓防震规） + # 对小孔关键点：只用像素射线交到 workpiece_surface_z_m，不读深度图。 hole_visual_only: true - # 本分支实验：true=悬停拍一次并走完到孔；main 保存点应保持 false。 + # 本分支实验（exp/hole-keypoint-once）：true=悬停拍一次，框心→k1 一次走完 + # （40mm 上限）。main 保存点是 false（多走多拍）。 hole_keypoint_once: true - # true=悬停走完后一条厂家 MoveL 贴面。对照观察高补交时改 false。 + # true=悬停走完后一条厂家 MoveL 贴面（不补拍）。对照 37b85aa 改回 false。 hole_once_lock_xy_descend: true - # 0=笛卡尔锁 XY 只降 Z。k>0 才沿 -look 加前馈。 + # 贴面相对悬停：沿 -look 走 k×剩余下降 [m/m]。0=笛卡尔锁 XY 只降 Z。 + # 0.018 在框心改平面交点后两条链路都 +1.3mm X 过补，插不进。 once_ff_look_per_m: 0.0 - final_gap_mm: 2.0 # 对准后贴面目标：规尖离面 [mm]。CLI --final-gap 覆盖。 + final_gap_mm: 2.0 # 不进孔时的贴面目标：规尖离面 [mm]。笛卡尔锁 XY + # 时法兰 Z = 工具长 + 工件面 + 间隙。CLI --final-gap 覆盖。 + hole_entry_mm: 1.0 # 规尖停在工件上表面以下 [mm]。2026-10-19 12:20 左臂： + # 4mm 指令在面下 1.6mm 被挡住（厂家 302064 到不了位）， + # 流水线接着空等 90s。1.0mm 留在挡住点之上，仍进孔口。 + # 0 = 停在 final_gap（面上）。上限 8mm（板厚 12mm）。 observe_gap_mm: 15.0 # --no-descent 时的观测下降：只降到离面这么高 [mm]， # 方便看规尖偏没偏。必须 ≥ final_gap_mm。 - # 左臂89：悬停间隙约 75mm（0.320−0.223−0.022）⇒ 大约下降 60mm。 + # 左臂89：悬停间隙约 71.4mm（0.320−0.226587−0.022）⇒ 观测下降约 56.4mm。 final_descent_speed: 0.005 # 最后轻降速度 [m/s]（很慢，柔顺） # ✅ 2026-10-08 定稿：下降量**只由几何决定**（法兰 FK − 工具长 − 工件面 − final_gap）， # 深度读数只做交叉核对、不再参与任何决策。两段真机事故方向相反、都指向这条： @@ -792,7 +805,7 @@ hole_detect: # ⇒ 读数往**小**错一次就永远到不了目标。 # 同一套读数两个方向各错一次、幅度 150mm 量级、随位姿乱跳 —— 它对"能降多少" # 和"该不该停"都没有发言权。现在它只在偏差 >10mm 时喊一句"深度测距与几何不符"。 - # 现行几何：悬停 0.320 − 工具长 0.223 − 工件面 0.022 = 75mm 间隙 ⇒ 降 73mm 到 2mm。 + # 现行几何：悬停 0.320 − 工具长 0.226587 − 工件面 0.022 = 71.4mm 间隙 ⇒ 降 69.4mm 到 2mm。 # 三道**物理/几何**保险：descent_budget_m（配置上限）+ 腕部 FT 接触检测（停+退 1mm） # + 任一步 _goto 走不到位就停。 # 缺几何（工具长/工件面没配）时**拒绝下降**并报错（保持悬停），绝不拿测距去赌。 @@ -923,8 +936,9 @@ gauge_assembly: # ---- S01 回 home 后按工具系逆时针预绕到极限角 ---- # 预绕是为了给 S08 的棘轮留出回转行程：不预绕，拧两圈就贴关节限位。 extreme_angle_deg: 150.0 # 想转多少度（按工具系逆时针） - extreme_speed_deg_s: 20.0 # 关节角速度 [deg/s]，慢一点防冲击 - extreme_direction: 1 # +1=逆时针 / -1=顺时针（改符号，别去改代码） + extreme_speed_deg_s: 10.0 # 关节角速度 [deg/s]。2026-10-19 左臂 A7 咬合从 20 降到 10， + # 限位到限位约 34s（原先约 17s） + extreme_direction: -1 # +1=逆时针 / -1=顺时针（2026-10-19 真机 A7 转反了，关节角增大是顺时针） extreme_margin_deg: 3.0 # 距关节限位留的余量 [deg]；撞限位不算失败（按 JOINT_LIMIT 继续） # 但余量太小会让规划器频繁拒收，表现为"偶尔转不到位" diff --git a/config/site.yaml b/config/site.yaml index 4942fa3..a4bdda9 100644 --- a/config/site.yaml +++ b/config/site.yaml @@ -768,8 +768,12 @@ hole_detect: # 贴面相对悬停：沿 -look 走 k×剩余下降 [m/m]。0=笛卡尔锁 XY 只降 Z。 # 0.018 在框心改平面交点后两条链路都 +1.3mm X 过补，插不进。 once_ff_look_per_m: 0.0 - final_gap_mm: 2.0 # 对准后贴面目标：规尖离面 [mm]（不接触）。笛卡尔锁 XY - # 时法兰 Z = 工具长 + 工件面 + 本值。CLI --final-gap 覆盖。 + final_gap_mm: 2.0 # 不进孔时的贴面目标：规尖离面 [mm]。笛卡尔锁 XY + # 时法兰 Z = 工具长 + 工件面 + 间隙。CLI --final-gap 覆盖。 + hole_entry_mm: 1.0 # 规尖停在工件上表面以下 [mm]。2026-10-19 12:20 左臂： + # 4mm 指令在面下 1.6mm 被挡住（厂家 302064 到不了位）， + # 流水线接着空等 90s。1.0mm 留在挡住点之上，仍进孔口。 + # 0 = 停在 final_gap（面上）。上限 8mm（板厚 12mm）。 observe_gap_mm: 15.0 # --no-descent 时的观测下降：只降到离面这么高 [mm]， # 方便看规尖偏没偏。必须 ≥ final_gap_mm。 # 左臂89：悬停间隙约 71.4mm（0.320−0.226587−0.022）⇒ 观测下降约 56.4mm。 @@ -916,8 +920,9 @@ gauge_assembly: # ---- S01 回 home 后按工具系逆时针预绕到极限角 ---- # 预绕是为了给 S08 的棘轮留出回转行程：不预绕，拧两圈就贴关节限位。 extreme_angle_deg: 150.0 # 想转多少度（按工具系逆时针） - extreme_speed_deg_s: 20.0 # 关节角速度 [deg/s]，慢一点防冲击 - extreme_direction: 1 # +1=逆时针 / -1=顺时针（改符号，别去改代码） + extreme_speed_deg_s: 10.0 # 关节角速度 [deg/s]。2026-10-19 左臂 A7 咬合从 20 降到 10， + # 限位到限位约 34s（原先约 17s） + extreme_direction: -1 # +1=逆时针 / -1=顺时针（2026-10-19 真机 A7 转反了，关节角增大是顺时针） extreme_margin_deg: 3.0 # 距关节限位留的余量 [deg]；撞限位不算失败（按 JOINT_LIMIT 继续） # 但余量太小会让规划器频繁拒收，表现为"偶尔转不到位" diff --git a/docs/HANDBOOK.md b/docs/HANDBOOK.md index 1440686..a0ddee3 100644 --- a/docs/HANDBOOK.md +++ b/docs/HANDBOOK.md @@ -79,7 +79,8 @@ ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。D435 这 启动高度用 `--init-z`（法兰 Z，米），对孔悬停用 `--hover-z`。两个都是法兰高度。 下臂贴面用 `--final-gap`（规尖离面，毫米；site 默认 `final_gap_mm` 2）。笛卡尔锁 XY 时只改 Z：法兰 = 工具长 + 工件面 + 间隙。不能和 `--no-descent` 一起写。 -会让工具尖进入工件、或超出工作盒的值直接拒绝，不会改成另一个高度再往下走。 +`--final-gap` ≤ 0、或超出工作盒的值直接拒绝，不会改成另一个高度再往下走。 +往孔里进只走 `stage2.hole_entry_mm`（正值，上限 8 mm）。 不写则一阶段锁高仍按 site，对孔悬停仍是 `hole_hover_z_m`（89 为 0.320）。 框心对准连续 `servo.wrist_align_sec`（0.5 s）才 `/visual_servo/locked`，然后才精拍下臂。 本分支（`exp/hole-keypoint-once`）二阶段是**单次精拍**：悬停拍一次 k1/k2， @@ -90,7 +91,9 @@ ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。D435 这 `main` 保存点仍是多走多拍。 孔坐标把关键点像素交到 `workpiece_surface_z_m`，不读深度图。下臂停在 `tool10120938` 的 TCP 长度（0.226587 m，2026-10-16 下午 RDK 重读）加上 `final_gap_mm` 之上，不按旧 tool1 的 0.223 m。 -贴面之后就停在这个法兰高度，不抬回 `hole_hover_z_m`。没有接触传感器，到位只认 `final_gap_mm`，不另设一个 Z。 +贴面之后就停在这个法兰高度，不抬回 `hole_hover_z_m`。没有接触传感器。 +`stage2.hole_entry_mm`（现为 1）> 0 时，插孔目标改成规尖在工件上表面以下这么深（孔口凹槽），再转 A7；为 0 时仍停在 `final_gap_mm`。进孔超过 8 mm（板厚 12 mm）直接拒绝。2026-10-19 12:20 写成 4 mm 时，臂在面下 1.6 mm 被挡住。 +然后只转左臂 A7（厂家 `SendJointPosition` 的第 7 轴，模式 `NRT_JOINT_POSITION`）：先逆时针转到软件限位内侧 `extreme_margin_deg`，再顺时针转到另一侧。顺时针到位记为「首轮咬合结束」。接着 `run_gripper.sh --arm <臂号> --cli pos 400` 把平行爪只张开到 400（不全开），A7 逆时针回到限位，`--cli close` 夹住规，再顺时针转到限位。前六轴不动，不改法兰 Z。顺时针是进孔，逆时针是出孔。夹爪命令失败或状态没到，就停在当前关节。 只测 D435 这一段位移时加 `--d435-only`：不交 D405、不进二阶段，并且要求先停 `run_real.sh`。 ```bash diff --git a/src/arm_application/arm_application/gauge_kin.py b/src/arm_application/arm_application/gauge_kin.py index d7b7855..b004510 100644 --- a/src/arm_application/arm_application/gauge_kin.py +++ b/src/arm_application/arm_application/gauge_kin.py @@ -26,6 +26,9 @@ import numpy as np #: 直接拒绝整条轨迹 —— 那不是我们要的"极限角"，是报错。 DEFAULT_LIMIT_MARGIN_DEG = 3.0 +#: 厂家 A7。``Robot::SendJointPosition`` 的关节向量按 A1..A7 排列，A7 是最后一轴。 +A7_INDEX = 6 + _JOINT_RE = re.compile(r" Optional[np.ndarray]: + """只把 A7 收到软件限位内侧。前六轴原样拷回，不许在这里改。 + + 厂家接口是 ``Robot::SendJointPosition``（模式 ``NRT_JOINT_POSITION``）。 + 向量按 A1..A7 排，A7 是下标 ``A7_INDEX``。限位是 ``RobotInfo::q_min/q_max`` + 的那一对，真机上与 URDF ``joint7`` 的 lower/upper 是同一组数。 + + ``direction`` 与 ``prewind`` 相同：``+1`` 关节角增大 = 逆时针，目标取上限； + ``-1`` 关节角减小 = 顺时针，目标取下限。现场若发现正负反了，改 + ``site.yaml`` 的 ``extreme_direction``，不要改这里的符号。 + + 没有限位、方向不是 ±1、或者余量把上下限挤没了，返回 None。 + 调用方必须停，不许猜一个角度发出去。 + """ + if direction not in (-1, 1): + raise ValueError(f"方向只能是 +1 / -1，收到 {direction}") + arr = np.asarray(q, dtype=float).reshape(-1) + if arr.size != 7: + raise ValueError(f"A7 目标要 7 个关节角，收到 {arr.size}") + if limits is None: + return None + lo = float(limits[0]) + up = float(limits[1]) + margin = math.radians(float(margin_deg)) + if not math.isfinite(lo) or not math.isfinite(up) or up - lo <= 2.0 * margin: + return None + target = (up - margin) if direction > 0 else (lo + margin) + out = arr.copy() + out[A7_INDEX] = target + return out + + def rotation_about_axis_deg(R_before: np.ndarray, R_after: np.ndarray, axis: np.ndarray) -> float: """从两个旋转矩阵算出"绕 ``axis`` 转了多少度"（带符号）。 diff --git a/src/arm_application/arm_application/overhead_oneshot.py b/src/arm_application/arm_application/overhead_oneshot.py index e8863dd..38bab9b 100644 --- a/src/arm_application/arm_application/overhead_oneshot.py +++ b/src/arm_application/arm_application/overhead_oneshot.py @@ -688,10 +688,60 @@ def hole_pair_span_ok(span_m: float) -> bool: def flange_z_for_gap_m(tip_len_m: float, surface_z_m: float, gap_m: float) -> float: - """规尖离面 ``gap_m`` 时的法兰 Z [m]。与轻降几何同一条式子。""" + """规尖离面 ``gap_m`` 时的法兰 Z [m]。与轻降几何同一条式子。 + + ``gap_m`` 为正是停在面上；为负是规尖进入孔口（上表面以下）。 + """ return float(tip_len_m) + float(surface_z_m) + float(gap_m) +# 工件板厚 12mm。进孔超过这个值会朝下表面穿，直接拒绝，不改成更浅再走。 +HOLE_ENTRY_MAX_M = 0.008 + + +def insert_target_gap_m(final_gap_m: float, hole_entry_m: float) -> float: + """插孔停在哪 [m]。``hole_entry_m`` > 0 时规尖在工件上表面以下这么深；否则停在面上 ``final_gap_m``。""" + entry = float(hole_entry_m) + if entry > 1e-9: + return -entry + return float(final_gap_m) + + +def hole_entry_refusal( + entry_m: float, + tool_len_m: float, + surface_z_m: float, + z_min_m: float, + z_max_m: float, +) -> str: + """进孔深度能不能用。空串表示可以（含 0 = 不进孔）。非空就是拒绝原因。""" + entry = float(entry_m) + if not np.isfinite(entry): + return "进孔深度不是有限数，拒绝开跑" + if entry < -1e-9: + return f"进孔深度 {entry * 1000:.1f}mm < 0，拒绝开跑" + if entry <= 1e-9: + return "" + if entry > HOLE_ENTRY_MAX_M + 1e-9: + return ( + f"进孔深度 {entry * 1000:.1f}mm 超过 {HOLE_ENTRY_MAX_M * 1000:.0f}mm" + "（板厚 12mm，不许穿出下表面），拒绝开跑" + ) + tool = float(tool_len_m) + surf = float(surface_z_m) + if tool <= 0.0 or surf <= 0.0: + return "写了进孔深度，但工具长度或工件面未知，无法确认尖端进了多深，拒绝开跑" + flange = flange_z_for_gap_m(tool, surf, -entry) + z_lo = float(z_min_m) + z_hi = float(z_max_m) + if flange < z_lo - 1e-6 or flange > z_hi + 1e-6: + return ( + f"进孔法兰 Z {flange:.3f}m（面上再进 {entry * 1000:.1f}mm）" + f"超出工作盒 Z [{z_lo:.3f}, {z_hi:.3f}]，拒绝开跑" + ) + return "" + + def final_gap_refusal( gap_m: float, tool_len_m: float, diff --git a/src/arm_application/arm_application/pipeline_node.py b/src/arm_application/arm_application/pipeline_node.py index a27c828..a1f350c 100644 --- a/src/arm_application/arm_application/pipeline_node.py +++ b/src/arm_application/arm_application/pipeline_node.py @@ -11,20 +11,29 @@ from __future__ import annotations import json +import math import os import signal import subprocess import time from pathlib import Path -from typing import Optional +from typing import Optional, Tuple import numpy as np import rclpy from geometry_msgs.msg import PointStamped -from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy -from std_msgs.msg import Bool, String +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from std_msgs.msg import Bool, Float32MultiArray, String +from trajectory_msgs.msg import JointTrajectory +from arm_application.gauge_kin import ( + a7_limit_target, + match_limits, + parse_joint_limits, +) from arm_application.overhead_oneshot import D435_MOVEL_GAVE_UP +from arm_application.site_io import load_site_dict, ros2_root +from arm_application.stage2_locate_node import _pt from arm_application.stage3_search_node import Stage3Search @@ -165,6 +174,19 @@ class AssemblyPipeline(Stage3Search): self._s1: Optional[subprocess.Popen] = None self._locked = False + self._robot_description: Optional[str] = None + # robot_state_publisher 用 TRANSIENT_LOCAL 只发一次。默认 VOLATILE + # 订阅在它之后才起来，3s 内一条都收不到，A7 会因“没有限位”拒绝旋转。 + self.create_subscription( + String, + "/robot_description", + self._on_robot_description, + QoSProfile( + depth=1, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + reliability=ReliabilityPolicy.RELIABLE, + ), + ) self.create_subscription(Bool, "/visual_servo/locked", self._on_lock, 10) # 诊断用：一阶段超时时靠它区分"感知没给点"和"给了点没收敛"。 # 只计数，不参与控制 —— 它绝不能影响流程本身的判断。 @@ -208,6 +230,235 @@ class AssemblyPipeline(Stage3Search): self._state_pub.publish(msg) self._pipe_pub.publish(msg) + def _on_robot_description(self, msg: String) -> None: + self._robot_description = str(msg.data) + + def _a7_limits(self, timeout_s: float = 3.0) -> Optional[Tuple[float, float]]: + """A7 软件限位。来自 ``/robot_description`` 的 joint7，与厂家 ``q_min/q_max`` 同一对。""" + t0 = time.monotonic() + while self._robot_description is None and time.monotonic() - t0 < timeout_s: + self._spin(0.05) + if not self._robot_description: + self.get_logger().error("拿不到 /robot_description，A7 没有限位，拒绝旋转") + return None + got = match_limits( + parse_joint_limits(self._robot_description), [self.joint_names[-1]] + ) + if got is None: + self.get_logger().error( + f"/robot_description 里没有 {self.joint_names[-1]} 的限位，拒绝旋转" + ) + return got + + def _send_a7_only(self, a7_rad: float, speed_deg_s: float, tag: str) -> bool: + """只下发 A7。前六轴用这一拍的当前角，厂家 ``SendJointPosition`` 收到的也是这一组。""" + q, Tee = self._wait_state(5.0) + if q is None or Tee is None or int(q.size) != 7: + self.get_logger().error(f"{tag}: 拿不到 7 轴关节，不下发") + return False + z0 = float(Tee[2, 3]) + q_hold = q.copy() + q_hold[6] = float(a7_rad) + delta_deg = abs(math.degrees(float(q_hold[6] - q[6]))) + dur = max(0.8, delta_deg / max(float(speed_deg_s), 1.0)) + zeros = np.zeros(q.size) + traj = JointTrajectory() + traj.header.stamp.sec = 0 + traj.header.stamp.nanosec = 0 + traj.joint_names = list(self.joint_names) + traj.points = [_pt(q, zeros, 0.2), _pt(q_hold, zeros, 0.2 + dur)] + self.get_logger().info( + f"{tag}: A7 {math.degrees(float(q[6])):+.1f}° → " + f"{math.degrees(float(q_hold[6])):+.1f}°，" + f"{speed_deg_s:.0f}°/s，前六轴不动，法兰 Z={z0:.4f}m" + ) + self._send_action(traj, wait_s=dur + 8.0) + q2, Tee2 = self._wait_state(5.0) + if q2 is None or Tee2 is None: + self.get_logger().error(f"{tag}: 走完读不到关节") + return False + others = float(np.max(np.abs(q2[:6] - q[:6]))) + dz = abs(float(Tee2[2, 3]) - z0) + a7_err = abs(float(q2[6] - q_hold[6])) + self.get_logger().info( + f"{tag}: 前六轴最大偏差 {others:.4f} rad，" + f"法兰 ΔZ={dz * 1000:.1f}mm，A7 残差 {math.degrees(a7_err):.1f}°" + ) + if others > 0.02: + self.get_logger().error(f"{tag}: 前六轴动了 {others:.3f} rad，中止") + return False + if dz > 0.003: + self.get_logger().error(f"{tag}: 法兰 Z 变了 {dz * 1000:.1f}mm，中止") + return False + if a7_err > 0.05: + self.get_logger().error( + f"{tag}: A7 还差 {math.degrees(a7_err):.1f}°，没到限位内侧" + ) + return False + self._pin(q2) + return True + + def _arm_short_id(self) -> str: + """当前臂号。与 ``export_site_env.arm_short_id`` 一样，取 SN 末两位。""" + short = os.environ.get("SITE_ROBOT_SHORT_ID", "").strip() + if short: + return short + sn = str( + self.get_parameter("robot_sn").value + or os.environ.get("SITE_ROBOT_SN") + or "" + ).strip() + tail = sn.rsplit("-", 1)[-1] + if tail.isdigit() and len(tail) >= 2: + return tail[-2:] + return "" + + def _gripper_cli(self, action: str, value: str = "") -> bool: + """左平行爪。``close`` 合拢；``pos`` 走到 0–1000 的指定开度，不全开。 + + 脚本返回只说明命令已交给 ``/gripper/ctrl``。没在状态里看到目标开度或夹住， + 就不许接着转 A7。 + """ + short = self._arm_short_id() + if not short: + self.get_logger().error("不知道当前臂号，拒绝开合夹爪") + return False + if action == "close": + extra: list[str] = [] + elif action == "pos": + try: + pos = int(value) + except (TypeError, ValueError): + pos = -1 + if pos < 0 or pos > 1000: + self.get_logger().error(f"夹爪位置只接受 0–1000，收到 {value!r}") + return False + extra = [str(pos)] + else: + self.get_logger().error(f"夹爪命令只接受 pos/close，收到 {action}") + return False + script = ros2_root() / "scripts" / "run_gripper.sh" + cmd = ["bash", str(script), "--arm", short, "--cli", action, *extra] + self.get_logger().info("夹爪：" + " ".join(cmd)) + try: + proc = subprocess.run( + cmd, capture_output=True, text=True, timeout=30, check=False + ) + except subprocess.TimeoutExpired: + self.get_logger().error(f"夹爪 {action} 30s 没有返回，A7 不再转") + return False + text = ((proc.stdout or "") + "\n" + (proc.stderr or "")).strip() + if text: + tail = " | ".join(text.splitlines()[-4:]) + self.get_logger().info(f"夹爪脚本：{tail}") + if proc.returncode != 0: + self.get_logger().error( + f"夹爪 {action} 失败 exit={proc.returncode}，A7 不再转" + ) + return False + return self._wait_gripper(action, value) + + def _wait_gripper(self, action: str, value: str = "", timeout_s: float = 8.0) -> bool: + """``/gripper/states``：``pos`` 要停在目标附近；闭合要报夹住（grip=2）。""" + box: dict = {"data": None} + + def _on_state(msg: Float32MultiArray) -> None: + box["data"] = list(msg.data) + + sub = self.create_subscription(Float32MultiArray, "/gripper/states", _on_state, 10) + t0 = time.monotonic() + last = None + try: + while time.monotonic() - t0 < timeout_s: + self._spin(0.05) + data = box["data"] + if not data or len(data) < 3: + continue + grip = int(data[1]) + pos = float(data[2]) + last = (grip, pos) + if action == "pos" and grip != 0 and abs(pos - float(value)) <= 40.0: + self.get_logger().info( + f"夹爪已到 pos={pos:.0f}（目标 {value}，不全开）" + ) + return True + if action == "close" and grip == 2: + self.get_logger().info(f"夹爪已夹住 pos={pos:.0f}") + return True + finally: + self.destroy_subscription(sub) + shown = f"grip={last[0]} pos={last[1]:.0f}" if last else "没有 /gripper/states" + self.get_logger().error( + f"夹爪 {action} 后 {timeout_s:.0f}s 未到位（{shown}），A7 不再转" + ) + return False + + def _a7_to_limit( + self, limits, direction: int, margin_deg: float, speed: float, tag: str + ) -> bool: + """只把 A7 转到这一侧限位内侧。前六轴和法兰 Z 的检查在 ``_send_a7_only``。""" + q, _Tee = self._wait_state(5.0) + if q is None: + self.get_logger().error(f"{tag}: 没有当前关节") + return False + target = a7_limit_target(q, limits, direction, float(margin_deg)) + if target is None: + self.get_logger().error(f"{tag}: 算不出 A7 限位目标") + return False + return self._send_a7_only(float(target[6]), speed, tag) + + def _run_a7_seat(self) -> bool: + """贴面后咬螺纹。只转 A7，不改法兰位姿。 + + 前两步是首轮：逆时针到限位，再顺时针到限位。顺时针到位即首轮咬合结束。 + 第三步松爪、逆时针回限位、合爪、再顺时针到限位。 + """ + limits = self._a7_limits() + if limits is None: + return False + ga = load_site_dict().get("gauge_assembly") or {} + try: + margin = float(ga["extreme_margin_deg"]) + speed = float(ga["extreme_speed_deg_s"]) + ccw = int(ga["extreme_direction"]) + except (KeyError, TypeError, ValueError) as exc: + self.get_logger().error( + f"site.yaml gauge_assembly 缺 A7 旋转参数（{exc}），拒绝旋转" + ) + return False + if ccw not in (-1, 1): + self.get_logger().error( + f"extreme_direction 只能是 +1/-1，收到 {ccw}，拒绝旋转" + ) + return False + # extreme_direction 是逆时针的符号。第一步出孔，第二步顺时针进孔。 + first = ( + (ccw, "A7 逆时针至极限（出孔）"), + (-ccw, "A7 顺时针至极限（进孔）"), + ) + for direction, tag in first: + if not self._a7_to_limit(limits, direction, margin, speed, tag): + return False + self._state("首轮咬合结束") + self.get_logger().info("首轮咬合结束：A7 已顺时针转到限位。") + # 第三步：只张开到 400（不全开），逆时针空回，再夹住顺时针拧。 + if not self._gripper_cli("pos", "400"): + return False + if not self._a7_to_limit( + limits, ccw, margin, speed, "A7 逆时针至极限（松爪回转）" + ): + return False + if not self._gripper_cli("close"): + return False + if not self._a7_to_limit( + limits, -ccw, margin, speed, "A7 顺时针至极限（夹紧再进）" + ): + return False + self.get_logger().info( + "A7 第三步完成：张开到 400、逆时针回限位、合爪、顺时针到限位。只转了 A7。" + ) + return True + def _on_lock(self, msg: Bool) -> None: if bool(msg.data): self._locked = True @@ -406,7 +657,8 @@ class AssemblyPipeline(Stage3Search): f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm — " f"插孔停在法兰 Z={final.get('flange_z_m')}m，" f"规尖离面 {final.get('tip_gap_mm')}mm" - f"（目标 {final.get('commanded_gap_mm')}mm，来自 final_gap_mm）。" + f"（目标 {final.get('commanded_gap_mm')}mm，" + f"来自 {final.get('insert_z_source')}）。" "没有接触传感器，不另设 Z，不抬回悬高。" ) else: @@ -434,8 +686,13 @@ class AssemblyPipeline(Stage3Search): self._pin(q) self.get_logger().info( "插孔已停在贴面高度，不进搜孔、不抬回悬高。" - "15s 后本进程退出，臂保持这个 Z。" + "接着只转 A7：先逆时针到限位，再顺时针到限位（首轮咬合）。" + "然后平行爪只张开到 400、逆时针回限位、合爪、再顺时针到限位。" ) + if not self._run_a7_seat(): + self.get_logger().error( + "A7 旋转没走完，臂停在当前关节。不抬 Z。" + ) else: self.get_logger().info( "跳过搜孔 —— 臂停在当前高度。请看规尖与孔是否对中；" diff --git a/src/arm_application/arm_application/stage2_locate_node.py b/src/arm_application/arm_application/stage2_locate_node.py index a3575d2..fb2e5a0 100644 --- a/src/arm_application/arm_application/stage2_locate_node.py +++ b/src/arm_application/arm_application/stage2_locate_node.py @@ -63,6 +63,8 @@ from arm_application.overhead_oneshot import ( decompose_base_xy, flange_z_for_gap_m, final_gap_refusal, + hole_entry_refusal, + insert_target_gap_m, hole_pair_span_m, hole_pair_span_ok, once_ff_movel_from_tcp, @@ -260,6 +262,8 @@ class Stage2Locate(TolerantNode): self.final_gap_m = _gap_override_mm / 1000.0 else: self.final_gap_m = float(stage2.get("final_gap_mm") or 1.5) / 1000.0 + # 缺键 = 不进孔（停在面上）。进孔必须在 site 里写明，不许默认钻进去。 + self.hole_entry_m = float(stage2.get("hole_entry_mm") or 0.0) / 1000.0 # 观测下降：对准后降到这个间隙就停，方便看规尖是不是对着孔。 # 比 final_gap（贴面 2mm）高一截，对偏了也不会把规压上孔沿。 self.observe_gap_m = float(stage2.get("observe_gap_mm") or 15.0) / 1000.0 @@ -325,7 +329,8 @@ class Stage2Locate(TolerantNode): "（这次没写 --hover-z，沿用 site 的几何抬高）" ) self.hover_z = _used - # 轻降**最多能降多少**：从悬停高度降到"规尖停在工件面上方 final_gap"为止。 + self.insert_gap_m = insert_target_gap_m(self.final_gap_m, self.hole_entry_m) + # 轻降**最多能降多少**：从悬停高度降到插孔目标（面上 final_gap，或面下 hole_entry）。 # 这是纯几何量，与深度测量无关 —— 测量的唯一职责是让下降**提前**收工， # 绝不允许它把下降量**放大**（2026-09-29 实测就是这么撞的：测量出的 # gap0=141mm 比几何真值大 ~86mm，于是多降 86mm 把规撞进孔）。 @@ -333,7 +338,7 @@ class Stage2Locate(TolerantNode): self.descent_budget_m: Optional[float] = None if _tool_len > 0.0 and _surface_z > 0.0: self.descent_budget_m = max( - 0.0, self.hover_z - _tool_len - _surface_z - self.final_gap_m + 0.0, self.hover_z - _tool_len - _surface_z - self.insert_gap_m ) # 轻降结束后**按几何**算出的真实间隙 [m]，供收尾日志如实汇报（不是把目标值 # 当结果念一遍 —— 那会让"停在离面 50mm"也显示成"~2.0mm"，R1.1 的静默失效）。 @@ -369,6 +374,17 @@ class Stage2Locate(TolerantNode): if _why_gap: self._final_gap_refused = _why_gap self.get_logger().error(_why_gap) + else: + _why_entry = hole_entry_refusal( + self.hole_entry_m, + _tool_len, + _surface_z, + float(self.ws_min[2]), + float(self.ws_max[2]), + ) + if _why_entry: + self._final_gap_refused = _why_entry + self.get_logger().error(_why_entry) self.vert_tol_rad = math.radians(float(stage2.get("verticalize_tol_deg") or 0.8)) self.settle_sec = float(stage2.get("settle_sec") or 0.45) self.snap_frames = int(stage2.get("snap_frames") or 5) @@ -490,14 +506,17 @@ class Stage2Locate(TolerantNode): f"hole_keypoint_once={self.hole_keypoint_once} " f"lock_xy_descend={self.hole_once_lock_xy_descend} " f"ff_look={self.once_ff_look_per_m:.3f} " - f"final_gap={self.final_gap_m * 1000:.1f}mm" + f"final_gap={self.final_gap_m * 1000:.1f}mm " + f"hole_entry={self.hole_entry_m * 1000:.1f}mm " + f"插孔间隙={self.insert_gap_m * 1000:.1f}mm" + ( - f"→法兰Z={flange_z_for_gap_m(self.tool_tip_below_flange_m, self.surface_z_m, self.final_gap_m):.4f}m" + f"→法兰Z={flange_z_for_gap_m(self.tool_tip_below_flange_m, self.surface_z_m, self.insert_gap_m):.4f}m" if self.tool_tip_below_flange_m > 0.0 and self.surface_z_m > 0.0 else "" ) + "\n" - f" 工具长={self.tool_tip_below_flange_m:.4f}m（法兰到尖，下臂不得低于面+间隙）" + f" 工具长={self.tool_tip_below_flange_m:.4f}m（法兰到尖；" + "hole_entry>0 时规尖停在上表面以下）" ) if self.hole_keypoint_once and self.hole_once_lock_xy_descend: k = float(self.once_ff_look_per_m) @@ -3363,7 +3382,7 @@ class Stage2Locate(TolerantNode): if self.hole_keypoint_once and self.hole_once_lock_xy_descend: # 视线前馈斜线：不经过观察高、不补交。一条 MoveL 收到贴面。 lock_gap = ( - self.final_gap_m if self.final_descent_enable else self.observe_gap_m + self.insert_gap_m if self.final_descent_enable else self.observe_gap_m ) okd, q, Tee = self._once_ff_diagonal_to_gap( q, Tee, target_gap_m=lock_gap @@ -3406,8 +3425,9 @@ class Stage2Locate(TolerantNode): ) # ---- 5) 锁定保持 ---- - # 贴面之后就停在这个法兰 Z。没有接触传感器，到位只认 final_gap_mm - # （规尖离面）。三阶段开局会抬回悬高，这里标明已经插到位，流水线不再上抬。 + # 插孔之后就停在这个法兰 Z。没有接触传感器。 + # hole_entry_mm>0：规尖在上表面以下；否则停在 final_gap（面上）。 + # 三阶段开局会抬回悬高，这里标明已经插到位，流水线不再上抬。 if ( self.descent_final_gap_m is not None and self.final_descent_enable @@ -3417,8 +3437,11 @@ class Stage2Locate(TolerantNode): final["insert_held"] = True final["flange_z_m"] = round(float(Tee[2, 3]), 4) final["tip_gap_mm"] = round(float(self.descent_final_gap_m) * 1000.0, 2) - final["commanded_gap_mm"] = round(float(self.final_gap_m) * 1000.0, 2) - final["insert_z_source"] = "final_gap_mm" + final["commanded_gap_mm"] = round(float(self.insert_gap_m) * 1000.0, 2) + final["hole_entry_mm"] = round(float(self.hole_entry_m) * 1000.0, 2) + final["insert_z_source"] = ( + "hole_entry_mm" if self.hole_entry_m > 1e-9 else "final_gap_mm" + ) self._state("hold") q_fin = self._q() self._pin(q_fin if q_fin is not None else q) @@ -3426,7 +3449,8 @@ class Stage2Locate(TolerantNode): self.get_logger().info( f"=== 二阶段定位完成: {self.target_hole} 残差 {final.get('residual_mm', -1)}mm，" f"法兰 Z={final.get('flange_z_m')}m，规尖离面 {gap_txt}mm" - f"（final_gap {self.final_gap_m * 1000:.1f}mm，无接触传感器）。" + f"（目标 {self.insert_gap_m * 1000:.1f}mm，" + f"hole_entry {self.hole_entry_m * 1000:.1f}mm，无接触传感器）。" "停在这一高度，不抬回悬高。" f"结果: /stage2/result, /tmp/stage2_result.json ===" ) diff --git a/src/arm_application/test/test_gauge_kin.py b/src/arm_application/test/test_gauge_kin.py index 9fb8716..d764e74 100644 --- a/src/arm_application/test/test_gauge_kin.py +++ b/src/arm_application/test/test_gauge_kin.py @@ -14,6 +14,7 @@ import pytest from arm_application.gauge_kin import ( DEFAULT_LIMIT_MARGIN_DEG, + a7_limit_target, axis_misalignment_deg, match_limits, parse_joint_limits, @@ -200,6 +201,21 @@ def test_rotation_helpers_reject_zero_axis(): rotation_about_axis_deg(np.eye(3), _rot_z(10.0), [0.0, 0.0, 0.0]) +def test_a7_limit_moves_only_the_last_joint(): + """逆时针到上限、顺时针到下限。前六轴必须原样，没有限位就拒绝。""" + q = np.array([0.1, -0.2, 0.3, -0.4, 0.5, -0.6, 0.2]) + limits = (-3.0543, 3.0543) + ccw = a7_limit_target(q, limits, direction=1, margin_deg=3.0) + cw = a7_limit_target(q, limits, direction=-1, margin_deg=3.0) + assert ccw is not None and cw is not None + np.testing.assert_allclose(ccw[:6], q[:6]) + np.testing.assert_allclose(cw[:6], q[:6]) + assert ccw[6] == pytest.approx(3.0543 - math.radians(3.0)) + assert cw[6] == pytest.approx(-3.0543 + math.radians(3.0)) + assert a7_limit_target(q, None, 1) is None + assert a7_limit_target(q, limits, 1, margin_deg=200.0) is None + + def test_tool_z_and_vertical_down_error(): """工具 Z 朝世界 -Z = 0°；水平 = 90°；朝上 = 180°。""" R_down = np.diag([1.0, -1.0, -1.0]) diff --git a/src/arm_application/test/test_overhead_oneshot.py b/src/arm_application/test/test_overhead_oneshot.py index 443ba2b..61972b3 100644 --- a/src/arm_application/test/test_overhead_oneshot.py +++ b/src/arm_application/test/test_overhead_oneshot.py @@ -61,7 +61,10 @@ from arm_application.overhead_oneshot import ( hole_pair_span_ok, oneshot_align_step_m, flange_z_for_gap_m, + HOLE_ENTRY_MAX_M, final_gap_refusal, + hole_entry_refusal, + insert_target_gap_m, d435_movel_from_tcp, once_ff_movel_from_tcp, once_ff_tcp_xyz, @@ -929,6 +932,23 @@ class TestHoleBiasAxes: assert flange_8 == pytest.approx(0.2545) assert flange_8 < hover + def test_hole_entry_goes_below_the_face_and_refuses_through_the_plate(self): + """孔口凹槽：进孔深度把规尖放到上表面以下；超过 8mm 或出工作盒必须红。""" + tool, surf = 0.226587, 0.022 + z_lo, z_hi = 0.18, 0.65 + assert insert_target_gap_m(0.002, 0.0) == pytest.approx(0.002) + assert insert_target_gap_m(0.002, 0.004) == pytest.approx(-0.004) + assert hole_entry_refusal(0.0, tool, surf, z_lo, z_hi) == "" + assert hole_entry_refusal(0.004, tool, surf, z_lo, z_hi) == "" + flange = flange_z_for_gap_m(tool, surf, -0.004) + assert flange == pytest.approx(tool + surf - 0.004) + assert flange > z_lo + why_deep = hole_entry_refusal(HOLE_ENTRY_MAX_M + 0.004, tool, surf, z_lo, z_hi) + assert "穿出" in why_deep + assert "未知" in hole_entry_refusal(0.004, 0.0, surf, z_lo, z_hi) + why_box = hole_entry_refusal(0.004, tool, surf, 0.40, z_hi) + assert "工作盒" in why_box + def test_final_gap_cli_overrides_site_and_conflicts_with_no_descent(self): """--final-gap 必须接到 launch/节点；和 --no-descent 一起写必须在脚本里红。""" from pathlib import Path @@ -1060,6 +1080,11 @@ class TestRetractHold: ).read_text(encoding="utf-8") assert "stage1_log_fatal(" in pipe assert "finish_vendor_movel" in cpp + running = cpp[cpp.find("if (vendor_movel_phase_ == VendorMovelPhase::kRunning)") :] + running = running[: running.find("reachedTarget")] + assert "robot_->fault()" in running + assert "目标到不了" in running + assert running.find("robot_->fault()") < running.find("> vendor_movel_deadline_") fin = cpp[cpp.find("void FlexivHardwareInterface::finish_vendor_movel") :] fin = fin[: fin.find("bool FlexivHardwareInterface::poll_vendor_movel")] assert "hold_current_joint_commands()" in fin diff --git a/src/arm_application/test/test_pipeline_stage1_diag.py b/src/arm_application/test/test_pipeline_stage1_diag.py index 624df10..f72a647 100644 --- a/src/arm_application/test/test_pipeline_stage1_diag.py +++ b/src/arm_application/test/test_pipeline_stage1_diag.py @@ -36,13 +36,34 @@ def test_insert_hold_does_not_return_to_hover(): ).read_text(encoding="utf-8") hold = src[src.find("# ---- 5) 锁定保持 ----") : src.find("def _linger_hold")] assert 'final["insert_held"] = True' in hold - assert 'final["insert_z_source"] = "final_gap_mm"' in hold + assert 'final["insert_z_source"]' in hold + assert "hole_entry_mm" in hold + assert "self.insert_gap_m" in hold pipe = ( Path(__file__).resolve().parents[1] / "arm_application" / "pipeline_node.py" ).read_text(encoding="utf-8") run = pipe[pipe.find("def run") :] assert "hold_at_insert_z(" in run assert "不抬回悬高" in run + assert "_run_a7_seat(" in run + seat = pipe[pipe.find("def _run_a7_seat") : pipe.find("def _on_lock")] + marks = ( + "首轮咬合结束", + '_gripper_cli("pos", "400")', + "松爪回转", + '_gripper_cli("close")', + "夹紧再进", + ) + pos = [seat.find(m) for m in marks] + assert all(i >= 0 for i in pos) + assert pos == sorted(pos) + grip = pipe[pipe.find("def _gripper_cli") : pipe.find("def _wait_gripper")] + assert "run_gripper.sh" in grip + assert '"--cli"' in grip + assert "_send_a7_only" in pipe[pipe.find("def _a7_to_limit") : pipe.find("def _run_a7_seat")] + # /robot_description 是锁存话题。VOLATILE 订阅会错过已经发出的 URDF。 + sub = pipe[pipe.find('"/robot_description"') : pipe.find('"/robot_description"') + 400] + assert "TRANSIENT_LOCAL" in sub def test_zero_points_blames_perception_not_calibration(tmp_path: Path): diff --git a/src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp b/src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp index d1e9545..8ccb25c 100644 --- a/src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp +++ b/src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp @@ -866,6 +866,23 @@ bool FlexivHardwareInterface::poll_vendor_movel() } if (vendor_movel_phase_ == VendorMovelPhase::kRunning) { try { + // 目标在孔底外面时厂家约 5s 报 302064，原语不会报到位。 + // 不在这里收，流水线会干等到 90s deadline。 + if (robot_->fault()) { + RCLCPP_ERROR(getLogger(), + "厂家 MoveL 机器人报故障，停止等待原语到位"); + try { + robot_->ClearFault(3); + } catch (const std::exception&) { + } + try { + robot_->SwitchMode(rdk_control_mode_); + } catch (const std::exception&) { + } + finish_vendor_movel( + vendor_movel_seq_, false, "MoveL 中机器人报故障，目标到不了"); + return true; + } if (std::chrono::steady_clock::now() > vendor_movel_deadline_) { try { robot_->SwitchMode(rdk_control_mode_);