P3a：手眼标定集中到 handeye/，修复三套板尺寸矛盾与从未生效的稳定性参数

新建 handeye/ 专用文件夹（用户要求：专用文件夹 + 使用说明） - handeye/README.md：从小白视角讲清楚两种模式怎么选、板尺寸怎么量、完整流程、出问题先看哪里。以前这些知识散在 docs/ 和代码注释里。 - 结构：calibrate.yaml（参数）/ poses/（采点输入）/ results/（结果输出）/ tmp/（临时，不进 git） - 文件用 git mv 迁移，历史保留命名约定改为 <臂号>_<模式>.yaml - 如 89_eye_in_hand.yaml、91_eye_to_hand.yaml - 臂号 = Flexiv 序列号后两位；模式 = eye_in_hand | eye_to_hand - 换模式 = 改 site.yaml 里 paths.handeye_file 一行，不动代码 - 迁移只需改 site.yaml（4 个 launch + servo_node 都经 paths 读取，一行没改）修复：三套互相矛盾的标定板尺寸（真实 bug）同一个"板子多大"分散在三处，且值各不相同： handeye_calibrate.py 默认 9x6 / 0.025 handeye/calibrate.yaml 11x16 / 0.011 site.yaml + 结果文件头 8x11 / 0.015 ← 实测的真实板子谁生效取决于覆盖顺序。症状是"标定流程跑得很顺、不报错，但结果明显不对"，极难排查。已全部统一为 8x11 / 0.015，README 里写明换板必须三处同改。修复：site.yaml 里五项稳定性配置从未生效 run_handeye.sh 一直在传 settle_qd_max / settle_stable_sec / settle_timeout_sec / pin_holds / pin_hold_sec，但 handeye_calibrate.py 从未声明这些参数 —— ROS 2 会静默忽略未声明的覆盖参数，所以"等机械臂停稳再拍照"这件事其实没实现，实际只有一次固定 settle_sec。现已声明并实现： - _wait_settled()：用关节速度 |qd|_max 判定"真的停了"，比固定 sleep 可靠（负载/姿态不同刹车距离差别很大；等太短会在臂还动时拍照导致角点抖动） - _pin_hold()：静止后再保持几拍，抑制到位后的微小回弹新增：自动标定进度条一条可视化进度条（[ 4/12] [████░░░░] 33.3%），无人值守的长流程能一眼看出还剩多久。清理重复文件（内容逐字节比对确认后删除） - config/handeye_result.yaml 与 _89 逐字节相同 → 删 - config/handeye_poses.yaml 与 _89 逐字节相同 → 删迁移后 src/arm_application/config/ 下已无 handeye 文件。测试：224 passed（原 198，新增 26）新增 src/arm_application/test/test_handeye_lib.py —— 手眼模块此前零测试。关键用例是合成数据往返：用人为设定的 X_true 反推出每个视图的 (gripper→base, target→cam)，再让求解器还原。坐标链方向/求逆/转置只要有一处写错就必然失败。 - 真值恢复（旋转+平移，atol 1e-6） - 单独验证平移精确（旋转对而平移错是最易漏的一类 bug） - 结果必须是 det=+1 的正交阵（排除镜像解） - 退化输入（全同位姿）不得悄悄给出"看起来能用"的结果 - 稳定性判据：窗口未满不判稳、丢帧后重新计数、抖动超阈不判稳 - 写盘再读回不漂移
2026-09-23 11:46:03 +08:00
parent 52c108de07
commit a3a589b9e3
18 changed files with 735 additions and 85 deletions
@@ -0,0 +1,214 @@
+# handeye/ —— 手眼标定
+
+这个文件夹放**手眼标定所需的全部配置、采点与结果**。以前这些东西散在
+`src/arm_application/config/` 里，和别的配置混在一起，改板子尺寸要翻三个文件 ——
+现在集中在这里，改一个地方就够了。
+
+---
+
+## 1. 先搞懂"手眼标定"在干什么
+
+机械臂要"看着东西去操作"，就必须知道**相机看到的位置**和**机械臂自己认为的位置**
+之间的对应关系。这个对应关系就是一个 4×4 的变换矩阵，标定过程就是把它解出来。
+
+两种装法，决定了解出来的矩阵含义不同：
+
+| 模式 | 相机装在哪 | 解出的 X 是什么 | 本项目的用法 |
+|------|-----------|----------------|-------------|
+| `eye_in_hand` | **相机装在机械臂末端（法兰/手腕）上** | 相机 → 法兰 | **D405 腕部相机（当前使用）** |
+| `eye_to_hand` | **相机固定在旁边不动** | 相机 → 机械臂基座 | D435 顶置相机（预留） |
+
+**怎么选**：看相机是不是跟着机械臂动。
+- 跟着动 → `eye_in_hand`
+- 不动 → `eye_to_hand`
+
+选错模式的后果：标定能算完、不报错，但结果**完全不对**（点云/抓取会系统性偏掉）。
+所以模式在文件名里就标出来了（见第 4 节），避免拿错文件。
+
+---
+
+## 2. 文件夹结构
+
+```
+handeye/
+├── README.md                      ← 你正在看的这份
+├── calibrate.yaml                 ← 标定节点参数（板尺寸、运动、稳定性）
+├── waypoints.yaml                 ← 旧的"巡航记点"配置（legacy，一般用不到）
+├── poses/                         ← 【输入】示教采点（每个点一组关节角）
+│   ├── 89_eye_in_hand.yaml
+│   └── 91_eye_in_hand.yaml
+├── results/                       ← 【输出】标定结果（4×4 变换矩阵）
+│   ├── 89_eye_in_hand.yaml
+│   └── 91_eye_in_hand.yaml
+└── tmp/                           ← 临时产物（采点图、taught_poses.txt、captures.json）
+                                     不进 git
+```
+
+**命名约定**：`<臂号>_<模式>.yaml`
+- 臂号 = Flexiv 序列号后两位（`Rizon4-063989` → `89`，`Rizon4-063991` → `91`）
+- 模式 = `eye_in_hand` | `eye_to_hand`
+
+运行时谁读结果：`config/site.yaml` 里 `robots[89].paths.handeye_file` 指向
+`handeye/results/89_eye_in_hand.yaml`，再由各 launch 文件发布成静态 TF。
+**换模式 = 把 site.yaml 里那个路径改成 `_eye_to_hand` 那份**，不需要改代码。
+
+---
+
+## 3. 标定板（最容易出错的地方）
+
+当前使用的板：
+
+```
+内角点  8 列 × 11 行      (注意是"内角点"，不是方格数！)
+方格边长 15 mm (0.015 m)
+```
+
+**这个尺寸必须三处一致**，否则标定结果会系统性偏错：
+
+| 位置 | 内容 |
+|------|------|
+| `config/site.yaml` → `handeye.board_cols/rows/square_size_m` | 8 / 11 / 0.015 |
+| `handeye/calibrate.yaml` | `board_cols/rows/square_size_m` |
+| `src/.../handeye_calibrate.py` 的参数默认值 | 代码兜底 |
+
+> ⚠️ **历史上这里出过问题**：曾经三处分别写着 `9×6/0.025`、`11×16/0.011`、
+> `8×11/0.015`，谁生效取决于覆盖顺序。症状是"标定流程跑得很顺、不报错，
+> 但结果明显不对"，非常难查。现已统一为 `8×11/0.015`。
+> **换板子时，请把上面三处一起改。**
+
+怎么看内角点：把棋盘当成一张网格，**数内部的黑白交叉点**，
+是「方格数 − 1」。比如 A5 板上画了 9×12 个格子 → 内角点是 8×11。
+
+---
+
+## 4. 完整流程
+
+三条命令，按顺序执行。**先决条件**：标定板摆在相机视野里，机械臂能自由拖动。
+
+### 第一步：采点（拖动示教）
+
+```bash
+bash ~/ros2/scripts/run_handeye.sh teach --arm 89
+```
+
+会弹出实时预览窗口，里面能看到标定板有没有被识别到。操作：
+
+| 按键 | 作用 |
+|------|------|
+| `d` | 打开软拖拽（此时可以用手推机械臂） |
+| `s` | 关闭软拖拽（锁住） |
+| `Enter` / `空格` | **拍照记点**（记录当前关节角 + 图像） |
+| `q` / `Esc` | 结束采点 |
+
+要点：
+- **点位要分散**：让标定板在画面里出现在不同位置、不同角度（远/近/左/右/倾斜）。
+  全挤在一个角落会让求解退化，结果不可信。
+- **至少 5 个点**，推荐 **10～15 个**。数量在 `poses/*.yaml` 的 `num_poses` 里，
+  也可以直接多按几次 `Enter`。
+- 采点数据写在 `handeye/tmp/`，**确认无误后再 merge**（下一步）。
+
+### 第二步：合并点位
+
+```bash
+bash ~/ros2/scripts/run_handeye.sh merge --arm 89
+```
+
+把临时采到的关节角写入 `handeye/poses/89_eye_in_hand.yaml`，
+并自动同步 `num_poses`。这一步不做数学计算，只是搬运+格式化。
+
+### 第三步：自动标定
+
+```bash
+bash ~/ros2/scripts/run_handeye.sh calibrate --arm 89
+```
+
+机械臂会**自动**走一遍刚才记下的所有点位，每个点：
+
+1. 移动到目标位姿
+2. 校验确实到位了（防止控制器假报成功）
+3. **等机械臂真的停下来**（看关节速度，不是干等固定秒数）
+4. 等画面里的标定板**连续多帧稳定**（角点抖动小于阈值）
+5. 拍照、记录法兰位姿 + 板位姿
+6. 屏幕上打一条进度条
+
+全部走完后一次性解算出结果，写入
+`handeye/results/89_eye_in_hand.yaml`。
+
+```
+[ 4/12] [████████░░░░░░░░░░░░░░░░]  33.3%  moving...
+```
+
+某个点看不到板时会**跳过该点**继续（`skip_unstable_poses: true`），
+不会整轮中止 —— 但如果跳掉的点很多，说明采点时板的位置不好，建议重采。
+
+### 查看当前状态
+
+```bash
+bash ~/ros2/scripts/run_handeye.sh show --arm 89
+```
+
+打印臂号、配置文件路径、板尺寸、已记录点数，以及当前的标定结果是否存在。
+
+---
+
+## 5. 两种模式
+
+- **自动模式** = 上面的 `calibrate`。机械臂自己走点位，人不用管，
+  适合"点位已经采好、只想重算一遍"。
+- **手动模式** = 上面的 `teach`。人拖着机械臂摆姿势、手动决定什么时候记这个点。
+  适合第一次标定，或者想补几个点位。
+
+两者共用同一份 `poses/*.yaml`，顺序是 `teach`（采）→ `merge`（存）→ `calibrate`（算）。
+
+---
+
+## 6. 结果怎么被用起来
+
+`handeye/results/89_eye_in_hand.yaml` 的内容长这样：
+
+```yaml
+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]
+```
+
+它被以下地方读取，最终以**静态 TF** 的形式发布出去：
+
+| 谁读 | 在哪 |
+|------|------|
+| `pipeline.launch.py` | 发布 `{法兰}` → `{相机}_color_optical_frame` 静态 TF |
+| `visual_servo.launch.py` | 同上，并把路径传给 `servo_node` |
+| `stage2_locate.launch.py` | 同上；文件缺失会直接报错 |
+| `stage3_search.launch.py` | 同上 |
+| `servo_node.py` | 用 `T_4x4` 算相机倾角、做 hover/lookat |
+
+也就是说：**标定结果不对，上面这些全会跟着错**。改完结果建议重启相关节点。
+
+---
+
+## 7. 出了问题先看这里
+
+| 现象 | 多半是 |
+|------|--------|
+| 标定跑完但抓取/定位系统性偏 | 板尺寸三处不一致；或模式选错（eye-in-hand / eye-to-hand 弄反） |
+| 一直"棋盘不稳定"、点位被跳过 | 标定板没完整入镜 / 反光过曝 / 太远太小；调低 `stable_pixel_std` 或改善光照 |
+| `ERROR: 机械臂控制器未激活` | 先另开终端跑 `bash ~/ros2/scripts/run_real.sh --arm 89` |
+| `hand-eye result missing` | `site.yaml` 里的 `paths.handeye_file` 指到了不存在的文件 |
+| 拖动时臂不动 | 软拖拽没开（按 `d`）；或 RDK 与 `run_real.sh` 抢占，见 `docs/INTERFACES.md` |
+| 采的点少于 5 个 | `merge` 会拒绝（求解至少需要 5 组，且姿态要有区分度） |
+
+---
+
+## 8. 相关文件
+
+- 拖动引擎：`scripts/rdk_drag_bridge.py`
+- 拖拽的 ROS 服务：`/soft_drag`（在 `arm_driver_flexiv` 里）
+- 数学与视觉：`src/arm_application/arm_application/handeye_lib.py`
+- 采点节点：`src/arm_application/arm_application/handeye_teach.py`
+- 求解节点：`src/arm_application/arm_application/handeye_calibrate.py`
+- 单元测试：`src/arm_application/test/test_handeye_lib.py`
+  （用合成数据验证求解正确性，无需硬件：
+  `python3 -m pytest src/arm_application/test/test_handeye_lib.py`）
@@ -0,0 +1,87 @@
+# Hand-eye auto calibration config.
+#
+# Workspace: ~/ros2
+#
+# Teach poses, then solve (prefer the wrapper):
+#   bash ~/ros2/scripts/run_handeye.sh teach
+#   bash ~/ros2/scripts/run_handeye.sh merge
+#   bash ~/ros2/scripts/run_handeye.sh calibrate
+
+handeye_calibrate:
+  ros__parameters:
+    mode: "eye_in_hand"
+
+    joint_prefix: "Rizon4-063989_"
+    joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]
+    velocity_scale: 0.10
+    action_name: "follow_joint_trajectory"
+    move_time_sec: 4.0
+    start_index: 0
+    min_poses: 5
+
+    # Camera on flange (eye-in-hand): D405
+    camera_source: "d405"            # d405 | d435 | manual
+    camera_serial: ""
+    camera_namespace: ""
+    realsense_width: 1280
+    realsense_height: 720
+    image_topic: ""
+    camera_info_topic: ""
+
+    camera_fx: 0.0
+    camera_fy: 0.0
+    camera_cx: 0.0
+    camera_cy: 0.0
+    camera_dist: [0.0, 0.0, 0.0, 0.0, 0.0]
+
+    # 真实使用的标定板：内角点 8x11，方格 15mm。
+    # 依据是 handeye/results/*.yaml 头部记录的实测板子，以及 site.yaml 的 handeye.board_*。
+    # 这里原先写的是 11x16 / 0.011（另一张早期用过的板）—— 三处配置曾互相矛盾，
+    # 谁生效取决于覆盖顺序，因此存在"标定跑完但结果不对"的风险。
+    board_cols: 8
+    board_rows: 11
+    square_size_m: 0.015
+
+    settle_sec: 0.4
+    stable_window: 8
+    stable_pixel_std: 0.45
+    stable_timeout_sec: 25.0
+
+    # 临时产物（采点图、taught_poses.txt、captures.json）落在 handeye/tmp/ 下。
+    # 注意 handeye/tmp/ 不进 git。
+    output_dir: "/home/cc/ros2/handeye/tmp"
+    # 留空 = 由 run_handeye.sh 按"设备ID + 模式"给出路径，
+    # 即 handeye/results/<臂号>_<模式>.yaml（如 89_eye_in_hand.yaml）。
+    result_file: ""
+    cleanup_images: true
+
+    num_poses: 13
+    # Taught 2026-08-28 via handeye_teach (backend:=rdk)
+    waypoint_positions: [
+      # pose 0
+      -1.107107, -0.744489, 0.982447, 1.856279, -0.735517, 0.946910, -0.118163,
+      # pose 1
+      -0.982764, -1.163516, 1.128527, 1.406787, -0.502416, 0.571857, 0.224024,
+      # pose 2
+      -1.012149, -1.180698, 1.184095, 1.390763, -0.559587, 0.407246, 0.249883,
+      # pose 3
+      -0.984199, -1.175286, 1.175012, 1.397197, -0.796608, 0.459682, 0.247304,
+      # pose 4
+      -0.982355, -1.196864, 1.076502, 1.590187, -1.106339, 0.636800, 0.481410,
+      # pose 5
+      -1.121428, -1.173892, 1.152070, 1.762215, -1.147604, 0.641314, 0.434178,
+      # pose 6
+      -1.121716, -1.174258, 1.203286, 1.643879, -1.200144, 0.459669, 0.315685,
+      # pose 7
+      -1.100360, -1.337467, 1.235742, 1.377341, -1.211692, 0.569701, 0.220985,
+      # pose 8
+      -1.160070, -1.473164, 1.364572, 1.369112, -0.784077, 0.489467, 0.378945,
+      # pose 9
+      -0.575560, -1.470411, 1.348577, 0.982228, -1.163608, -0.208091, 0.410659,
+      # pose 10
+      -0.716065, -1.411332, 1.272539, 1.753754, -1.856130, -0.098214, 0.341287,
+      # pose 11
+      -0.870910, -1.412227, 1.243149, 1.900543, -1.886769, 0.153908, -0.399394,
+      # pose 12
+      -0.656925, -1.412314, 1.216671, 1.820324, -1.850224, -0.103640, -0.399964,
+    ]
@@ -0,0 +1,38 @@
+# Hand-eye taught joint poses (7 joints × N). Shared by calibrate + waypoints.
+#
+# Edit here, or:
+#   bash ~/ros2/scripts/run_handeye.sh teach
+#   bash ~/ros2/scripts/run_handeye.sh merge
+#
+# /** so any node name receives these params.
+
+/**:
+  ros__parameters:
+    num_poses: 12
+    # Taught 2026-09-02 via handeye_teach (RDK free-drive)
+    waypoint_positions: [
+      # pose 0
+      -0.751865, -0.769544, 0.713051, 1.140021, -0.189690, 0.133294, -0.342184,
+      # pose 1
+      -0.541318, -0.867448, 0.864868, 1.233428, -0.348623, 0.146046, -0.249719,
+      # pose 2
+      -0.343594, -0.948720, 0.848287, 1.273279, -0.954339, 0.019624, 0.071314,
+      # pose 3
+      -0.379884, -1.002193, 1.166394, 1.579194, -1.097644, -0.115142, -0.347640,
+      # pose 4
+      -0.694826, -0.943610, 0.820178, 0.963907, -0.427763, 0.165888, -0.761562,
+      # pose 5
+      -0.964023, -1.025531, 1.013898, 1.032490, -0.167151, 0.017429, -0.566073,
+      # pose 6
+      -0.812664, -0.984951, 1.072695, 1.473414, -0.772970, 0.267382, 0.401278,
+      # pose 7
+      -0.967527, -1.067191, 1.101680, 1.434183, -0.915388, 0.259486, 0.205330,
+      # pose 8
+      -0.451483, -1.257408, 0.895481, 0.806594, -0.652401, -0.221453, 0.184234,
+      # pose 9
+      -0.981877, -1.415295, 1.339004, 1.131348, -0.863194, 0.170952, 1.615403,
+      # pose 10
+      -0.967163, -1.377051, 1.337671, 1.399175, -1.241886, 0.284547, 1.927756,
+      # pose 11
+      -0.575271, -1.141574, 1.121935, 1.502172, -1.452029, 0.088544, 2.200019,
+    ]
@@ -0,0 +1,36 @@
+# Right arm 91 (Rizon4-063991) taught joints for hand-eye.
+# Fill with:
+#   bash ~/ros2/scripts/run_handeye.sh --arm 91 teach
+#   bash ~/ros2/scripts/run_handeye.sh --arm 91 merge
+#
+# Do not reuse 89's waypoint_positions.
+
+/**:
+  ros__parameters:
+    num_poses: 12
+    waypoint_positions: [
+      # pose 0
+      0.387144, -0.514770, -0.072695, 1.399918, 0.326501, 0.308501, -0.298212,
+      # pose 1
+      0.459205, -0.747691, 0.042380, 0.908430, 0.436933, -0.099413, -0.398336,
+      # pose 2
+      0.113162, -0.929267, -0.132969, 0.776759, 0.579499, -0.081271, -0.588715,
+      # pose 3
+      0.823056, -1.181803, -0.157161, 0.344860, 0.073450, -0.436449, -0.333484,
+      # pose 4
+      0.561660, 0.126479, -0.217063, 2.400061, 1.116363, 1.205544, -0.955972,
+      # pose 5
+      0.749011, 0.058548, -0.125615, 1.951037, 0.399065, 0.388437, -1.057397,
+      # pose 6
+      0.707611, -0.557010, -0.302672, 1.263980, 0.333093, 0.208229, -0.938372,
+      # pose 7
+      0.197846, -0.400530, 0.263064, 1.879643, 0.062586, 1.133715, -0.791285,
+      # pose 8
+      0.804027, -0.518162, 0.353947, 1.985246, -0.941195, 0.481238, -1.551113,
+      # pose 9
+      0.296161, -0.030159, 0.317117, 2.335652, 1.072158, 1.524893, -0.591372,
+      # pose 10
+      0.346492, -0.906418, -0.118014, 0.810448, 0.420788, -0.122104, -0.197859,
+      # pose 11
+      1.031802, -0.422436, -0.640939, 1.656629, 0.375163, 0.470365, -0.328041,
+    ]
@@ -0,0 +1,14 @@
+# Left arm 89 (Rizon4-063989) eye-in-hand. Do not use for 91.
+# Hand-eye result
+# 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]
@@ -0,0 +1,13 @@
+# Hand-eye result
+# mode: eye_in_hand
+# camera_source: d405
+# views: 11
+# utc: 2026-09-15T08:44:59.380472+00:00
+# board: 8x11 square_m=0.015
+# T_cam_to_gripper (eye_in_hand)
+translation_m: [-0.06856274, -0.00768546, 0.13773073]
+T_4x4:
+  - [0.00611182, -0.87098536, 0.49127096, -0.06856274]
+  - [0.99997742, 0.00395004, -0.00543744, -0.00768546]
+  - [0.00279539, 0.49129310, 0.87098983, 0.13773073]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
@@ -0,0 +1,25 @@
+# Hand-eye interactive waypoint tour (preview + Free Drive confirm).
+# Poses: sibling handeye_poses.yaml
+#
+# Run:
+#   bash ~/ros2/scripts/run_handeye.sh waypoints
+
+handeye_waypoints:
+  ros__parameters:
+    joint_prefix: "Rizon4-063989_"
+    joint_names: [joint1, joint2, joint3, joint4, joint5, joint6, joint7]
+    velocity_scale: 0.05
+    action_name: "follow_joint_trajectory"
+    dwell_sec: 2.5
+    capture_mode: "preview"       # auto | keypress | preview
+    start_index: 0
+    log_csv: ""
+    capture_topic: "handeye/capture"
+
+    camera_source: "d405"
+    camera_namespace: ""
+    image_topic: ""
+    preview_window: true
+    append_poses_file: "/tmp/handeye_calib/taught_poses.txt"
+
+    waypoint_times: [4.0, 4.0, 4.0, 4.0, 4.0, 4.5, 4.5, 4.5, 4.5, 4.5, 4.5, 4.0, 4.0]