arm_dev
a3a589b9e3
新建 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 的正交阵（排除镜像解） - 退化输入（全同位姿）不得悄悄给出"看起来能用"的结果 - 稳定性判据：窗口未满不判稳、丢帧后重新计数、抖动超阈不判稳 - 写盘再读回不漂移
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 ~/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 ~/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
机械臂会自动走一遍刚才记下的所有点位，每个点：
- 移动到目标位姿
- 校验确实到位了（防止控制器假报成功）
- 等机械臂真的停下来（看关节速度，不是干等固定秒数）
- 等画面里的标定板连续多帧稳定（角点抖动小于阈值）
- 拍照、记录法兰位姿 + 板位姿
- 屏幕上打一条进度条
全部走完后一次性解算出结果，写入
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（算）。
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）