ros_contorl/handeye

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）
    ├── 89/                           **按臂号分目录**：两条臂同时标定不会互相覆盖
    └── 91/                           不进 git

⚠️ tmp/ 下是按臂号分目录的（tmp/89、tmp/91），不是所有臂共用一个。 真实生效的值来自 site.yaml 的 arms.<臂>.handeye_workdir 与顶层 handeye_workdir。两条臂若共用一个工作目录，先标定的那份采点会被后 标定的覆盖，而症状是"标定结果突然变得很差"，不容易联想到是目录冲突。

命名约定：<臂号>_<模式>.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 ~/ros2/scripts/run_handeye.sh teach --arm 89

会弹出实时预览窗口，里面能看到标定板有没有被识别到。操作：

按键	作用
d	打开软拖拽（此时可以用手推机械臂）
s	关闭软拖拽（锁住）
Enter / 空格	拍照记点（记录当前关节角 + 图像）
q / Esc	结束采点

要点：

• 点位要分散：让标定板在画面里出现在不同位置、不同角度（远/近/左/右/倾斜）。 全挤在一个角落会让求解退化，结果不可信。
• 至少 5 个点，推荐 10～15 个。数量在 poses/*.yaml 的 num_poses 里， 也可以直接多按几次 Enter。
• 采点数据写在 handeye/tmp/<臂号>/（如 handeye/tmp/89/），确认无误后再 merge（下一步）。

第二步：合并点位

bash ~/ros2/scripts/run_handeye.sh merge --arm 89

把临时采到的关节角写入 handeye/poses/89_eye_in_hand.yaml， 并自动同步 num_poses。这一步不做数学计算，只是搬运+格式化。

第三步：自动标定

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 ~/ros2/scripts/run_handeye.sh show --arm 89

打印臂号、配置文件路径、板尺寸、已记录点数，以及当前的标定结果是否存在。

---

5. 两种模式

• 自动模式 = 上面的 calibrate。机械臂自己走点位，人不用管， 适合"点位已经采好、只想重算一遍"。
• 手动模式 = 上面的 teach。人拖着机械臂摆姿势、手动决定什么时候记这个点。 适合第一次标定，或者想补几个点位。

两者共用同一份 poses/*.yaml，顺序是 teach（采）→ merge（存）→ calibrate（算）。

一条命令跑完（手动模式 + 自动求解）

# 需要先在另一个终端跑 run_real.sh
bash ~/ros2/scripts/run_handeye.sh teach --ros --auto-solve --arm 89

采完点按 q 之后，脚本会自动 merge、重启相机、接着 calibrate， 不用再敲第二条命令。

⚠️ --auto-solve 只在 --ros（示教器）模式能全程跑通。

原因是一个硬件约束：标定必须通过 run_real.sh 的控制器驱动机械臂走点位， 而 --rdk 软拖拽要求 RDK 独占，两者无法同时存在。 所以 --rdk --auto-solve 会照常自动 merge，然后明确告诉你剩下那两条命令 —— 这是物理限制，不是脚本能绕过的。

想要真正一条命令，就用示教器 Free Drive：先在示教器上打开 Free Drive， 再用 --ros --auto-solve。

切换 eye-in-hand / eye-to-hand

# 用腕部相机（默认）
bash ~/ros2/scripts/run_handeye.sh --arm 89 teach

# 用固定相机
bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --mode eye_to_hand
bash ~/ros2/scripts/run_handeye.sh --arm 89 merge --mode eye_to_hand
bash ~/ros2/scripts/run_handeye.sh --arm 89 calibrate --mode eye_to_hand

--mode 会把读写的文件切到 <臂号>_<模式>.yaml，所以：

• 两种模式互不覆盖，可以分别标定、共存
• 跑完想切换使用，改 config/site.yaml 里 robots[89].paths.handeye_file 指向对应那份即可，代码不用动

传了非法模式会立刻报错并退出（不会悄悄退回默认值 —— 那会导致"以为标的是 eye-to-hand、其实算的是 eye-in-hand"这种最难查的错）。

设定采集点数

bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --points 12

采点过程中会显示 [3/12] ... 还差 9 个点，采够时提示可以收工。 不会强制关窗 —— 常常采到一半发现某个角度还没覆盖，强制退出反而要重开； 想多采就继续按 Enter，想结束按 q。

不加 --points 则不限数量（默认），建议 8～12 个点，至少 5 个。

---

6. 结果怎么被用起来

handeye/results/89_eye_in_hand.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）