orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

左臂换成 tool10120938 后，手眼仍按旧 tool1 和 8×11 板求解，顶视迁臂又按短跳 rest-to-rest，真机上要么整体偏一截，要么中途弹射启停。

Browse Source 
眼在手上改用该 TCP 做 AX=XB，再折回相机→法兰；A5 内角点改为 11×8，09-04 / 10-09 旧结果留在 backup。一阶段 D435 迁臂改成一条 FollowJointTrajectory（余弦加减速，近处降速，XY 进门槛后再单独收垂直），跟踪锁高与悬停齐平，避免倾斜相机下先降一截把孔位重投影偏掉。--no-descent 只降到离面 15 mm；跳过三阶段时触觉 USB 打不开不再空等几分钟。

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
2026-10-12 15:04:23 +08:00
parent 7d22f777c8
commit b903161113
45 changed files with 3555 additions and 405 deletions
Download Patch File Download Diff File Expand all files Collapse all files
config/site.example.yaml
+43 -22
View File
@@ -26,6 +26,7 @@ robot:
# >>> 切换机械臂：改成 robots 里某一台的完整 SN <<<
sn: "Rizon4-XXXXXX"
# Studio Tools 名（一字不差）。空=不切换。查：bash ~/ros2/scripts/rdk_switch_tool.sh --list
# 左臂 89 的激活工具写在 robots[] 里（tool10120938）；这里只给未覆盖的臂当缺省。
tool_name: "tool1"
# 每台登记 SN + IP，以及该臂自己的相机/夹爪/手眼/点位（覆盖下面的全局段）。
robots:
@@ -33,11 +34,13 @@ robot:
arm: left
ip: "192.168.2.XXX"
pc_ip: "192.168.2.XXX"
iface: "enxXXXXXXXXXXXX"
iface: "eth0"
label: "左臂89"
# 本臂装了 Orisys 视触觉（只有左臂有）。脚本靠这一项决定要不要拉触觉服务，
# 而不是靠"SN 等于某个具体值"这种写死的判断（换台臂就会失效）。
tactile: true
# 2026-10-12 示教的工具 TCP（tool + 1012 + 0938）。眼在手上 AX=XB 用这份 TCP。
tool_name: "tool10120938"
camera:
name: d405_left # 话题 /d405_left/... 与 91 分开
serial: "<序列号>" # 左腕 D405（一直在用的那台）
@@ -63,6 +66,11 @@ robot:
handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml"
handeye_waypoints: "/home/USER/ros2/handeye/poses/89_eye_in_hand.yaml"
handeye_workdir: "/home/USER/ros2/handeye/tmp/89"
handeye:
# Studio tcp_location（法兰系，米 + qwxyz）。与 rdk Tool.params 必须一致。
gripper_tool: "tool10120938"
gripper_tcp_xyz: [-0.0005, 0.0067, 0.2245]
gripper_tcp_qwxyz: [1.0, 0.0, 0.0, 0.0]
# 左臂跟踪高度（只在 --arm 89 时生效，改 91 的 servo 不影响这里）
# 锚点 `&servo_left`：下面的"仿真台"条目直接引用它 —— 仿真建的就是左臂，
# 悬停/锁高必须与 89 **一模一样**，抄两份迟早漂移。
@@ -70,8 +78,10 @@ robot:
# 2026-09-29 底座加高后由 0.35 → 0.36：工件上表面实测抬高 10mm
# （0.012 → 0.022），锁高同步 +0.011 ⇒ 相机到工件的视距**与加高前一致**
# （≈0.213m，跟踪那一档已实拍验证过检出稳定），不是随手抬的。
# 规矩：ee_z_m 要高于 servo.hole_hover_z_m（二阶段得往**下**走，不能往上返）。
ee_z_m: 0.36 # 跟踪锁高：法兰相对基座 Z [m]
# 跟踪锁高与 hole_hover_z_m 相同：先到这个悬高，一阶段只动 XY。
# 对孔完成后再下降。以前 ee_z 高于悬停（0.36），对孔前会先降一截，
# 倾斜相机下孔位被重投影，偏差变大。
ee_z_m: 0.320 # 跟踪锁高：法兰相对基座 Z [m]，与 hole_hover_z_m 同一值
min_ee_z_m: 0.20
safe_z_m: 0.45
z_min_m: 0.22
@@ -276,7 +286,7 @@ robot:
arm: right
ip: "192.168.2.XXX"
pc_ip: "192.168.2.XXX"
iface: "enxXXXXXXXXXXXX"
iface: "eth0"
label: "右臂91"
camera:
name: d405_right # 话题 /d405_right/... 禁止和 89 的 /d405_left 混用
@@ -356,9 +366,9 @@ handeye:
# 内角点 = 方格数 − 1；列/行顺序不用纠结，检测端两种顺序都会试
#（见 handeye_lib.detect_chessboard）。
boards:
eye_in_hand: # A5 板：9×12 方格
cols: 8 # 内角点列数
rows: 11 # 内角点行数
eye_in_hand: # A5 板：12×9 方格 → 内角点 11×8
cols: 11 # 内角点列数
rows: 8 # 内角点行数
square_size_m: 0.015 # 方格边长 [m]
eye_to_hand: # A4 板：12×9 方格
cols: 11
@@ -579,9 +589,10 @@ servo:
deadzone_m: 0.003 # 3 mm；收紧提高对准精度。若抖动调回 0.006
deadzone_z_m: 0.012
lock_hold_sec: 0.7 # 旧死区驻留（混合模式不再用它进二阶段）
# D405 跟住后，法兰 XY 连续这么久位移不超过 lock_still_move_m → /visual_servo/locked
# 顶视一次性把臂迁过去的那段路不算：必须是腕部有效目标。大幅一动就重计时。
lock_still_sec: 3.0
lock_still_move_m: 0.015
lock_still_move_m: 0.015 # 15 mm；超过就重计时，避免"晃一下刚好满 3 秒"
e_xy_clip_m: 0.200 # 大误差仍按 v_max 走，靠 a_max 加速
v_max_xy: 0.45 # 法兰 XY 上限 450 mm/s
v_max_z: 0.04
@@ -732,7 +743,9 @@ hole_detect:
# 是拿法兰对的孔）。收到 0.25mm 才逼它必须真的对上规尖。
align_max_iters: 12 # 拍照→修正→复拍 迭代上限（收紧容差后需要更多拍）
align_tip_vision: true # true=精对准用「视觉规尖」；false=退回旧的「法兰+偏置」
tip_near_m: 0.15 # 规尖量取：近端阈值，规比这更近 [m]
tip_near_m: 0.15 # 规尖量取：无工件面深度时的近端上限 [m]；
# 精对准同一帧有 z_wp 时改用「工件面 − 15mm」
# （3mm 挡不住噪声，会把工件面当成规尖）
tip_min_m: 0.04 # 有效深度下限 [m]
tip_max_m: 0.60 # 有效深度上限 [m]
tip_search_band_frac: 0.25 # 中央竖带半宽 = 该比例 × 图宽（规夹在爪里，不会靠边）
@@ -752,7 +765,10 @@ hole_detect:
# —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 ——
align_speed: 0.014 # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
move_speed: 0.022 # 升降/粗居中段速度 [m/s]（原0.05，放缓防震规）
final_gap_mm: 2.0 # 对准后轻降目标：规尖悬在孔面上方这么高 [mm]（不接触；安全下限1.2mm自动停）
final_gap_mm: 2.0 # 对准后贴面轻降目标：规尖悬在孔面上方这么高 [mm]（不接触）
observe_gap_mm: 15.0 # --no-descent 时的观测下降：只降到离面这么高 [mm]，
# 方便看规尖偏没偏。必须 ≥ final_gap_mm。
# 左臂89：悬停间隙约 75mm（0.320−0.223−0.022）⇒ 大约下降 60mm。
final_descent_speed: 0.005 # 最后轻降速度 [m/s]（很慢，柔顺）
# ✅ 2026-10-08 定稿：下降量**只由几何决定**（法兰 FK − 工具长 − 工件面 − final_gap），
# 深度读数只做交叉核对、不再参与任何决策。两段真机事故方向相反、都指向这条：
@@ -769,7 +785,7 @@ hole_detect:
# 三道**物理/几何**保险：descent_budget_m（配置上限）+ 腕部 FT 接触检测（停+退 1mm）
# + 任一步 _goto 走不到位就停。
# 缺几何（工具长/工件面没配）时**拒绝下降**并报错（保持悬停），绝不拿测距去赌。
final_descent_enable: true # 对准后是否轻降到 final_gap_mm（false=停在悬停高）
final_descent_enable: true # true=对准后降到 final_gap_mm；false / --no-descent=只降到 observe_gap_mm
verticalize_tol_deg: 0.8 # 垂直朝下判据 [deg]
settle_sec: 0.45 # 到位后静止多久再拍照
snap_frames: 5 # 每次精拍取中值的帧数
@@ -842,22 +858,26 @@ hole_detect:
# =============================================================================
perception:
source: "yolo"
# 一阶段：wrist=D405 主跟踪，画面内无 conf≥wrist_handoff_conf 则 D435 一次性定 K0。
# overhead=整段只用顶视（实验）。非法值启动即报错。
# 一阶段跟踪用哪台相机。wrist=腕部 D405（主跟踪）+ 顶视 D435 一次性兜底；
# overhead=整段只用 D435（实验分支才切）。非法值启动即报错，不会静默退回。
track_camera: "wrist"
# 画面内最高检测 conf 低于此 → 马上拍一张 D435，把工件 / k0 定到基座系。
# 与 perception / servo 共用这一处，不许两边各写一个 0.5。
wrist_handoff_conf: 0.5
# 顶视深度量程 + 一次性参数。腕部那组 0.04–0.55m 会把 D435 的点全丢掉。
# 缺键启动即红，不许借腕部量程、不许用代码默认冷却时间。
overhead:
min_depth_m: 0.25
max_depth_m: 1.80
feature_min_range_m: 0.30
work_z_max_m: 1.50
z_ref_m: 0.70
feature_mode: "plane"
hold_point_on_miss: true
oneshot_conf: 0.5
cooldown_s: 1.5
frame_max_age_s: 0.4
oneshot_hold_s: 8.0
feature_mode: "plane" # 顶视与腕部视角差太大，pose 的 k0 经常看不见；优先 k0，否则框心
hold_point_on_miss: true # 仅 overhead 整段跟踪时用；混合模式一次性不续发
oneshot_conf: 0.5 # D435 这一张也必须 conf≥此值，不许拿噪声框当 K0
cooldown_s: 1.5 # 两次一次性之间的最短间隔，避免跟 D405 抢同一条 YOLO 队列
frame_max_age_s: 0.4 # 彩图/深度比这更旧就拒绝，不许拿过期图反投影
oneshot_hold_s: 8.0 # 伺服保持这一次设定的最长时间；超时丢掉，等下一张或 D405
truth:
# 真值目标物体。留空 = 场景文件里的 operating.part_object（推荐留空：
# "哪个是工件"是场景的知识，换场景不该改这里）。
@@ -1089,10 +1109,11 @@ go_home:
network:
# 2026-09-29 改线：两台臂 → 交换机 → USB-C 千兆网卡（ASIX AX88179）
# enxXXXXXXXXXXXX = 该 USB 网卡（名字由网卡 MAC 派生，换网卡会变）
# MAC 6c:1f:f7:d0:6f:17。内核现在叫 eth0（不再用 enxXXXXXXXXXXXX）。
# 本机 <PC_IP> ↔ <左臂SN>(左89)=.101 / <右臂SN>(右91)=.100
# NM 配置名 robot-usb-lan（静态 .10/24，不当默认路由）。
# 说明：enp4s0 仍是办公网（192.168.1.x）；两台臂已不在 enp4s0 上。
prefer_iface: "enxXXXXXXXXXXXX"
prefer_iface: "eth0"
# 仿真平台（当前 Gazebo 尚未接线，详见 devices/sim/gazebo/README.md）
# 设计约束：仿真与真机共用同一套参数（servo.* / hole_detect.stage2.*），
docs/HANDBOOK.md
+23 -7
View File
@@ -68,10 +68,26 @@ bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview # T2 相机预览，
bash ~/ros2/scripts/run_all.sh --arm 89 # T3 三段串联（默认不拧入）
```
一阶段默认 **D405 主跟踪**：画面里没有 conf≥`perception.wrist_handoff_conf`（0.5）的目标，
马上用顶视 D435 **拍一张** 把工件 / k0 定到基座系（眼在手外 T，不再乘法兰），把臂迁过去
让 D405 重新看见。D405 跟住后，左臂法兰 XY 连续 `servo.lock_still_sec`（3 s）位移不超过
`lock_still_move_m`（15 mm）→ `/visual_servo/locked` → 二阶段对孔。
一阶段默认 **D405 实时推理**，D435 待命。连续 `D405_MISS_BEFORE_D435_S`（0.5 s）没有
conf≥`perception.wrist_handoff_conf`（0.5）的目标，才用顶视 D435 **拍一张** 把工件 / k0
定到基座系（眼在手外 T，不再乘法兰），左臂按二阶段同一套 **`FollowJointTrajectory` action**
先收齐锁高，再把全部 XY 串成一条关节折线，余弦加速（100 mm）/
巡航封顶 60 mm/s / 余弦减速（80 mm），头尾速度为 0。中途不重测、不停住。
40 mm 爬到 80 mm/s 会在真机上像弹射，那是加速段太短，不是初速度或角速度。
工件上方（剩差 ≤80 mm）巡航改 30 mm/s，只走 XY，不混 Z、不夹收垂直。
结果 + 0.25 s 再重测 TF（带符号 `dxy`）。冻结 J 若还剩 >15 mm，只允许 **一刀补**
（`tag=fix`）。XY 进 15 mm 门槛后再单独收法兰朝下。结果回来之前不往话题插点。
到位成功不再发话题 hold。几乎没走近就钉住，不许按同一剩差再刷。
走到该点并且法兰 XY 连续 `servo.lock_still_sec`（3 s）位移不超过
`lock_still_move_m`（15 mm）之前，D405 的点不下发（推理不关）。停稳之后改回 D405
跟踪；再按同一静止判据 → `/visual_servo/locked` → 二阶段对孔。
只测 D435 这一段位移时加 `--d435-only`：D405 点不下发，到位停稳后钉住并结束一阶段，
不交 D405、不进二阶段。眼外标定误差只会让冻结点整体偏一截，不会造成中途启停；
启停来自以前按 80/15 mm 一跳一跳 rest-to-rest。
```bash
bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera --d435-only
```
```bash
bash ~/ros2/scripts/run_real.sh --arm 89
@@ -93,7 +109,7 @@ bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control
| 按螺距旋入（螺纹规） | ✅ 有参数，`--depth`/`--pitch` 控制 | `run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0` |
| Orisys 双指触觉（法向 + 两路切向） | ⚠️ 能读，但**轴向映射尚未实测确认** | 见 [`TACTILE.md`](TACTILE.md) |
| Orisys 的深度图 / 力矢量图 | ⚠️ 消息结构与话题已在，真机值待确认 | 同上 |
| D435 手外相机（一阶段一次性 + S03） | ⚠️ **89 已标定**；一阶段 D405 看不见时拍一张定 K0（混合跟踪）；S03 粗定位真机未对过 | `handeye/results/89_d435_eye_to_hand.yaml` |
| D435 手外相机（一阶段一次性 + S03） | ⚠️ 89 已按 `tool10120938` 重标（24 视角，10-09 旧结果在 `results/backup/`）；S03 粗定位真机未对过 | `handeye/results/89_d435_eye_to_hand.yaml` |
| 自动启动全部服务（一条命令拉起所有进程） | ❌ 还没做，目前靠三个终端手工起 | 见第 6 节待办 |
| 「规有没有被夹住」自检 | ❌ 还没做 | 见第 6 节待办 |
| 装配合格判定（通/止） | ⚠️ **通止规装配流程**能按力矩判据拧到底/拧出来，但"通/止合格"结论还没做 | `run_gauge_assembly.sh --arm 89 --dry-run` |
@@ -485,7 +501,7 @@ bash scripts/run_unit_tests.sh -k gauge # 只跑名字带 gauge 的
|----|------|
| **通止规** | 检验螺纹的量具。通端应能旋到底，止端应旋不进（或只进几圈） |
| **89 / 91** | 左臂 / 右臂的现场编号，**各有独立的手眼、相机、夹爪、锁高，禁止混用** |
| **法兰** | 机械臂末端安装面；手眼标定求的就是相机→法兰的 `T_4x4` |
| **法兰** | 机械臂末端安装面；手眼 `T_4x4` 仍是相机→法兰。左臂 89 现行按 Studio `tool10120938` 的 TCP 求解，再折回法兰 |
| **eye-in-hand / eye-to-hand** | 相机装在手上 / 装在手上之外（D405 在手上，D435 在外部） |
| **stage1/2/3** | 跟踪 / 对孔 / 搜孔，见 1.3 |
| **S03 / S04 / S07 / S08** | 整体装配任务书里的阶段编号（S03 粗定位、S04 跟踪、S07 柔顺下压、S08 夹爪旋转循环）。代码里对应的是 stage1/2/3 的拆分实现，**没有一一对应**，别硬套 |
@@ -501,7 +517,7 @@ bash scripts/run_unit_tests.sh -k gauge # 只跑名字带 gauge 的
且顺序错了会出"控制器没起来但脚本继续跑"这类问题。
2. **夹爪持规自检** —— 启动后先用触觉判断"规真的被夹着"，没有就报错停机
（现在没有这一步，规掉了会一路跑到撞工件才发现）。
3. **D435 手眼标定（91）** —— 89 已写入 `handeye/results/89_d435_eye_to_hand.yaml`（18 视角）。91 还没标。S03 粗定位要用这份 TF 在真机上对一次工件才算链路验过。
3. **D435 手眼标定（91）** —— 89 按 `tool10120938` 重标（旧 10-09 / 18 视角在 `handeye/results/backup/`）。91 还没标。S03 粗定位要用这份 TF 在真机上对一次工件才算链路验过。
4. **装配合格判定** —— 把力曲线转成"通/止"结论。当前只输出原始几何与力，
判定逻辑（含螺纹旋入圈数）还没写。
docs/HANDEYE.md
+32 -18
View File
@@ -4,8 +4,8 @@
| 模式 | 相机 | 板 | 板怎么摆 | 结果文件 | `T_4x4` 含义 |
|------|------|----|----------|----------|--------------|
| `eye_in_hand` | 腕部 D405 | A5，内角点 8×11，15 mm | 板放台面不动 | `handeye/results/<臂号>_eye_in_hand.yaml` | 相机 → 法兰 |
| `eye_to_hand` | 顶置 D435 | A4，内角点 11×8，20 mm | **板装到法兰上** | `handeye/results/<臂号>_d435_eye_to_hand.yaml` | 相机 → 这条臂的基座 |
| `eye_in_hand` | 腕部 D405 | A5，内角点 11×8，15 mm | 板放台面不动 | `handeye/results/<臂号>_eye_in_hand.yaml` | 相机 → 法兰（89 按 `tool10120938` TCP 求解后再折算） |
| `eye_to_hand` | 顶置 D435 | A4，内角点 11×8，20 mm | **板夹在 `tool10120938` 上** | `handeye/results/<臂号>_d435_eye_to_hand.yaml` | 相机 → 这条臂的基座 |
| 臂 | 眼在手上结果 | 眼在手外结果 | 示教关节 | 工作目录 |
|----|--------------|--------------|----------|----------|
@@ -18,9 +18,19 @@
## 眼在手上（腕部 D405 + A5 板）
左臂 89 现行工具是 Studio **`tool10120938`**（2026-10-12 示教的 TCP）。
`run_real.sh --arm 89` 会切到这个工具；标定用 `/tcp_pose`（即该 TCP）做
AX=XB，再左乘 `T_flange_to_tool` 折成 **相机 → 法兰** 写入 `T_4x4`
（伺服 / 静态 TF 的父系仍是法兰）。旧的 2026-09-04 / tool1 结果在
`handeye/results/backup/89_eye_in_hand_flange_tool1_20260904.yaml`。
`teach`（RDK 软拖拽）与 `run_real.sh` **互斥**。全程带同一 `--arm`。
板：A5，内角点 **11×8**，方格 15 mm，放台面后不要动。
```bash
# 0) 确认配置（应看到 tool10120938 与 11x8 / 0.015 m）
bash ~/ros2/scripts/run_handeye.sh --arm 89 show
# 1) 停掉 run_real.sh 和视觉伺服
# 2) 示教 8–12 个姿态（棋盘清晰、占画面 30–70%，姿态拉开）
bash ~/ros2/scripts/run_handeye.sh --arm 89 teach
@@ -30,7 +40,7 @@ bash ~/ros2/scripts/run_handeye.sh --arm 89 teach
# 3) 写入该臂 handeye/poses/89_eye_in_hand.yaml
bash ~/ros2/scripts/run_handeye.sh --arm 89 merge
# 4) 再开实机 + 腕部相机，棋盘不要动
# 4) 再开实机（会切到 tool10120938）+ 腕部相机，棋盘不要动
bash ~/ros2/scripts/run_real.sh --arm 89
bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
bash ~/ros2/scripts/run_handeye.sh --arm 89 calibrate
@@ -43,32 +53,36 @@ bash ~/ros2/scripts/run_handeye.sh --arm 89 calibrate
物理摆法与上面**反过来**：相机固定，板跟着臂动。板若还摆在台面上，
`target→camera` 不变，方程退化（解不出来或解出垃圾）。
左臂 89 的“手”同样是 **`tool10120938`**：把 A4 板**刚性夹在该工具的夹爪里**
（夹正、标定过程不要松）。`run_real.sh` 会切到这个工具，AX=XB 用 `/tcp_pose`。
`T_4x4` 仍是 **相机 → 这条臂的基座**，**不要**再乘 `T_flange_to_tool`
（相机和基座都在世界里不动）。2026-10-09 那份在
`handeye/results/backup/89_d435_eye_to_hand_20261009.yaml`。
板：A4，内角点 **11×8**，方格 20 mm（12×9 格）。印面朝上对着 D435。
```bash
# 1) 把 A4 板刚性固定在法兰 / 夹爪上（夹在正中间，标定过程不要松）
# 2) 起臂 + 顶置相机（两个终端，常驻）
bash ~/ros2/scripts/run_real.sh --arm 89
bash ~/ros2/scripts/run_camera_d435.sh --preview
# 0) 确认配置（应看到 d435、11x8 / 0.02 m、89_d435_eye_to_hand.yaml、tool10120938）
bash ~/ros2/scripts/run_handeye.sh --arm 89 show --mode eye_to_hand
# 3) 采点：推着臂走，让板在 D435 画面里出现在不同位置、不同角度
# teach 与 run_real.sh 互斥 —— 用 RDK 拖拽前先停掉 run_real.sh
# 1) 夹好 A4 板。停掉 run_real.sh、视觉伺服、腕部 D405（RDK + USB 独占）
# 2) 示教 8–12 个姿态：板在 D435 画面里换位置、换倾角，占画面 30–70%
bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --mode eye_to_hand --points 12
# 窗口：d=拖拽 Enter=拍照 q=结束（窗口只显示画面，不做棋盘检测）
# 4) 合并 + 自动走点求解（此时再开 run_real.sh + D435）
# 3) 写入 handeye/poses/89_eye_to_hand.yaml（不要手抄眼在手上那份）
bash ~/ros2/scripts/run_handeye.sh --arm 89 merge --mode eye_to_hand
# 4) 再开实机（会切到 tool10120938）+ 顶置 D435，板不要松
bash ~/ros2/scripts/run_real.sh --arm 89
bash ~/ros2/scripts/run_camera_d435.sh --preview
bash ~/ros2/scripts/run_handeye.sh --arm 89 calibrate --mode eye_to_hand
```
查看当前模式选中的板 / 相机 / 文件：
`show` 必须同时看到 `camera=d435`、`board 11x8 inner, square=0.02 m`、
`89_d435_eye_to_hand.yaml`、`tool10120938`。不是这四样就停。
```bash
bash ~/ros2/scripts/run_handeye.sh --arm 89 show --mode eye_to_hand
```
应看到 `camera=d435`、`board 11x8 inner, square=0.02 m`、
结果路径 `handeye/results/89_d435_eye_to_hand.yaml`。不是这三样就停，
不要带着错配置开标定。
**不要**改 `paths.handeye_file` 去指向这份结果 —— 那会把腕部 D405 的 TF 换掉。
## 结果怎么用
handeye/README.md
+4 -4
View File
@@ -65,8 +65,8 @@ handeye/
| 模式 | 物理板 | 内角点（列 × 行） | 方格边长 | `site.yaml` 里的位置 |
|------|--------|-------------------|----------|----------------------|
| `eye_in_hand` | A5（腕部相机看台面） | 8 × 11 | 15 mm (`0.015`) | `handeye.boards.eye_in_hand` |
| `eye_to_hand` | A4（板装在法兰上） | 11 × 8 | 20 mm (`0.020`) | `handeye.boards.eye_to_hand` |
| `eye_in_hand` | A5（腕部相机看台面） | 11 × 8 | 15 mm (`0.015`) | `handeye.boards.eye_in_hand` |
| `eye_to_hand` | A4（板夹在 `tool10120938` 上） | 11 × 8 | 20 mm (`0.020`) | `handeye.boards.eye_to_hand` |
权威只有 `config/site.yaml` 的 `handeye.boards.<模式>`。`run_handeye.sh` 按
`--mode` 取对应那一组，用 `-p` 传给节点。`handeye/calibrate.yaml` 和节点默认值
@@ -74,8 +74,8 @@ handeye/
尺寸去标（那会让结果系统性偏，还不报错）。
怎么看内角点：把棋盘当成一张网格，**数内部的黑白交叉点**，
是「方格数 − 1」。A5 板上画了 9×12 个格子 → 内角点 8×11；
A4 板上画了 12×9 个格子 → 内角点 11×8。列/行顺序不用纠结，检测端两种顺序都会试。
是「方格数 − 1」。A5 板上画了 12×9 个格子 → 内角点 11×8；
A4 板上画了 12×9 个格子 → 内角点 11×8（方格 20 mm）。列/行顺序不用纠结，检测端两种顺序都会试。
> ⚠️ **历史上这里出过问题**：曾经三处分别写着 `9×6/0.025`、`11×16/0.011`、
> `8×11/0.015`，谁生效取决于覆盖顺序。后来又把两块板合成一组键，于是标眼在手外
handeye/calibrate.yaml
+3 -3
View File
@@ -49,11 +49,11 @@ handeye_calibrate:
camera_dist: [0.0, 0.0, 0.0, 0.0, 0.0]
# 板尺寸的权威在 site.yaml → handeye.boards.<模式>。
# 这里只写眼在手上那块板的兜底（A5 / 8×11 / 15mm）；run_handeye.sh 会按
# 这里只写眼在手上那块板的兜底（A5 / 11×8 / 15mm）；run_handeye.sh 会按
# --mode 用 -p 覆盖成对应模式的值。没配过的模式会被脚本当场拦下，
# 不会静默用这份兜底去标另一块板。
board_cols: 8
board_rows: 11
board_cols: 11
board_rows: 8
square_size_m: 0.015
settle_sec: 0.4
handeye/poses/89_eye_in_hand.yaml
+43 -25
View File
@@ -8,31 +8,49 @@
/**:
ros__parameters:
num_poses: 12
num_poses: 21
# 2026-09-02 通过 handeye_teach 示教（RDK 自由驱动）
waypoint_positions: [
# 位姿 0
-0.751865, -0.769544, 0.713051, 1.140021, -0.189690, 0.133294, -0.342184,
# 位姿 1
-0.541318, -0.867448, 0.864868, 1.233428, -0.348623, 0.146046, -0.249719,
# 位姿 2
-0.343594, -0.948720, 0.848287, 1.273279, -0.954339, 0.019624, 0.071314,
# 位姿 3
-0.379884, -1.002193, 1.166394, 1.579194, -1.097644, -0.115142, -0.347640,
# 位姿 4
-0.694826, -0.943610, 0.820178, 0.963907, -0.427763, 0.165888, -0.761562,
# 位姿 5
-0.964023, -1.025531, 1.013898, 1.032490, -0.167151, 0.017429, -0.566073,
# 位姿 6
-0.812664, -0.984951, 1.072695, 1.473414, -0.772970, 0.267382, 0.401278,
# 位姿 7
-0.967527, -1.067191, 1.101680, 1.434183, -0.915388, 0.259486, 0.205330,
# 位姿 8
-0.451483, -1.257408, 0.895481, 0.806594, -0.652401, -0.221453, 0.184234,
# 位姿 9
-0.981877, -1.415295, 1.339004, 1.131348, -0.863194, 0.170952, 1.615403,
# 位姿 10
-0.967163, -1.377051, 1.337671, 1.399175, -1.241886, 0.284547, 1.927756,
# 位姿 11
-0.575271, -1.141574, 1.121935, 1.502172, -1.452029, 0.088544, 2.200019,
# pose 0
-0.928421, -1.237477, 1.528823, 1.526334, -1.230146, 0.098562, 0.538531,
# pose 1
-0.970203, -1.315554, 1.553230, 1.669383, -1.407869, 0.125907, 0.578173,
# pose 2
-0.967819, -1.525572, 1.706057, 1.357062, -1.211051, 0.023232, 0.193541,
# pose 3
-1.065106, -1.624218, 1.793644, 1.279402, -1.120929, 0.019151, 0.139658,
# pose 4
-0.386279, -1.551838, 1.563578, 1.552444, -1.775337, -0.341974, 0.957445,
# pose 5
-0.227779, -1.219414, 1.026882, 0.828801, -0.916809, -0.166985, 0.034154,
# pose 6
-0.324968, -1.273082, 1.092344, 0.723132, -0.922095, -0.206413, 0.152082,
# pose 7
-0.421966, -1.309101, 1.143065, 0.606456, -0.906673, -0.219282, 0.237953,
# pose 8
-0.851598, -1.260600, 1.302895, 1.127033, -0.969809, 0.032856, 0.642673,
# pose 9
-0.951418, -0.550124, 0.848598, 2.139787, -1.025778, 1.184418, 0.598394,
# pose 10
-0.834498, -0.635767, 0.970038, 2.058410, -1.035138, 1.032040, 0.738619,
# pose 11
-0.519528, -0.883321, 1.135035, 1.830274, -1.100901, 0.234065, -0.069068,
# pose 12
-0.347358, -1.081426, 1.192233, 1.514557, -1.244594, -0.116668, -0.564362,
# pose 13
-0.364832, -1.102689, 1.224844, 1.520320, -1.265760, -0.136855, -0.582459,
# pose 14
-1.130796, -1.316186, 1.711089, 1.821409, -1.278003, 0.010885, -1.337210,
# pose 15
-0.732222, -1.376363, 1.812339, 1.293854, -1.410826, -0.226771, -0.217230,
# pose 16
-0.885017, -1.493416, 1.894070, 1.379021, -1.520015, -0.257115, 1.025331,
# pose 17
-0.911769, -1.532695, 1.944123, 1.427787, -1.560513, -0.267425, 1.004398,
# pose 18
-1.436271, -1.768595, 1.559397, 2.244717, -2.057112, 0.341785, 0.749883,
# pose 19
-1.511828, -1.759337, 1.620075, 2.131574, -1.946595, 0.328227, 0.584657,
# pose 20
-1.180482, -1.643241, 1.865851, 1.597699, -1.464982, 0.097605, -0.217780,
]
handeye/poses/89_eye_to_hand.yaml
+31 -21
View File
@@ -1,4 +1,4 @@
# 眼在手外示教关节（7 关节 × N）。板装在法兰上，顶置 D435 看它。
# 眼在手外示教关节（7 关节 × N）。板夹在 tool10120938 上，顶置 D435 看它。
#
# 由 teach → merge 写入，不要手抄眼在手上那份（两种模式的姿态完全不同）。
#
@@ -7,44 +7,54 @@
/**:
ros__parameters:
num_poses: 19
num_poses: 24
waypoint_positions: [
# pose 0
-1.045969, -0.575935, 0.132913, 2.251024, -0.297265, 1.274437, -0.219817,
-1.241563, -0.359631, 0.346153, 2.172565, -0.179027, 0.929920, -0.778876,
# pose 1
-1.221272, -0.582225, 0.269777, 2.060852, -0.180051, 1.157081, -0.070498,
-1.388832, -0.554698, 0.416075, 1.897617, -0.358341, 0.827495, -0.762302,
# pose 2
-1.222672, -0.605722, 0.273858, 2.111021, -0.181634, 1.193047, -0.059758,
-1.436048, -0.573046, 0.461762, 1.870967, -0.387696, 0.813090, -0.762917,
# pose 3
-1.219127, -0.576923, 0.281368, 2.310613, -0.162188, 1.339573, -0.026065,
-1.614491, -0.519365, 0.693991, 2.156193, -0.499113, 0.968762, 0.073287,
# pose 4
-1.100853, -0.552692, 0.291246, 2.473326, -0.116403, 1.504852, 0.051283,
-2.020087, -0.869088, 1.059166, 2.155478, -1.198473, 0.740567, 0.478840,
# pose 5
-1.022050, -0.394038, 0.375788, 2.311711, -0.503617, 1.217768, 1.268088,
-2.082145, -1.122210, 1.203011, 1.950272, -1.269847, 0.434138, -0.641975,
# pose 6
-1.180556, -0.574370, 0.471095, 2.055400, -0.490664, 1.086456, 1.216527,
-1.983153, -0.886082, 0.999329, 2.324821, -1.272773, 0.754127, 0.313613,
# pose 7
-2.039707, -0.492556, 1.462952, 2.222064, -0.329990, 0.209789, 0.523258,
-2.021479, -0.886609, 1.055679, 2.337716, -1.282773, 0.669319, -0.028355,
# pose 8
-1.959748, -0.554596, 1.469405, 2.293419, -0.165008, 0.422761, 0.844081,
-2.022792, -0.947211, 1.118194, 2.127275, -1.211995, 0.527528, -0.169900,
# pose 9
-1.666347, -0.747468, 1.236192, 2.176000, -0.303646, 0.773895, 1.279040,
-2.004766, -1.065820, 1.183799, 1.901084, -1.217488, 0.380285, -0.312733,
# pose 10
-1.689100, -0.879982, 1.168013, 2.057620, -0.424417, 0.752234, 1.241290,
-1.942863, -1.219617, 1.256733, 1.653113, -1.222302, 0.261818, -0.420706,
# pose 11
-1.691875, -0.889908, 1.154014, 2.062930, -0.549154, 0.850985, 1.257704,
-2.039558, -1.082441, 1.189833, 1.996967, -1.241252, 0.521591, 0.526563,
# pose 12
-1.715420, -0.986765, 1.125361, 1.948461, -0.638012, 0.809355, 1.173290,
-2.098074, -1.006236, 1.538183, 2.118382, -0.737422, 0.019903, 0.648802,
# pose 13
-1.729057, -1.004106, 1.126932, 1.961482, -0.727920, 0.900058, 1.188069,
-2.039609, -1.159222, 1.488501, 1.908579, -1.168793, -0.140451, 0.034889,
# pose 14
-2.034166, -0.748356, 1.234076, 2.252718, -1.542249, 0.093972, 0.640260,
-1.880783, -1.104601, 1.579382, 2.036427, -0.665049, 0.254105, 1.518504,
# pose 15
-2.155450, -0.769613, 1.249491, 2.275528, -1.648348, 0.073256, 0.495109,
-2.015530, -1.535468, 1.711220, 1.836908, -1.707450, -0.400234, -0.708683,
# pose 16
-1.959788, -0.979802, 1.271261, 2.222882, -1.560991, 0.057204, 0.296625,
-1.997672, -1.626362, 1.529443, 2.200206, -1.291013, 0.398664, 1.954208,
# pose 17
-1.933247, -0.398944, 1.211885, 2.312756, -0.424981, 0.358678, 0.347203,
-1.981875, -1.620651, 1.545443, 2.373140, -1.167365, 0.396487, 2.012547,
# pose 18
-1.944530, -0.399220, 1.244635, 2.293245, -0.423596, 0.317991, 0.358565,
-2.179415, -1.511968, 1.578832, 2.401706, -1.326480, -0.051353, 1.137181,
# pose 19
-2.173463, -1.530413, 1.575534, 2.173673, -1.289673, -0.090117, 0.867214,
# pose 20
-2.096314, -1.569352, 1.579820, 1.942057, -1.458598, -0.057003, 0.572953,
# pose 21
-2.129376, -1.566787, 1.903585, 1.962858, -1.454137, -0.335990, -0.148441,
# pose 22
-2.020280, -1.527165, 1.887130, 1.728957, -1.491867, -0.370585, -0.277940,
# pose 23
-2.140817, -1.607357, 1.617236, 2.153850, -1.571057, -0.147540, 0.770535,
]
handeye/results/89_d435_eye_to_hand.yaml
+17 -8
View File
@@ -1,13 +1,22 @@
# 手眼标定结果
# mode: eye_to_hand
# camera_source: d435
# views: 18
# utc: 2026-10-09T02:56:24.621687+00:00
# views: 24
# utc: 2026-10-12T05:20:30.137123+00:00
# board: 11x8 square_m=0.02
# T_cam_to_base (eye_to_hand)
translation_m: [0.37495201, -0.52156937, 0.72589194]
T_4x4:
- [-0.04575158, 0.99819969, -0.03878355, 0.37495201]
- [0.99893621, 0.04594039, 0.00399073, -0.52156937]
- [0.00576527, -0.03855971, -0.99923967, 0.72589194]
# gripper_tool: tool10120938
# T_4x4 = cam→基座（AX=XB 的手是 TCP，不折到法兰）
# 旧结果备份在 results/backup/
gripper_tool: tool10120938
T_flange_to_tool:
- [1.00000000, 0.00000000, 0.00000000, -0.00050000]
- [0.00000000, 1.00000000, 0.00000000, 0.00670000]
- [0.00000000, 0.00000000, 1.00000000, 0.22450000]
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
# T_cam_to_base (eye_to_hand)
translation_m: [0.37094529, -0.52805928, 0.73745585]
T_4x4:
- [-0.00990083, 0.99909706, -0.04131632, 0.37094529]
- [0.99991380, 0.01024832, 0.00820703, -0.52805928]
- [0.00862304, -0.04123150, -0.99911241, 0.73745585]
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
handeye/results/89_eye_in_hand.bak_20260904_ee-was-tool1.yaml
+18
View File
@@ -0,0 +1,18 @@
# 【备份】2026-10-12 从 89_eye_in_hand.yaml 挪出。
# 这是 2026-09-04 那份：AX=XB 的 gripper 用的是 GetArmState.ee_pose
# （当时激活工具是 tool1，不是法兰，也不是后来的 tool10120938）。
# 棋盘当时按 8x11 / 15mm 写。新标定不要覆盖本文件。
# 左臂 89 eye-in-hand. 不要用于 91。
# 手眼标定结果
# mode: eye_in_hand
# camera_source: d405
# views: 12
# utc: 2026-09-04T10:15:27.480727+00:00
# board: 8x11 square_m=0.015
# T_cam_to_gripper (eye_in_hand)
translation_m: [-0.06085181, -0.00864320, 0.12364093]
T_4x4:
- [0.01784499, -0.86878381, 0.49486994, -0.06085181]
- [0.99977570, 0.02115147, 0.00108124, -0.00864320]
- [-0.01140659, 0.49473964, 0.86896638, 0.12364093]
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
handeye/results/89_eye_in_hand.yaml
+25 -11
View File
@@ -1,14 +1,28 @@
# 左臂 89 eye-in-hand. 不要用于 91。
# 手眼标定结果
# mode: eye_in_hand
# camera_source: d405
# views: 12
# utc: 2026-09-04T10:15:27.480727+00:00
# board: 8x11 square_m=0.015
# T_cam_to_gripper (eye_in_hand)
translation_m: [-0.06085181, -0.00864320, 0.12364093]
T_4x4:
- [0.01784499, -0.86878381, 0.49486994, -0.06085181]
- [0.99977570, 0.02115147, 0.00108124, -0.00864320]
- [-0.01140659, 0.49473964, 0.86896638, 0.12364093]
# camera_source: d405_left
# views: 21
# utc: 2026-10-12T03:29:51.243487+00:00
# board: 11x8 square_m=0.015
# gripper_tool: tool10120938
# T_4x4 = cam→法兰（AX=XB 在 TCP 上解，再左乘 T_flange_to_tool）
# T_cam_to_tool 是原始求解；旧的法兰/tool1 备份在 results/backup/
# 2026-10-12 第一次写出的 T_4x4 乘序反了（tz≈-296 mm），已按同一份 T_cam_to_tool 重折。
gripper_tool: tool10120938
T_flange_to_tool:
- [1.00000000, 0.00000000, 0.00000000, -0.00050000]
- [0.00000000, 1.00000000, 0.00000000, 0.00670000]
- [0.00000000, 0.00000000, 1.00000000, 0.22450000]
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
T_cam_to_tool:
- [-0.03435184, -0.86951953, 0.49270249, -0.05961995]
- [0.99932767, -0.03620511, 0.00577974, -0.01523625]
- [0.01281276, 0.49256977, 0.87017863, -0.09761517]
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
# T_cam_to_flange (eye_in_hand，由工具 TCP 折算)
translation_m: [-0.06011995, -0.00853625, 0.12688483]
T_4x4:
- [-0.03435184, -0.86951953, 0.49270249, -0.06011995]
- [0.99932767, -0.03620511, 0.00577974, -0.00853625]
- [0.01281276, 0.49256977, 0.87017863, 0.12688483]
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
handeye/results/backup/89_d435_eye_to_hand_20261009.yaml
+19
View File
@@ -0,0 +1,19 @@
# 备份：不要当现行结果用。
# 原文件：handeye/results/89_d435_eye_to_hand.yaml
# 备份原因：2026-10-12 起左臂 89 眼在手外改按 Studio 工具 tool10120938
# 的 TCP 做 AX=XB（板夹在该工具上）。本份是 2026-10-09、18 视角、
# A4 11×8 / 20 mm 的结果，当时激活工具未经这次对齐。
# T_4x4 仍是 cam→基座。现行结果仍写回 89_d435_eye_to_hand.yaml。
# 手眼标定结果
# mode: eye_to_hand
# camera_source: d435
# views: 18
# utc: 2026-10-09T02:56:24.621687+00:00
# board: 11x8 square_m=0.02
# T_cam_to_base (eye_to_hand)
translation_m: [0.37495201, -0.52156937, 0.72589194]
T_4x4:
- [-0.04575158, 0.99819969, -0.03878355, 0.37495201]
- [0.99893621, 0.04594039, 0.00399073, -0.52156937]
- [0.00576527, -0.03855971, -0.99923967, 0.72589194]
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
handeye/results/backup/89_eye_in_hand_flange_tool1_20260904.yaml
+20
View File
@@ -0,0 +1,20 @@
# 备份：不要当现行结果用。
# 原文件：handeye/results/89_eye_in_hand.yaml
# 备份原因：2026-10-12 起左臂 89 眼在手上改按 Studio 工具 tool10120938 求解。
# 本份是 2026-09-04 那次标定：AX=XB 的 gripper 按当时激活的 tool1 / 法兰口径，
# 板为内角点 8×11、方格 15 mm。T_4x4 当时标注为 T_cam_to_gripper。
# 现行结果仍写回 89_eye_in_hand.yaml；要比对旧外参请打开本文件。
# 左臂 89 eye-in-hand. 不要用于 91。
# 手眼标定结果
# mode: eye_in_hand
# camera_source: d405
# views: 12
# utc: 2026-09-04T10:15:27.480727+00:00
# board: 8x11 square_m=0.015
# T_cam_to_gripper (eye_in_hand)
translation_m: [-0.06085181, -0.00864320, 0.12364093]
T_4x4:
- [0.01784499, -0.86878381, 0.49486994, -0.06085181]
- [0.99977570, 0.02115147, 0.00108124, -0.00864320]
- [-0.01140659, 0.49473964, 0.86896638, 0.12364093]
- [0.00000000, 0.00000000, 0.00000000, 1.00000000]
scripts/fix_robot_lan.sh
+23 -1
View File
@@ -42,7 +42,29 @@ pick_iface() {
echo "${SITE_NET_IFACE}"
return
fi
for d in enp4s0 enp5s0 eth0; do
# 内核有时把 USB 网卡叫 eth0，不再用 enx+MAC。按 MAC 找回，禁止误选办公网 enp4s0。
local want="${SITE_NET_IFACE:-}"
if [[ "${want}" == enx* && ${#want} -ge 15 ]]; then
local mac
mac="$(printf '%s' "${want#enx}" | sed 's/../&:/g;s/:$//')"
local d
for d in /sys/class/net/*; do
[[ -f "${d}/address" ]] || continue
if [[ "$(cat "${d}/address")" == "${mac}" ]]; then
echo "$(basename "$d")"
return
fi
done
fi
local d
for d in /sys/class/net/*; do
[[ -f "${d}/address" ]] || continue
if ip -4 -o addr show "$(basename "$d")" 2>/dev/null | grep -q '192.168.2.10'; then
echo "$(basename "$d")"
return
fi
done
for d in eth0 enp4s0 enp5s0; do
[[ -d /sys/class/net/$d ]] || continue
echo "$d"
return
scripts/lib/export_site_env.py
+11
View File
@@ -627,6 +627,13 @@ def flatten(data: dict) -> dict:
# 两个模式各自的名字在 *_EYE_IN_HAND / *_EYE_TO_HAND 里。
"SITE_HANDEYE_FILE": he_modes_out["SITE_HANDEYE_FILE_EYE_IN_HAND"],
"SITE_HANDEYE_MODE": he_mode,
"SITE_HANDEYE_GRIPPER_TOOL": str(he.get("gripper_tool") or ""),
"SITE_HANDEYE_GRIPPER_TCP_XYZ": ",".join(
f"{float(v):.6f}" for v in list(he.get("gripper_tcp_xyz") or [])
),
"SITE_HANDEYE_GRIPPER_TCP_QWXYZ": ",".join(
f"{float(v):.6f}" for v in list(he.get("gripper_tcp_qwxyz") or [])
),
**he_modes_out,
# 不带后缀的板尺寸 = **当前模式**那一组，供 `show` 之类只看当前模式的调用方；
# 标定脚本用带后缀的那三组（按 --mode 选），不依赖这个。
@@ -985,6 +992,10 @@ def main() -> int:
print(f" 手眼 : {flat['SITE_HANDEYE_FILE']}")
print(f" 手眼路点 : {flat['SITE_HANDEYE_WAYPOINTS']}")
print(f" 手眼目录 : {flat['SITE_HANDEYE_WORKDIR']}")
print(
f" 手眼工具 : {flat.get('SITE_HANDEYE_GRIPPER_TOOL') or '（无，ee_pose 原样）'} "
f"tcp=[{flat.get('SITE_HANDEYE_GRIPPER_TCP_XYZ') or ''}]"
)
print(f" 命名位姿 : {flat.get('SITE_NAMED_POSES_FILE')}")
print(f" 眼在手外结果 : {flat['SITE_HANDEYE_FILE_EYE_TO_HAND']}")
print(f" 眼在手外路点 : {flat['SITE_HANDEYE_WAYPOINTS_EYE_TO_HAND']}")
scripts/lib/rdk_helpers.py
+72
View File
@@ -23,6 +23,7 @@ RDK 对同一台机器人是**独占访问**：同时有两个进程连着同一
from __future__ import annotations
import os
import time
from typing import Any, Dict, List, Optional
@@ -155,6 +156,77 @@ def tool_params(tool: Any, group: Any) -> Dict[str, Any]:
}
def iface_ipv4s(name: str) -> List[str]:
"""网卡名 → 其上的 IPv4。FastDDS 白名单只认地址，不认 ``eth0`` / ``enx…``。"""
import fcntl
import socket
import struct
import subprocess
name = (name or "").strip()
if not name:
return []
if name.count(".") == 3:
return [name]
addrs: List[str] = []
try:
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
ifreq = struct.pack("256s", name.encode("utf-8")[:15])
addrs.append(socket.inet_ntoa(fcntl.ioctl(s.fileno(), 0x8915, ifreq)[20:24]))
s.close()
except OSError:
pass
try:
out = subprocess.check_output(
["ip", "-4", "-o", "addr", "show", "dev", name],
text=True,
stderr=subprocess.DEVNULL,
)
for line in out.splitlines():
parts = line.split()
if "inet" in parts:
addrs.append(parts[parts.index("inet") + 1].split("/")[0])
except Exception:
pass
seen = set()
uniq: List[str] = []
for a in addrs:
if a and a not in seen:
seen.add(a)
uniq.append(a)
return uniq
def rdk_whitelist_lists(iface: str = "", pc_ip: str = "") -> List[List[str]]:
"""给 ``flexivrdk.Robot(sn, listen)`` 用的白名单候选。
只放 IPv4。把 ``eth0`` 这种网卡名塞进去，FastDDS 会把真地址全部滤掉，
症状是 ping 通、RDK 报 Failed to connect。
"""
lists: List[List[str]] = []
pc = (pc_ip or os.environ.get("SITE_PC_IP") or "").strip()
if pc.count(".") == 3:
lists.append([pc])
iface = (iface or "").strip()
if iface.count(".") == 3:
lists.append([iface])
elif iface:
lan = [a for a in iface_ipv4s(iface) if a.startswith("192.168.2.")]
if lan:
lists.append(lan)
all_ips = iface_ipv4s(iface)
if all_ips and all_ips != lan:
lists.append(all_ips)
seen = set()
out: List[List[str]] = []
for item in lists:
key = tuple(item)
if key not in seen:
seen.add(key)
out.append(item)
return out
def switch_tool(tool: Any, group: Any, target: str) -> None:
"""切换工具。失败抛异常。"""
if group is None:
scripts/merge_handeye_poses.py
+22 -2
View File
@@ -61,6 +61,16 @@ def parse_poses(path: Path) -> list[list[float]]:
return poses
def unique_poses(poses: list[list[float]], tol: float = 0.015) -> list[list[float]]:
"""关节几乎不动的连拍只留一条。阈值 0.015 rad ≈ 0.86°。"""
kept: list[list[float]] = []
for pose in poses:
if any(max(abs(a - b) for a, b in zip(pose, old)) < tol for old in kept):
continue
kept.append(pose)
return kept
def format_positions_block(poses: list[list[float]], indent: str = " ") -> str:
rows = [f"{indent}waypoint_positions: ["]
for i, q in enumerate(poses):
@@ -131,9 +141,19 @@ def main(argv: list[str]) -> int:
print(f"Not found: {dst}", file=sys.stderr)
return 3
poses = parse_poses(src)
if len(poses) < 5:
print(f"Need >=5 poses, got {len(poses)}", file=sys.stderr)
uniq = unique_poses(poses)
if len(uniq) < 5:
print(
f"Need >=5 unique poses, got {len(poses)} lines / {len(uniq)} unique",
file=sys.stderr,
)
return 4
if len(uniq) < len(poses):
print(
f"警告: {len(poses)} 条里只有 {len(uniq)} 个姿态不同，已丢掉连按重复",
file=sys.stderr,
)
poses = uniq
merge(dst, poses)
print(f"Merged {len(poses)} poses -> {dst}")
return 0
scripts/rdk_drag_bridge.py
+6 -2
View File
@@ -55,6 +55,7 @@ from rdk_helpers import ( # noqa: E402
clear_fault as rdk_clear_fault,
ensure_idle,
ensure_operational,
rdk_whitelist_lists,
resolve_tool_name,
single_arm_groups,
switch_tool,
@@ -228,8 +229,11 @@ def main() -> int:
robot = None
last_err = None
attempts = []
if iface:
attempts.extend([(sn, [iface]), (sn, [iface], True)])
# 白名单必须是 IPv4。传 eth0 / enx… 会被 FastDDS 整段滤掉，
# ping 通也会 Failed to connect。
for listen in rdk_whitelist_lists(iface):
log('白名单: ' + ', '.join(listen))
attempts.extend([(sn, listen), (sn, listen, True)])
attempts.append((sn,))
for args in attempts:
try:
scripts/rdk_goto_xyz.py
+204
View File
@@ -0,0 +1,204 @@
#!/usr/bin/env python3
"""用厂家 Flexiv RDK 的原语 MoveL，把当前工具尖走到世界系里的一个点。
必须用 RDK 自带的 Python 3.10，不能用 ROS 的 3.11：
# 先在跑 run_real.sh 的那个终端 Ctrl-C。RDK 同时只能有一个连接。
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_goto_xyz.py
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_goto_xyz.py --xyz 0.45 -0.30 0.40
不给 --xyz 时只打印当前工具尖坐标，不动臂。
这个脚本做的事，按顺序只有这些：
1. 用网卡上的 IPv4 连上序列号那台臂。网卡名（eth0）不能当白名单。
2. 清故障、使能，等到 operational。示教器要在 AUTO Remote，急停松开。
3. 读 states().tcp_pose。这 7 个数是
[x, y, z, qw, qx, qy, qz]，单位米，坐标系是机器人世界系（基座那套 WORLD）。
位置是当前 Studio 工具的 TCP（现在是 tool10120938 的尖），不是法兰，也不是相机。
4. 目标只换 x、y、z。姿态沿用现在的朝向，折成厂家 MoveL 要的欧拉角（度，ZYX）。
5. SwitchMode(NRT_PRIMITIVE_EXECUTION)，ExecutePrimitive("MoveL", ...)。
6. 轮询 primitive_states()["reachedTarget"]。到了再读一次 tcp_pose，印出还差多少毫米。
厂家示例：flexiv_rdk example_py/basics3_primitive_execution.py 第 (3) 段。
"""
from __future__ import annotations
import argparse
import math
import sys
import time
from pathlib import Path
ROOT = Path(__file__).resolve().parents[1]
sys.path.insert(0, str(ROOT / "scripts" / "lib"))
from rdk_helpers import rdk_whitelist_lists # noqa: E402
def site_robot() -> tuple[str, str]:
"""读 config/site.yaml 里第一台臂的序列号和这台电脑的机器人网 IP。"""
text = (ROOT / "config" / "site.yaml").read_text(encoding="utf-8")
sn = ""
pc_ip = ""
for raw in text.splitlines():
line = raw.split("#", 1)[0].strip()
if line.startswith("sn:") and not sn:
sn = line.split(":", 1)[1].strip().strip('"').strip("'")
elif line.startswith("pc_ip:") and not pc_ip:
pc_ip = line.split(":", 1)[1].strip().strip('"').strip("'")
if sn and pc_ip:
break
if not sn:
raise RuntimeError(f"config/site.yaml 里没有 sn：{ROOT / 'config' / 'site.yaml'}")
return sn, pc_ip
def quat_wxyz_to_euler_xyz_deg(q) -> list[float]:
"""[qw,qx,qy,qz] → MoveL 要的 [rot_x, rot_y, rot_z] 度。
与厂家 example_py/utility.py 的 quat2eulerZYX 相同：
scipy 的 from_quat([x,y,z,w]).as_euler('xyz', degrees=True)。
这里不 import scipy，避免和 ROS 环境的 scipy 打架。
"""
qw, qx, qy, qz = (float(v) for v in q)
n = math.sqrt(qw * qw + qx * qx + qy * qy + qz * qz)
if n < 1e-12:
raise RuntimeError(f"四元数是零：{q}")
qw, qx, qy, qz = qw / n, qx / n, qy / n, qz / n
xx, yy, zz = qx * qx, qy * qy, qz * qz
xy, xz, yz = qx * qy, qx * qz, qy * qz
wx, wy, wz = qw * qx, qw * qy, qw * qz
r00 = 1.0 - 2.0 * (yy + zz)
r10 = 2.0 * (xy + wz)
r20 = 2.0 * (xz - wy)
r21 = 2.0 * (yz + wx)
r22 = 1.0 - 2.0 * (xx + yy)
r01 = 2.0 * (xy - wz)
r11 = 1.0 - 2.0 * (xx + zz)
r12 = 2.0 * (yz - wx)
# R = Rx(x) @ Ry(y) @ Rz(z)
sy = math.sqrt(r00 * r00 + r10 * r10)
if sy > 1e-6:
x = math.atan2(r21, r22)
y = math.atan2(-r20, sy)
z = math.atan2(r10, r00)
else:
x = math.atan2(-r12, r11)
y = math.atan2(-r20, sy)
z = 0.0
return [math.degrees(x), math.degrees(y), math.degrees(z)]
def connect(flexivrdk, sn: str, pc_ip: str):
"""白名单只放 IPv4。连不上就换下一组，最后再试不带白名单。"""
last = None
attempts = []
for listen in rdk_whitelist_lists(pc_ip=pc_ip):
attempts.append((sn, listen))
attempts.append((sn,))
for args in attempts:
try:
print(f"连接 {args}")
return flexivrdk.Robot(*args)
except Exception as exc: # noqa: BLE001 — 下一组白名单再试，最后一组失败才抛
last = exc
print(f" 失败：{exc}")
raise RuntimeError(f"RDK 连不上 {sn}：{last}")
def wait_operational(robot, timeout_s: float) -> None:
if robot.fault():
print("有故障，尝试 ClearFault")
if not robot.ClearFault():
raise RuntimeError("故障清不掉。看示教器。")
print("Enable …（示教器：松开急停，AUTO Remote）")
robot.Enable()
t0 = time.monotonic()
while not robot.operational():
if time.monotonic() - t0 > timeout_s:
raise RuntimeError(f"{timeout_s:.0f}s 内没有 operational")
time.sleep(0.2)
print("operational")
def fmt_pose(pose) -> str:
x, y, z = pose[0], pose[1], pose[2]
return f"xyz=({x:.4f}, {y:.4f}, {z:.4f}) m qwxyz=({pose[3]:.4f}, {pose[4]:.4f}, {pose[5]:.4f}, {pose[6]:.4f})"
def main() -> int:
parser = argparse.ArgumentParser(description="RDK MoveL：工具尖走到世界系的一个点")
parser.add_argument("--xyz", nargs=3, type=float, metavar=("X", "Y", "Z"), help="目标点，米，机器人世界系")
parser.add_argument("--vel", type=float, default=0.05, help="TCP 线速度 m/s，默认 0.05")
parser.add_argument("--max-step-m", type=float, default=0.40, help="离当前点超过这么多米就拒绝，除非 --go")
parser.add_argument("--go", action="store_true", help="允许超过 --max-step-m")
parser.add_argument("--timeout", type=float, default=60.0, help="等 reachedTarget 的秒数")
args = parser.parse_args()
sn, pc_ip = site_robot()
print(f"臂 {sn} 本机 {pc_ip or '（site 里没有 pc_ip）'}")
import flexivrdk
robot = connect(flexivrdk, sn, pc_ip)
try:
wait_operational(robot, args.timeout)
pose = list(robot.states().tcp_pose)
if len(pose) < 7:
raise RuntimeError(f"tcp_pose 不是 7 个数：{pose}")
print("当前 TCP（世界系）：", fmt_pose(pose))
if args.xyz is None:
print("没有 --xyz，只看坐标，不动。")
return 0
target_xyz = [float(v) for v in args.xyz]
dist = math.dist(pose[:3], target_xyz)
print(f"目标 xyz=({target_xyz[0]:.4f}, {target_xyz[1]:.4f}, {target_xyz[2]:.4f}) m")
print(f"直线距离 {dist * 1000:.0f} mm 速度 {args.vel:.3f} m/s 姿态保持当前")
if dist > args.max_step_m and not args.go:
print(f"超过 {args.max_step_m * 1000:.0f} mm，拒绝。确认无误后加 --go。")
return 2
euler_deg = quat_wxyz_to_euler_xyz_deg(pose[3:7])
print(f"MoveL 欧拉角 deg xyz=({euler_deg[0]:.2f}, {euler_deg[1]:.2f}, {euler_deg[2]:.2f})")
robot.SwitchMode(flexivrdk.Mode.NRT_PRIMITIVE_EXECUTION)
robot.ExecutePrimitive(
"MoveL",
{
"target": flexivrdk.Coord(
target_xyz,
euler_deg,
["WORLD", "WORLD_ORIGIN"],
),
"vel": float(args.vel),
},
)
print("MoveL 已下发，等 reachedTarget …")
t0 = time.monotonic()
while True:
states = robot.primitive_states()
if states.get("reachedTarget"):
break
if robot.fault():
raise RuntimeError(f"运动中故障：{states}")
if time.monotonic() - t0 > args.timeout:
raise RuntimeError(f"{args.timeout:.0f}s 内没有 reachedTarget。states={states}")
time.sleep(0.2)
end = list(robot.states().tcp_pose)
err_mm = math.dist(end[:3], target_xyz) * 1000.0
print("到达 TCP：", fmt_pose(end))
print(f"相对目标还差 {err_mm:.1f} mm")
return 0
finally:
try:
robot.Stop()
except Exception:
pass
if __name__ == "__main__":
raise SystemExit(main())
scripts/rdk_switch_tool.py
+4 -20
View File
@@ -158,27 +158,11 @@ def _iface_ipv4s(name: str) -> list:
def _rdk_whitelist(iface: str) -> list[list[str]]:
"""RDK 手册要的是「连机器人的那张网卡」。enp4s0 上还有办公网 IP，不能整张卡的地址都塞进白名单。"""
import os
"""只放 IPv4。网卡名进白名单会被 FastDDS 滤掉（见 rdk_helpers）。"""
sys.path.insert(0, str(__import__("pathlib").Path(__file__).resolve().parent / "lib"))
from rdk_helpers import rdk_whitelist_lists # noqa: PLC0415
lists: list[list[str]] = []
pc = (os.environ.get("SITE_PC_IP") or "").strip()
if pc:
lists.append([pc])
if iface:
lists.append([iface])
robot_lan = [a for a in _iface_ipv4s(iface) if a.startswith("192.168.2.")]
if robot_lan:
lists.append(robot_lan)
# 去重
seen: set[tuple[str, ...]] = set()
out: list[list[str]] = []
for item in lists:
key = tuple(item)
if key not in seen:
seen.add(key)
out.append(item)
return out
return rdk_whitelist_lists(iface)
def connect_rdk(sn: str, iface: str = ""):
scripts/run_all.sh
+29 -9
View File
@@ -20,7 +20,8 @@
# bash ~/ros2/scripts/run_all.sh --arm 89 --camera-preview # 相机带 Qt 预览窗
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera # 相机已在别处拉起了
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-tactile # 不起触觉服务
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent # 只对准不轻降（验证精对准）
# bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent # 对准后只降到观测高度（默认离面 15mm）
# bash ~/ros2/scripts/run_all.sh --arm 89 --d435-only # 只测 D435 单次位移，到位后不交 D405、不进二阶段
#
# 前置（另开一个终端，先跑起来）：
# bash ~/ros2/scripts/run_real.sh --arm 89
@@ -43,6 +44,7 @@ WITH_CAMERA=true
WITH_TACTILE=true
CAMERA_PREVIEW=false
NO_DESCENT=false
D435_ONLY=false
# --sn/--arm 必须先解析：load_site_config.sh 靠 ROBOT_SN 决定加载哪套机器人配置
# shellcheck disable=SC1091
@@ -59,14 +61,15 @@ while [[ $# -gt 0 ]]; do
--depth) DEPTH="${2:?--depth 需要毫米数}"; shift 2 ;;
--pitch) PITCH="${2:?--pitch 需要螺距毫米数}"; shift 2 ;;
--resnap) RESNAP=true; shift ;;
# 只验证**精对准**、不轻降。对准逻辑改完先这么跑，确认规真被对到孔心再放开下降
# （对不准就下压会把规顶在孔沿上：2026-10-08 真机顶到 63N）。
# 不到贴面 2mm。对准后仍降到 site 的 observe_gap_mm（默认离面 15mm），
# 方便看规尖偏没偏；对不准就下到 2mm 会把规顶在孔沿上（2026-10-08 真机 63N）。
--no-descent) NO_DESCENT=true; shift ;;
--d435-only) D435_ONLY=true; SKIP_STAGE2=true; SKIP_STAGE3=true; shift ;;
--no-preview) PIPELINE_PREVIEW=false; shift ;;
--camera-preview) CAMERA_PREVIEW=true; shift ;;
--no-camera) WITH_CAMERA=false; shift ;;
--no-tactile) WITH_TACTILE=false; shift ;;
-h|--help) sed -n '2,30p' "$0"; exit 0 ;;
-h|--help) sed -n '2,31p' "$0"; exit 0 ;;
*) echo "Unknown: $1" >&2; exit 1 ;;
esac
done
@@ -174,12 +177,21 @@ else
echo "=== --no-camera：跳过相机检查 ==="
fi
# ---- 3) 触觉服务。只有左臂装了传感器 ----
# ---- 3) 触觉服务。只有左臂装了传感器；只有三阶段搜孔才用力 ----
# --skip-stage3 时如果还去等，USB 打不开会假死：话题在、connected 却是 false，
# 每次 echo 吃满超时 × 60 次 ≈ 3 分钟，终端上没有任何新行（2026-10-09 真机）。
_SKIP_TACTILE_REASON=""
if [[ "${SKIP_STAGE3}" == true && "${WITH_TACTILE}" == true ]]; then
echo "=== --skip-stage3：一/二阶段不用触觉，跳过拉起 ==="
WITH_TACTILE=false
_SKIP_TACTILE_REASON="stage3"
fi
# 同样先查 topic 列表再 echo，避免服务没起时反复撞超时。
TACTILE_TOPIC="/tactile/left/status"
_tactile_ok() {
timeout 2 ros2 topic list 2>/dev/null | grep -qx "${TACTILE_TOPIC}" || return 1
timeout 3 ros2 topic echo "${TACTILE_TOPIC}" --once 2>/dev/null | grep -q "connected: true"
timeout 1 ros2 topic echo "${TACTILE_TOPIC}" --once 2>/dev/null | grep -q "connected: true"
}
if [[ "${WITH_TACTILE}" == true ]]; then
@@ -199,12 +211,19 @@ if [[ "${WITH_TACTILE}" == true ]]; then
sleep 1.0
nohup ros2 launch arm_application tactile.launch.py > /tmp/run_all_tactile.log 2>&1 &
echo " tactile pid=$! log=/tmp/run_all_tactile.log"
echo " 等触觉 connected（USB 打不开会在十几秒内跳过，不会空等几分钟）"
t_ok=false
for _i in $(seq 1 60); do # 最多等 ~30s；传感器要在 Python 侧加载模型
for _i in $(seq 1 8); do
if _tactile_ok; then
t_ok=true
break
fi
# 日志已经写明打不开：再给两轮自愈，然后放行。不许在这里空转到超时。
if [[ "${_i}" -ge 4 ]] && grep -q "connect failed" /tmp/run_all_tactile.log 2>/dev/null; then
echo " 触觉 USB 打不开，停止等待" >&2
break
fi
echo " 触觉还未 connected（${_i}/8）"
sleep 0.5
done
if [[ "${t_ok}" != true ]]; then
@@ -219,7 +238,7 @@ if [[ "${WITH_TACTILE}" == true ]]; then
else
echo "=== 触觉只有左臂装了；当前 ${SITE_ROBOT_ARM:-?}，跳过 ==="
fi
else
elif [[ "${_SKIP_TACTILE_REASON}" != "stage3" ]]; then
echo "=== --no-tactile：跳过触觉检查 ==="
fi
@@ -239,10 +258,11 @@ ARGS=(
"preview:=${PIPELINE_PREVIEW}"
"resnap:=${RESNAP}"
"no_descent:=${NO_DESCENT}"
"d435_move_only:=${D435_ONLY}"
)
[[ -n "${HOLE}" ]] && ARGS+=("target_hole:=${HOLE}")
[[ -n "${DEPTH}" ]] && ARGS+=("insert_depth_mm:=${DEPTH}")
[[ -n "${PITCH}" ]] && ARGS+=("screw_pitch_mm:=${PITCH}")
echo "=== launch pipeline hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} no_descent=${NO_DESCENT} ==="
echo "=== launch pipeline hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} no_descent=${NO_DESCENT} d435_only=${D435_ONLY} ==="
exec ros2 launch arm_application pipeline.launch.py "${ARGS[@]}"
scripts/run_handeye.sh
+31 -1
View File
@@ -202,6 +202,7 @@ cmd_show() {
echo " result : ${RES}"
echo " workdir : ${WORKDIR}"
echo " camera : ${CAM} serial=${CAM_SERIAL:-（空）}"
echo " tool : ${SITE_ROBOT_TOOL_NAME:-（不切换）} gripper_tool=${SITE_HANDEYE_GRIPPER_TOOL:-（无，ee_pose 原样）}"
echo " board : $(_he BOARD_COLS)x$(_he BOARD_ROWS) inner, square=$(_he SQUARE) m (来源=$(_he BOARD_SRC))"
echo " （另一种模式的板见下面 export 的 标定板[...] 两行）"
python3 "${ROOT}/scripts/lib/export_site_env.py" --print || true
@@ -240,6 +241,20 @@ cmd_calibrate() {
exit 2
fi
_need_files "${CAL}" "${WP}"
local grip_tool="${SITE_HANDEYE_GRIPPER_TOOL:-}"
local grip_xyz="${SITE_HANDEYE_GRIPPER_TCP_XYZ:-}"
local grip_q="${SITE_HANDEYE_GRIPPER_TCP_QWXYZ:-}"
if [[ -n "${grip_tool}" ]]; then
if [[ "${SITE_ROBOT_TOOL_NAME:-}" != "${grip_tool}" ]]; then
echo "ERROR: 手眼要按工具 ${grip_tool} 求解，但 site 当前激活工具是 '${SITE_ROBOT_TOOL_NAME:-}'。" >&2
echo " robots[].tool_name 必须和 handeye.gripper_tool 同一份名字。" >&2
exit 2
fi
if [[ -z "${grip_xyz}" || -z "${grip_q}" ]]; then
echo "ERROR: 配了 gripper_tool=${grip_tool} 但没写 gripper_tcp_xyz / gripper_tcp_qwxyz。" >&2
exit 2
fi
fi
_need_robot
_need_camera
echo "=== kill visual_servo (avoid traj fight) ==="
@@ -260,6 +275,19 @@ cmd_calibrate() {
echo " motion v=${vel} T≥${mt}s settle=${st}s qd≤${qd} pin=${ph}x${ps}s"
echo " poses=${WP}"
echo " result → ${RES}"
local grip_args=()
if [[ -n "${grip_tool}" ]]; then
if [[ "${HANDEYE_MODE}" == "eye_in_hand" ]]; then
echo " gripper_tool=${grip_tool} tcp_xyz=[${grip_xyz}] （AX=XB 用 TCP，T_4x4 折成 cam→法兰）"
else
echo " gripper_tool=${grip_tool} tcp_xyz=[${grip_xyz}] （AX=XB 用 TCP，T_4x4 仍是 cam→基座，不折）"
fi
grip_args+=(
-p gripper_tool:="${grip_tool}"
-p gripper_tcp_xyz:="[${grip_xyz}]"
-p gripper_tcp_qwxyz:="[${grip_q}]"
)
fi
exec ros2 run arm_application handeye_calibrate --ros-args \
--params-file "${CAL}" \
--params-file "${WP}" \
@@ -280,7 +308,8 @@ cmd_calibrate() {
-p settle_timeout_sec:="${sto}" \
-p pin_holds:="${ph}" \
-p pin_hold_sec:="${ps}" \
-p skip_unstable_poses:="${skip}"
-p skip_unstable_poses:="${skip}" \
"${grip_args[@]}"
}
cmd_waypoints() {
@@ -359,6 +388,7 @@ cmd_teach() {
-p joint_prefix:="${PREFIX}"
-p camera_source:="${CAM}"
-p output_dir:="${WORKDIR}"
-p poses_file:="${TAUGHT}"
-p target_points:="${points}"
)
if [[ -n "${CAM_SERIAL}" ]]; then
src/arm_application/arm_application/handeye_calibrate.py
+92 -10
View File
@@ -33,8 +33,11 @@ from arm_application.handeye_lib import (
BoardConfig,
CameraIntrinsics,
ChessboardStabilizer,
T_from_flexiv_tcp,
calibrate_hand_eye,
cam_to_flange_via_tool,
detect_chessboard,
format_matrix_yaml,
format_transform_yaml,
is_pose_filled,
pose_to_Rt,
@@ -113,11 +116,10 @@ class HandeyeCalibrate(TolerantNode):
# 棋盘（可自定义）
# 默认值必须与 site.yaml 的 handeye.board_* 及结果文件头里的实际板子一致。
# 真实使用的板：内角点 8x11，方格 15mm（见 handeye/results/*.yaml 的头部注释）。
# 这里原先写的是 9x6 / 0.025（另一张早期的板），一旦 site.yaml 漏配就会被
# 静默启用 —— 症状是"标定能顺利跑完，但结果明显不对"，极难排查。
self.declare_parameter('board_cols', 8) # 内角点
self.declare_parameter('board_rows', 11)
# 真实使用的板：A5 内角点 11x8，方格 15mm（site.yaml handeye.boards.eye_in_hand）。
# 这里原先写的是 9x6 / 0.025，后来是 8x11；漏配就会静默用错尺寸。
self.declare_parameter('board_cols', 11) # 内角点
self.declare_parameter('board_rows', 8)
self.declare_parameter('square_size_m', 0.015)
self.declare_parameter('stable_window', 8)
@@ -138,6 +140,11 @@ class HandeyeCalibrate(TolerantNode):
self.declare_parameter('output_dir', '/home/cc/ros2/handeye/tmp')
self.declare_parameter('result_file', '')
self.declare_parameter('cleanup_images', True)
# 眼在手上：AX=XB 的 gripper 用 Studio 工具 TCP，再折成 cam→法兰写 T_4x4。
# 空 = 旧口径（ee_pose 当 gripper，T_4x4 原样写出）。配了就必须带 TCP。
self.declare_parameter('gripper_tool', '')
self.declare_parameter('gripper_tcp_xyz', [0.0, 0.0, 0.0])
self.declare_parameter('gripper_tcp_qwxyz', [1.0, 0.0, 0.0, 0.0])
prefix = str(self.get_parameter('joint_prefix').value or '')
base = list(
@@ -152,6 +159,21 @@ class HandeyeCalibrate(TolerantNode):
self.start_index = max(0, int(self.get_parameter('start_index').value))
self.min_poses = int(self.get_parameter('min_poses').value)
self.cleanup_images = bool(self.get_parameter('cleanup_images').value)
self.gripper_tool = str(self.get_parameter('gripper_tool').value or '').strip()
tcp_xyz = list(
self.get_parameter('gripper_tcp_xyz').get_parameter_value().double_array_value
or [0.0, 0.0, 0.0]
)
tcp_q = list(
self.get_parameter('gripper_tcp_qwxyz').get_parameter_value().double_array_value
or [1.0, 0.0, 0.0, 0.0]
)
self.T_flange_to_tool = T_from_flexiv_tcp(tcp_xyz[:3], tcp_q[:4])
if self.gripper_tool and float(np.linalg.norm(self.T_flange_to_tool[:3, 3])) < 1e-4:
raise RuntimeError(
f'gripper_tool={self.gripper_tool!r} 但 gripper_tcp_xyz 几乎是零：'
'等于没把 Studio TCP 传进来。不许按法兰口径假装在用工具系。'
)
self.camera_source = str(self.get_parameter('camera_source').value).strip().lower()
self.camera_serial = str(self.get_parameter('camera_serial').value or '').strip()
@@ -265,10 +287,12 @@ class HandeyeCalibrate(TolerantNode):
self.images_dir.mkdir(parents=True, exist_ok=True)
eye_to_hand = self.mode == 'eye_to_hand'
grip = self.gripper_tool or '（ee_pose 原样，无工具折算）'
self.get_logger().info(
f'手眼标定 [{self.mode}] camera_source={self.camera_source}：'
f'{len(poses)} 个位姿，棋盘={self.board.cols}x{self.board.rows} '
f'方格={self.board.square_size}m，v={self.velocity_scale} '
f'gripper_tool={grip}'
)
self.get_logger().warn('清理工作空间；确认急停就绪。Ctrl+C 可中止。')
@@ -368,11 +392,57 @@ class HandeyeCalibrate(TolerantNode):
self.get_logger().info(f'用 {len(Rg)} 个视角求解手眼标定...')
R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=eye_to_hand)
name = (
'T_cam_to_base (eye_to_hand)'
if eye_to_hand
else 'T_cam_to_gripper (eye_in_hand)'
extra = ''
if eye_to_hand:
name = 'T_cam_to_base (eye_to_hand)'
yaml_txt = format_transform_yaml(R, t, name)
if self.gripper_tool:
extra = (
f'gripper_tool: {self.gripper_tool}\n'
+ format_matrix_yaml(self.T_flange_to_tool, 'T_flange_to_tool')
)
self.get_logger().info(
f'gripper={self.gripper_tool}：AX=XB 的手是 TCP；'
f'T_4x4 是 cam→基座，不折 t={t.reshape(3)*1000} mm'
)
elif self.gripper_tool:
T_cam_tool = np.eye(4)
T_cam_tool[:3, :3] = R
T_cam_tool[:3, 3] = t.reshape(3)
T_cam_flange = cam_to_flange_via_tool(T_cam_tool, self.T_flange_to_tool)
delta = float(np.linalg.norm(T_cam_flange[:3, 3] - T_cam_tool[:3, 3]))
tool_len = float(np.linalg.norm(self.T_flange_to_tool[:3, 3]))
if tool_len > 0.05 and delta < 1e-4:
self.get_logger().error(
'T_cam_to_flange 与 T_cam_to_tool 平移几乎相同，'
'折算没生效。结果作废。'
)
return 11
# 腕部相机在法兰前方（法兰 +Z）。乘反了 Z 会落到 −200 mm 一带。
if (
self.T_flange_to_tool[2, 3] > 0.05
and float(T_cam_flange[2, 3]) < 0.0
):
self.get_logger().error(
f'T_cam_to_flange tz={float(T_cam_flange[2, 3])*1000:.1f} mm '
'在法兰后方，乘序反了。结果作废。'
)
return 12
R, t = T_cam_flange[:3, :3], T_cam_flange[:3, 3].reshape(3, 1)
name = 'T_cam_to_flange (eye_in_hand，由工具 TCP 折算)'
yaml_txt = format_transform_yaml(R, t, name)
extra = (
f'gripper_tool: {self.gripper_tool}\n'
+ format_matrix_yaml(self.T_flange_to_tool, 'T_flange_to_tool')
+ format_matrix_yaml(T_cam_tool, 'T_cam_to_tool')
)
self.get_logger().info(
f'gripper={self.gripper_tool}：'
f'T_cam_to_tool t={T_cam_tool[:3, 3]*1000} mm → '
f'T_cam_to_flange t={T_cam_flange[:3, 3]*1000} mm'
)
else:
name = 'T_cam_to_gripper (eye_in_hand)'
yaml_txt = format_transform_yaml(R, t, name)
header = (
f'# 手眼标定结果\n'
@@ -383,8 +453,20 @@ class HandeyeCalibrate(TolerantNode):
f'# board: {self.board.cols}x{self.board.rows} '
f'square_m={self.board.square_size}\n'
)
if self.gripper_tool and eye_to_hand:
header += (
f'# gripper_tool: {self.gripper_tool}\n'
f'# T_4x4 = cam→基座（AX=XB 的手是 TCP，不折到法兰）\n'
f'# 旧结果备份在 results/backup/\n'
)
elif self.gripper_tool:
header += (
f'# gripper_tool: {self.gripper_tool}\n'
f'# T_4x4 = cam→法兰（AX=XB 在 TCP 上解，再左乘 T_flange_to_tool）\n'
f'# T_cam_to_tool 是原始求解；旧的法兰/tool1 备份在 results/backup/\n'
)
self.result_file.parent.mkdir(parents=True, exist_ok=True)
self.result_file.write_text(header + yaml_txt, encoding='utf-8')
self.result_file.write_text(header + extra + yaml_txt, encoding='utf-8')
(self.output_dir / 'captures.json').write_text(
json.dumps(meta, indent=2), encoding='utf-8'
)
src/arm_application/arm_application/handeye_lib.py
+40
View File
@@ -342,6 +342,38 @@ def calibrate_hand_eye(
return calibrate_hand_eye_tsai(Rg, tg, R_target2cam, t_target2cam)
def T_from_flexiv_tcp(xyz_m, qwxyz) -> np.ndarray:
"""Studio ``tcp_location`` → 法兰→工具 4×4。
Flexiv 顺序是 ``[x, y, z, q_w, q_x, q_y, q_z]``，四元数是 wxyz，
和本仓库其余地方的 xyzw 不一样。只在这里换一次。
"""
xyz = np.asarray(xyz_m, dtype=float).reshape(3)
q = np.asarray(qwxyz, dtype=float).reshape(7) if np.asarray(qwxyz).size == 7 else np.asarray(qwxyz, dtype=float).reshape(4)
if q.size == 7:
qw, qx, qy, qz = (float(q[3]), float(q[4]), float(q[5]), float(q[6]))
else:
qw, qx, qy, qz = (float(q[0]), float(q[1]), float(q[2]), float(q[3]))
R = quat_xyzw_to_R((qx, qy, qz, qw))
return Rt_to_T(R, xyz.reshape(3, 1))
def cam_to_flange_via_tool(T_cam_to_tool: np.ndarray, T_flange_to_tool: np.ndarray) -> np.ndarray:
"""AX=XB 在工具 TCP 上解出 cam→工具后，再折成 cam→法兰。
``T_flange_to_tool`` 是 Studio TCP：``p_法兰 = T_flange_to_tool · p_工具``。
OpenCV 的 ``T_cam_to_tool`` 是 ``p_工具 = T_cam_to_tool · p_相机``。
所以 ``T_cam_to_flange = T_flange_to_tool · T_cam_to_tool``。
乘反了（``T_cam_to_tool · inv(T_ft)``）旋转看着还像，平移会把相机
扔到法兰后方约 30 cm。忘了乘则 ``T_4x4`` 停在 TCP 上，沿工具偏 ~225 mm。
两种都不报错。
"""
T_ct = np.asarray(T_cam_to_tool, dtype=float).reshape(4, 4)
T_ft = np.asarray(T_flange_to_tool, dtype=float).reshape(4, 4)
return T_ft @ T_ct
def format_transform_yaml(R: np.ndarray, t: np.ndarray, name: str) -> str:
T = Rt_to_T(R, t)
lines = [
@@ -354,6 +386,14 @@ def format_transform_yaml(R: np.ndarray, t: np.ndarray, name: str) -> str:
return '\n'.join(lines) + '\n'
def format_matrix_yaml(T: np.ndarray, key: str) -> str:
arr = np.asarray(T, dtype=float).reshape(4, 4)
lines = [f'{key}:']
for row in arr:
lines.append(' - [' + ', '.join(f'{v:.8f}' for v in row) + ']')
return '\n'.join(lines) + '\n'
def is_pose_filled(joints: Sequence[float]) -> bool:
"""若全为零 / NaN 占位则返回 False。"""
arr = np.asarray(joints, dtype=float)
src/arm_application/arm_application/handeye_teach.py
+24 -1
View File
@@ -122,6 +122,8 @@ class RdkBridge:
'PATH': f'{Path(py).parent}:/usr/bin:/bin',
'LD_LIBRARY_PATH': flexiv_lib,
'LANG': 'C.UTF-8',
'SITE_PC_IP': os.environ.get('SITE_PC_IP', ''),
'SITE_NET_IFACE': os.environ.get('SITE_NET_IFACE', ''),
}
# 不要继承 conda 的 PYTHONPATH。
# 重要：stderr 不能用 PIPE —— RDK 日志会填满管道并让桥接进程死锁
@@ -254,6 +256,9 @@ class HandeyeTeach(TolerantNode):
# 设了就只是个"提醒"：采够时提示可以收工，不强制关窗 ——
# 常常采到一半发现某个角度还没覆盖，强制退出反而要重开。
self.declare_parameter('target_points', 0)
# 必须和 run_handeye.sh 的 TAUGHT 是同一个文件。只写 taught_poses.txt、
# 脚本去读 taught_poses_<模式>.txt，merge 就会「文件在、内容空」。
self.declare_parameter('poses_file', '')
self.backend = str(self.get_parameter('backend').value).strip().lower()
self.robot_sn = str(self.get_parameter('robot_sn').value)
@@ -261,8 +266,12 @@ class HandeyeTeach(TolerantNode):
self.output_dir = Path(str(self.get_parameter('output_dir').value)).expanduser()
self.images_dir = self.output_dir / 'images'
self.poses_file = self.output_dir / 'taught_poses.txt'
_pf = str(self.get_parameter('poses_file').value).strip()
self.poses_file = (
Path(_pf).expanduser() if _pf else self.output_dir / 'taught_poses.txt'
)
self.yaml_file = self.output_dir / 'taught_waypoint_positions.yaml'
self._last_capture_t = 0.0
self.images_dir.mkdir(parents=True, exist_ok=True)
self._captures: List[dict] = []
@@ -531,6 +540,20 @@ class HandeyeTeach(TolerantNode):
if q is None:
self.get_logger().error('读取关节失败')
return
now = time.monotonic()
if now - self._last_capture_t < 0.8:
self.get_logger().warn('按太快，忽略（防 Enter 连发）')
return
if self._captures:
last = self._captures[-1]['joints']
delta = max(abs(a - b) for a, b in zip(q, last))
if delta < 0.015:
self.get_logger().warn(
f'关节几乎没动（Δ={delta:.4f} rad），忽略这次拍照。'
'先拖到另一个姿态再按 Enter。'
)
return
self._last_capture_t = now
idx = len(self._captures)
stamp = datetime.now(timezone.utc).strftime('%Y%m%d_%H%M%S')
src/arm_application/arm_application/overhead_oneshot.py
+321
View File
@@ -17,6 +17,32 @@ from typing import Iterable, Optional
import numpy as np
from arm_application.handeye_lib import cam_point_to_base
from arm_application.path_profile import ( # 伺服和测试从本模块再导出，不另抄一份
D435_FULL_ACC_M,
D435_FULL_CRUISE_MPS,
D435_FULL_DEC_M,
D435_MID_HOP_MAX_M,
D435_MID_LEFTOVER_M,
D435_MOVE_ACC_MPS2,
D435_MOVE_SPEED_SCALE,
D435_NEAR_CRUISE_MPS,
D435_NEAR_HOP_MAX_M,
D435_NEAR_LEFTOVER_M,
D435_VERT_TILT_RAD,
D435_SETTLE_S,
FROZEN_J_HOP_MAX_M,
build_arc_xy_move,
flange_down_w,
flange_tilt_rad,
hybrid_cruise_vxy,
oneshot_hop_limits,
smooth_joint_schedule,
trap_acc_for_hop,
translate_hold_ori_waypoints,
)
# 上一跳实际少走这么多，就认定冻结 J 在当前姿态走不动，不许按同一剩差再刷。
D435_MIN_PROGRESS_FRAC = 0.40
def best_det_conf(dets: Optional[Iterable]) -> Optional[float]:
@@ -30,6 +56,301 @@ def best_det_conf(dets: Optional[Iterable]) -> Optional[float]:
return max(vals)
def z_locked_qos():
"""``visual_servo/z_locked`` 的 QoS。发布端和订阅端必须用这一份。
感知比伺服晚启动。默认 volatile 会丢掉伺服启动时已经发出的「已到锁高」，
感知就停在初值 False，D435 永远不拍。
"""
from rclpy.qos import ( # noqa: PLC0415 — 纯函数测试不需要在导入时拉起 ROS
DurabilityPolicy,
HistoryPolicy,
QoSProfile,
ReliabilityPolicy,
)
return QoSProfile(
history=HistoryPolicy.KEEP_LAST,
depth=1,
reliability=ReliabilityPolicy.RELIABLE,
durability=DurabilityPolicy.TRANSIENT_LOCAL,
)
def at_lock_height(z_m: float, lock_z_m: float, gate_m: float) -> bool:
"""法兰是否在锁高带内。
低于锁高**不算**到位。上一轮观测下降停在 0.26 m 时，若把「不高于锁高」
当成到位，XY 会在低位开跑，D435 又因高度不对拒绝拍照，两边都不再动。
``lock_z_m <= 0`` 表示没启用锁高。
"""
lock = float(lock_z_m)
if lock <= 0.0:
return True
gate = max(0.02, float(gate_m))
return abs(float(z_m) - lock) <= gate
def lock_z_velocity(z_m: float, lock_z_m: float, *, kp: float, v_lim: float) -> float:
"""回到锁高的竖直速度 [m/s]，基座 +Z 朝上。
低于锁高为正（上抬），高于为负（下降）。4 mm 内视为到位。
不许在低于锁高时把速度写成 0：那会和「没到锁高不拍 D435」卡死。
"""
lock = float(lock_z_m)
if lock <= 0.0:
return 0.0
ez = lock - float(z_m)
if abs(ez) < 0.004:
return 0.0
lim = abs(float(v_lim))
return float(np.clip(float(kp) * ez, -lim, lim))
def hold_translation_vz(z_m: float, z_hold_m: float, *, kp: float, v_lim: float) -> float:
"""平移时把法兰钉在**这一段开始时的高度**。
只写 v_z=0 不够：大范围平移加姿态校正时，阻尼求逆会把竖直速度吃掉，
2026-10-09 真机从 0.327 m 漂到 0.307 m 又抬到 0.337 m。这里用开始那一
帧的高度当设定，4 mm 内不动，避免来回抖。不追 site 里的锁高数字，
也不因此停掉 XY。下臂仍只在二阶段。
"""
ez = float(z_hold_m) - float(z_m)
if abs(ez) < 0.004:
return 0.0
lim = abs(float(v_lim))
return float(np.clip(float(kp) * ez, -lim, lim))
# D405 连续这么久没有高置信度目标，才让待命的 D435 拍一张。
# 单帧漏检就拍，会在平移半路把目标换掉。
D405_MISS_BEFORE_D435_S = 0.5
# 拍了但伺服没 ``d435_commanding``（工作盒拒收等）：超过这时限就允许再拍。
# 不清的话 ``_oneshot_shot`` 会永远真，臂停着、感知还以为在赶路。
ONESHOT_ACCEPT_TIMEOUT_S = 0.8
def oneshot_shot_timed_out(
*,
shot: bool,
commanding: bool,
age_s: float,
timeout_s: float = ONESHOT_ACCEPT_TIMEOUT_S,
) -> bool:
"""拍过一张、伺服却没接住 → True。已经在赶路不许清。"""
if not bool(shot) or bool(commanding):
return False
return float(age_s) >= float(timeout_s)
def d405_commands_arm(
*,
using_d435: bool,
arrived: bool,
settled: bool,
move_only: bool = False,
) -> bool:
"""D405 这一帧的点能不能下发给左臂。
推理一直开着。正在用 D435 那一个点时，点到并且停稳之前不下发。
停稳沿用一阶段的 ``StillLock``（site 里的 ``lock_still_sec`` /
``lock_still_move_m``），不另写一套计时。
``move_only``：只测 D435 位移，D405 点一律不下发（到位之后也不交）。
"""
if move_only:
return False
if not using_d435:
return True
return bool(arrived) and bool(settled)
def oneshot_needs_verticalize(
tilt_rad: float,
leftover_m: float,
arrive_m: float,
*,
tilt_lim: float = D435_VERT_TILT_RAD,
already_done: bool = False,
) -> bool:
"""XY 已经进到位门槛、倾角还大，才单独收垂直。不许和平移捆在一起。"""
if already_done:
return False
if float(leftover_m) > float(arrive_m):
return False
return float(tilt_rad) > float(tilt_lim)
def oneshot_next_hop_m(
dist_m: float, hop_max_m: float = FROZEN_J_HOP_MAX_M
) -> float:
"""这一跳最多走 ``hop_max_m``。冻结 J 一次规划 400 mm 会少走一大截。"""
return min(max(0.0, float(dist_m)), float(hop_max_m))
def oneshot_cartesian_dp(
p_ee,
goal_xy,
lock_z: float,
*,
gate_m: float,
hop_max_m: Optional[float] = None,
leftover_xy_m: Optional[float] = None,
):
"""这一跳的笛卡尔位移。没到锁高只收 Z；远处 XY 带锁高；近处只走 XY。
工件上方最后一跳若再把 7 mm 的 Z 和 60 mm 的 XY 捆在一起，冻结 J 会把腕部带偏。
远处仍带 ``ee_z_m``，避免长跳把高度抬出锁高带。
"""
p = np.asarray(p_ee, dtype=float).reshape(3)
z_lock = float(lock_z)
leftover = (
float(leftover_xy_m)
if leftover_xy_m is not None
else float(
np.hypot(float(goal_xy[0]) - float(p[0]), float(goal_xy[1]) - float(p[1]))
)
)
hop_cap = (
float(hop_max_m)
if hop_max_m is not None
else oneshot_hop_limits(leftover)[0]
)
if not at_lock_height(float(p[2]), z_lock, float(gate_m)):
dz = z_lock - float(p[2])
mag = oneshot_next_hop_m(abs(dz), hop_cap)
if mag < 1e-6:
return np.zeros(3, dtype=float)
return np.array([0.0, 0.0, float(np.sign(dz)) * mag], dtype=float)
dz = 0.0 if leftover <= D435_NEAR_LEFTOVER_M else (z_lock - float(p[2]))
dp = np.array(
[
float(goal_xy[0]) - float(p[0]),
float(goal_xy[1]) - float(p[1]),
dz,
],
dtype=float,
)
n = float(np.linalg.norm(dp))
hop = oneshot_next_hop_m(n, hop_cap)
if n > hop > 1e-9:
dp = dp * (hop / n)
return dp
def oneshot_full_segments(
p_ee,
goal_xy,
lock_z: float,
*,
gate_m: float,
):
"""整段一次走完的笛卡尔分段。先收齐锁高，再走全部 XY，不切 80/15 mm。
近处（剩差 ≤80 mm）仍然只走 XY，不混 Z。收垂直不在这里。
返回 ``[(dp, tag), ...]``，``tag`` 是 ``lift_z`` 或 ``xy``。
"""
p = np.asarray(p_ee, dtype=float).reshape(3).copy()
z_lock = float(lock_z)
goal = np.asarray(goal_xy, dtype=float).reshape(2)
out = []
if not at_lock_height(float(p[2]), z_lock, float(gate_m)):
dz = z_lock - float(p[2])
if abs(dz) >= 1e-6:
out.append((np.array([0.0, 0.0, dz], dtype=float), "lift_z"))
p[2] = z_lock
leftover = float(np.hypot(float(goal[0]) - float(p[0]), float(goal[1]) - float(p[1])))
dz = 0.0 if leftover <= D435_NEAR_LEFTOVER_M else (z_lock - float(p[2]))
dp = np.array(
[float(goal[0]) - float(p[0]), float(goal[1]) - float(p[1]), dz],
dtype=float,
)
if float(np.linalg.norm(dp)) >= 1e-6:
out.append((dp, "xy"))
return out
def oneshot_concat_hold_ori(q0, J, segments, *, R=None):
"""把分段 ``dp`` 用同一份冻结 J 串成一条关节折线。中途不回零速。"""
q = np.asarray(q0, dtype=float).copy()
wps = [q.copy()]
planned = 0.0
asked = 0.0
tags = []
for dp, tag in segments:
hop = float(np.linalg.norm(np.asarray(dp, dtype=float).reshape(3)))
if hop < 1e-6:
continue
asked += hop
part, remain = translate_hold_ori_waypoints(
q, J, dp, R=R, verticalize=False
)
if len(part) < 2:
break
wps.extend(part[1:])
q = wps[-1]
planned += hop - float(remain)
tags.append(str(tag))
tag = "full"
if tags == ["lift_z"]:
tag = "lift_z"
elif "lift_z" in tags and "xy" in tags:
tag = "full"
elif tags == ["xy"]:
tag = "full"
return wps, planned, asked, tag
def oneshot_full_cruise_mps(leftover_m: float, far_cruise_mps: float) -> float:
"""整段巡航：工件上方 30 mm/s；远处封顶 ``D435_FULL_CRUISE_MPS``，不许再弹到 80。"""
far = max(1e-4, float(far_cruise_mps))
if float(leftover_m) <= D435_NEAR_LEFTOVER_M:
return min(D435_NEAR_CRUISE_MPS, far)
return min(D435_FULL_CRUISE_MPS, far)
def oneshot_may_send_another(
leftover_m: float,
last_dist_m: Optional[float],
*,
arrive_m: float,
hop_max_m: float = FROZEN_J_HOP_MAX_M,
min_progress_frac: float = D435_MIN_PROGRESS_FRAC,
) -> bool:
"""上一跳走完后还要不要再发一条。
剩差已经进到位门槛 → 钉住。
上一跳几乎没缩短剩差 → 不许按同一距离再刷（真机 28 mm 死循环）。
"""
if float(leftover_m) <= float(arrive_m):
return False
if last_dist_m is None:
return True
progressed = float(last_dist_m) - float(leftover_m)
planned = min(float(last_dist_m), float(hop_max_m))
if progressed < float(min_progress_frac) * max(planned, 1e-6):
return False
return True
def oneshot_should_submit(
*,
handoff: bool,
low_for_s: float,
already_shot: bool,
wait_s: float = D405_MISS_BEFORE_D435_S,
) -> bool:
"""腕部连续 ``wait_s`` 秒没有高置信度目标、这一轮还没拍成，才让 D435 拍一张。
高度不在这里再判一次。进一阶段之前已经抬到锁高；再等伺服报「已到锁高」
会和「没到锁高就只收 Z、不平移」卡死。拍成一张之后不许连拍，顶视不是第二套跟踪。
"""
return (
bool(handoff)
and (not already_shot)
and float(low_for_s) >= float(wait_s)
)
def select_oneshot_det(dets: Optional[Iterable], min_conf: float):
"""顶视一次性：只收 conf≥门槛的框，取最高分。没有就 None，不许降级。"""
if not dets:
src/arm_application/arm_application/path_profile.py
+550
View File
@@ -0,0 +1,550 @@
"""路径弧长上的梯形速度，以及 XY 圆弧。
二阶段下臂和一阶段 D435 迁臂共用这一份。不许再抄一套加减速。
"""
from __future__ import annotations
import math
from dataclasses import dataclass, field
from typing import Optional, Tuple
import numpy as np
# 整段同一巡航：site ``v_slow_xy`` × 本系数。1.0 → 80 mm/s（二阶段对孔同档上限）。
D435_MOVE_SPEED_SCALE = 1.0
# 缺路程时的兜底加速度。真机 hop 用 ``trap_acc_for_hop``，和二阶段同一条刹车。
D435_MOVE_ACC_MPS2 = 0.125
# action 走完后再停这么久，才重测 TF。二阶段 ``_goto`` 的 settle_s。
D435_SETTLE_S = 0.25
# 冻结雅可比一次只信这么远。二阶段 ``_plan_and_send`` 默认 ``max_seg`` 同源。
# 400 mm 一次规划，真机剩 117 mm，随后 28 mm 死循环刷轨迹。
FROZEN_J_HOP_MAX_M = 0.08
# 剩差进入这一带 = 工件上方。真机最后一跳 37–75 mm @ 80 mm/s 会晃并偏转。
D435_NEAR_LEFTOVER_M = 0.08
D435_NEAR_HOP_MAX_M = 0.015
D435_NEAR_CRUISE_MPS = 0.030
# 法兰倾角超过这个才单独收垂直。和 XY 捆在同一跳会把腕部漏成 Y 过冲。
D435_VERT_TILT_RAD = 0.015
D435_MID_LEFTOVER_M = 0.15
D435_MID_HOP_MAX_M = 0.040
# 弦长的 12% 当矢高，封顶 50 mm，避免大圆弧甩出工作盒。
D435_ARC_SAGITTA_FRAC = 0.12
D435_ARC_SAGITTA_MAX_M = 0.05
# 整段一次走完。起步必须够长：40 mm 就爬到 80 mm/s 是真机那下弹射
# （头尾速度本来就是 0，不是初速度；平移也不下发角速度）。
# 峰值加速度 a_max = π v² / (4 d_acc)；100 mm @ 60 mm/s → 约 0.028 m/s²。
D435_FULL_ACC_M = 0.100
D435_FULL_DEC_M = 0.080
D435_FULL_CRUISE_MPS = 0.060
def hybrid_cruise_vxy(
v_slow_xy: float, scale: float = D435_MOVE_SPEED_SCALE
) -> float:
"""远处巡航。近处由 ``oneshot_hop_limits`` 另给一档，不许再抄 15 mm/s。"""
return max(1e-4, float(v_slow_xy) * float(scale))
def oneshot_hop_limits(
leftover_m: float, far_cruise_mps: float = 0.08
) -> Tuple[float, float]:
"""这一跳的 ``(hop_max_m, cruise_mps)``。近处必须比 80 mm / 80 mm/s 短且慢。"""
d = max(0.0, float(leftover_m))
far = max(1e-4, float(far_cruise_mps))
if d <= D435_NEAR_LEFTOVER_M:
return D435_NEAR_HOP_MAX_M, min(D435_NEAR_CRUISE_MPS, far)
if d <= D435_MID_LEFTOVER_M:
return D435_MID_HOP_MAX_M, far
return FROZEN_J_HOP_MAX_M, far
def flange_down_w(
R,
*,
dead_rad: float = 0.006,
gain: float = 2.0,
mag_lim: float = 0.5,
) -> Tuple[np.ndarray, float]:
"""让法兰 +Z 指向世界 −Z 的角速度 + 当前倾角。
和二阶段 ``_w_down`` 同一份。死区 0.006 rad（约 0.34°）以下不再修。
"""
z_now = np.asarray(R, dtype=float).reshape(3, 3)[:, 2]
z_des = np.array([0.0, 0.0, -1.0], dtype=float)
c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0))
ang = float(math.acos(c))
if ang < float(dead_rad):
return np.zeros(3, dtype=float), ang
axis = np.cross(z_now, z_des)
n = float(np.linalg.norm(axis))
axis = axis / n if n > 1e-9 else np.array([1.0, 0.0, 0.0], dtype=float)
mag = float(np.clip(float(gain) * ang, -float(mag_lim), float(mag_lim)))
return axis * mag, ang
def flange_tilt_rad(R) -> float:
"""法兰 +Z 相对世界 −Z 的倾角 [rad]。只读角度，不带死区。"""
z_now = np.asarray(R, dtype=float).reshape(3, 3)[:, 2]
z_des = np.array([0.0, 0.0, -1.0], dtype=float)
c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0))
return float(math.acos(c))
def trap_acc_for_hop(dist: float, v: float) -> float:
"""二阶段 ``_plan_and_send`` 同一条刹车：路程越长刹车越远，不许另写一套。"""
v_cmd = max(min(float(v), 0.08), 1e-3)
d_brake = min(0.030, max(0.006, 0.32 * max(float(dist), 1e-6)))
return max(v_cmd * v_cmd / (2.0 * d_brake), 0.030)
def trap_params(dist: float, v: float, a: float) -> Tuple[float, float, float, float, float]:
"""Rest-to-rest 梯形：返回 (T, v_peak, t_acc, t_cruise, a)。巡航速度保持 v。"""
dist = max(float(dist), 1e-6)
v = max(float(v), 1e-4)
a = max(float(a), 1e-3)
d_acc = v * v / (2.0 * a)
if 2.0 * d_acc >= dist:
v_peak = math.sqrt(max(a * dist, 1e-12))
t_acc = v_peak / a
return 2.0 * t_acc, v_peak, t_acc, 0.0, a
t_acc = v / a
t_cruise = (dist - 2.0 * d_acc) / v
return 2.0 * t_acc + t_cruise, v, t_acc, t_cruise, a
def trap_sv(
t: float, dist: float, v_peak: float, t_acc: float, t_cruise: float, a: float,
) -> Tuple[float, float]:
"""时刻 t 的路径弧长 s 与切向速度。"""
t = max(0.0, float(t))
t_dec0 = t_acc + t_cruise
t_end = t_dec0 + t_acc
if t >= t_end:
return dist, 0.0
if t < t_acc:
return 0.5 * a * t * t, a * t
if t < t_dec0:
return 0.5 * a * t_acc * t_acc + v_peak * (t - t_acc), v_peak
td = t - t_dec0
s0 = 0.5 * a * t_acc * t_acc + v_peak * t_cruise
return s0 + v_peak * td - 0.5 * a * td * td, max(0.0, v_peak - a * td)
def smooth_path_params(
dist: float,
v: float,
*,
d_acc: float = D435_FULL_ACC_M,
d_dec: float = D435_FULL_DEC_M,
) -> Tuple[float, float, float, float, float, float]:
"""整段一次走完的余弦加减速：(T, v_peak, t_acc, t_cruise, d_acc, d_dec)。
加速/减速段速度按半个余弦走，两端加速度为 0，没有梯形那种加速度台阶。
路程不够巡航时整段一条余弦，峰值不超过 ``v``。
"""
dist = max(float(dist), 1e-6)
v = max(float(v), 1e-4)
acc = min(max(float(d_acc), 1e-4), 0.45 * dist)
dec = min(max(float(d_dec), 1e-4), 0.45 * dist)
if acc + dec >= dist - 1e-6:
# 整段一条正弦速度：v = v_peak sin(π t / T)，s(T)=2 v_peak T / π。
T = math.pi * dist / (2.0 * v)
return T, v, 0.0, 0.0, 0.0, dist
t_acc = 2.0 * acc / v
t_dec = 2.0 * dec / v
t_cruise = (dist - acc - dec) / v
T = t_acc + t_cruise + t_dec
return T, v, t_acc, t_cruise, acc, dec
def cosine_peak_acc(v: float, d_acc: float) -> float:
"""余弦加速段的峰值切向加速度 [m/s²]。``d_acc`` 越长，起步越不弹。"""
d = max(float(d_acc), 1e-6)
return math.pi * float(v) * float(v) / (4.0 * d)
def smooth_path_sv(
t: float,
dist: float,
v_peak: float,
t_acc: float,
t_cruise: float,
d_acc: float,
d_dec: float,
) -> Tuple[float, float]:
"""时刻 t 的弧长与切向速度。头尾速度为 0。"""
t = max(0.0, float(t))
dist = max(float(dist), 1e-6)
v_peak = max(float(v_peak), 1e-4)
if t_acc <= 1e-9 and t_cruise <= 1e-9:
T = math.pi * dist / (2.0 * v_peak)
if t >= T:
return dist, 0.0
u = math.pi * t / T
s = (v_peak * T / math.pi) * (1.0 - math.cos(u))
vs = v_peak * math.sin(u)
return min(s, dist), max(0.0, vs)
t_dec = 2.0 * float(d_dec) / v_peak if v_peak > 1e-9 else 0.0
t_end = float(t_acc) + float(t_cruise) + t_dec
if t >= t_end:
return dist, 0.0
if t < t_acc:
u = math.pi * t / t_acc
s = 0.5 * v_peak * (t - t_acc / math.pi * math.sin(u))
vs = 0.5 * v_peak * (1.0 - math.cos(u))
return s, vs
if t < t_acc + t_cruise:
return float(d_acc) + v_peak * (t - t_acc), v_peak
td = t - t_acc - t_cruise
u = math.pi * td / t_dec if t_dec > 1e-9 else math.pi
s0 = float(d_acc) + v_peak * float(t_cruise)
s = s0 + 0.5 * v_peak * (td + t_dec / math.pi * math.sin(u))
vs = 0.5 * v_peak * (1.0 + math.cos(u))
return min(s, dist), max(0.0, vs)
def _circle_3(
a: np.ndarray, b: np.ndarray, c: np.ndarray
) -> Optional[Tuple[np.ndarray, float]]:
ax, ay = float(a[0]), float(a[1])
bx, by = float(b[0]), float(b[1])
cx, cy = float(c[0]), float(c[1])
d = 2.0 * (ax * (by - cy) + bx * (cy - ay) + cx * (ay - by))
if abs(d) < 1e-9:
return None
a2 = ax * ax + ay * ay
b2 = bx * bx + by * by
c2 = cx * cx + cy * cy
ux = (a2 * (by - cy) + b2 * (cy - ay) + c2 * (ay - by)) / d
uy = (a2 * (cx - bx) + b2 * (ax - cx) + c2 * (bx - ax)) / d
center = np.array([ux, uy], dtype=float)
r = float(np.hypot(ax - ux, ay - uy))
if r < 1e-4:
return None
return center, r
def _ccw(a0: float, a1: float) -> float:
return (a1 - a0) % (2.0 * math.pi)
def _xy_in_box(xy: np.ndarray, lo: np.ndarray, hi: np.ndarray) -> bool:
return bool(
float(lo[0]) <= float(xy[0]) <= float(hi[0])
and float(lo[1]) <= float(xy[1]) <= float(hi[1])
)
@dataclass
class ArcXyMove:
"""从 p0 走到 p1 的 XY 圆弧（或退化成直线），弧长上跑梯形速度。"""
p0: np.ndarray
p1: np.ndarray
length: float
v_cruise: float
T: float
v_peak: float
t_acc: float
t_cruise: float
a: float
t0: float
linear: bool
center: Optional[np.ndarray] = None
radius: float = 0.0
ang0: float = 0.0
sweep: float = 0.0
s_last: float = 0.0
_dir: np.ndarray = field(default_factory=lambda: np.array([1.0, 0.0]))
def sample(self, now: float) -> Tuple[np.ndarray, np.ndarray, bool]:
"""返回 (计划 XY, 切向速度, 是否走完)。"""
t = max(0.0, float(now) - float(self.t0))
s, sdot = trap_sv(
t, self.length, self.v_peak, self.t_acc, self.t_cruise, self.a
)
s = min(float(s), float(self.length))
self.s_last = s
done = t >= float(self.T) - 1e-4 or s >= float(self.length) - 1e-5
if self.linear or self.radius <= 1e-6 or self.center is None:
L = max(float(self.length), 1e-6)
u = s / L
xy = (1.0 - u) * self.p0 + u * self.p1
vxy = self._dir * float(sdot)
return xy, vxy, done
frac = 0.0 if abs(self.sweep) < 1e-9 else s / (self.radius * abs(self.sweep))
frac = min(1.0, max(0.0, frac))
ang = self.ang0 + self.sweep * frac
xy = self.center + self.radius * np.array(
[math.cos(ang), math.sin(ang)], dtype=float
)
tang = np.array([-math.sin(ang), math.cos(ang)], dtype=float)
if self.sweep < 0.0:
tang = -tang
return xy, tang * float(sdot), done
def build_arc_xy_move(
p0,
p1,
*,
v: float,
a: float,
now: float,
ws_min=None,
ws_max=None,
sag_frac: float = D435_ARC_SAGITTA_FRAC,
sag_max: float = D435_ARC_SAGITTA_MAX_M,
) -> ArcXyMove:
"""p0→p1 的圆弧。工作盒装不下就换侧，再不行走直线。速度整段同一档。"""
p0 = np.asarray(p0, dtype=float).reshape(2)
p1 = np.asarray(p1, dtype=float).reshape(2)
chord = p1 - p0
L = float(np.hypot(chord[0], chord[1]))
direction = (
chord / L if L > 1e-6 else np.array([1.0, 0.0], dtype=float)
)
cruise = max(float(v), 1e-4)
acc = max(float(a), 1e-3)
def _linear() -> ArcXyMove:
T, v_peak, t_acc, t_cruise, a_use = trap_params(max(L, 1e-6), cruise, acc)
return ArcXyMove(
p0=p0.copy(),
p1=p1.copy(),
length=max(L, 1e-6),
v_cruise=cruise,
T=T,
v_peak=v_peak,
t_acc=t_acc,
t_cruise=t_cruise,
a=a_use,
t0=float(now),
linear=True,
_dir=direction,
)
if L < 0.003:
return _linear()
n = np.array([-direction[1], direction[0]], dtype=float)
h = min(float(sag_max), float(sag_frac) * L)
mid = 0.5 * (p0 + p1)
lo = None if ws_min is None else np.asarray(ws_min, dtype=float).reshape(-1)[:2]
hi = None if ws_max is None else np.asarray(ws_max, dtype=float).reshape(-1)[:2]
def _try(sign: float) -> Optional[ArcXyMove]:
apex = mid + n * (h * sign)
cir = _circle_3(p0, apex, p1)
if cir is None:
return None
center, r = cir
a0 = math.atan2(p0[1] - center[1], p0[0] - center[0])
a1 = math.atan2(p1[1] - center[1], p1[0] - center[0])
a_via = math.atan2(apex[1] - center[1], apex[0] - center[0])
ccw_end = _ccw(a0, a1)
ccw_via = _ccw(a0, a_via)
if 1e-6 < ccw_via < ccw_end - 1e-6:
sweep = ccw_end
else:
sweep = ccw_end - 2.0 * math.pi
if abs(sweep) < 1e-6:
return None
length = r * abs(sweep)
if lo is not None and hi is not None:
for k in range(9):
frac = k / 8.0
ang = a0 + sweep * frac
xy = center + r * np.array([math.cos(ang), math.sin(ang)])
if not _xy_in_box(xy, lo, hi):
return None
T, v_peak, t_acc, t_cruise, a_use = trap_params(length, cruise, acc)
return ArcXyMove(
p0=p0.copy(),
p1=p1.copy(),
length=length,
v_cruise=cruise,
T=T,
v_peak=v_peak,
t_acc=t_acc,
t_cruise=t_cruise,
a=a_use,
t0=float(now),
linear=False,
center=center,
radius=r,
ang0=a0,
sweep=sweep,
)
for sign in (1.0, -1.0):
got = _try(sign)
if got is not None:
return got
return _linear()
def path_interp(waypoints, u: float):
"""沿关节路点折线插值。u∈[0,1] → (q, dq/du)。"""
nseg = len(waypoints) - 1
if nseg <= 0:
z = np.zeros_like(waypoints[0])
return waypoints[0].copy(), z
u = float(min(1.0, max(0.0, u)))
x = u * nseg
i = min(nseg - 1, int(math.floor(x)))
a = x - i
q = (1.0 - a) * waypoints[i] + a * waypoints[i + 1]
dq_du = (waypoints[i + 1] - waypoints[i]) * nseg
return q, dq_du
def translate_hold_ori_waypoints(
q0,
J,
dp,
*,
damping: float = 0.08,
max_dq: float = 0.06,
R=None,
verticalize: bool = False,
):
"""冻结当前雅可比，把 ``dp`` 拆成关节路点。
和二阶段 ``_plan_and_send`` 同一份逆解。``verticalize`` 时顺带收法兰朝下，
不许另抄一份 ``_w_down``。返回 ``(路点, 还剩多少米)``。
"""
from arm_application.geom_ik import dls_translate_hold_ori, dls_twist
q = np.asarray(q0, dtype=float).copy()
remain = np.asarray(dp, dtype=float).reshape(3).copy()
dist = float(np.linalg.norm(remain))
wps = [q.copy()]
inner_tol = min(0.00012, max(0.00005, 0.05 * max(dist, 1e-6)))
n_max = int(math.ceil(dist / 0.008)) + 16
Jj = np.asarray(J, dtype=float)
w_step = np.zeros(3, dtype=float)
ang = 0.0
if verticalize:
if R is None:
raise ValueError("verticalize 必须传入当前法兰 R，不许假装已经朝下")
w_step, ang = flange_down_w(R)
for _ in range(n_max):
if float(np.linalg.norm(remain)) < inner_tol and (
(not verticalize) or ang < 0.006
):
break
nseg = max(1, int(math.ceil(float(np.linalg.norm(remain)) / 0.012)))
step = remain / nseg
if verticalize and ang > 0.008:
qdot = dls_twist(Jj, step / 0.12, w_base=w_step, damping=damping, max_qdot=0.45)
dq = np.clip(qdot * 0.12, -float(max_dq), float(max_dq))
else:
dq = dls_translate_hold_ori(Jj, step, damping=damping, max_dq=max_dq)
if float(np.max(np.abs(dq))) < 1e-5:
break
q = q + dq
wps.append(q.copy())
remain = remain - (Jj[:3, :] @ dq)
ang = max(0.0, ang - float(np.linalg.norm(w_step)) * 0.12)
return wps, float(np.linalg.norm(remain))
def trap_joint_schedule(
waypoints,
dist: float,
v: float,
a: float,
*,
t0: float = 0.06,
dt: float = 0.04,
t_hold: float = 0.25,
):
"""路点 + 梯形速度 → ``[(q, qdot, t), ...]``。头尾速度为 0，时间严格递增。
一次下发整段。中途再发下一条会把这条掐掉，臂就重新加速 —— 那就是抖。
"""
dist = max(float(dist), 1e-6)
T_move, v_peak, t_acc, t_cruise, a_use = trap_params(dist, v, a)
n_samp = max(8, int(math.ceil(T_move / max(float(dt), 1e-3))))
zeros = np.zeros_like(waypoints[0])
out = []
last_t = -1.0
for k in range(n_samp + 1):
t_local = T_move * k / n_samp
s, vs = trap_sv(t_local, dist, v_peak, t_acc, t_cruise, a_use)
u = s / dist
qi, dq_du = path_interp(waypoints, u)
qdot = dq_du * (vs / dist) if k < n_samp else zeros
if k >= n_samp:
qi = waypoints[-1]
qdot = zeros
t = float(t0) + t_local
if t <= last_t:
t = last_t + 0.02
out.append((np.asarray(qi, dtype=float).copy(), np.asarray(qdot, dtype=float).copy(), t))
last_t = t
hold = max(0.12, float(t_hold))
for frac in (0.40, 0.70, 1.00):
t = float(t0) + T_move + hold * frac
if t < last_t + 0.05:
t = last_t + 0.05
out.append((np.asarray(waypoints[-1], dtype=float).copy(), zeros.copy(), t))
last_t = t
return out, T_move, v_peak
def smooth_joint_schedule(
waypoints,
dist: float,
v: float,
*,
t0: float = 0.06,
dt: float = 0.04,
t_hold: float = 0.25,
d_acc: float = D435_FULL_ACC_M,
d_dec: float = D435_FULL_DEC_M,
):
"""路点 + 余弦加减速 → ``[(q, qdot, t), ...]``。头尾速度为 0。
D435 整段一次下发用这一份。二阶段短跳仍走 ``trap_joint_schedule``。
"""
dist = max(float(dist), 1e-6)
T_move, v_peak, t_acc, t_cruise, acc_m, dec_m = smooth_path_params(
dist, v, d_acc=d_acc, d_dec=d_dec
)
n_samp = max(12, int(math.ceil(T_move / max(float(dt), 1e-3))))
zeros = np.zeros_like(waypoints[0])
out = []
last_t = -1.0
for k in range(n_samp + 1):
t_local = T_move * k / n_samp
s, vs = smooth_path_sv(
t_local, dist, v_peak, t_acc, t_cruise, acc_m, dec_m
)
u = min(1.0, s / dist)
qi, dq_du = path_interp(waypoints, u)
qdot = dq_du * (vs / dist) if k < n_samp else zeros
if k >= n_samp:
qi = waypoints[-1]
qdot = zeros
t = float(t0) + t_local
if t <= last_t:
t = last_t + 0.02
out.append(
(
np.asarray(qi, dtype=float).copy(),
np.asarray(qdot, dtype=float).copy(),
t,
)
)
last_t = t
hold = max(0.12, float(t_hold))
for frac in (0.40, 0.70, 1.00):
t = float(t0) + T_move + hold * frac
if t < last_t + 0.05:
t = last_t + 0.05
out.append((np.asarray(waypoints[-1], dtype=float).copy(), zeros.copy(), t))
last_t = t
return out, T_move, v_peak
src/arm_application/arm_application/perception_node.py
+97 -5
View File
@@ -31,7 +31,7 @@ from rclpy.qos import (
qos_profile_sensor_data,
)
from sensor_msgs.msg import CameraInfo, Image
from std_msgs.msg import Float32, Float32MultiArray
from std_msgs.msg import Bool, Float32, Float32MultiArray
from tf2_ros import TransformBroadcaster
from arm_application.box_tracker import BoxTracker
@@ -46,11 +46,16 @@ from arm_application.handeye_realsense import (
resolve_realsense_topics,
)
from arm_application.overhead_oneshot import (
D405_MISS_BEFORE_D435_S,
ONESHOT_ACCEPT_TIMEOUT_S,
best_det_conf,
depth_raw_to_m,
oneshot_k0_base,
oneshot_shot_timed_out,
oneshot_should_submit,
select_oneshot_det,
wrist_needs_handoff,
z_locked_qos,
)
from arm_application.kalman_filter import KalmanFilter3D
from arm_application.plane_feature import (
@@ -161,6 +166,7 @@ class PerceptionNode(TolerantNode):
# ---- D405 主跟踪 + D435 一次性（键必须从 site 接入，缺了启动即红）----
self.declare_parameter("wrist_handoff_conf", -1.0)
self.declare_parameter("overhead_oneshot_enable", False)
self.declare_parameter("d435_move_only", False)
self.declare_parameter("overhead_min_depth_m", -1.0)
self.declare_parameter("overhead_max_depth_m", -1.0)
self.declare_parameter("overhead_oneshot_conf", -1.0)
@@ -210,9 +216,10 @@ class PerceptionNode(TolerantNode):
# 跟踪点三道守卫（单一实现见 feature_guard.py）
self.keypoint_border_px = float(self.get_parameter("keypoint_border_px").value)
self.track_max_step_m = float(self.get_parameter("track_max_step_m").value)
self.reacquire_far_px = float(self.get_parameter("reacquire_far_px").value)
self._slew = TrackSlewGuard(self.track_max_step_m)
self._far = FarReacquireConfirm(
float(self.get_parameter("reacquire_far_px").value),
self.reacquire_far_px,
int(self.get_parameter("reacquire_confirm_frames").value),
)
self.plane_inlier_m = float(self.get_parameter("plane_inlier_m").value)
@@ -220,6 +227,7 @@ class PerceptionNode(TolerantNode):
self.overhead_oneshot_enable = bool(
self.get_parameter("overhead_oneshot_enable").value
)
self.d435_move_only = bool(self.get_parameter("d435_move_only").value)
self._T_oh: Optional[np.ndarray] = None
self._oh_image: Optional[Image] = None
self._oh_depth: Optional[Image] = None
@@ -230,6 +238,11 @@ class PerceptionNode(TolerantNode):
self._oneshot_t = -1.0e9
self._need_oneshot = False
self._oneshot_inflight = False
self._oneshot_shot = False
self._oneshot_shot_t = -1.0e9
self._low_since: Optional[float] = None
self._d435_commanding = False
self._z_locked = False
self._oneshot_img: Optional[Image] = None
self._oneshot_depth: Optional[Image] = None
if self.overhead_oneshot_enable:
@@ -441,10 +454,28 @@ class PerceptionNode(TolerantNode):
CameraInfo, oh_info, self._on_oh_info, image_qos,
callback_group=self._io_cbg,
)
self.create_subscription(
Bool, "visual_servo/z_locked", self._on_z_locked, z_locked_qos(),
callback_group=self._io_cbg,
)
self.create_subscription(
Bool,
"visual_servo/d435_commanding",
self._on_d435_commanding,
z_locked_qos(),
callback_group=self._io_cbg,
)
extra = (
"；d435_move_only：不等 D405 丢目标，0.5s 后必拍一张"
if self.d435_move_only
else ""
)
self.get_logger().info(
f"D435 一次性 image={oh_img} depth={oh_dep} "
f"D435 待命 image={oh_img} depth={oh_dep} "
f"handoff_conf={self.wrist_handoff_conf:.2f} "
f"看不到满 {D405_MISS_BEFORE_D435_S:.1f}s 才拍一张 "
f"T={self.get_parameter('overhead_handeye_file').value}"
f"{extra}"
)
point_qos = QoSProfile(
@@ -865,9 +896,52 @@ class PerceptionNode(TolerantNode):
self._publish_wrist_conf(best_conf)
handoff = bool(
self.overhead_oneshot_enable
and wrist_needs_handoff(best_conf, self.wrist_handoff_conf)
and (
self.d435_move_only
or wrist_needs_handoff(best_conf, self.wrist_handoff_conf)
)
)
now_m = time.monotonic()
if self._d435_commanding:
# 臂还在赶 D435 那个点。漏检计时清掉，走到并停稳之后再重新计。
self._low_since = None
self._need_oneshot = False
elif not handoff:
self._low_since = None
self._oneshot_shot = False
self._need_oneshot = False
else:
if oneshot_shot_timed_out(
shot=self._oneshot_shot,
commanding=self._d435_commanding,
age_s=now_m - float(self._oneshot_shot_t),
timeout_s=ONESHOT_ACCEPT_TIMEOUT_S,
):
self.get_logger().warn(
"D435 拍了但伺服没接住（工作盒拒收等），允许再拍"
)
self._oneshot_shot = False
self._low_since = None
if self._low_since is None:
self._low_since = now_m
low_for = now_m - self._low_since
self._need_oneshot = oneshot_should_submit(
handoff=True,
low_for_s=low_for,
already_shot=self._oneshot_shot,
wait_s=D405_MISS_BEFORE_D435_S,
)
if self._oneshot_shot:
self.get_logger().info(
"D435 这一轮已经拍过一张，等这一段走完，不再连拍",
throttle_duration_sec=2.0,
)
elif not self._need_oneshot:
self.get_logger().info(
f"D405 已连续 {low_for:.2f}s 没有高置信度目标，"
f"未满 {D405_MISS_BEFORE_D435_S:.1f}s，D435 继续待命",
throttle_duration_sec=0.5,
)
self._need_oneshot = handoff
now = time.monotonic()
pred_at_img = self._box_trk.predict(t_img, self._img_wh())
with self._lock:
@@ -1326,6 +1400,21 @@ class PerceptionNode(TolerantNode):
msg.data = float("nan") if best_conf is None else float(best_conf)
self._wrist_conf_pub.publish(msg)
def _on_z_locked(self, msg: Bool) -> None:
"""锁高状态只记着。拍不拍 D435 不看它，看 D405 连续漏检。"""
self._z_locked = bool(msg.data)
def _on_d435_commanding(self, msg: Bool) -> None:
"""伺服还在执行 D435 那一个点时，不再提交下一张。
这一段结束（到位并停稳）后清掉「已拍过」，漏检重新计 0.5 s。
"""
commanding = bool(msg.data)
if self._d435_commanding and not commanding:
self._oneshot_shot = False
self._low_since = None
self._d435_commanding = commanding
def _publish_overhead_k0(self, p_base: np.ndarray, is_k0: bool) -> None:
pt = PointStamped()
pt.header.stamp = self.get_clock().now().to_msg()
@@ -1447,6 +1536,9 @@ class PerceptionNode(TolerantNode):
)
return
p_base, is_k0 = got
self._oneshot_shot = True
self._oneshot_shot_t = now
self._need_oneshot = False
self._publish_overhead_k0(p_base, is_k0)
self.get_logger().info(
f"D435 一次性定位 {'k0' if is_k0 else '框心（顶视看不见 k0）'} "
src/arm_application/arm_application/pipeline_node.py
+62 -11
View File
@@ -26,16 +26,33 @@ from std_msgs.msg import Bool, String
from arm_application.stage3_search_node import Stage3Search
#: 一阶段跟踪高相对二阶段悬停高的**抬升量** [m]。
#: 悬停时规尖离面不够，跟踪高再抬的量 [m]。
#:
#: 为什么要抬：跟踪时通止规已经夹在爪上，规尖比相机更靠下；悬停高是"相机拍清
#: 孔"的高度，直接在那儿跟踪会让规尖刮到工件。抬 5cm 让它离开工件，二阶段再
#: 小幅下降（只需 5cm 以内的行程）。
#: 只在「悬停高上规尖离工件面 < :data:`TRACK_LOCK_GAP_M`」时加上。仿真悬停按
#: 工具几何只留约 2mm，不抬就会在跟踪时刮到工件。真机 89 悬停 0.320 时规尖
#: 已经在面上方约 75mm，再抬 50mm 会让对孔前先改一次高度（0.37→0.32）。
#:
#: ⚠️ 抽成常量是因为它不只被流程用：``scripts/check_sim_scene.py`` 要用它核对
#: "跟踪高有没有超出工作盒上界"（仿真里悬停 0.632 + 0.05 = 0.682 > 真机那套
#: 0.65 上界，就是被这条查出来的）。两处各写一个字面量迟早漂移。
TRACK_CLEARANCE_M = 0.05
#: 悬停时规尖离面已经不少于这个值 [m]，一阶段就锁在悬停高，对孔完成后再下降。
TRACK_LOCK_GAP_M = 0.040
def track_lock_z(hover_z: float, tool_len_m: float, surface_z_m: float) -> float:
"""一阶段锁高。
悬停时规尖离面已经 ≥ ``TRACK_LOCK_GAP_M``：锁在悬停高，跟踪和对孔都不改 Z，
对孔完成后再下降。离面不够（仿真悬停只留 2mm）：再抬 ``TRACK_CLEARANCE_M``，
否则跟踪会刮到工件。工具长或工件面缺失时抬升 —— 缺数时锁在悬停高等于没检查。
"""
tool = float(tool_len_m)
surf = float(surface_z_m)
hover = float(hover_z)
if tool > 0.0 and surf > 0.0 and (hover - tool - surf) >= TRACK_LOCK_GAP_M:
return hover
return hover + TRACK_CLEARANCE_M
def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) -> str:
@@ -79,6 +96,7 @@ class AssemblyPipeline(Stage3Search):
self.declare_parameter("skip_stage1", False)
self.declare_parameter("skip_stage2", False)
self.declare_parameter("skip_stage3", False) # 默认搜孔+轻压；拧入靠 --depth
self.declare_parameter("d435_move_only", False)
self.declare_parameter("stage1_timeout_sec", 150.0)
self.linger3 = 1.2 # 串联结束短保持；单独跑 stage3 仍用 site linger_sec
@@ -113,9 +131,11 @@ class AssemblyPipeline(Stage3Search):
skip1 = bool(self.get_parameter("skip_stage1").value)
skip2 = bool(self.get_parameter("skip_stage2").value)
skip3 = bool(self.get_parameter("skip_stage3").value)
self.d435_move_only = bool(self.get_parameter("d435_move_only").value)
self.get_logger().info(
f"pipeline arm_ee={self.ee_frame} hole={self.target_hole} "
f"skip1={skip1} skip2={skip2} skip3={skip3} insert={self.insert_depth_mm:.1f}mm"
f"skip1={skip1} skip2={skip2} skip3={skip3} "
f"d435_only={self.d435_move_only} insert={self.insert_depth_mm:.1f}mm"
)
def _state(self, s: str) -> None:
@@ -163,8 +183,12 @@ class AssemblyPipeline(Stage3Search):
self._kill_old_stage1()
def _track_z(self) -> float:
"""跟踪高度 = 孔悬停高 + :data:`TRACK_CLEARANCE_M`，通止规离开工件。"""
return float(self.hover_z) + TRACK_CLEARANCE_M
"""一阶段锁高：见 :func:`track_lock_z`。对孔完成前不再改这个高度。"""
return track_lock_z(
float(self.hover_z),
float(getattr(self, "tool_tip_below_flange_m", 0.0) or 0.0),
float(getattr(self, "surface_z_m", 0.0) or 0.0),
)
def _ensure_clearance(self) -> None:
"""上一轮轻压可能还在孔里：先纯 Z 抬到跟踪高，禁止 0.23↔0.35 来回。"""
@@ -177,7 +201,7 @@ class AssemblyPipeline(Stage3Search):
return
self._state("pre_lift")
self.get_logger().info(
f"先离开工件/孔口：Z {z:.3f} → {tz:.3f}m（跟踪高=hover+50mm）"
f"先到锁高：Z {z:.3f} → {tz:.3f}m（跟踪与对孔都停在这个高度，对完孔再下降）"
)
_, q, Tee = self._goto(
q, Tee, np.array([float(Tee[0, 3]), float(Tee[1, 3]), tz]),
@@ -206,7 +230,13 @@ class AssemblyPipeline(Stage3Search):
"publish_handeye_tf:=false",
f"ee_z_m:={lock_z:.3f}",
f"use_sim_time:={str(use_sim).lower()}",
f"d435_move_only:={str(self.d435_move_only).lower()}",
]
if self.d435_move_only:
self.get_logger().info(
f"启动 D435 位移测试（锁高 {lock_z:.3f}m，到位后不交 D405、不进二阶段）"
)
else:
self.get_logger().info(
f"启动一阶段跟踪（锁高 {lock_z:.3f}m，锁定后自动交二阶段）"
)
@@ -229,6 +259,11 @@ class AssemblyPipeline(Stage3Search):
stage1_timeout_reason(self._point_msgs, timeout, log_path)
)
return 1
if self.d435_move_only:
self.get_logger().info(
"D435 位移测试结束 — 停跟踪、钉住当前关节，跳过二阶段"
)
else:
self.get_logger().info("一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段")
self._stop_stage1()
time.sleep(0.35)
@@ -241,6 +276,9 @@ class AssemblyPipeline(Stage3Search):
skip1 = bool(self.get_parameter("skip_stage1").value)
skip2 = bool(self.get_parameter("skip_stage2").value)
skip3 = bool(self.get_parameter("skip_stage3").value)
if self.d435_move_only:
skip2 = True
skip3 = True
if not self._cli.wait_for_server(timeout_sec=5.0):
self.get_logger().error("JTC action 不在 —— 先开 T1: run_real.sh --arm 89")
return 2
@@ -268,17 +306,30 @@ class AssemblyPipeline(Stage3Search):
return code
self.get_logger().info(
f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm"
+ (" — 跳过搜孔，保持悬停" if skip3 else " — 立刻下压搜孔并轻压（不拧入）")
+ (
" — 跳过搜孔，停在观测高度（看规尖是否对中；进程退出后臂保持）"
if skip3
else " — 立刻下压搜孔并轻压（不拧入）"
)
)
else:
self._state("skip_stage2")
if skip3:
self._state("skip_stage3")
self.get_logger().info("跳过搜孔 —— 臂保持在孔上方")
if self.d435_move_only:
self.get_logger().info(
"D435 位移测试结束 —— 臂停在当前高度，不交 D405、不对孔；"
"15s 后本进程退出，臂保持不动"
)
else:
self.get_logger().info(
"跳过搜孔 —— 臂停在当前高度。请看规尖与孔是否对中；"
"15s 后本进程退出，臂保持不动"
)
if final:
self._publish_result(final)
self._linger_hold(final if final else {"ok": True}, seconds=0.8)
self._linger_hold(final if final else {"ok": True}, seconds=15.0)
payload = {"ok": True, "stage": "pipeline", "exit": 0, "skipped": "stage3"}
try:
payload["stage2"] = json.loads(
src/arm_application/arm_application/servo_node.py
+543 -94
View File
@@ -24,9 +24,17 @@ import rclpy
from geometry_msgs.msg import PointStamped, Twist
from std_msgs.msg import Bool, Float32
from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup
from rclpy.action import ActionClient
from rclpy.executors import MultiThreadedExecutor
from rclpy.node import Node
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy, qos_profile_system_default
from control_msgs.action import FollowJointTrajectory
from rclpy.qos import (
DurabilityPolicy,
HistoryPolicy,
QoSProfile,
ReliabilityPolicy,
qos_profile_system_default,
)
from sensor_msgs.msg import JointState
from tf2_ros import Buffer, TransformListener
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
@@ -39,7 +47,26 @@ from arm_application.geom_ik import (
quat_xyzw_to_R,
)
from arm_application.handeye_lib import cam_point_to_base, cam_to_base_T
from arm_application.overhead_oneshot import StillLock, wrist_needs_handoff
from arm_application.overhead_oneshot import (
D435_NEAR_CRUISE_MPS,
D435_SETTLE_S,
StillLock,
at_lock_height,
d405_commands_arm,
flange_tilt_rad,
hold_translation_vz,
hybrid_cruise_vxy,
lock_z_velocity,
oneshot_concat_hold_ori,
oneshot_full_cruise_mps,
oneshot_full_segments,
oneshot_hop_limits,
oneshot_may_send_another,
oneshot_needs_verticalize,
wrist_needs_handoff,
z_locked_qos,
)
from arm_application.path_profile import smooth_joint_schedule, translate_hold_ori_waypoints
from arm_application.pid_controller import PidController
from arm_application.kalman_filter import KalmanFilter3D
from arm_application.feature_jump_guard import (
@@ -83,6 +110,9 @@ class ServoNode(TolerantNode):
self.declare_parameter("ee_frame", ee_frame_default())
self.declare_parameter("camera_frame", "d405_color_optical_frame")
self.declare_parameter("traj_topic", "rizon_arm_controller/joint_trajectory")
self.declare_parameter(
"action_name", "rizon_arm_controller/follow_joint_trajectory"
)
self.declare_parameter("point_topic", "visual_servo/filtered_point")
# 默认值只作兜底：真机运行一定由 launch 传入 site.yaml 的 paths.handeye_file。
# 指向 /tmp 是历史遗留（标定临时产物），会让人误以为"标定结果在那里"。
@@ -187,6 +217,8 @@ class ServoNode(TolerantNode):
self.declare_parameter("lock_still_move_m", -1.0)
self.declare_parameter("wrist_handoff_conf", -1.0)
self.declare_parameter("overhead_hold_s", -1.0)
# 只测 D435 位移：到位后不交 D405、不靠 D405 静止去锁二阶段。
self.declare_parameter("d435_move_only", False)
self.declare_parameter("overhead_k0_topic", "visual_servo/overhead_k0")
self.declare_parameter("wrist_conf_topic", "visual_servo/wrist_conf")
@@ -271,6 +303,7 @@ class ServoNode(TolerantNode):
self.lock_still_move_m = float(self.get_parameter("lock_still_move_m").value)
self.wrist_handoff_conf = float(self.get_parameter("wrist_handoff_conf").value)
self.overhead_hold_s = float(self.get_parameter("overhead_hold_s").value)
self.d435_move_only = bool(self.get_parameter("d435_move_only").value)
self._hybrid = (
self.lock_still_sec >= 0.0
and self.lock_still_move_m >= 0.0
@@ -281,6 +314,26 @@ class ServoNode(TolerantNode):
still_sec=max(0.0, self.lock_still_sec),
still_move_m=max(0.0, self.lock_still_move_m),
)
# D435 那一段的「到位并停稳」，和进二阶段用同一套静止判据。
self._arrive = StillLock(
still_sec=max(0.0, self.lock_still_sec),
still_move_m=max(0.0, self.lock_still_move_m),
)
self._d435_settled = False
self._d435_exec_until = 0.0
self._d435_pinned = False
self._d435_sent = False
self._d435_last_dist = None
self._d435_give_up = False
self._d435_plan_lock = threading.Lock()
self._d435_inflight = False
self._d435_settle_until = 0.0
self._d435_last_z_err = None
self._d435_z_stall = 0
self._d435_remeasure_log = False
self._d435_vert_done = False
self._d435_full_done = False
self._d435_fix_done = False
self._track_source = "wrist" # wrist | overhead
self._wrist_conf: Optional[float] = None
self._wrist_conf_t = -1.0
@@ -337,6 +390,11 @@ class ServoNode(TolerantNode):
f"optical_vs_flangeZ={self._cam_tilt_deg:.1f}deg "
f"— 悬停使用这个 T，而不是用 TF 查询"
)
if self.d435_move_only:
self.get_logger().warn(
"d435_move_only：只跑 D435 单次位移。到位停稳后钉住，"
"不交 D405；locked 只用来结束一阶段，不进二阶段"
)
self.pid_x = PidController(
float(self.get_parameter("kp_xy").value),
@@ -391,6 +449,7 @@ class ServoNode(TolerantNode):
self._jump_gate = FeatureJumpGate(self.predict_jump_m, self.jump_confirm_frames)
# 只有当法兰 Z 已经落到锁定平面上（深度新鲜）才开启 XY 悬停。
self._xy_armed = bool(self.ee_z_m <= 0.0)
self._z_hold: Optional[float] = None
# Seek（搜索）状态：在锁定高度且没有目标时扫描工件。
self._seek_t0 = 0.0
self._seek_theta = 0.0
@@ -447,11 +506,30 @@ class ServoNode(TolerantNode):
)
# 一阶段锁定信号（串联编排器用它判断可以进入二阶段）
self._lock_pub = self.create_publisher(Bool, "visual_servo/locked", 1)
# TRANSIENT_LOCAL：感知节点比伺服晚起来，默认 QoS 会丢掉启动时那一拍
# True，感知就永远停在「还没到锁高」、D435 不拍。两边必须同一份 QoS。
self._z_lock_pub = self.create_publisher(
Bool, "visual_servo/z_locked", z_locked_qos()
)
# 感知用它挡住「平移半路再拍一张」。晚启动也能拿到当前值。
self._d435_cmd_pub = self.create_publisher(
Bool, "visual_servo/d435_commanding", z_locked_qos()
)
self._d435_cmd_sent: Optional[bool] = None
self._z_locked_sent: Optional[bool] = None
self._z_lock_pub_mono = 0.0
self._lock_since: Optional[float] = None
self._locked = False
self._lock_pub_t = 0.0
traj_topic = str(self.get_parameter("traj_topic").value)
self._traj_pub = self.create_publisher(JointTrajectory, traj_topic, cmd_qos)
self._jtc_cbg = ReentrantCallbackGroup()
self._jtc = ActionClient(
self,
FollowJointTrajectory,
str(self.get_parameter("action_name").value),
callback_group=self._jtc_cbg,
)
self._tf = Buffer()
TransformListener(self._tf, self, spin_thread=True)
@@ -477,6 +555,36 @@ class ServoNode(TolerantNode):
f"hold_xy={int(self.ee_hold_xy)} down={int(self.ee_vertical_down)} "
f"XY_after_lock_z±{self.approach_xy_gate_m*1000:.0f}mm"
)
self._publish_z_locked(self._xy_armed)
self._publish_d435_commanding(False)
def _publish_z_locked(self, locked: bool) -> None:
"""法兰是否在锁高带里。只记日志，不拦 D435。
状态没变也每 0.5 s 再发一次：只发一次的话，晚订阅的感知收不到。
"""
locked = bool(locked)
now = time.monotonic()
changed = self._z_locked_sent is not locked
if (not changed) and (now - self._z_lock_pub_mono) < 0.5:
return
self._z_locked_sent = locked
self._z_lock_pub_mono = now
self._z_lock_pub.publish(Bool(data=locked))
if not changed:
return
if locked:
self.get_logger().info("锁高已到位")
else:
self.get_logger().info("离开锁高带 —— 平移仍保持当前高度，不改去追锁高")
def _publish_d435_commanding(self, commanding: bool) -> None:
"""正在用 D435 那一个点赶路。感知看到 True 就不再拍第二张。"""
commanding = bool(commanding)
if self._d435_cmd_sent is commanding:
return
self._d435_cmd_sent = commanding
self._d435_cmd_pub.publish(Bool(data=commanding))
def _on_js(self, msg: JointState) -> None:
self._last_js = msg
@@ -504,8 +612,19 @@ class ServoNode(TolerantNode):
if not np.all(np.isfinite(p_base)):
return
if not self._feat_workspace_ok(p_base):
m = float(self.feat_ws_margin_m)
self.get_logger().warn(
f"D435 一次性点 ({p_base[0]:.3f},{p_base[1]:.3f}) 在工作盒外 —— 丢弃",
f"D435 一次性点 ({p_base[0]:.3f},{p_base[1]:.3f}) 在工作盒外 "
f"（x[{self.ws_min[0]:.2f},{self.ws_max[0]:.2f}] "
f"y[{self.ws_min[1]:.2f},{self.ws_max[1]:.2f}] ±{m:.2f}）"
"—— 丢弃，不发轨迹。这不是起步加速度。"
)
return
with self._lock:
busy = self._track_source == "overhead" and not self._d435_settled
if busy:
self.get_logger().info(
"D435 这一段还没走完，不再换目标",
throttle_duration_sec=1.0,
)
return
@@ -520,15 +639,31 @@ class ServoNode(TolerantNode):
self._have_target = True
self._target_lost_flag = False
self._track_source = "overhead"
self._d435_settled = False
self._overhead_t = now_m
self._last_seen_time = now_m
self._stamp_sec = now_sec
self._seek_t0 = 0.0
self._seek_warned = False
self._still.reset()
self._arrive.reset()
self._d435_exec_until = 0.0
self._d435_pinned = False
self._d435_sent = False
self._d435_last_dist = None
self._d435_give_up = False
self._d435_inflight = False
self._d435_settle_until = 0.0
self._d435_last_z_err = None
self._d435_z_stall = 0
self._d435_remeasure_log = False
self._d435_vert_done = False
self._d435_full_done = False
self._d435_fix_done = False
self._publish_d435_commanding(True)
self.get_logger().info(
f"D435 一次性设定 feat_base=({p_base[0]:.3f},{p_base[1]:.3f},{p_base[2]:.3f}) "
f"—— 迁过去等 D405 重新看见"
f"—— 走到这个点并停稳之前，不下发 D405 的点"
)
def _on_point(self, msg: PointStamped) -> None:
@@ -541,16 +676,26 @@ class ServoNode(TolerantNode):
# 已经收到腕部 conf 且低于门槛：这条 D405 点不能当跟踪源。
# conf 还是 None = 感知还没报到，不要把开场几帧也丢掉。
return
with self._lock:
using_d435 = self._track_source == "overhead"
settled = self._d435_settled
if self._hybrid and not d405_commands_arm(
using_d435=using_d435,
arrived=settled,
settled=settled,
move_only=self.d435_move_only,
):
# 推理服务不关。这一帧的点不下发给臂，目标仍是 D435 那一个点。
self.get_logger().info(
"D435 位置还没到并且停稳，D405 这帧不下发",
throttle_duration_sec=1.0,
)
return
p_cam = np.array(
[float(msg.point.x), float(msg.point.y), float(msg.point.z)], dtype=float
)
T_ee = self._lookup_T(self.base_frame, self.ee_frame)
p_base = self._feature_in_base(T_ee, p_cam) if T_ee is not None else None
at_lock = True
if self.ee_z_m > 0.0 and T_ee is not None:
at_lock = abs(float(T_ee[2, 3]) - self.ee_z_m) <= max(
0.02, float(self.approach_xy_gate_m)
)
if not self._he_cmp_done:
self._he_cmp_done = True
self._log_handeye_tf_vs_yaml()
@@ -558,12 +703,9 @@ class ServoNode(TolerantNode):
rejected = False
with self._lock:
was_lost = self._target_lost_flag
if self._track_source == "overhead":
# 从顶视切回腕部：允许一次重定位，跳变门不拿顶视点比。
was_lost = True
self._track_source = "wrist"
self._p_meas[:] = p_cam
if p_base is not None and at_lock and self._xy_armed:
if p_base is not None:
verdict = self._update_feat_track(p_base, was_lost=was_lost, now_m=now_m)
if verdict == ACCEPT:
self._p_feat_base = np.asarray(p_base, dtype=float).copy()
@@ -747,28 +889,25 @@ class ServoNode(TolerantNode):
lost = self._target_lost_flag
source = self._track_source
if self._hybrid:
self._tick_d435_arrive()
self._tick_still_lock()
if self.enable_control and self._d435_oneshot_holds_the_arm():
return
# ----- 启动阶段：**先**下降到锁定高度，甚至在任何检测到来之前。
# 在第一个 PointStamped 到达之前 have 一直是 False，所以这是一个纯启动
# 阶段：只动 Z 下降，XY 锁死，姿态朝下。
if (
self.enable_control
and self.ee_z_m > 0.0
and not have
and not self._xy_armed
):
# 还没有目标、人又停在锁高下面：只抬一次 Z。已经有 D435 点或腕部点之后
# 不再插这一步 —— 平移途中改高度会抖，也会把正在走的 XY 掐掉。
if self.enable_control and self.ee_z_m > 0.0 and not have:
T_ee0 = self._lookup_T(self.base_frame, self.ee_frame)
if T_ee0 is None:
return # TF 还没就绪 —— 等待，不要触发保持急停
self._update_xy_arm(float(T_ee0[2, 3]))
return
z_now = float(T_ee0[2, 3])
self._update_xy_arm(z_now)
if not self._xy_armed:
self.get_logger().info(
f"启动下降至 lock_z {self.ee_z_m:.3f}m "
f"当前={T_ee0[2, 3]:.3f}m（还没有目标，XY 锁死）",
f"先回锁高 {self.ee_z_m:.3f}m 当前={z_now:.3f}m（还没有目标，只动 Z）",
throttle_duration_sec=1.0,
)
self._target_lost_flag = False # 盲降期间允许执行
self._target_lost_flag = False
self._execute_twist(np.zeros(3, dtype=float), p_cam=None, hold_xy=True)
self._lock_feedback(False)
return
@@ -821,13 +960,10 @@ class ServoNode(TolerantNode):
seen_age = now_mono - last_seen
stamp_age = now_sec - stamp if stamp >= 0.0 else 1e9
if self._hybrid and source == "overhead":
if seen_age > self.overhead_hold_s:
self._lock_feedback(False)
self._watchdog_trip(
f"D435 一次性设定已过期 {seen_age:.1f}s > {self.overhead_hold_s:.1f}s"
)
return
stamp_age = 0.0 # 一次性不是流：别按图像戳判过期
# 一次性设定一直用到 D405 接上。按秒数冻住的话，臂还在平移就被看门狗
# 掐掉，D435 又不再拍，停在半路。
stamp_age = 0.0
seen_age = 0.0
elif seen_age > self.watchdog_timeout or stamp_age > self.lost_timeout:
self._watchdog_trip(
f"目标丢失 seen_age={seen_age:.3f}s stamp_age={stamp_age:.3f}s"
@@ -1176,12 +1312,343 @@ class ServoNode(TolerantNode):
self._xy_in_dead = dead
return dead
def _jacobian_at(self, p_ee: np.ndarray):
link_R: List[np.ndarray] = []
link_p: List[np.ndarray] = []
for fr in self.link_frames:
Tl = self._lookup_T(self.base_frame, fr)
if Tl is None:
return None
link_R.append(Tl[:3, :3])
link_p.append(Tl[:3, 3])
return geometric_jacobian(link_R, link_p, p_ee, RIZON_LINK_AXES)
def _d435_oneshot_holds_the_arm(self) -> bool:
"""D435 到点平移：action 短跳，等结果再重测。D405 主跟踪仍走原来的伺服。"""
if self.d435_move_only and self._hybrid and self._track_source == "overhead" and self._d435_settled:
# 位移测完了：守着，不许掉进 D405 的 _execute_twist。hold 只在到位那一拍发一次。
return True
if not (self._hybrid and self._track_source == "overhead" and not self._d435_settled):
return False
if not self._d435_plan_lock.acquire(blocking=False):
return True
try:
return self._d435_oneshot_holds_locked()
finally:
self._d435_plan_lock.release()
def _d435_pin_and_stop_hops(self, why: str) -> None:
if not self._d435_pinned:
self._send_hold()
self._d435_pinned = True
self._d435_give_up = True
self.get_logger().error(why)
def _sanitize_d435_traj(self, traj: JointTrajectory, min_dt: float = 0.02) -> JointTrajectory:
"""与二阶段 ``_sanitize_traj`` 同一条：time_from_start 必须严格递增。"""
last = -min_dt
for p in traj.points:
t = float(p.time_from_start.sec) + float(p.time_from_start.nanosec) * 1e-9
if t < last + min_dt:
t = last + min_dt
ns = int(round(t * 1e9))
p.time_from_start.sec = ns // 1_000_000_000
p.time_from_start.nanosec = ns % 1_000_000_000
last = t
return traj
def _d435_send_action(
self,
sched,
tag: str,
hop_m: float,
leftover_m: float,
v_peak: float,
T_move: float,
*,
ee_z: float,
tilt_deg: float,
max_dq: float,
) -> None:
"""先占住 in-flight 再 ``send_goal_async``。话题插点会把这条掐掉。"""
traj = JointTrajectory()
traj.header.stamp.sec = 0
traj.header.stamp.nanosec = 0
traj.joint_names = list(self.joint_names)
for qi, qdot, t_sec in sched:
pt = JointTrajectoryPoint()
pt.positions = [float(v) for v in np.asarray(qi).tolist()]
pt.velocities = [float(v) for v in np.asarray(qdot).tolist()]
ns = int(round(max(0.0, float(t_sec)) * 1e9))
pt.time_from_start.sec = ns // 1_000_000_000
pt.time_from_start.nanosec = ns % 1_000_000_000
traj.points.append(pt)
traj = self._sanitize_d435_traj(traj)
goal = FollowJointTrajectory.Goal()
goal.trajectory = traj
self._d435_inflight = True
self._d435_sent = True
self._d435_pinned = False
fut = self._jtc.send_goal_async(goal)
fut.add_done_callback(self._d435_on_goal)
self.get_logger().info(
f"D435 action 下发 {len(sched)} 点 tag={tag} 本跳={hop_m*1000:.0f}mm "
f"剩={leftover_m*1000:.0f}mm v={v_peak*1000:.0f}mm/s T={T_move:.1f}s "
f"ee_z={ee_z:.3f} tilt={tilt_deg:.2f}deg max|Δq|={max_dq:.4f}rad "
f"—— 结果回来之前不再发下一条"
)
def _d435_on_goal(self, fut) -> None:
try:
handle = fut.result()
except Exception as exc:
self.get_logger().error(f"D435 JTC 目标回调失败：{exc}")
self._d435_inflight = False
self._d435_give_up = True
return
if handle is None or not bool(getattr(handle, "accepted", False)):
self.get_logger().error("JTC 拒绝了 D435 轨迹")
self._d435_inflight = False
self._d435_give_up = True
return
handle.get_result_async().add_done_callback(self._d435_on_result)
def _d435_on_result(self, fut) -> None:
ok = True
try:
r = fut.result()
inner = getattr(r, "result", None)
code = getattr(inner, "error_code", 0) if inner is not None else 0
if code not in (0, None):
ok = False
msg = getattr(inner, "error_string", "")
self.get_logger().error(f"D435 JTC 执行失败 error_code={code} {msg}")
except Exception as exc:
ok = False
self.get_logger().error(f"D435 JTC 结果失败：{exc}")
self._d435_inflight = False
self._d435_settle_until = time.monotonic() + float(D435_SETTLE_S)
self._d435_remeasure_log = True
if not ok:
self._d435_give_up = True
def _d435_oneshot_holds_locked(self) -> bool:
now = time.monotonic()
if self._d435_inflight:
return True
if now < self._d435_settle_until:
return True
T_ee = self._lookup_T(self.base_frame, self.ee_frame)
q = self._current_joints()
if T_ee is None or q is None:
return True
# 只更新锁高标志。这一段不许把控制权交给回锁高流式。
self._update_xy_arm(float(T_ee[2, 3]))
with self._lock:
goal = None if self._p_feat_base is None else self._p_feat_base[:2].copy()
if goal is None:
return True
p = np.asarray(T_ee[:3, 3], dtype=float)
dist = float(np.hypot(float(goal[0]) - p[0], float(goal[1]) - p[1]))
arrive = float(self.lock_still_move_m)
z_lock = float(self.ee_z_m)
tilt = flange_tilt_rad(T_ee[:3, :3])
at = at_lock_height(float(p[2]), z_lock, self.approach_xy_gate_m)
if self._d435_remeasure_log:
self._d435_remeasure_log = False
self.get_logger().info(
f"D435 重测 leftover={dist*1000:.0f}mm "
f"dxy=({(p[0]-goal[0])*1000:+.0f},{(p[1]-goal[1])*1000:+.0f})mm "
f"ee_xy=({p[0]:.3f},{p[1]:.3f}) goal=({goal[0]:.3f},{goal[1]:.3f}) "
f"ee_z={p[2]:.3f} tilt={math.degrees(tilt):.2f}deg"
)
if self._d435_give_up:
self._d435_pinned = True
return True
hop_max, _cruise_unused = oneshot_hop_limits(
dist, hybrid_cruise_vxy(self.v_slow_xy)
)
if at and dist <= arrive:
if oneshot_needs_verticalize(
tilt, dist, arrive, already_done=self._d435_vert_done
):
return self._d435_send_verticalize(q, T_ee, leftover=dist, tilt=tilt)
# action 已经 rest-to-rest。到位成功不许再往话题插 hold，那会再抖一下。
self._d435_pinned = True
return True
if self._d435_full_done and self._d435_fix_done:
self._d435_pin_and_stop_hops(
f"D435 整段+补刀后还差 {dist*1000:.0f}mm —— 钉住不再补刀"
)
return True
if at and not oneshot_may_send_another(
dist, self._d435_last_dist, arrive_m=arrive, hop_max_m=hop_max
):
self._d435_pin_and_stop_hops(
f"D435 上一跳几乎没走近，剩差还是 {dist*1000:.0f}mm "
f"—— 钉住不再补刀，等停稳后交回 D405"
)
return True
if not at:
z_err = abs(float(p[2]) - z_lock)
if self._d435_last_z_err is not None and z_err > self._d435_last_z_err * 0.8:
self._d435_z_stall += 1
else:
self._d435_z_stall = 0
self._d435_last_z_err = z_err
if self._d435_z_stall >= 2:
self._d435_pin_and_stop_hops(
f"D435 收锁高连续无进展 当前={p[2]:.3f}m 期望={z_lock:.3f}m"
)
return True
if not self._jtc.server_is_ready():
self.get_logger().warn("D435 等 JTC action 就绪", throttle_duration_sec=1.0)
return True
J = self._jacobian_at(p)
if J is None:
self.get_logger().warn("D435 平移缺 link TF，等下一拍", throttle_duration_sec=1.0)
return True
segs = oneshot_full_segments(
p, goal, z_lock, gate_m=self.approach_xy_gate_m
)
if not segs:
return True
# XY / 收 Z 都不许夹收垂直：冻结 J 会把腕部转动漏成 Y 过冲。
wps, planned, asked, tag = oneshot_concat_hold_ori(
q, J, segs, R=T_ee[:3, :3]
)
if self._d435_full_done:
tag = "fix"
if len(wps) < 2 or asked < 1e-5 or planned < 0.75 * asked:
self._d435_pin_and_stop_hops(
f"D435 平移规划停滞 {planned*1000:.0f}/{asked*1000:.0f}mm —— 钉住不瞎发"
)
return True
max_dq = float(np.max(np.abs(wps[-1] - q)))
if max_dq > 1.0:
self._d435_pin_and_stop_hops("D435 平移关节行程 >1 rad，拒绝执行")
return True
cruise = oneshot_full_cruise_mps(dist, hybrid_cruise_vxy(self.v_slow_xy))
sched, T_move, v_peak = smooth_joint_schedule(wps, asked, cruise)
if tag in ("full", "fix", "xy"):
self._d435_last_dist = dist
if not self._d435_full_done:
self._d435_full_done = True
else:
self._d435_fix_done = True
self._d435_send_action(
sched,
tag,
asked,
dist,
v_peak,
T_move,
ee_z=float(p[2]),
tilt_deg=float(np.degrees(tilt)),
max_dq=max_dq,
)
return True
def _d435_send_verticalize(self, q, T_ee, *, leftover: float, tilt: float) -> bool:
"""XY 已到位后单独收法兰朝下。和平移拆开，避免 Y 过冲。"""
if not self._jtc.server_is_ready():
self.get_logger().warn("D435 收垂直等 JTC action 就绪", throttle_duration_sec=1.0)
return True
p = np.asarray(T_ee[:3, 3], dtype=float)
J = self._jacobian_at(p)
if J is None:
self.get_logger().warn("D435 收垂直缺 link TF，等下一拍", throttle_duration_sec=1.0)
return True
wps, _remain = translate_hold_ori_waypoints(
q, J, np.zeros(3, dtype=float), R=T_ee[:3, :3], verticalize=True
)
if len(wps) < 2:
self._d435_vert_done = True
self.get_logger().info(
f"D435 收垂直无需再动 tilt={math.degrees(tilt):.2f}deg"
)
return True
max_dq = float(np.max(np.abs(wps[-1] - q)))
if max_dq < 1e-4:
self._d435_vert_done = True
return True
if max_dq > 1.0:
self._d435_pin_and_stop_hops("D435 收垂直关节行程 >1 rad，拒绝执行")
return True
hop = max(0.008, float(tilt) * 0.05)
cruise = D435_NEAR_CRUISE_MPS
sched, T_move, v_peak = smooth_joint_schedule(wps, hop, cruise)
self._d435_vert_done = True
self._d435_send_action(
sched,
"vert",
hop,
leftover,
v_peak,
T_move,
ee_z=float(p[2]),
tilt_deg=float(np.degrees(tilt)),
max_dq=max_dq,
)
return True
def _tick_d435_arrive(self) -> None:
"""走到 D435 那个点，并且法兰按一阶段的静止判据停稳，才允许下发 D405。"""
if not self._hybrid:
return
if self._d435_inflight or time.monotonic() < self._d435_settle_until:
self._arrive.reset()
return
with self._lock:
if self._track_source != "overhead" or self._d435_settled:
return
goal = None if self._p_feat_base is None else self._p_feat_base[:2].copy()
if goal is None:
return
T = self._lookup_T(self.base_frame, self.ee_frame)
if T is None:
return
xy = np.asarray(T[:2, 3], dtype=float)
dist = float(np.hypot(float(xy[0]) - goal[0], float(xy[1]) - goal[1]))
arrived = dist <= float(self.lock_still_move_m) or self._d435_give_up
settled = self._arrive.update(
xy, time.monotonic(), tracking_ok=arrived
)
if not settled:
return
with self._lock:
self._d435_settled = True
self._d435_exec_until = 0.0
if self.d435_move_only:
# 保持 d435_commanding=True，感知不会重拍；D405 被 move_only 挡住。
# locked 只让 pipeline 结束一阶段，后面会 skip_stage2。
# action 已 rest-to-rest，到位不再往话题插 hold。
now_m = time.monotonic()
if not self._locked:
self._locked = True
self.get_logger().info(
f"已到 D435 位置（还差 {dist*1000:.0f}mm）并停稳 "
f"{self.lock_still_sec:.0f}s —— D435 位移测试结束，"
"不交 D405、不进二阶段"
)
self._lock_pub.publish(Bool(data=True))
self._lock_pub_t = now_m
elif now_m - self._lock_pub_t > 1.0:
self._lock_pub_t = now_m
self._lock_pub.publish(Bool(data=True))
return
self._publish_d435_commanding(False)
self.get_logger().info(
f"已到 D435 位置（还差 {dist*1000:.0f}mm）并停稳 "
f"{self.lock_still_sec:.0f}s —— 改回下发 D405"
)
def _tick_still_lock(self) -> None:
"""D405 跟住且法兰 XY 连续 still_sec 秒没大幅动 → 锁定进二阶段。
顶视一次性迁臂不算。门槛与位移上限只从 site 来，这里不再写死 3 / 15mm。
"""
if not self._hybrid:
if not self._hybrid or self.d435_move_only:
return
T = self._lookup_T(self.base_frame, self.ee_frame)
xy = None if T is None else np.asarray(T[:2, 3], dtype=float)
@@ -1284,48 +1751,31 @@ class ServoNode(TolerantNode):
return out
def _update_xy_arm(self, ee_z: float) -> bool:
"""只有当法兰到达 lock_z（真实深度、自适应悬停）后才做 XY 悬停。"""
"""只有法兰落在锁高带内才做 XY。低于锁高要先抬回去，不算到位。"""
if self.ee_z_m <= 0.0:
self._xy_armed = True
self._publish_z_locked(True)
return True
gate = max(0.02, float(self.approach_xy_gate_m))
z = float(ee_z)
# 已经在锁高或更低：立刻开 XY，禁止再抬回 0.35（否则二阶段又要降下来）。
if z <= float(self.ee_z_m) + gate:
if not self._xy_armed:
at = at_lock_height(z, self.ee_z_m, self.approach_xy_gate_m)
if at and not self._xy_armed:
self._xy_armed = True
self._feat_kf.initialized = False
self._p_feat_base = None
self._v_xy_cmd[:] = 0.0
self.pid_x.reset()
self.pid_y.reset()
self.get_logger().warn(
f"lock-z 就绪 ee_z={z:.3f}m（期望 {self.ee_z_m:.3f}，不再回抬） "
"—— 在实时 handeye 上开始 XY 悬停"
self.get_logger().info(
f"锁高到位 ee_z={z:.3f}m（期望 {self.ee_z_m:.3f}）"
" —— 开始 XY；腕部无目标时 D435 可以拍一张"
)
return True
err = abs(z - float(self.ee_z_m))
if not self._xy_armed:
if err <= gate:
self._xy_armed = True
self._feat_kf.initialized = False
self._p_feat_base = None
self._v_xy_cmd[:] = 0.0
self.pid_x.reset()
self.pid_y.reset()
self.get_logger().warn(
f"已到达 lock-z ee_z={z:.3f}m（期望 {self.ee_z_m:.3f}）"
" —— 在实时 handeye 上开始 XY 悬停"
)
elif err > gate + 0.05:
elif (not at) and self._xy_armed:
self._xy_armed = False
self._p_feat_base = None
self._feat_kf.initialized = False
self._v_xy_cmd[:] = 0.0
self.get_logger().info(
f"已离开 lock-z 区间 ee_z={z:.3f}m —— XY 保持，重新下降",
f"离开锁高 ee_z={z:.3f}m（期望 {self.ee_z_m:.3f}）"
" —— 先回高度，XY 暂停",
throttle_duration_sec=1.0,
)
self._publish_z_locked(at)
return self._xy_armed
def _seek_velocity(self, p_ee: np.ndarray) -> Optional[np.ndarray]:
@@ -1392,7 +1842,8 @@ class ServoNode(TolerantNode):
now_m = time.monotonic()
dt_cmd = now_m - self._last_traj_mono
if self._last_traj_mono > 0.0 and dt_cmd < (1.0 / self.traj_stream_hz):
min_dt = 1.0 / self.traj_stream_hz
if self._last_traj_mono > 0.0 and dt_cmd < min_dt:
return
dt_use = float(np.clip(dt_cmd if self._last_traj_mono > 0.0 else 0.04, 0.015, 0.08))
@@ -1422,6 +1873,7 @@ class ServoNode(TolerantNode):
# hover：在任意锁定高度上把法兰 XY 停在三维特征点上。
# lookat：把光轴命中点（图像中心）驱到特征点上。
xy_ok = self._update_xy_arm(float(p_ee[2]))
cruise = hybrid_cruise_vxy(self.v_slow_xy)
if (
xy_ok
and (not hold_xy)
@@ -1437,19 +1889,10 @@ class ServoNode(TolerantNode):
if nerr > clip_xy:
err = err * (clip_xy / nerr)
nerr = clip_xy
# ---- 远处限速：抓错目标时只走一小段 ----
# 2026-10-08 真机：一阶段被一串高置信度假 k0 顶掉，伺服 0.7s 内以
# v_max_xy(0.45m/s) 把臂从 home (0.686,-0.113) 带到工作盒角落
# (0.083,-0.515)，之后 148s 全是"检测漏检"。留一道**与"目标真不真"
# 无关**的防线：目标离得远就慢走 —— 真目标（真 k0 离 145mm）只是晚
# ~1.8s 到位，假目标则最多把臂带走几厘米。
v_xy, why_slow = self._slow_xy_limit(nerr, v_xy)
if why_slow:
self.get_logger().info(
f"{why_slow} —— 限速 {self.v_slow_xy*1000:.0f}mm/s 逼近"
"（防追错目标大范围移动）",
throttle_duration_sec=1.0,
)
# 锁定前整段同一巡航速度。不再用 50mm 远近两档：
# 远处本来就慢，近处一放开就按 kp×误差冲，越近越抖。
if self._hybrid and not self._locked:
v_xy = min(float(v_xy), cruise)
if self._xy_deadzone(nerr, update=False):
v_base[0] = 0.0
v_base[1] = 0.0
@@ -1480,9 +1923,10 @@ class ServoNode(TolerantNode):
)
nxy = float(np.hypot(v_base[0], v_base[1]))
if nxy > v_xy > 0.0:
v_base[0] *= v_xy / nxy
v_base[1] *= v_xy / nxy
cap_xy = cruise if (self._hybrid and not self._locked) else v_xy
if nxy > cap_xy > 0.0:
v_base[0] *= cap_xy / nxy
v_base[1] *= cap_xy / nxy
# 在 base 系里保持法兰 XYZ（可配置，默认约 250 mm）。当 ee_hold_xy
# 为真时，视觉算出的 XY 会被覆盖。
@@ -1493,28 +1937,33 @@ class ServoNode(TolerantNode):
v_base[0] = 0.0
if abs(self.ee_y_m - p_ee[1]) < 0.004:
v_base[1] = 0.0
if (not xy_ok) or hold_xy:
# 有目标：XY 平移，Z 钉在这一段开始的高度。没目标才回锁高。
translating = (not hold_xy) and self._have_target
if (not translating) and ((not xy_ok) or hold_xy):
v_base[0] = 0.0
v_base[1] = 0.0
self._v_xy_cmd[:] = 0.0
if self.ee_z_m > 0.0:
self.get_logger().info(
f"下降至 lock_z {self.ee_z_m:.3f}m 当前={p_ee[2]:.3f}m XY 保持",
f"回锁高 {self.ee_z_m:.3f}m 当前={p_ee[2]:.3f}m XY 保持",
throttle_duration_sec=1.0,
)
shaped = self._shape_xy_vel(v_base[:2], dt_use)
v_base[0] = float(shaped[0])
v_base[1] = float(shaped[1])
if self.ee_z_m > 0.0:
ez_b = float(self.ee_z_m - p_ee[2])
# 已在锁高以下：不再抬臂。二阶段会降到 hole_hover_z。
if ez_b > 0.0:
v_base[2] = 0.0
if translating:
if self._z_hold is None:
self._z_hold = float(p_ee[2])
v_base[2] = hold_translation_vz(
float(p_ee[2]), self._z_hold, kp=kp, v_lim=float(self.v_max_z)
)
else:
vz_lim = float(self.v_max_z) if xy_ok else max(float(self.v_max_z), 0.08)
v_base[2] = float(np.clip(kp * ez_b, -vz_lim, vz_lim))
if abs(ez_b) < 0.004:
v_base[2] = 0.0
self._z_hold = None
if self.ee_z_m > 0.0:
vz_lim = max(float(self.v_max_z), 0.08)
v_base[2] = lock_z_velocity(
float(p_ee[2]), self.ee_z_m, kp=kp, v_lim=vz_lim
)
w_base = (
self._w_align_flange_down(R_ee)
@@ -1523,9 +1972,9 @@ class ServoNode(TolerantNode):
)
p_next = p_ee + v_base * T
if self.ee_z_m > 0.0:
# 绝不指令到配置平面以下
floor = max(self.min_ee_z, self.ee_z_m - 0.02)
if self.ee_z_m > 0.0 and not translating:
# 向下只许到锁高死区内。已经低于锁高时 v_z>0，这条不挡上抬。
floor = float(self.ee_z_m) - 0.004
if p_next[2] < floor and v_base[2] < 0.0:
v_base[2] = 0.0
p_next = p_ee + v_base * T
src/arm_application/arm_application/stage2_locate_node.py
+230 -96
View File
@@ -45,6 +45,12 @@ from arm_application.geom_ik import (
geometric_jacobian,
quat_xyzw_to_R,
)
from arm_application.path_profile import (
FROZEN_J_HOP_MAX_M,
path_interp as _path_interp,
trap_params as _trap_params,
trap_sv as _trap_sv,
)
from arm_application.handeye_realsense import resolve_realsense_topics
from arm_application.gauge_boot import perception_source_of
from arm_application.gauge_fsm import drag_verdict, hover_z_guard, wrench_all_zero
@@ -82,54 +88,6 @@ def _pt(q: np.ndarray, qdot: np.ndarray, t_sec: float) -> JointTrajectoryPoint:
return pt
def _trap_params(dist: float, v: float, a: float) -> Tuple[float, float, float, float, float]:
"""Rest-to-rest 梯形：返回 (T, v_peak, t_acc, t_cruise, a)。巡航速度保持 v。"""
dist = max(float(dist), 1e-6)
v = max(float(v), 1e-4)
a = max(float(a), 1e-3)
d_acc = v * v / (2.0 * a)
if 2.0 * d_acc >= dist:
v_peak = math.sqrt(max(a * dist, 1e-12))
t_acc = v_peak / a
return 2.0 * t_acc, v_peak, t_acc, 0.0, a
t_acc = v / a
t_cruise = (dist - 2.0 * d_acc) / v
return 2.0 * t_acc + t_cruise, v, t_acc, t_cruise, a
def _trap_sv(
t: float, dist: float, v_peak: float, t_acc: float, t_cruise: float, a: float,
) -> Tuple[float, float]:
"""时刻 t 的路径弧长 s 与切向速度。"""
t = max(0.0, float(t))
t_dec0 = t_acc + t_cruise
t_end = t_dec0 + t_acc
if t >= t_end:
return dist, 0.0
if t < t_acc:
return 0.5 * a * t * t, a * t
if t < t_dec0:
return 0.5 * a * t_acc * t_acc + v_peak * (t - t_acc), v_peak
td = t - t_dec0
s0 = 0.5 * a * t_acc * t_acc + v_peak * t_cruise
return s0 + v_peak * td - 0.5 * a * td * td, max(0.0, v_peak - a * td)
def _path_interp(waypoints: List[np.ndarray], u: float) -> Tuple[np.ndarray, np.ndarray]:
"""沿关节路点折线插值。u∈[0,1] → (q, dq/du)。"""
nseg = len(waypoints) - 1
if nseg <= 0:
z = np.zeros_like(waypoints[0])
return waypoints[0].copy(), z
u = float(min(1.0, max(0.0, u)))
x = u * nseg
i = min(nseg - 1, int(math.floor(x)))
a = x - i
q = (1.0 - a) * waypoints[i] + a * waypoints[i + 1]
dq_du = (waypoints[i + 1] - waypoints[i]) * nseg
return q, dq_du
def _sanitize_traj(traj: JointTrajectory, min_dt: float = 0.02) -> JointTrajectory:
"""JTC 要求 time_from_start 严格递增；短 hop 保持点曾经倒退导致拒轨。"""
last = -min_dt
@@ -246,6 +204,15 @@ class Stage2Locate(TolerantNode):
self.coarse_border_px = int(stage2.get("coarse_border_px") or 2)
# 最后轻降到孔面上方 final_gap_mm（不接触）
self.final_gap_m = float(stage2.get("final_gap_mm") or 1.5) / 1000.0
# 观测下降：对准后降到这个间隙就停，方便看规尖是不是对着孔。
# 比 final_gap（贴面 2mm）高一截，对偏了也不会把规压上孔沿。
self.observe_gap_m = float(stage2.get("observe_gap_mm") or 15.0) / 1000.0
if self.observe_gap_m < self.final_gap_m:
self.get_logger().warn(
f"observe_gap_mm={self.observe_gap_m*1000:.1f} 比贴面目标 "
f"{self.final_gap_m*1000:.1f}mm 还近 —— 收到贴面高度，避免观测下降钻进孔"
)
self.observe_gap_m = self.final_gap_m
# 悬停高度**必须**让工具尖端留在工件面上方（2026-09-24 的教训，完整说明见
# gauge_fsm.hover_z_guard）：悬停高是按相机视距定的，而工具（夹爪+通止规）
# 比相机长 —— 实测悬停时尖端已经钻进工件 25mm，于是 S05 的横向对准把工件
@@ -511,18 +478,10 @@ class Stage2Locate(TolerantNode):
return geometric_jacobian(link_R, link_p, p_ee, RIZON_LINK_AXES)
def _w_down(self, R: np.ndarray) -> Tuple[np.ndarray, float]:
"""让法兰 +Z 指向世界 -Z（垂直朝下）的角速度向量 + 当前倾角。"""
z_now = R[:, 2]
z_des = np.array([0.0, 0.0, -1.0])
c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0))
ang = float(np.arccos(c))
if ang < 0.006: # 0.34° 以下不再修，避免原地抖动
return np.zeros(3), ang
axis = np.cross(z_now, z_des)
n = float(np.linalg.norm(axis))
axis = axis / n if n > 1e-9 else np.array([1.0, 0.0, 0.0])
mag = float(np.clip(2.0 * ang, -0.5, 0.5))
return axis * mag, ang
"""让法兰 +Z 指向世界 -Z。公式只在 ``path_profile.flange_down_w``。"""
from arm_application.path_profile import flange_down_w
return flange_down_w(R)
def _wait_state(self, timeout_s: float) -> Tuple[Optional[np.ndarray], Optional[np.ndarray]]:
t0 = time.monotonic()
@@ -595,7 +554,7 @@ class Stage2Locate(TolerantNode):
target: np.ndarray,
verticalize: bool,
tag: str,
max_seg: float = 0.08,
max_seg: float = FROZEN_J_HOP_MAX_M,
v: float = 0.05,
min_dur: float = 1.6,
) -> bool:
@@ -982,9 +941,12 @@ class Stage2Locate(TolerantNode):
except Exception:
return None
# 两孔统一用工件表面深度 z_wp（中值，稳健）。
# 孔是垂直孔，XY 位置与深度无关；而 _ring_depth 在通止规逼近目标孔时会采到
# 更近的通止规（~88mm vs 面 ~115mm），把孔深拉偏 → 孔基座坐标随手臂移动漂移、
# 精对准追不动目标而振荡。倾斜仅 ~0.4°，左右孔 camera-Z 差 <0.5mm，对 XY 影响 <0.2mm。
# 不用每孔的环形深度：通止规逼近时环上会采到更近的规（~88mm vs 面 ~115mm），
# 孔的基座坐标就跟着臂飘、精对准追不动。
# 法兰相对竖直大约 0.4°，但相机光轴相对法兰大约 30°。深度误差会进 XY：
# 2026-10-09 观测高度实测 dXY/dz ≈ 0.46 mm/mm。z_wp 必须是工件面，
# 不能是规。同一孔在悬停和观测高度上反投影，基座 XY 可以差约 2mm，
# 所以下降之后还要在当前高度再对一次（见 _trim_xy_after_descent）。
z_left = z_right = z_wp
try:
left_b = self._cam_to_base(self._px_to_cam(pts[0][0], pts[0][1], z_left, intr))
@@ -1360,65 +1322,108 @@ class Stage2Locate(TolerantNode):
return np.array([p[0] + off[0], p[1] + off[1]])
def _tip_offset_from_depth(
self, depth_m: np.ndarray, intr: Dict[str, float], Tee: np.ndarray
self,
depth_m: np.ndarray,
intr: Dict[str, float],
Tee: np.ndarray,
z_wp_cam: Optional[float] = None,
) -> Tuple[Optional[np.ndarray], Optional[float], str]:
"""从一帧深度里找**规尖**，返回 (规尖相对法兰的基座 XY 偏移, 工具长, 说明)。
找不到/不可信时返回 (None, None, 原因)。判据（每条都能真的失败）：
1. 只看法兰轴线附近的**中央竖带**——规就夹在爪里，不会跑到画面边上；
2. 近端连通域要够大（规杆），且连通域内最深 3% 就是**规尖**；
3. 反投出的"法兰 z − 规尖 z"必须落在 [tip_len_min, tip_len_max] ——
这一条同时挡住"深度读到别的东西"（如台面/工件），是主干守卫。
2. 多个连通域里取画面**更靠下**（v 更大）的那一个，不按面积、也不按
「最接近配置工具长」——按面积会选爪子；贴配置长会选法兰轴上的假深点
（2026-10-09 真机工具长 225mm、偏移约 0，残差 0.14mm 但孔偏了）；
3. 有同一帧工件面深度时，只留**比工件面至少近 8mm** 的像素。相机前倾
~30°，规尖沿光轴只比面近十来毫米（2026-10-09 观测高度：面 127mm、
尖 115mm）。再往前砍 15mm 会把尖切掉，只剩更近的夹爪/杆身（~80mm），
反投影偏移 ~2mm，残差 0.2mm 看着达标，针却偏在孔沿上。
没给工件面深度时，退回固定近端（默认 150mm）；
4. 反投出的"法兰 z − 规尖 z"必须落在 [tip_len_min, tip_len_max]，
并且基座 z 要比工件面高出一截 —— 挡住"深度读到台面/工件"。
⚠️ 工具长这一项**只在单帧内**可信；实测它会跳（192~223mm）——规尖细、反光，
掠射角下深度容易丢，丢了以后"最深有效像素"就往上跑、工具长只会**变短**。
所以：
· **XY 偏移**对这件事不敏感（规杆细且接近竖直，杆上任一像素反投回基座 XY
都在杆轴 ~3mm 内）⇒ 逐帧拿来用是稳的；
· **工具长**必须挑"端头看得全"的拍（最长那一簇），见 _refresh_tip_offset。
XY 偏移对这件事不敏感（杆细且接近竖直），见 _refresh_tip_offset。
"""
import cv2
h, w = depth_m.shape
# 8mm：3mm 挡不住深度噪声（工件漏进来，工具长测成 296–315mm）；
# 15mm 又把真规尖切掉（面 127mm、尖 115mm，只剩夹爪）。两条都在真机上红过。
wp_gap = 0.008
# z_wp 只在像相机深度时才当上限（真机孔 ~0.13–0.18m）。基座 z~0.03 会把近端切空。
_z_wp = float(z_wp_cam) if z_wp_cam is not None else 0.0
if 0.08 <= _z_wp <= 0.45:
ceiling = min(_z_wp - wp_gap, self.tip_max_m)
ceil_s = f"比工件面{_z_wp*1000:.0f}近{wp_gap*1000:.0f}"
else:
ceiling = self.tip_near_m
ceil_s = f"固定近端{self.tip_near_m*1000:.0f}"
valid = (depth_m > self.tip_min_m) & (depth_m < self.tip_max_m)
near = (valid & (depth_m < self.tip_near_m)).astype(np.uint8)
near = (valid & (depth_m < ceiling)).astype(np.uint8)
band = np.zeros_like(near)
half = max(1, int(self.tip_band_frac * w))
c0, c1 = int(intr["cx"]) - half, int(intr["cx"]) + half
band[:, max(0, c0):min(w, c1)] = 1
near = near & band
if int(near.sum()) < self.tip_min_px:
return None, None, f"近端(<{self.tip_near_m*1000:.0f}mm)中央竖带内像素 {int(near.sum())} < {self.tip_min_px}"
return None, None, (
f"近端(<{ceiling*1000:.0f}mm,{ceil_s})中央竖带内像素 "
f"{int(near.sum())} < {self.tip_min_px}"
)
n, lab, stats, _ = cv2.connectedComponentsWithStats(near, 8)
if n <= 1:
return None, None, "近端无连通域（规不见了吗）"
big = max(range(1, n), key=lambda i: stats[i, 4])
ys, xs = np.where(lab == big)
# 掩膜已经丢掉贴着工件面的像素。基座 z 再挡一层：反投影落在面上的
# 仍不是规尖。裕量 8mm 而不是 20mm——观测高度规尖离面只有约 15mm，
# 20mm 会把真尖一起丢掉（2026-10-09）。悬停时尖离面约 75mm，8mm 仍够。
surf = float(getattr(self, "surface_z_m", 0.0) or 0.0)
above_surface_m = 0.008
cands = []
stuck = 0
for i in range(1, n):
if int(stats[i, 4]) < self.tip_min_px:
continue
ys, xs = np.where(lab == i)
dv = depth_m[ys, xs]
k = np.percentile(dv, 97) # 连通域内最深 3% ≈ 规尖
k = float(np.percentile(dv, 97))
sel = dv >= k - 1e-6
u = float(np.median(xs[sel]))
v = float(np.median(ys[sel]))
y0, y1 = max(0, int(v) - 2), min(h, int(v) + 3)
x0, x1 = max(0, int(u) - 2), min(w, int(u) + 3)
patch = depth_m[y0:y1, x0:x1]
pv = patch[(patch > self.tip_min_m) & (patch < self.tip_max_m)]
if pv.size == 0:
return None, None, "规尖邻域深度无效"
z_tip = float(np.median(pv))
# 用连通域自己的近端深度，不用周围 5×5：规尖只有几像素宽，
# 邻域中值会落到工件面上，尖又被当成面丢掉。
z_tip = float(np.median(dv[sel]))
try:
p_tip = self._cam_to_base(self._px_to_cam(u, v, z_tip, intr))
except Exception as exc: # noqa: BLE001
return None, None, f"规尖反投影失败（手眼 TF 缺？）: {exc}"
tool_len = float(Tee[2, 3]) - float(p_tip[2])
if not (self.tip_len_min <= tool_len <= self.tip_len_max):
continue
if surf > 0.0 and float(p_tip[2]) < surf + above_surface_m:
stuck += 1
continue
off = np.array(
[float(p_tip[0]) - float(Tee[0, 3]), float(p_tip[1]) - float(Tee[1, 3])]
)
cands.append((off, tool_len, u, v))
if not cands:
if stuck:
return None, None, (
f"反投影出的工具长 {tool_len*1000:.0f}mm 不在 "
f"近端点贴着工件面（高出不足 {above_surface_m*1000:.0f}mm），不是规尖"
)
return None, None, (
f"反投影出的工具长不在 "
f"[{self.tip_len_min*1000:.0f},{self.tip_len_max*1000:.0f}]mm —— "
f"近端那个物体不是规"
)
off = np.array([float(p_tip[0]) - float(Tee[0, 3]), float(p_tip[1]) - float(Tee[1, 3])])
return off, tool_len, f"px=({u:.0f},{v:.0f}) 工具长 {tool_len*1000:.0f}mm"
# v 优先：画面更靠下的才是伸向工件的那一端。工具长只作同分时的次序，
# 不拿去贴近 site.yaml —— 贴配置长会把法兰轴上的点当成规尖。
off, tool_len, u, v = max(cands, key=lambda c: (c[3], c[1]))
return off, tool_len, f"px=({u:.0f},{v:.0f}) 工具长 {tool_len*1000:.0f}mm {ceil_s}"
def _refresh_tip_offset(self) -> None:
"""攒够**一致**样本后，定下并**冻结**「规尖相对法兰」的偏移。
@@ -1440,6 +1445,10 @@ class Stage2Locate(TolerantNode):
med = np.median(arr, axis=0)
inl = np.linalg.norm(arr - med, axis=1) <= self.tip_off_tol
if int(inl.sum()) < self.tip_calib_n:
self.get_logger().warn(
f" 规尖样本 {len(self._tip_samp)} 拍、一致的只有 {int(inl.sum())}/"
f"{self.tip_calib_n}（tol={self.tip_off_tol*1000:.0f}mm）—— 还冻不住，继续采"
)
return
idx = np.where(inl)[0]
self._tip_off = arr[idx].mean(0)
@@ -1540,8 +1549,11 @@ class Stage2Locate(TolerantNode):
2. 腕部 FT 接触检测：``force.z`` 相对悬空基线突变 ⇒ 立即停 + 回退 1mm；
3. 任一步 ``_goto`` 没走到位（多半是碰到东西/被挡）⇒ 停止，保持当前高度。
"""
if not self.final_descent_enable:
return True, q, Tee
# final_descent_enable：降到贴面 final_gap（约 2mm）。
# 关掉时（--no-descent）仍做**观测下降**，停在 observe_gap（约 15mm），
# 让人能看出规尖偏没偏，又不把 φ6 规压到 φ7 孔沿上。
target_gap = self.final_gap_m if self.final_descent_enable else self.observe_gap_m
kind = "贴面轻降" if self.final_descent_enable else "观测下降"
self._state("descend_above_hole")
tip_len = float(self.tool_tip_below_flange_m)
surf = float(self.surface_z_m)
@@ -1557,7 +1569,7 @@ class Stage2Locate(TolerantNode):
return False, q, Tee
gap0 = float(Tee[2, 3]) - tip_len - surf # 几何间隙（唯一决定下降量的量）
descent = gap0 - self.final_gap_m
descent = gap0 - target_gap
if descent > self.descent_budget_m + 5e-4:
self.get_logger().warn(
f"轻降超过配置几何预算：按 FK 算需降 {descent * 1000:.1f}mm > "
@@ -1585,9 +1597,9 @@ class Stage2Locate(TolerantNode):
)
self.get_logger().info(
f"轻降准备: 几何间隙 {gap0 * 1000:.1f}mm → 目标 {self.final_gap_m * 1000:.1f}mm，"
f"{kind}准备: 几何间隙 {gap0 * 1000:.1f}mm → 目标 {target_gap * 1000:.1f}mm，"
f"需下降 {descent * 1000:.1f}mm（法兰 {Tee[2, 3]:.4f} − 工具长 {tip_len * 1000:.0f}mm"
f" − 工件面 {surf:.4f}；很慢很柔）"
f" − 工件面 {surf:.4f}；很慢，XY 保持）"
)
if descent <= 0.001:
self.get_logger().info("规尖已在目标间隙附近，无需下降")
@@ -1632,7 +1644,7 @@ class Stage2Locate(TolerantNode):
self.descent_final_gap_m = float(Tee[2, 3]) - tip_len - surf
return True, q, Tee
# 用 FK 现算剩余下降量（不是累加指令）—— 单步没走到也不会积累误差。
remaining = (float(Tee[2, 3]) - tip_len - surf) - self.final_gap_m
remaining = (float(Tee[2, 3]) - tip_len - surf) - target_gap
if remaining <= 1e-4:
break
step = min(remaining, 0.004) # 每步 ≤4mm，很慢很柔
@@ -1653,12 +1665,128 @@ class Stage2Locate(TolerantNode):
self.descent_final_gap_m = gap_f
lvl = self.get_logger().info if gap_f >= 0.0 else self.get_logger().warn
lvl(
f"=== 轻降完成: 规尖悬在孔面上方 ≈{gap_f * 1000:.1f}mm（几何：法兰 "
f"=== {kind}完成: 规尖悬在孔面上方 ≈{gap_f * 1000:.1f}mm（几何：法兰 "
f"{Tee[2, 3]:.4f} − 工具长 {tip_len * 1000:.0f}mm − 工件面 {surf:.4f}；"
f"目标 {self.final_gap_m * 1000:.1f}mm，不接触）==="
f"目标 {target_gap * 1000:.1f}mm，不接触）==="
)
return True, q, Tee
def _trim_xy_after_descent(self, q, Tee, final: Dict[str, Any]):
"""下降之后、保持当前高度，用这一高度的规尖和孔再对一次 XY。
悬停残差不能拿来当下降后的孔位。相机光轴相对法兰约 30°，孔的基座 XY
对深度大约 0.5 mm/mm。2026-10-09：同一孔在悬停（相机深度 180mm，残差
0.22mm）和观测高度（127mm）上反投影，基座 XY 差约 2mm；法兰 XY 下降时
只动了 0.2mm。画面里针在孔的上方一截，主要是 15mm 间隙的透视，横着的
那 2mm 才是要补的。
只用本高度新量的规尖，不沿用悬停时冻住的偏移。总修正超过 8mm 就不动
—— 那一档是夹爪/杆身，不是针。量不到规尖也不动，悬停对准的结果留着。
"""
if not self.tip_vision:
return q, Tee, final
final_in = final
z_hold = float(Tee[2, 3])
saved = self._tip_off
budget = 0.008
moved = 0.0
prev: Optional[np.ndarray] = None
for it in range(4):
if not rclpy.ok():
break
try:
_bgr, depth_m, intr, res = self._snap_holes(skip_center_gate=True)
except Exception as exc: # noqa: BLE001
self.get_logger().warn(f"下降后复拍失败，保持悬停对准的位置：{exc}")
self._tip_off = saved
return q, Tee, final
if not res.get("status"):
self.get_logger().warn(f"下降后丢孔（{res.get('msg')}），不再改 XY")
self._tip_off = saved
return q, Tee, final
hb = res.get("hole_base")
z_wp = float(res.get("z_wp") or 0.0)
if hb is None or len(hb) != 2 or z_wp <= 0.0:
self.get_logger().warn("下降后没有孔的基座坐标，不再改 XY")
self._tip_off = saved
return q, Tee, final
left_b = np.asarray(hb[0], dtype=float)
right_b = np.asarray(hb[1], dtype=float)
k1_b, k2_b = (left_b, right_b) if self.k1_label == "left" else (right_b, left_b)
tgt = k1_b if self.target_hole == "k1" else k2_b
Tee_now = self._lookup(self.base_frame, self.ee_frame)
if Tee_now is not None:
Tee = Tee_now
off, tlen, why = self._tip_offset_from_depth(
depth_m, intr, Tee, z_wp_cam=z_wp
)
if off is None or tlen is None:
self.get_logger().warn(f"下降后没量到规尖（{why}），不再改 XY")
self._tip_off = saved
return q, Tee, final
if prev is not None and float(np.linalg.norm(off - prev)) > 0.003:
self.get_logger().warn(
f"下降后规尖两拍差 {float(np.linalg.norm(off - prev))*1000:.1f}mm，这一拍不动"
)
prev = np.array(off, dtype=float)
continue
prev = np.array(off, dtype=float)
if it == 0:
continue
gxy = np.array([
float(Tee[0, 3]) + float(off[0]), float(Tee[1, 3]) + float(off[1])
])
raw = np.array([float(tgt[0]) - gxy[0], float(tgt[1]) - gxy[1]])
err = float(np.linalg.norm(raw))
final = dict(final)
final["residual_mm"] = round(err * 1000.0, 2)
final["tip_offset_m"] = [round(float(off[0]), 5), round(float(off[1]), 5)]
final["tip_len_m"] = round(float(tlen), 5)
final["gauge_xy_m"] = [round(float(gxy[0]), 5), round(float(gxy[1]), 5)]
final["gauge_source"] = "视觉规尖(下降后重测)"
self._tip_off = np.array(off, dtype=float)
self.get_logger().info(
f"[下降后 {it}] 工具=({gxy[0]:.4f},{gxy[1]:.4f}) "
f"残差={err*1000:.2f}mm {why}"
)
if err <= self.align_tol:
self.get_logger().info(
f"下降后 XY 已在容差内（{err*1000:.2f}mm），高度保持 {z_hold:.4f}"
)
return q, Tee, final
if moved + err > budget:
self.get_logger().error(
f"下降后要对 {err*1000:.1f}mm（已走 {moved*1000:.1f}mm），"
f"超过 {budget*1000:.0f}mm —— 不像针，停在原地"
)
self._tip_off = saved
return q, Tee, final_in
step = raw
if err > 0.004:
step = raw * (0.004 / err)
move = np.array([
float(Tee[0, 3]) + float(step[0]),
float(Tee[1, 3]) + float(step[1]),
z_hold,
])
ok, q, Tee = self._goto(
q, Tee, move, verticalize=True,
tol_m=0.0004, tol_tilt=self.vert_tol_rad * 2.0,
tag="trim_xy", v=min(float(self.align_speed), 0.010),
max_hops=4, settle_s=0.30, strict_xy=True,
)
self._pin(q)
moved += float(np.linalg.norm(step))
if not ok:
self.get_logger().warn("下降后 XY 修正没走到，停")
return q, Tee, final
if abs(float(Tee[2, 3]) - z_hold) > 0.002:
self.get_logger().error(
f"下降后修正把高度从 {z_hold:.4f} 带到了 {float(Tee[2, 3]):.4f}，停下"
)
return q, Tee, final
return q, Tee, final
def _snap_holes(self, *, skip_center_gate: bool) -> Tuple[np.ndarray, np.ndarray, Dict[str, float], dict]:
"""多帧拍孔：先对 N 帧做时间中值得到干净图，再在其上检测螺纹嵌件对。
@@ -2059,7 +2187,9 @@ class Stage2Locate(TolerantNode):
# 把法兰对到孔上等于把规偏着对过去。偏移量准一次后冻结（刚体量）。
tip_src = "法兰+偏置"
if self.tip_vision and self._tip_off is None:
off, tlen, why = self._tip_offset_from_depth(depth_m, intr, Tee)
off, tlen, why = self._tip_offset_from_depth(
depth_m, intr, Tee, z_wp_cam=float(z_wp)
)
if off is not None and tlen is not None:
self._tip_samp.append((off, tlen))
self._refresh_tip_offset()
@@ -2263,6 +2393,10 @@ class Stage2Locate(TolerantNode):
okd, q, Tee = self._descend_above_hole(q, Tee)
if not okd:
self.get_logger().warn("轻降未完全到位，保持当前高度（安全优先）")
else:
# 悬停残差在下降 60mm 之后会变大约 2mm（相机 30°，同一孔两个高度
# 反投影对不齐）。在观测高度重测规尖，只补 XY。
q, Tee, final = self._trim_xy_after_descent(q, Tee, final)
# 如实汇报**实际**间隙（几何量）——不是把目标值念一遍。停在离面 50mm 时
# 也显示 ~50mm，现场才看得出"轻降没做到"（R1.1：不许把没做到说成做到）。
gap_txt = (
src/arm_application/launch/pipeline.launch.py
+6 -2
View File
@@ -81,6 +81,7 @@ def _launch_setup(context, *args, **kwargs):
# 对到孔心"时先别让规往下走 —— 对不准就下压会把规顶在孔沿上（2026-10-08 真机
# 顶到 63N）。默认关；验证对准时用 --no-descent 打开。
no_descent = _as_bool(LaunchConfiguration("no_descent").perform(context))
d435_only = _as_bool(LaunchConfiguration("d435_move_only").perform(context))
target_hole = LaunchConfiguration("target_hole").perform(context).strip()
depth = LaunchConfiguration("insert_depth_mm").perform(context).strip()
pitch = LaunchConfiguration("screw_pitch_mm").perform(context).strip()
@@ -117,8 +118,9 @@ def _launch_setup(context, *args, **kwargs):
"insert_depth_mm": float(depth) if depth else -1.0,
"screw_pitch_mm": float(pitch) if pitch else -999.0,
"skip_stage1": skip1,
"skip_stage2": skip2,
"skip_stage3": skip3,
"skip_stage2": skip2 or d435_only,
"skip_stage3": skip3 or d435_only,
"d435_move_only": d435_only,
# -1=按 site 的 stage2.final_descent_enable；0=强制不降（调试）
"final_descent_enable": 0 if no_descent else -1,
}
@@ -131,6 +133,7 @@ def _launch_setup(context, *args, **kwargs):
LogInfo(
msg=f"[pipeline] hole={target_hole or 'site'} depth={depth or 'site'} "
f"pitch={pitch or 'site'} skip1={skip1} skip2={skip2} skip3={skip3}"
+ (" [d435_only：只测 D435 位移]" if d435_only else "")
+ (" [no_descent：只对准，不轻降]" if no_descent else "")
),
*nodes,
@@ -156,6 +159,7 @@ def generate_launch_description() -> LaunchDescription:
DeclareLaunchArgument("resnap", default_value="false"),
# 只验证精对准、不轻降（见 _launch_setup 里的说明）
DeclareLaunchArgument("no_descent", default_value="false"),
DeclareLaunchArgument("d435_move_only", default_value="false"),
DeclareLaunchArgument("preview", default_value="true"),
OpaqueFunction(function=_launch_setup),
]
src/arm_application/launch/visual_servo.launch.py
+15
View File
@@ -60,6 +60,10 @@ def _load_site():
return merged, path, perception_overrides(data), servo_overrides(data), robot
def _as_bool(s: str) -> bool:
return s.strip().lower() in ("1", "true", "yes")
def _handeye_xyz_xyzw(path: str):
"""读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。
@@ -171,6 +175,9 @@ def _launch_setup(context, *args, **kwargs):
servo_params["lock_hold_sec"] = float(lock_hold_arg)
except ValueError:
pass
d435_only = _as_bool(LaunchConfiguration("d435_move_only").perform(context))
perc_params["d435_move_only"] = d435_only
servo_params["d435_move_only"] = d435_only
# ------------------------------------------------------------------
# 感知来源：谁来产生"视觉测量"（site.yaml 的 perception.source）
@@ -240,6 +247,10 @@ def _launch_setup(context, *args, **kwargs):
),
LogInfo(msg=f"[site] 感知来源 perception.source = {perc_src}"),
]
if d435_only:
nodes.append(
LogInfo(msg="[d435_only] 只测 D435 单次位移：不交 D405、不进二阶段")
)
if perc_src == "yolo":
nodes.append(LogInfo(msg=f"[site] YOLO weights: {weights}"))
nodes.append(
@@ -345,6 +356,10 @@ def generate_launch_description() -> LaunchDescription:
"lock_hold_sec",
default_value="",
),
DeclareLaunchArgument(
"d435_move_only",
default_value="false",
),
OpaqueFunction(function=_launch_setup),
]
)
src/arm_application/package.xml
+1
View File
@@ -16,6 +16,7 @@
<depend>std_msgs</depend>
<depend>std_srvs</depend>
<depend>trajectory_msgs</depend>
<depend>control_msgs</depend>
<exec_depend>tf2_ros</exec_depend>
<exec_depend>python3-yaml</exec_depend>
<exec_depend>python3-serial</exec_depend>
src/arm_application/test/test_feature_guard.py
+3
View File
@@ -287,6 +287,9 @@ def test_perception_wires_slew_and_reacquire_guards():
src = _perception_src()
assert "TrackSlewGuard(" in src, "位移上限没接"
assert "FarReacquireConfirm(" in src, "远框重捕获确认没接"
assert "self.reacquire_far_px =" in src, (
"参数读了没存成属性 —— 远框确认会 AttributeError，看门狗跟着急停"
)
def _export_site_env():
src/arm_application/test/test_gauge_exit_lift.py
+3 -3
View File
@@ -12,8 +12,8 @@
所以这里有两条断言，缺一不可：
* **结构**：``_step_unscrew`` 在"成功返回"之前必须调用提出动作 —— 顺序错了
（比如把提出挪到 S10 里）就等于没做，因为横向动作已经发生了；
* **行为**：提出目标是**跟踪高**（``_track_z()`` = 悬停高 + 50mm），不是写死的
毫米数；已经在跟踪高上时不动（否则每次换孔白等 1 秒多）。
* **行为**：提出目标是**跟踪高**（``_track_z()``：悬停时尖端离面不够才再抬
50mm），不是写死的毫米数；已经在跟踪高上时不动（否则每次换孔白等 1 秒多）。
"""
from __future__ import annotations
@@ -79,7 +79,7 @@ def test_unscrew_lifts_out_of_the_hole_before_succeeding():
def test_lift_helper_exists_and_uses_the_track_height_idiom():
"""提出高度用 ``_track_z()``（悬停高 + 50mm），不写死毫米数。"""
"""提出高度用 ``_track_z()``，不写死毫米数。"""
fn = _find_method("_lift_clear_of_hole")
assert _call_lines(fn, "_track_z"), (
"提出高度必须用 _track_z()：这是流程里『工具已离开工件』的既有定义，"
src/arm_application/test/test_handeye_lib.py
+40
View File
@@ -18,8 +18,10 @@ from arm_application.handeye_lib import (
CameraIntrinsics,
ChessboardStabilizer,
Rt_to_T,
T_from_flexiv_tcp,
board_object_points,
calibrate_hand_eye,
cam_to_flange_via_tool,
format_transform_yaml,
invert_Rt,
is_pose_filled,
@@ -420,3 +422,41 @@ def test_is_pose_filled_rejects_wrong_length():
def test_is_pose_filled_accepts_real_pose():
assert is_pose_filled([-1.107, -0.744, 0.982, 1.856, -0.735, 0.947, -0.118])
def test_flexiv_tcp_identity_is_pure_translation():
"""Studio tool10120938：姿态单位四元数，只沿法兰 Z 偏 224.5 mm。"""
T = T_from_flexiv_tcp([-0.0005, 0.0067, 0.2245], [1.0, 0.0, 0.0, 0.0])
assert np.allclose(T[:3, :3], np.eye(3), atol=1e-12)
assert T[2, 3] == pytest.approx(0.2245)
def test_cam_to_flange_via_tool_must_not_leave_t_unchanged():
"""忘了乘时 T_4x4 停在 TCP；乘反了相机会到法兰后方。"""
T_cam_tool = np.eye(4)
T_cam_tool[:3, 3] = [-0.05961995, -0.01523625, -0.09761517]
T_flange_tool = T_from_flexiv_tcp([-0.0005, 0.0067, 0.2245], [1.0, 0.0, 0.0, 0.0])
T_cam_flange = cam_to_flange_via_tool(T_cam_tool, T_flange_tool)
delta = float(np.linalg.norm(T_cam_flange[:3, 3] - T_cam_tool[:3, 3]))
tool_len = float(np.linalg.norm(T_flange_tool[:3, 3]))
assert delta == pytest.approx(tool_len, abs=1e-9)
assert tool_len > 0.20
# 相机在法兰前方（+Z ≈ 126 mm），与 2026-09-04 旧结果同量级。
assert T_cam_flange[2, 3] == pytest.approx(0.12688483, abs=1e-8)
assert T_cam_flange[0, 3] == pytest.approx(-0.06011995, abs=1e-8)
# 负向：乘反了会到法兰后方约 −296 mm。
wrong = T_cam_tool @ np.linalg.inv(T_flange_tool)
assert wrong[2, 3] < 0.0
# 负向：直接把 cam→工具当成 cam→法兰，平移几乎不动。
assert float(np.linalg.norm(T_cam_tool[:3, 3] - T_cam_tool[:3, 3])) < 1e-12
# 折回去必须还原：T_ct = inv(T_ft) · T_cf
T_back = np.linalg.inv(T_flange_tool) @ T_cam_flange
assert np.allclose(T_back, T_cam_tool, atol=1e-12)
def test_cam_at_tcp_is_the_tool_pose_in_flange():
"""相机与 TCP 重合时，cam→法兰必须等于法兰→工具，不能是它的逆。"""
T_ft = T_from_flexiv_tcp([-0.0005, 0.0067, 0.2245], [1.0, 0.0, 0.0, 0.0])
T_cf = cam_to_flange_via_tool(np.eye(4), T_ft)
assert np.allclose(T_cf, T_ft, atol=1e-12)
assert T_cf[2, 3] == pytest.approx(0.2245)
src/arm_application/test/test_handeye_mode.py
+55 -1
View File
@@ -339,6 +339,7 @@ class TestRunHandeyeScriptMode:
assert "camera=d435" in out
assert "11x8 inner, square=0.02 m" in out
assert "89_d435_eye_to_hand.yaml" in out
assert "tool10120938" in out
# 负向：不能再去开腕部相机。
assert "camera=d405" not in out
@@ -346,7 +347,8 @@ class TestRunHandeyeScriptMode:
out = _show("eye_in_hand", _real_site)
assert "mode=eye_in_hand" in out
assert "d405" in out
assert "8x11 inner, square=0.015 m" in out
assert "11x8 inner, square=0.015 m" in out
assert "tool10120938" in out
assert "89_eye_in_hand.yaml" in out
def test_missing_board_blocks_calibrate(self, tmp_path: Path):
@@ -392,6 +394,58 @@ class TestRunHandeyeScriptMode:
assert proc.returncode != 0
assert "没有配置 [eye_to_hand] 用的标定板尺寸" in (proc.stdout + proc.stderr)
def test_left_arm_calibrate_wires_tool10120938(self):
"""89 必须按 tool10120938 求解；脚本和节点都要接到这份 TCP。"""
cal = (
ROOT / "src" / "arm_application" / "arm_application" / "handeye_calibrate.py"
).read_text(encoding="utf-8")
sh = SCRIPT.read_text(encoding="utf-8")
assert "cam_to_flange_via_tool(" in cal
assert "gripper_tool" in cal
assert "gripper_tool:=" in sh
assert "T_4x4 仍是 cam→基座，不折" in sh
backup = ROOT / "handeye" / "results" / "backup" / "89_eye_in_hand_flange_tool1_20260904.yaml"
assert backup.is_file()
e2h_bak = ROOT / "handeye" / "results" / "backup" / "89_d435_eye_to_hand_20261009.yaml"
assert e2h_bak.is_file()
e2h = (ROOT / "handeye" / "results" / "89_d435_eye_to_hand.yaml").read_text(
encoding="utf-8"
)
assert "gripper_tool: tool10120938" in e2h
assert "cam→基座" in e2h
import re
m_e2h = re.search(r"translation_m:\s*\[([^]]+)\]", e2h)
assert m_e2h, e2h
xyz_e2h = [float(x) for x in m_e2h.group(1).split(",")]
# 顶置相机在基座上方约 0.7 m；折错或写到法兰会差几十厘米。
assert 0.60 < xyz_e2h[2] < 0.90, xyz_e2h
# 眼在手外不得走 cam_to_flange_via_tool（相机和基座都在世界里）。
e2h_block = cal.split("if eye_to_hand:")[1].split("elif self.gripper_tool:")[0]
assert "cam_to_flange_via_tool" not in e2h_block
live = ROOT / "handeye" / "results" / "89_eye_in_hand.yaml"
text = live.read_text(encoding="utf-8")
assert "BACKUP" in text or "backup/" in text
# 乘反了 tz 会是负的（法兰后方）；现行必须在法兰前方。
import re
m = re.search(r"translation_m:\s*\[([^]]+)\]", text)
assert m, text
xyz = [float(x) for x in m.group(1).split(",")]
assert xyz[2] > 0.10, xyz
assert "inv(T_flange_to_tool)" not in text
def test_teach_and_merge_share_the_same_taught_file(self):
"""示教写 A、merge 读 B → 采了点也报 got 0。两边必须用 TAUGHT。"""
sh = SCRIPT.read_text(encoding="utf-8")
teach = (
ROOT / "src" / "arm_application" / "arm_application" / "handeye_teach.py"
).read_text(encoding="utf-8")
assert 'poses_file:="${TAUGHT}"' in sh
assert "taught_poses_${HANDEYE_MODE}" in sh
assert "declare_parameter('poses_file'" in teach
assert "get_parameter('poses_file')" in teach
def test_illegal_mode_exits_immediately(self):
proc = subprocess.run(
["bash", str(SCRIPT), "--arm", "89", "show", "--mode", "eye_on_foot"],
src/arm_application/test/test_merge_handeye_poses.py
+69
View File
@@ -0,0 +1,69 @@
"""merge 必须能失败：空文件、连按重复点都不够 5 个独立姿态。"""
from __future__ import annotations
import importlib.util
import sys
from pathlib import Path
ROOT = Path(__file__).resolve().parents[3]
_SPEC = importlib.util.spec_from_file_location(
"merge_handeye_poses", ROOT / "scripts" / "merge_handeye_poses.py"
)
assert _SPEC and _SPEC.loader
_mod = importlib.util.module_from_spec(_SPEC)
sys.modules[_SPEC.name] = _mod
_SPEC.loader.exec_module(_mod)
def _pose(i: int) -> list[float]:
return [0.1 * i, -1.0, 1.0, 1.4, -0.9, 0.2, 0.4]
def test_parse_taught_txt_skips_comments(tmp_path: Path):
text = "\n".join(
[
"# pose 0 20261012_030533",
"-0.730276, -1.078914, 1.182093, 1.378457, -0.914310, 0.247653, 0.409611,",
"# image /tmp/a.png",
]
)
p = tmp_path / "taught_parse_once.txt"
p.write_text(text, encoding="utf-8")
poses = _mod.parse_poses(p)
assert len(poses) == 1
assert abs(poses[0][0] + 0.730276) < 1e-9
def test_unique_drops_enter_repeat():
same = _pose(0)
poses = [same, list(same), list(same), _pose(1)]
uniq = _mod.unique_poses(poses)
assert len(uniq) == 2
def test_merge_refuses_repeat_only_file(tmp_path: Path):
src = tmp_path / "taught.txt"
dst = tmp_path / "wp.yaml"
same = ", ".join(f"{v:.6f}" for v in _pose(0)) + ","
src.write_text("\n".join(f"# pose {i}\n{same}" for i in range(12)) + "\n")
dst.write_text(" num_poses: 0\n waypoint_positions: []\n", encoding="utf-8")
rc = _mod.main(["merge_handeye_poses.py", str(src), str(dst)])
assert rc == 4
assert "waypoint_positions: []" in dst.read_text(encoding="utf-8")
def test_merge_keeps_five_distinct(tmp_path: Path):
src = tmp_path / "taught.txt"
dst = tmp_path / "wp.yaml"
lines = []
for i in range(5):
lines.append(f"# pose {i}")
lines.append(", ".join(f"{v:.6f}" for v in _pose(i)) + ",")
src.write_text("\n".join(lines) + "\n", encoding="utf-8")
dst.write_text(" num_poses: 0\n waypoint_positions: []\n", encoding="utf-8")
rc = _mod.main(["merge_handeye_poses.py", str(src), str(dst)])
assert rc == 0
text = dst.read_text(encoding="utf-8")
assert "num_poses: 5" in text
assert text.count("# pose ") == 5
src/arm_application/test/test_overhead_oneshot.py
+428
View File
@@ -11,14 +11,49 @@ from types import SimpleNamespace
import numpy as np
import pytest
from arm_application.path_profile import (
D435_FULL_ACC_M,
D435_FULL_CRUISE_MPS,
D435_FULL_DEC_M,
cosine_peak_acc,
smooth_joint_schedule,
smooth_path_params,
smooth_path_sv,
trap_joint_schedule,
)
from arm_application.overhead_oneshot import (
StillLock,
at_lock_height,
best_det_conf,
depth_raw_to_m,
k0_uv_from_det,
lock_z_velocity,
map_uv_to_depth,
D405_MISS_BEFORE_D435_S,
D435_MOVE_SPEED_SCALE,
D435_NEAR_CRUISE_MPS,
D435_NEAR_HOP_MAX_M,
D435_NEAR_LEFTOVER_M,
D435_SETTLE_S,
FROZEN_J_HOP_MAX_M,
build_arc_xy_move,
d405_commands_arm,
flange_down_w,
hold_translation_vz,
hybrid_cruise_vxy,
oneshot_cartesian_dp,
oneshot_concat_hold_ori,
oneshot_full_cruise_mps,
oneshot_full_segments,
oneshot_hop_limits,
oneshot_k0_base,
oneshot_may_send_another,
oneshot_needs_verticalize,
oneshot_shot_timed_out,
oneshot_next_hop_m,
oneshot_should_submit,
select_oneshot_det,
trap_acc_for_hop,
wrist_needs_handoff,
)
@@ -38,6 +73,399 @@ class TestHandoff:
assert wrist_needs_handoff(0.5, 0.5) is False
assert wrist_needs_handoff(0.87, 0.5) is False
def test_below_lock_height_is_not_ready_and_must_lift(self):
"""0.26 m 是观测下降停的高度，不是锁高。必须判没到位，并且速度朝上。"""
assert at_lock_height(0.257, 0.320, 0.040) is False
assert at_lock_height(0.327, 0.320, 0.040) is True
assert at_lock_height(0.250, 0.320, 0.040) is False
vz = lock_z_velocity(0.257, 0.320, kp=1.0, v_lim=0.08)
assert vz > 0.0, "低于锁高必须上抬，写成 0 会和 D435 拒拍卡死"
assert lock_z_velocity(0.360, 0.320, kp=1.0, v_lim=0.08) < 0.0
assert lock_z_velocity(0.321, 0.320, kp=1.0, v_lim=0.08) == 0.0
def test_z_locked_qos_is_latched(self):
"""晚订阅必须能拿到已经发出的锁高状态，否则 D435 会一直拒拍。"""
from rclpy.qos import DurabilityPolicy
from arm_application.overhead_oneshot import z_locked_qos
qos = z_locked_qos()
assert qos.durability == DurabilityPolicy.TRANSIENT_LOCAL
assert qos.depth == 1
def test_oneshot_waits_half_second_then_fires_once(self):
"""连续 0.5 s 没有高置信度才拍。差一帧不算。拍成之后不许连拍。
不再另等「已到锁高」——那一闸和回锁高互相卡住。
"""
wait = D405_MISS_BEFORE_D435_S
assert wait == 0.5
assert oneshot_should_submit(
handoff=True, low_for_s=wait - 0.01, already_shot=False
) is False
assert oneshot_should_submit(
handoff=True, low_for_s=wait, already_shot=False
) is True
assert oneshot_should_submit(
handoff=True, low_for_s=wait + 1.0, already_shot=True
) is False
assert oneshot_should_submit(
handoff=False, low_for_s=wait + 1.0, already_shot=False
) is False
# 拍了伺服没接住（工作盒拒）必须能再拍；已经在赶路不许清。
assert oneshot_shot_timed_out(
shot=True, commanding=False, age_s=0.79
) is False
assert oneshot_shot_timed_out(
shot=True, commanding=False, age_s=0.80
) is True
assert oneshot_shot_timed_out(
shot=True, commanding=True, age_s=2.0
) is False
perc = (
__import__("pathlib").Path(__file__).resolve().parents[1]
/ "arm_application" / "perception_node.py"
).read_text(encoding="utf-8")
assert "oneshot_shot_timed_out(" in perc
assert "伺服没接住" in perc
def test_d405_points_stay_off_the_arm_until_arrived_and_settled(self):
"""用着 D435 那个点时，到位并且停稳之前，D405 的点不下发。推理不在这里关。"""
assert d405_commands_arm(using_d435=False, arrived=False, settled=False) is True
assert d405_commands_arm(using_d435=True, arrived=False, settled=False) is False
assert d405_commands_arm(using_d435=True, arrived=True, settled=False) is False
assert d405_commands_arm(using_d435=True, arrived=False, settled=True) is False
assert d405_commands_arm(using_d435=True, arrived=True, settled=True) is True
src = (
__import__("pathlib").Path(__file__).resolve().parents[1]
/ "arm_application" / "servo_node.py"
).read_text(encoding="utf-8")
assert "d405_commands_arm(" in src
assert "_arrive" in src
perc = (
__import__("pathlib").Path(__file__).resolve().parents[1]
/ "arm_application" / "perception_node.py"
).read_text(encoding="utf-8")
assert "D405_MISS_BEFORE_D435_S" in perc
assert "d435_commanding" in perc
def test_d435_move_only_never_hands_back_to_d405(self):
"""只测位移：D405 点一律不下发，到位停稳也不交。默认路径仍会交回。"""
assert d405_commands_arm(
using_d435=False, arrived=True, settled=True, move_only=True
) is False
assert d405_commands_arm(
using_d435=True, arrived=True, settled=True, move_only=True
) is False
assert d405_commands_arm(
using_d435=True, arrived=True, settled=True, move_only=False
) is True
root = __import__("pathlib").Path(__file__).resolve().parents[1]
servo = (root / "arm_application" / "servo_node.py").read_text(encoding="utf-8")
perc = (root / "arm_application" / "perception_node.py").read_text(
encoding="utf-8"
)
pipe = (root / "arm_application" / "pipeline_node.py").read_text(
encoding="utf-8"
)
launch = (root / "launch" / "pipeline.launch.py").read_text(encoding="utf-8")
vs = (root / "launch" / "visual_servo.launch.py").read_text(encoding="utf-8")
ws = root
while ws != ws.parent and not (ws / "scripts" / "run_all.sh").is_file():
ws = ws.parent
run_all = (ws / "scripts" / "run_all.sh").read_text(encoding="utf-8")
assert "move_only=self.d435_move_only" in servo
assert "不交 D405、不进二阶段" in servo
assert "改回下发 D405" in servo
assert "self.d435_move_only" in perc
assert "or wrist_needs_handoff(" in perc
assert "d435_move_only:={str(self.d435_move_only).lower()}" in pipe
assert "skip2 = True" in pipe
assert '"d435_move_only": d435_only' in launch
assert 'LaunchConfiguration("d435_move_only")' in vs
assert '"d435_move_only"' in vs
assert "--d435-only" in run_all
def test_whole_path_uses_one_cruise_and_an_arc(self):
"""远处仍 80 mm/s。近处必须另降一档。路径是圆弧，不是对着终点做 P。"""
assert D435_MOVE_SPEED_SCALE == 1.0
assert hybrid_cruise_vxy(0.08) == pytest.approx(0.08)
hop_far, v_far = oneshot_hop_limits(0.20, 0.08)
hop_mid, v_mid = oneshot_hop_limits(0.12, 0.08)
hop_near, v_near = oneshot_hop_limits(0.074, 0.08)
assert hop_far == pytest.approx(FROZEN_J_HOP_MAX_M)
assert v_far == pytest.approx(0.08)
assert hop_mid == pytest.approx(0.040)
assert v_mid == pytest.approx(0.08)
assert hop_near == pytest.approx(D435_NEAR_HOP_MAX_M)
assert v_near == pytest.approx(D435_NEAR_CRUISE_MPS)
assert hop_near == pytest.approx(0.015)
assert hop_near < 0.030
assert v_near < 0.08
assert D435_NEAR_LEFTOVER_M == pytest.approx(0.08)
assert D435_SETTLE_S == pytest.approx(0.25)
p0 = np.array([0.687, -0.113])
p1 = np.array([0.583, -0.502])
move = build_arc_xy_move(p0, p1, v=0.08, a=0.40, now=0.0)
assert move.linear is False
assert move.length > float(np.linalg.norm(p1 - p0))
assert move.v_cruise == pytest.approx(0.08)
xy0, v0, done0 = move.sample(0.0)
np.testing.assert_allclose(xy0, p0, atol=1e-6)
assert float(np.hypot(v0[0], v0[1])) == pytest.approx(0.0, abs=1e-6)
assert done0 is False
t_mid = 0.5 * (move.t_acc + (move.t_acc + move.t_cruise))
xy_m, v_m, _ = move.sample(t_mid)
chord = p1 - p0
# 中点不在弦上，否则还是直线冲。
rel = xy_m - p0
off = abs(float(chord[0] * rel[1] - chord[1] * rel[0])) / float(
np.linalg.norm(chord)
)
assert off > 0.005
assert float(np.hypot(v_m[0], v_m[1])) == pytest.approx(0.08, abs=0.008)
xy1, v1, done1 = move.sample(move.T + 0.01)
np.testing.assert_allclose(xy1, p1, atol=2e-3)
assert float(np.hypot(v1[0], v1[1])) == pytest.approx(0.0, abs=1e-4)
assert done1 is True
src = (
__import__("pathlib").Path(__file__).resolve().parents[1]
/ "arm_application" / "servo_node.py"
).read_text(encoding="utf-8")
assert "hybrid_cruise_vxy(" in src
assert "_d435_oneshot_holds_the_arm" in src
assert "smooth_joint_schedule(" in src
assert "oneshot_full_segments(" in src
assert "oneshot_concat_hold_ori(" in src
assert "oneshot_full_cruise_mps(" in src
assert "oneshot_may_send_another(" in src
assert "oneshot_hop_limits(" in src
assert "oneshot_needs_verticalize(" in src
assert "verticalize=near" not in src
oneshot = (
__import__("pathlib").Path(__file__).resolve().parents[1]
/ "arm_application" / "overhead_oneshot.py"
).read_text(encoding="utf-8")
assert "verticalize=False" in oneshot
assert "oneshot_concat_hold_ori" in oneshot
assert '_d435_send_verticalize' in src
assert 'tag="vert"' in src or '"vert"' in src
assert 'tag = "fix"' in src
assert "_d435_full_done" in src
assert "_d435_fix_done" in src
assert "flange_tilt_rad(" in src
assert "D435 重测 leftover=" in src
assert "dxy=(" in src
assert "_d435_remeasure_log" in src
assert "到位成功不许再往话题插 hold" in src
assert "FollowJointTrajectory" in src
assert "send_goal_async(" in src
assert "结果回来之前不再发下一条" in src
assert "钉住不再补刀" in src
hold = src[src.index("def _d435_oneshot_holds_the_arm") : src.index("def _tick_d435_arrive")]
assert 'self._track_source == "overhead"' in hold
assert "not self._d435_settled" in hold
assert "_publish_joint_schedule(" not in hold
assert "oneshot_cartesian_dp(" not in hold
assert "trap_joint_schedule(" not in hold
assert "_hybrid_goal_xy" not in src
assert "def _execute_twist(" in src
stage2 = (
__import__("pathlib").Path(__file__).resolve().parents[1]
/ "arm_application" / "stage2_locate_node.py"
).read_text(encoding="utf-8")
assert "from arm_application.path_profile import" in stage2
assert "FROZEN_J_HOP_MAX_M" in stage2
assert "def _trap_params(" not in stage2
assert "def _path_interp(" not in stage2
def test_frozen_j_hop_is_capped_and_same_leftover_must_not_repeat(self):
"""400 mm 一次规划会少走；28 mm 同剩差再发就是真机那趟死循环。"""
assert FROZEN_J_HOP_MAX_M == pytest.approx(0.08)
assert oneshot_next_hop_m(0.402) == pytest.approx(0.08)
assert oneshot_next_hop_m(0.028) == pytest.approx(0.028)
arrive = 0.015
assert oneshot_may_send_another(0.402, None, arrive_m=arrive) is True
assert oneshot_may_send_another(0.322, 0.402, arrive_m=arrive) is True
assert oneshot_may_send_another(0.028, 0.028, arrive_m=arrive) is False
assert oneshot_may_send_another(0.010, 0.028, arrive_m=arrive) is False
def test_oneshot_dp_lifts_z_first_and_caps_xy(self):
"""没到锁高只收 Z；400 mm 的 XY 必须被砍到 80 mm，并且带着锁高。"""
gate = 0.040
lift = oneshot_cartesian_dp(
[0.65, -0.32, 0.26], [0.58, -0.50], 0.320, gate_m=gate
)
np.testing.assert_allclose(lift[:2], [0.0, 0.0])
assert lift[2] > 0.0
assert float(np.linalg.norm(lift)) == pytest.approx(0.060, abs=1e-9)
xy = oneshot_cartesian_dp(
[0.65, -0.11, 0.320], [0.58, -0.50], 0.320, gate_m=gate
)
assert float(np.linalg.norm(xy)) == pytest.approx(FROZEN_J_HOP_MAX_M)
assert abs(float(xy[2])) < 1e-9
acc = trap_acc_for_hop(0.08, 0.08)
assert acc == pytest.approx(0.08 * 0.08 / (2.0 * 0.0256), abs=1e-6)
def test_near_leftover_is_short_slow_xy_only(self):
"""真机最后一跳 37–75 mm @ 80 mm/s 会晃。近处必须短、慢、不混 Z。"""
gate = 0.040
dp = oneshot_cartesian_dp(
[0.583, -0.465, 0.327], [0.583, -0.502], 0.320, gate_m=gate
)
leftover = 0.037
hop_max, cruise = oneshot_hop_limits(leftover, 0.08)
assert leftover <= D435_NEAR_LEFTOVER_M
assert hop_max == pytest.approx(0.015)
assert cruise == pytest.approx(0.030)
assert float(np.linalg.norm(dp)) == pytest.approx(0.015, abs=1e-9)
assert abs(float(dp[2])) < 1e-9
assert oneshot_needs_verticalize(0.03, 0.010, 0.015) is True
assert oneshot_needs_verticalize(0.03, 0.020, 0.015) is False
assert oneshot_needs_verticalize(0.03, 0.010, 0.015, already_done=True) is False
assert oneshot_needs_verticalize(0.005, 0.010, 0.015) is False
R = np.eye(3)
R[2, 2] = -1.0
w, ang = flange_down_w(R)
np.testing.assert_allclose(w, [0.0, 0.0, 0.0])
assert ang < 0.006
stage2 = (
__import__("pathlib").Path(__file__).resolve().parents[1]
/ "arm_application" / "stage2_locate_node.py"
).read_text(encoding="utf-8")
assert "return flange_down_w(R)" in stage2
def test_full_segments_do_not_chop_and_keep_z_then_xy(self):
"""整段一次走完：200 mm 不许被砍成 80 mm；没到锁高先整段收 Z。"""
gate = 0.040
segs = oneshot_full_segments(
[0.65, -0.11, 0.26], [0.58, -0.50], 0.320, gate_m=gate
)
assert segs[0][1] == "lift_z"
np.testing.assert_allclose(segs[0][0][:2], [0.0, 0.0])
assert segs[0][0][2] == pytest.approx(0.060)
assert segs[1][1] == "xy"
xy_len = float(np.linalg.norm(segs[1][0][:2]))
assert xy_len > 0.20
assert xy_len > FROZEN_J_HOP_MAX_M
near = oneshot_full_segments(
[0.583, -0.465, 0.327], [0.583, -0.502], 0.320, gate_m=gate
)
assert len(near) == 1
assert near[0][1] == "xy"
assert float(np.linalg.norm(near[0][0])) == pytest.approx(0.037, abs=1e-9)
assert abs(float(near[0][0][2])) < 1e-9
assert oneshot_full_cruise_mps(0.20, 0.08) == pytest.approx(D435_FULL_CRUISE_MPS)
assert oneshot_full_cruise_mps(0.20, 0.08) < 0.08
assert oneshot_full_cruise_mps(0.05, 0.08) == pytest.approx(D435_NEAR_CRUISE_MPS)
chopped = oneshot_cartesian_dp(
[0.65, -0.11, 0.320], [0.58, -0.50], 0.320, gate_m=gate
)
assert float(np.linalg.norm(chopped)) == pytest.approx(FROZEN_J_HOP_MAX_M)
def test_smooth_path_is_rest_to_rest_and_slows_at_the_end(self):
"""300 mm @ 60 mm/s：头尾速度 0，起步 ≥3 s，峰值加速度必须低于旧的弹射档。"""
assert D435_FULL_ACC_M == pytest.approx(0.100)
assert D435_FULL_DEC_M == pytest.approx(0.080)
assert D435_FULL_CRUISE_MPS == pytest.approx(0.060)
a_now = cosine_peak_acc(D435_FULL_CRUISE_MPS, D435_FULL_ACC_M)
a_old = cosine_peak_acc(0.08, 0.040)
assert a_old > 0.12
assert a_now < 0.035
assert a_now < 0.30 * a_old
T, v_peak, t_acc, t_cruise, d_acc, d_dec = smooth_path_params(0.30, 0.06)
assert v_peak == pytest.approx(0.06)
assert d_acc == pytest.approx(0.100)
assert d_dec == pytest.approx(0.080)
assert t_acc > 3.0
assert t_cruise > 0.5
s0, v0 = smooth_path_sv(0.0, 0.30, v_peak, t_acc, t_cruise, d_acc, d_dec)
assert s0 == pytest.approx(0.0, abs=1e-9)
assert v0 == pytest.approx(0.0, abs=1e-9)
s_mid, v_mid = smooth_path_sv(
t_acc + 0.5 * t_cruise, 0.30, v_peak, t_acc, t_cruise, d_acc, d_dec
)
assert v_mid == pytest.approx(0.06, abs=1e-6)
assert 0.10 < s_mid < 0.24
s1, v1 = smooth_path_sv(T, 0.30, v_peak, t_acc, t_cruise, d_acc, d_dec)
assert s1 == pytest.approx(0.30, abs=1e-6)
assert v1 == pytest.approx(0.0, abs=1e-6)
s_dec, v_dec = smooth_path_sv(
t_acc + t_cruise + 0.5 * (T - t_acc - t_cruise),
0.30,
v_peak,
t_acc,
t_cruise,
d_acc,
d_dec,
)
assert v_dec < 0.045
assert s_dec > 0.20
q0 = np.zeros(7)
q1 = np.zeros(7)
q1[0] = 0.3
sched, T_move, vp = smooth_joint_schedule([q0, q1], 0.30, 0.06)
assert T_move == pytest.approx(T, abs=1e-6)
assert vp == pytest.approx(0.06)
assert float(np.linalg.norm(sched[0][1])) == pytest.approx(0.0, abs=1e-9)
assert float(np.linalg.norm(sched[-1][1])) == pytest.approx(0.0, abs=1e-9)
times = [t for _, _, t in sched]
assert all(times[i] < times[i + 1] for i in range(len(times) - 1))
def test_concat_hold_ori_refuses_to_mix_verticalize(self):
"""串段必须 verticalize=False。混进收垂直会把腕部漏成 Y。"""
J = np.zeros((6, 7))
J[0, 0] = 1.0
J[1, 1] = 1.0
J[2, 2] = 1.0
segs = [
(np.array([0.0, 0.0, 0.02]), "lift_z"),
(np.array([0.04, -0.03, 0.0]), "xy"),
]
wps, planned, asked, tag = oneshot_concat_hold_ori(
np.zeros(7), J, segs, R=np.eye(3)
)
assert tag == "full"
assert asked == pytest.approx(0.02 + float(np.hypot(0.04, 0.03)))
assert len(wps) >= 3
src = (
__import__("pathlib").Path(__file__).resolve().parents[1]
/ "arm_application" / "overhead_oneshot.py"
).read_text(encoding="utf-8")
assert "verticalize=False" in src
assert "oneshot_concat_hold_ori" in src
def test_oneshot_schedule_is_rest_to_rest_and_times_increase(self):
"""一次下发的轨迹：头尾速度 0，时间递增。中途再插点就会把这条掐掉。"""
q0 = np.zeros(7)
q1 = np.zeros(7)
q1[0] = 0.3
sched, T_move, v_peak = trap_joint_schedule(
[q0, q1], 0.40, 0.08, 0.40
)
assert T_move > 4.0
assert v_peak == pytest.approx(0.08)
assert len(sched) > 8
assert float(np.linalg.norm(sched[0][1])) == pytest.approx(0.0, abs=1e-9)
assert float(np.linalg.norm(sched[-1][1])) == pytest.approx(0.0, abs=1e-9)
times = [t for _, _, t in sched]
assert times == sorted(times)
assert all(times[i] < times[i + 1] for i in range(len(times) - 1))
mid = sched[len(sched) // 3]
assert float(np.linalg.norm(mid[1])) > 0.01
def test_translation_holds_the_height_it_started_at(self):
"""平移钉住开始时的高度。沉下去要抬回来，已经在 4mm 内就不要再收。"""
assert hold_translation_vz(0.327, 0.327, kp=1.0, v_lim=0.05) == 0.0
assert hold_translation_vz(0.325, 0.327, kp=1.0, v_lim=0.05) == 0.0
assert hold_translation_vz(0.307, 0.327, kp=1.0, v_lim=0.05) > 0.0
assert hold_translation_vz(0.347, 0.327, kp=1.0, v_lim=0.05) < 0.0
src = (
__import__("pathlib").Path(__file__).resolve().parents[1]
/ "arm_application" / "servo_node.py"
).read_text(encoding="utf-8")
assert "hold_translation_vz(" in src
def test_threshold_is_the_caller_value_not_hardcoded(self):
# 负向：如果函数内部写死 0.5，改门槛 0.7 时 0.6 会判错
assert wrist_needs_handoff(0.6, 0.7) is True
src/arm_application/test/test_pipeline_stage1_diag.py
+19 -1
View File
@@ -13,7 +13,7 @@ from __future__ import annotations
import pytest
from arm_application.pipeline_node import stage1_timeout_reason
from arm_application.pipeline_node import stage1_timeout_reason, track_lock_z
def test_zero_points_blames_perception_not_calibration():
@@ -40,3 +40,21 @@ def test_timeout_seconds_and_log_path_are_in_the_message():
msg = stage1_timeout_reason(0, 42.0, "/tmp/x.log")
assert "42s" in msg
assert "/tmp/x.log" in msg
def test_real_arm_locks_track_height_at_hover():
"""真机 89：悬停时规尖已经离面约 75mm，跟踪锁在悬停高，对孔前不再抬。"""
z = track_lock_z(0.320, 0.223, 0.022)
assert z == pytest.approx(0.320)
def test_sim_still_lifts_when_hover_barely_clears_the_part():
"""仿真悬停只留约 2mm，不抬就会在跟踪时刮到工件。"""
z = track_lock_z(0.657, 0.243, 0.412)
assert z == pytest.approx(0.707)
def test_missing_geometry_does_not_pretend_the_hover_is_clear():
"""工具长或工件面缺失时必须抬升。缺数还锁在悬停高，等于没检查。"""
assert track_lock_z(0.320, 0.0, 0.022) == pytest.approx(0.370)
assert track_lock_z(0.320, 0.223, 0.0) == pytest.approx(0.370)
src/arm_application/test/test_stage2_descent_clamp.py
+18
View File
@@ -314,6 +314,24 @@ def test_goto_failure_stops_the_descent():
# --------------------------------------------------------------------------- #
# 不许回退到"把目标值当结果汇报"
# --------------------------------------------------------------------------- #
def test_no_descent_still_drops_to_the_observe_gap():
"""--no-descent 不是停在悬停高：要降到观测间隙（15mm），方便看规尖偏没偏。
不许降到贴面 2mm —— 对偏了再贴面会把规顶在孔沿上。
"""
st = _StubStage2()
st.final_descent_enable = False
st.observe_gap_m = 0.015
ok, Tee, st = _run(st)
assert ok is True
gap_mm = (Tee[2, 3] - _TOOL_LEN - _SURFACE_Z) * 1000
assert abs(gap_mm - 15.0) < 1.0, f"观测下降应停在离面 15mm，实际 {gap_mm:.1f}mm"
total_mm = sum(st.steps_mm)
assert total_mm < _BUDGET * 1000 - 5.0, "观测下降走到了贴面预算，等于把 --no-descent 吃掉了"
assert any("观测下降完成" in m for m in st.logs), "日志要写明这是观测下降，不是贴面轻降"
def test_wired_final_gap_is_reported_from_geometry_not_target():
"""收尾要汇报**几何实算**的间隙，不能把目标值（2.0mm）念一遍。
src/arm_application/test/test_stage2_tip_align.py
+231 -2
View File
@@ -27,6 +27,7 @@ from __future__ import annotations
from typing import Any, Dict, Optional, Tuple
import numpy as np
import pytest
from arm_application.stage2_locate_node import Stage2Locate
@@ -101,9 +102,9 @@ class _Stub:
return _R_CB @ np.asarray(p_cam, float) + self._t_cb
def _call(st: _Stub, depth: np.ndarray):
def _call(st: _Stub, depth: np.ndarray, z_wp_cam: Optional[float] = None):
return Stage2Locate._tip_offset_from_depth( # type: ignore[arg-type]
st, depth, _INTR, _TEE
st, depth, _INTR, _TEE, z_wp_cam=z_wp_cam
)
@@ -143,6 +144,129 @@ def test_offset_is_stable_frame_to_frame():
# --------------------------------------------------------------------------- #
# 判据各自都要能真的失败（负向用例）
# --------------------------------------------------------------------------- #
def _gripper_and_rod_frame() -> np.ndarray:
"""爪子块头大且更近 + 规杆更细更深：复现 2026-10-09 真机『按面积选爪子』。"""
depth = np.full((480, 800), 0.50, np.float32)
depth[20:90, 330:470] = 0.07 # 爪子：宽、浅、画面上方
for v in range(90, 280):
f = min(1.0, (v - 90) / 200.0)
depth[v, 392:408] = 0.08 + f * 0.05 # 规杆：细、伸向工件
return depth
def test_picks_the_lower_tip_not_the_config_length_blob():
"""画面上方、工具长刚好贴着配置的那一块不是规尖。
2026-10-09 真机：最接近配置 223mm 的点工具长 225mm、偏移约 0，冻住的是
法兰轴线。残差 0.14mm 达标，下降后规尖仍偏出约 10mm。规尖在画面更靠下。
"""
depth = np.full((480, 800), 0.50, np.float32)
depth[40:100, 370:430] = 0.080 # 靠上、居中，工具长贴着配置
for v in range(200, 301):
f = min(1.0, (v - 200) / 200.0)
depth[v, 442:458] = 0.10 + f * 0.02 # 靠下、偏出中心
st = _Stub()
st.tool_tip_below_flange_m = 0.185
off, tlen, why = _call(st, depth)
assert off is not None and tlen is not None, f"有规尖却没量到：{why}"
v = float(why.split("px=(")[1].split(")")[0].split(",")[1])
assert v > 180, f"采到的是贴着配置工具长的上方点，不是规尖：{why}"
assert abs(float(off[0])) > 0.005, (
f"规尖偏出中心，X 偏移不该≈0（那是法兰轴）：{why} off={off}"
)
def test_picks_rod_not_larger_gripper():
"""中央竖带里爪子面积更大时，必须仍选**更深的规杆**，不能按连通域面积。"""
depth = _gripper_and_rod_frame()
off, tlen, why = _call(_Stub(), depth)
assert off is not None and tlen is not None, f"有规杆却没量到：{why}"
assert "px=" in why
# 规尖在杆下端 v≈270，爪子在 v≈20–90；采到爪子 v 会 <100
v = float(why.split("px=(")[1].split(")")[0].split(",")[1])
assert v > 150, f"采到的是爪子不是规尖：{why}"
def test_workpiece_ceiling_keeps_the_real_tip():
"""同一帧工件面深度必须抬高近端上限，否则 150mm 会切掉真规尖。
2026-10-09 真机：孔 camera-Z ≈168mm，固定 tip_near=150mm 把尖切掉，
像素在 v=80（杆上半）和 v=188（尖偶尔漏进来）两档跳，6 拍冻不住。
上限要留在工件面前方（8mm）：贴着面的像素是工件，不是规。
再往前砍 15mm 会把只比面近十来毫米的真尖切掉。
"""
depth = np.full((480, 800), 0.50, np.float32)
for v in range(80, 200):
depth[v, 392:408] = 0.08 + (v - 80) / 120.0 * 0.06 # →0.14，在 150mm 内
for v in range(200, 280):
depth[v, 392:408] = 0.14 + (v - 200) / 80.0 * 0.024 # →0.164，会被 150mm 切掉
depth[300:400, 200:600] = 0.172 # 工件面；近端上限 = 面 − 15mm
cut, _t0, why0 = _call(_Stub(), depth) # 无 z_wp → 固定 150mm
full, tlen, why = _call(_Stub(), depth, z_wp_cam=0.172)
assert full is not None and tlen is not None, f"给了工件面仍没量到尖：{why}"
v_full = float(why.split("px=(")[1].split(")")[0].split(",")[1])
assert v_full > 200, f"有工件面上限仍没采到杆下端：{why}"
if cut is not None:
v_cut = float(why0.split("px=(")[1].split(")")[0].split(",")[1])
assert v_full > v_cut + 20, (
f"工件面上限没有比固定 150mm 采到更靠下的尖：cut={why0} full={why}"
)
def test_keeps_the_tip_when_it_is_only_slightly_in_front_of_the_surface():
"""规尖只比工件面近十来毫米时，必须采尖，不能退回更近的夹爪。
2026-10-09 观测高度实测：工件面相机深度 127mm，规尖 115mm，夹爪/杆身 80mm。
近端砍到「面 − 15mm」会把尖切掉，冻住的是杆身（偏移约 2mm、工具长贴着
配置 223mm）。残差 0.2mm 达标，针却偏在孔沿上。
"""
depth = np.full((480, 800), 0.50, np.float32)
depth[140:190, 360:430] = 0.080 # 夹爪：更近、更靠上
depth[210:236, 430:470] = 0.118 # 规尖：只比面近 12mm，画面更靠下
depth[250:420, 250:600] = 0.130 # 工件面
off, tlen, why = _call(_Stub(), depth, z_wp_cam=0.130)
assert off is not None and tlen is not None, f"只比面近 12mm 的尖被切掉了：{why}"
v = float(why.split("px=(")[1].split(")")[0].split(",")[1])
assert v > 200, f"采到的是夹爪不是规尖：{why}"
assert abs(float(off[0])) > 0.005, (
f"尖偏出中心，不该量成法兰轴：{why} off={off}"
)
def test_rejects_pixels_stuck_to_the_workpiece():
"""贴着工件面的近端块必须拒绝 —— 那是面，不是规尖。
2026-10-09 重跑：上限只让 3mm，噪声把工件漏进来，工具长测成 296–315mm
（规实际约 190mm），样本互斥，随后两孔中点一跳就被当成工件被推走。
"""
depth = np.full((480, 800), 0.50, np.float32)
depth[200:420, 360:440] = 0.170 # 大块、深度贴着 0.172 的工件面
off, tlen, why = _call(_Stub(), depth, z_wp_cam=0.172)
assert off is None and tlen is None, f"贴着工件面的块被当成规尖：{why}"
assert "工件" in why
def test_rejects_tip_that_lands_on_the_workpiece_surface():
"""反投影落在工件面上的点必须丢掉，哪怕相机深度还在近端上限之内。
2026-10-09：相机深度留了 15mm，工具长仍测成 298mm（配置 223mm），
基座 z 贴在工件面上。偏移冻不住 → exit=8 → 观测下降根本不会执行。
"""
st = _Stub()
st.surface_z_m = 1.0 # 高于任何反投影点
off, tlen, why = _call(st, _rod_frame())
assert off is None and tlen is None, f"贴在工件面上的点被当成规尖：{why}"
assert "工件" in why
def test_keeps_tip_that_clears_the_workpiece_surface():
"""规尖明显高于工件面时，表面守卫不许把真规尖一起丢掉。"""
st = _Stub()
st.surface_z_m = 0.05
off, tlen, why = _call(st, _rod_frame())
assert off is not None and tlen is not None, f"离面足够却被拒：{why}"
def test_rejects_when_nothing_is_near():
"""近端什么都没有（规不在视野）⇒ 必须判失败，不能返回一个假偏移。"""
depth = np.full((480, 800), 0.50, np.float32) # 全在远端
@@ -301,8 +425,113 @@ def test_default_is_tip_vision_on_in_site():
(Path(__file__).resolve().parents[3] / "config" / "site.yaml").read_text(encoding="utf-8")
)
st2 = ((site.get("hole_detect") or {}).get("stage2") or {})
robots = ((site.get("robot") or {}).get("robots") or [])
arm89 = next(r for r in robots if str(r.get("sn")) == "Rizon4-063989")
servo = arm89.get("servo") or {}
assert float(servo.get("ee_z_m")) == pytest.approx(
float(servo.get("hole_hover_z_m"))
), "左臂跟踪锁高必须等于孔悬停高：对孔完成前不许改高度"
assert st2.get("align_tip_vision") is True, "默认必须用视觉规尖"
assert float(st2.get("align_tol_m")) <= 0.00025 + 1e-9, (
f"align_tol_m={st2.get('align_tol_m')} 太松：会放过系统性的对孔偏差"
)
assert int(st2.get("align_max_iters")) >= 12, "收紧容差后迭代上限要跟上"
class _TrimStub:
"""下降后 XY 修正用到的成员。孔在基座里不动，规尖偏移是常数。"""
tip_vision = True
k1_label = "left"
target_hole = "k1"
align_tol = 0.00025
align_speed = 0.02
vert_tol_rad = 0.02
base_frame = "world"
ee_frame = "flange"
_tip_off = np.array([0.01, 0.0])
_tip_samp: list = []
_tip_len_m = 0.22
def __init__(self, err_xy=(0.0026, -0.0004), tip=None) -> None:
self.hole = np.array([0.50 + err_xy[0], -0.10 + err_xy[1], 0.022])
self.tip = None if tip is False else np.array([0.0015, -0.0012])
self.moves: list = []
self._logger = _StubLogger()
def get_logger(self):
return self._logger
def _lookup(self, *_a):
return None
def _pin(self, _q) -> None:
return None
def _snap_holes(self, *, skip_center_gate: bool):
res = {
"status": True,
"hole_base": [self.hole.tolist(), (self.hole + np.array([0.0, -0.060, 0.0])).tolist()],
"z_wp": 0.127,
}
return None, None, {}, res
def _tip_offset_from_depth(self, *_a, **_kw):
if self.tip is None:
return None, None, "没看到规"
return self.tip.copy(), 0.222, "px=(447,226)"
def _goto(self, _q, Tee, target, **_kw):
Tee = np.array(Tee, float)
self.moves.append(np.array(target, float))
Tee[0, 3] = float(target[0])
Tee[1, 3] = float(target[1])
Tee[2, 3] = float(target[2])
return True, _q, Tee
def _trim(st: _TrimStub, z: float = 0.260):
import rclpy
if not rclpy.ok():
rclpy.init()
Tee = np.eye(4)
Tee[0, 3] = 0.50
Tee[1, 3] = -0.10
Tee[2, 3] = z
final = {"ok": True, "residual_mm": 0.22, "gauge_source": "视觉规尖(已冻结)"}
return Stage2Locate._trim_xy_after_descent(st, np.zeros(7), Tee, final) # type: ignore[arg-type]
def test_trim_after_descent_corrects_xy_and_holds_z():
"""下降后孔位漂了 2mm 级，要在当前高度补上，不许改高度。
2026-10-09：悬停残差 0.22mm，降到观测高度后同一孔的基座 XY 差约 2.6mm。
"""
st = _TrimStub()
_q, Tee, final = _trim(st)
assert abs(float(Tee[2, 3]) - 0.260) < 1e-9, "下降后修正不许改高度"
tip = np.array([Tee[0, 3] + st.tip[0], Tee[1, 3] + st.tip[1]])
err = float(np.hypot(tip[0] - st.hole[0], tip[1] - st.hole[1]))
assert err <= 0.00025 + 1e-6, f"补完针还偏 {err*1000:.2f}mm"
assert final["residual_mm"] <= 0.25
assert "下降后" in final["gauge_source"]
def test_trim_refuses_a_centimeter_scale_correction():
"""要补的量到了厘米，就是采到夹爪了，必须停在原地。"""
st = _TrimStub(err_xy=(0.014, 0.0))
_q, Tee, final = _trim(st)
assert not st.moves, "厘米级修正被执行了"
assert abs(float(Tee[0, 3]) - 0.50) < 1e-9
assert final["residual_mm"] == 0.22, "拒绝修正时不能把悬停结果改写成没补上的残差"
assert any("不像针" in m for m in st._logger.msgs)
def test_trim_does_not_move_when_the_tip_is_missing():
"""下降后没量到规尖，保持悬停对准的位置。"""
st = _TrimStub(tip=False)
_q, Tee, final = _trim(st)
assert not st.moves
assert abs(float(Tee[0, 3]) - 0.50) < 1e-9
assert final["gauge_source"] == "视觉规尖(已冻结)"
src/arm_driver_flexiv/resource/arm_driver_flexiv
+1
View File
@@ -0,0 +1 @@
src/arm_kinematics/resource/arm_kinematics
+1
View File
@@ -0,0 +1 @@
src/arm_sim_gazebo/resource/arm_sim_gazebo
+1
View File
@@ -0,0 +1 @@