Initial import of ORISYS arm workstation
This commit is contained in:
cheng
@@ -0,0 +1,283 @@
|
||||
# handeye/ —— 手眼标定
|
||||
|
||||
这个文件夹放**手眼标定的入口、程序、配置、采点和结果**。四个程序在 `src/`，采点和结果在旁边。启动不要走 `scripts/`：
|
||||
|
||||
```bash
|
||||
bash ~/桌面/orisys_arm/handeye/run_handeye.sh --arm 89 teach
|
||||
bash ~/桌面/orisys_arm/handeye/run_handeye.sh --arm 89 merge
|
||||
bash ~/桌面/orisys_arm/handeye/run_handeye.sh --arm 89 calibrate
|
||||
```
|
||||
|
||||
眼在手外把命令里的模式改成 `--mode eye_to_hand`。
|
||||
|
||||
---
|
||||
|
||||
## 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`。两条臂若共用一个工作目录，先标定的那份采点会被后
|
||||
标定的覆盖，而症状是"标定结果突然变得很差"，不容易联想到是目录冲突。
|
||||
|
||||
**命名约定**：
|
||||
- 眼在手上：`<臂号>_eye_in_hand.yaml`（`paths.handeye_file` 指向它）
|
||||
- 眼在手外：`<臂号>_<相机>_eye_to_hand.yaml`（S03 按这个名字自己找，
|
||||
不要改 `paths.handeye_file`）
|
||||
- 臂号 = Flexiv 序列号后两位（`Rizon4-063989` → `89`）
|
||||
- 示教点：`<臂号>_<模式>.yaml`（两种模式的关节角完全不同，分开存）
|
||||
|
||||
---
|
||||
|
||||
## 3. 标定板（最容易出错的地方）
|
||||
|
||||
两种装法用**两块不同的物理板**，尺寸按模式分开写，不能共用一组键：
|
||||
|
||||
| 模式 | 物理板 | 内角点（列 × 行） | 方格边长 | `site.yaml` 里的位置 |
|
||||
|------|--------|-------------------|----------|----------------------|
|
||||
| `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` 和节点默认值
|
||||
只是兜底（眼在手上那块板），**没配过的模式会被脚本当场拦下**，不会静默用兜底
|
||||
尺寸去标（那会让结果系统性偏，还不报错）。
|
||||
|
||||
怎么看内角点：把棋盘当成一张网格，**数内部的黑白交叉点**，
|
||||
是「方格数 − 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`，谁生效取决于覆盖顺序。后来又把两块板合成一组键，于是标眼在手外
|
||||
> 时静默用了 A5 的 15 mm。症状都是"标定流程跑得很顺、不报错，但结果明显不对"。
|
||||
|
||||
---
|
||||
|
||||
## 4. 完整流程
|
||||
|
||||
三条命令，按顺序执行。**先决条件**：标定板摆在相机视野里，机械臂能自由拖动。
|
||||
|
||||
### 第一步：采点（拖动示教）
|
||||
|
||||
```bash
|
||||
bash ~/ros2/scripts/run_handeye.sh teach --arm 89
|
||||
```
|
||||
|
||||
会弹出实时预览窗口（**只显示画面，不做棋盘检测**——真正的角点稳定判定在
|
||||
`calibrate` 那一步）。操作：
|
||||
|
||||
| 按键 | 作用 |
|
||||
|------|------|
|
||||
| `d` | 打开软拖拽（此时可以用手推机械臂） |
|
||||
| `s` | 关闭软拖拽（锁住） |
|
||||
| `Enter` / `空格` | **拍照记点**（记录当前关节角 + 图像） |
|
||||
| `q` / `Esc` | 结束采点 |
|
||||
|
||||
要点：
|
||||
- **点位要分散**：让标定板在画面里出现在不同位置、不同角度（远/近/左/右/倾斜）。
|
||||
全挤在一个角落会让求解退化，结果不可信。
|
||||
- **至少 5 个点**，推荐 **10～15 个**。数量在 `poses/*.yaml` 的 `num_poses` 里，
|
||||
也可以直接多按几次 `Enter`。
|
||||
- 采点数据写在 `handeye/tmp/<臂号>/`（如 `handeye/tmp/89/`），**确认无误后再 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`（算）。
|
||||
|
||||
### 一条命令跑完（手动模式 + 自动求解）
|
||||
|
||||
```bash
|
||||
# 需要先在另一个终端跑 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
|
||||
# 用腕部相机 + A5 板（默认，板放台面不动）
|
||||
bash ~/ros2/scripts/run_handeye.sh --arm 89 teach
|
||||
|
||||
# 用固定相机 + A4 板（板必须装到法兰上，相机不动）
|
||||
# 前置：另开终端 bash ~/ros2/scripts/run_camera_d435.sh --preview
|
||||
bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --mode eye_to_hand --points 12
|
||||
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` 会同时切：标定板、相机、示教点文件、结果文件。所以：
|
||||
|
||||
- 两种模式**互不覆盖**，可以分别标定、共存
|
||||
- 眼在手上结果：`handeye/results/<臂号>_eye_in_hand.yaml`（`T` = 相机 → 法兰）
|
||||
- 眼在手外结果：`handeye/results/<臂号>_<相机>_eye_to_hand.yaml`
|
||||
（`T` = 相机 → 这条臂的基座；双臂共用一台 D435 时必须按臂分开）
|
||||
- **不要**把 `paths.handeye_file` 改成眼在手外那份 —— 那条路径是腕部相机
|
||||
（S05 精拍定位）用的。手外那份由 `gauge_assembly.launch` 按名字自己找，
|
||||
并以 **基座 ← 相机** 发布 TF（不是法兰 ← 相机）
|
||||
|
||||
传了非法模式会立刻报错并退出（不会悄悄退回默认值 —— 那会导致"以为标的是
|
||||
eye-to-hand、其实算的是 eye-in-hand"这种最难查的错）。
|
||||
|
||||
### 设定采集点数
|
||||
|
||||
```bash
|
||||
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` 的内容长这样：
|
||||
|
||||
```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`）
|
||||
Reference in New Issue
Block a user