diff --git a/.gitignore b/.gitignore index ae70316..cd471c4 100644 --- a/.gitignore +++ b/.gitignore @@ -42,3 +42,7 @@ __pycache__/ thirdparty/ vendor/ + +# 手眼标定的临时产物（采点图、taught_poses.txt、captures.json） +handeye/tmp/* +!handeye/tmp/.gitkeep diff --git a/config/site.example.yaml b/config/site.example.yaml index 139d34b..4ff2094 100644 --- a/config/site.example.yaml +++ b/config/site.example.yaml @@ -48,8 +48,8 @@ robot: stroke_mm: 20.0 named_poses_file: "/home/USER/ros2/src/arm_control/config/named_poses_89.yaml" paths: - handeye_file: "/home/USER/ros2/src/arm_application/config/handeye_result_89.yaml" - handeye_waypoints: "/home/USER/ros2/src/arm_application/config/handeye_poses_89.yaml" + 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: "/tmp/handeye_calib_89" # 左臂跟踪高度（只在 --arm 89 时生效，改 91 的 servo 不影响这里） servo: @@ -85,8 +85,8 @@ robot: auto_init: false named_poses_file: "/home/USER/ros2/src/arm_control/config/named_poses_91.yaml" paths: - handeye_file: "/home/USER/ros2/src/arm_application/config/handeye_result_91.yaml" - handeye_waypoints: "/home/USER/ros2/src/arm_application/config/handeye_poses_91.yaml" + handeye_file: "/home/USER/ros2/handeye/results/91_eye_in_hand.yaml" + handeye_waypoints: "/home/USER/ros2/handeye/poses/91_eye_in_hand.yaml" handeye_workdir: "/tmp/handeye_calib_91" # >>> 右臂跟踪高度：只改这里，89 仍用上面那段 <<< # 法兰 Z；RGI 比平行爪高，通常要比 89 略抬一点。改完重启： @@ -119,9 +119,9 @@ paths: # 入孔 YOLO（backend=yolo 时把孔当框）。two_stage 用上面的工件权重。 hole_weights: "/home/USER/ros2/models/hole_insert/best.pt" # 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。 - handeye_file: "/home/USER/ros2/src/arm_application/config/handeye_result_89.yaml" - handeye_waypoints: "/home/USER/ros2/src/arm_application/config/handeye_poses_89.yaml" - handeye_calibrate: "/home/USER/ros2/src/arm_application/config/handeye_calibrate.yaml" + 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_calibrate: "/home/USER/ros2/handeye/calibrate.yaml" handeye_workdir: "/tmp/handeye_calib_89" # 空=默认 DDS(SHM)；勿强制 UDP-only，否则相机预览收不到 /workpiece/detection fastdds_profile: "" diff --git a/handeye/README.md b/handeye/README.md new file mode 100644 index 0000000..33bc4ba --- /dev/null +++ b/handeye/README.md @@ -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`） diff --git a/src/arm_application/config/handeye_calibrate.yaml b/handeye/calibrate.yaml similarity index 75% rename from src/arm_application/config/handeye_calibrate.yaml rename to handeye/calibrate.yaml index 6934fbd..fa02c08 100644 --- a/src/arm_application/config/handeye_calibrate.yaml +++ b/handeye/calibrate.yaml @@ -34,17 +34,24 @@ handeye_calibrate: camera_cy: 0.0 camera_dist: [0.0, 0.0, 0.0, 0.0, 0.0] - # Physical board verified on teach images: inner corners 11x16 (not 10x15) - board_cols: 11 - board_rows: 16 - square_size_m: 0.011 + # 真实使用的标定板：内角点 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 - output_dir: "/tmp/handeye_calib" + # 临时产物（采点图、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 diff --git a/src/arm_application/config/handeye_poses_89.yaml b/handeye/poses/89_eye_in_hand.yaml similarity index 100% rename from src/arm_application/config/handeye_poses_89.yaml rename to handeye/poses/89_eye_in_hand.yaml diff --git a/src/arm_application/config/handeye_poses_91.yaml b/handeye/poses/91_eye_in_hand.yaml similarity index 100% rename from src/arm_application/config/handeye_poses_91.yaml rename to handeye/poses/91_eye_in_hand.yaml diff --git a/src/arm_application/config/handeye_result_89.yaml b/handeye/results/89_eye_in_hand.yaml similarity index 100% rename from src/arm_application/config/handeye_result_89.yaml rename to handeye/results/89_eye_in_hand.yaml diff --git a/src/arm_application/config/handeye_result_91.yaml b/handeye/results/91_eye_in_hand.yaml similarity index 100% rename from src/arm_application/config/handeye_result_91.yaml rename to handeye/results/91_eye_in_hand.yaml diff --git a/handeye/tmp/.gitkeep b/handeye/tmp/.gitkeep new file mode 100644 index 0000000..e69de29 diff --git a/src/arm_application/config/handeye_waypoints.yaml b/handeye/waypoints.yaml similarity index 100% rename from src/arm_application/config/handeye_waypoints.yaml rename to handeye/waypoints.yaml diff --git a/scripts/lib/export_site_env.py b/scripts/lib/export_site_env.py index 05e316c..2e91288 100644 --- a/scripts/lib/export_site_env.py +++ b/scripts/lib/export_site_env.py @@ -220,27 +220,31 @@ def flatten(data: dict) -> dict: "SITE_YOLO_WEIGHTS_RESOLVED": weights_resolved, "SITE_HANDEYE_WAYPOINTS": str( paths.get("handeye_waypoints") - or str(_root() / "src" / "arm_application" / "config" / "handeye_poses.yaml") + or str(_root() / "handeye" / "poses" / "89_eye_in_hand.yaml") ), "SITE_HANDEYE_CALIBRATE": str( paths.get("handeye_calibrate") - or str(_root() / "src" / "arm_application" / "config" / "handeye_calibrate.yaml") + or str(_root() / "handeye" / "calibrate.yaml") ), "SITE_HANDEYE_TOUR": str( paths.get("handeye_tour") - or str(_root() / "src" / "arm_application" / "config" / "handeye_waypoints.yaml") + or str(_root() / "handeye" / "waypoints.yaml") ), "SITE_HANDEYE_WORKDIR": str( paths.get("handeye_workdir") or "/tmp/handeye_calib" ), "SITE_HANDEYE_FILE": str( paths.get("handeye_file") - or str(_root() / "src" / "arm_application" / "config" / "handeye_result.yaml") + or str(_root() / "handeye" / "results" / "89_eye_in_hand.yaml") ), "SITE_HANDEYE_MODE": str(he.get("mode") or "eye_in_hand"), - "SITE_HANDEYE_BOARD_COLS": str(int(he.get("board_cols") or 11)), - "SITE_HANDEYE_BOARD_ROWS": str(int(he.get("board_rows") or 16)), - "SITE_HANDEYE_SQUARE_M": str(float(he.get("square_size_m") or 0.011)), + # 板尺寸默认值必须与 site.yaml / 结果文件头里的实际板子一致。 + # 这里原先是 11/16/0.011（一张早期用过、现已不存在的板），与 site.yaml 的 + # 8/11/0.015 冲突：一旦 site.yaml 缺这几个键就会静默用错尺寸， + # 表现为"标定能跑完但结果明显不对"。 + "SITE_HANDEYE_BOARD_COLS": str(int(he.get("board_cols") or 8)), + "SITE_HANDEYE_BOARD_ROWS": str(int(he.get("board_rows") or 11)), + "SITE_HANDEYE_SQUARE_M": str(float(he.get("square_size_m") or 0.015)), "SITE_HANDEYE_VEL_SCALE": str(float(he.get("velocity_scale") or 0.05)), "SITE_HANDEYE_MOVE_T": str(float(he.get("move_time_sec") or 8.0)), "SITE_HANDEYE_SETTLE_SEC": str(float(he.get("settle_sec") or 1.5)), diff --git a/scripts/run_handeye.sh b/scripts/run_handeye.sh index 4acfc45..a743e74 100755 --- a/scripts/run_handeye.sh +++ b/scripts/run_handeye.sh @@ -46,10 +46,14 @@ if [[ ! -d "${APP_CFG}" ]]; then APP_CFG="$(ros2 pkg prefix arm_application 2>/dev/null)/share/arm_application/config" || true fi -WP="${SITE_HANDEYE_WAYPOINTS:-${APP_CFG}/handeye_poses.yaml}" -CAL="${SITE_HANDEYE_CALIBRATE:-${APP_CFG}/handeye_calibrate.yaml}" -TOUR="${SITE_HANDEYE_TOUR:-${APP_CFG}/handeye_waypoints.yaml}" -RES="${SITE_HANDEYE_FILE:-${APP_CFG}/handeye_result.yaml}" +# 手眼标定相关文件现在集中在仓库根的 handeye/ 下（见 handeye/README.md）。 +# 这些 SHE 值正常情况下由 site.yaml → export_site_env.py 提供； +# 下面只是 shell 侧的兜底，路径必须与 export_site_env.py 的默认值一致。 +HE_DIR="${SITE_HANDEYE_DIR:-${ROOT}/handeye}" +WP="${SITE_HANDEYE_WAYPOINTS:-${HE_DIR}/poses/89_eye_in_hand.yaml}" +CAL="${SITE_HANDEYE_CALIBRATE:-${HE_DIR}/calibrate.yaml}" +TOUR="${SITE_HANDEYE_TOUR:-${HE_DIR}/waypoints.yaml}" +RES="${SITE_HANDEYE_FILE:-${HE_DIR}/results/89_eye_in_hand.yaml}" WORKDIR="${SITE_HANDEYE_WORKDIR:-/tmp/handeye_calib}" TAUGHT="${WORKDIR}/taught_poses.txt" PREFIX="${SITE_JOINT_PREFIX:-Rizon4-063989_}" @@ -123,9 +127,9 @@ cmd_calibrate() { echo "=== kill visual_servo (avoid traj fight) ===" _kill_servo mkdir -p "${WORKDIR}" - local cols="${SITE_HANDEYE_BOARD_COLS:-11}" - local rows="${SITE_HANDEYE_BOARD_ROWS:-16}" - local sq="${SITE_HANDEYE_SQUARE_M:-0.011}" + local cols="${SITE_HANDEYE_BOARD_COLS:-8}" + local rows="${SITE_HANDEYE_BOARD_ROWS:-11}" + local sq="${SITE_HANDEYE_SQUARE_M:-0.015}" local vel="${SITE_HANDEYE_VEL_SCALE:-0.05}" local mt="${SITE_HANDEYE_MOVE_T:-8.0}" local st="${SITE_HANDEYE_SETTLE_SEC:-1.5}" diff --git a/src/arm_application/arm_application/handeye_calibrate.py b/src/arm_application/arm_application/handeye_calibrate.py index deab99e..081965c 100644 --- a/src/arm_application/arm_application/handeye_calibrate.py +++ b/src/arm_application/arm_application/handeye_calibrate.py @@ -108,14 +108,27 @@ class HandeyeCalibrate(Node): self.declare_parameter('camera_dist', [0.0, 0.0, 0.0, 0.0, 0.0]) # Chessboard (customizable) - self.declare_parameter('board_cols', 9) # inner corners - self.declare_parameter('board_rows', 6) - self.declare_parameter('square_size_m', 0.025) + # 默认值必须与 site.yaml 的 handeye.board_* 及结果文件头里的实际板子一致。 + # 真实使用的板：内角点 8x11，方格 15mm（见 handeye/results/*.yaml 的头部注释）。 + # 这里原先写的是 9x6 / 0.025（另一张早期的板），一旦 site.yaml 漏配就会被 + # 静默启用 —— 症状是"标定能顺利跑完，但结果明显不对"，极难排查。 + self.declare_parameter('board_cols', 8) # inner corners + self.declare_parameter('board_rows', 11) + self.declare_parameter('square_size_m', 0.015) self.declare_parameter('stable_window', 8) self.declare_parameter('stable_pixel_std', 0.45) self.declare_parameter('stable_timeout_sec', 25.0) self.declare_parameter('settle_sec', 0.4) + # 到位后的"真的停住了"判据 —— 直接看关节速度，比固定 sleep 可靠。 + # 这几个参数原先 site.yaml 和 run_handeye.sh 一直在传，但本节点从未声明， + # ROS 2 会静默忽略未声明的覆盖参数，于是"等静止再拍照"这件事其实没实现， + # 实际只靠一次固定 settle_sec。现在补上。 + self.declare_parameter('settle_qd_max', 0.015) # |qd|_max [rad/s] 低于此值算静 + self.declare_parameter('settle_stable_sec', 0.6) # 需要连续静多久 + self.declare_parameter('settle_timeout_sec', 8.0) # 等静止的总超时 + self.declare_parameter('pin_holds', 2) # 静止后再保持几拍 + self.declare_parameter('pin_hold_sec', 0.45) # 每拍多长 self.declare_parameter('skip_unstable_poses', True) self.declare_parameter('output_dir', '/tmp/handeye_calib') @@ -155,6 +168,11 @@ class HandeyeCalibrate(Node): ) self.stable_timeout = float(self.get_parameter('stable_timeout_sec').value) self.settle_sec = float(self.get_parameter('settle_sec').value) + self.settle_qd_max = float(self.get_parameter('settle_qd_max').value) + self.settle_stable_sec = float(self.get_parameter('settle_stable_sec').value) + self.settle_timeout = float(self.get_parameter('settle_timeout_sec').value) + self.pin_holds = max(0, int(self.get_parameter('pin_holds').value)) + self.pin_hold_sec = max(0.0, float(self.get_parameter('pin_hold_sec').value)) self.skip_unstable = bool(self.get_parameter('skip_unstable_poses').value) out = Path(str(self.get_parameter('output_dir').value)).expanduser() @@ -259,7 +277,7 @@ class HandeyeCalibrate(Node): try: for i in range(self.start_index, len(poses)): q = poses[i] - self.get_logger().info(f'[{i + 1}/{len(poses)}] moving...') + self._log_progress(i, len(poses), 'moving...') code = self._move_one(q, self.move_time_sec) if code != 0: return code @@ -277,6 +295,10 @@ class HandeyeCalibrate(Node): while rclpy.ok() and time.monotonic() < end: rclpy.spin_once(self, timeout_sec=0.05) + # 固定 settle_sec 之后再看关节速度：真停了才算数，然后 pin 一拍。 + self._wait_settled() + self._pin_hold() + ok, std, corners, pattern, bgr = self._wait_stable() if not ok or corners is None or bgr is None: msg = f'[{i + 1}] chessboard not stable (std={std:.3f})' @@ -460,6 +482,68 @@ class HandeyeCalibrate(Node): ) return True + def _log_progress(self, i: int, total: int, suffix: str = '') -> None: + """带进度条的日志。 + + 自动标定是无人值守的长流程（十几个点位 × 移动+稳定+拍照）， + 单看 `[3/12]` 很难有"还要多久"的直觉，加一条可视化进度条。 + """ + frac = (i + 1) / max(1, total) + width = 24 + filled = int(round(frac * width)) + bar = '█' * filled + '░' * (width - filled) + self.get_logger().info( + f'[{i + 1}/{total}] [{bar}] {frac * 100:5.1f}% {suffix}' + ) + + def _wait_settled(self) -> bool: + """等机械臂真的停下来，再进入拍照。 + + 判据是关节速度 |qd|_max 连续 ``settle_stable_sec`` 低于 ``settle_qd_max``。 + 为什么不用固定 sleep：`settle_sec` 是拍脑袋的时长，臂负载/姿态不同， + 刹车距离差别很大 —— 等太短会在臂还在动时拍照（棋盘虚 → 角点抖动）， + 等太长则整轮标定被无谓拉长。直接看速度更准。 + + 超时不算致命：记一条警告后继续，让后面的棋盘稳定性检查去把关。 + """ + if self.settle_qd_max <= 0.0: + return True + deadline = time.monotonic() + self.settle_timeout + stable_since: Optional[float] = None + while rclpy.ok() and time.monotonic() < deadline: + rclpy.spin_once(self, timeout_sec=0.05) + js = self._latest_js + if js is None or not list(js.velocity): + continue + try: + vmax = float(np.max(np.abs(np.asarray(js.velocity, dtype=float)))) + except Exception: # noqa: BLE001 + continue + now = time.monotonic() + if vmax <= self.settle_qd_max: + if stable_since is None: + stable_since = now + elif now - stable_since >= self.settle_stable_sec: + return True + else: + stable_since = None # 又动了，重新计时 + self.get_logger().warn( + f'wait_settled 超时 ({self.settle_timeout:.1f}s)，臂可能仍在微动；' + '交给棋盘稳定性检查兜底' + ) + return False + + def _pin_hold(self) -> None: + """静止后再保持几拍，抑制到位后的微小回弹/漂移。 + + 语义取自 site.yaml 的注释"零速 pin 后再拍照"：不是重新规划轨迹， + 而是纯粹等一段时间让机械臂自己稳下来（控制器在到位后仍会做少量补偿）。 + """ + total = self.pin_holds * self.pin_hold_sec + end = time.monotonic() + total + while rclpy.ok() and time.monotonic() < end: + rclpy.spin_once(self, timeout_sec=0.05) + def _wait_stable( self, ) -> Tuple[bool, float, Optional[np.ndarray], Optional[np.ndarray]]: diff --git a/src/arm_application/arm_application/servo_node.py b/src/arm_application/arm_application/servo_node.py index 13166ad..fcc21ba 100644 --- a/src/arm_application/arm_application/servo_node.py +++ b/src/arm_application/arm_application/servo_node.py @@ -69,7 +69,9 @@ class ServoNode(Node): self.declare_parameter("camera_frame", "d405_color_optical_frame") self.declare_parameter("traj_topic", "rizon_arm_controller/joint_trajectory") self.declare_parameter("point_topic", "visual_servo/filtered_point") - self.declare_parameter("handeye_file", "/tmp/handeye_calib/handeye_result.yaml") + # 默认值只作兜底：真机运行一定由 launch 传入 site.yaml 的 paths.handeye_file。 + # 指向 /tmp 是历史遗留（标定临时产物），会让人误以为"标定结果在那里"。 + self.declare_parameter("handeye_file", "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml") self.declare_parameter("use_static_handeye_tf", False) self.declare_parameter("handeye_child_frame", "d405_color_optical_frame") diff --git a/src/arm_application/config/handeye_poses.yaml b/src/arm_application/config/handeye_poses.yaml deleted file mode 100644 index f32b974..0000000 --- a/src/arm_application/config/handeye_poses.yaml +++ /dev/null @@ -1,38 +0,0 @@ -# Legacy alias of 89 taught joints. Prefer handeye_poses_89.yaml / _91.yaml. -# -# 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, - ] diff --git a/src/arm_application/config/handeye_result.yaml b/src/arm_application/config/handeye_result.yaml deleted file mode 100644 index ed8b2c4..0000000 --- a/src/arm_application/config/handeye_result.yaml +++ /dev/null @@ -1,14 +0,0 @@ -# Legacy alias of 89 (Rizon4-063989). Calibrate into handeye_result_89.yaml / _91.yaml. -# 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] diff --git a/src/arm_application/launch/visual_servo.launch.py b/src/arm_application/launch/visual_servo.launch.py index 193b201..0d4fb91 100644 --- a/src/arm_application/launch/visual_servo.launch.py +++ b/src/arm_application/launch/visual_servo.launch.py @@ -102,7 +102,7 @@ def _launch_setup(context, *args, **kwargs): site_he = str(servo_over.get("handeye_file") or (site.get("paths") or {}).get("handeye_file") or "") if not handeye or handeye in ( "/tmp/handeye_calib/handeye_result.yaml", - "/home/cc/ros2/src/arm_application/config/handeye_result.yaml", + "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml", ): handeye = site_he or handeye if not Path(handeye).is_file(): @@ -218,7 +218,7 @@ def generate_launch_description() -> LaunchDescription: [ DeclareLaunchArgument( "handeye_file", - default_value="/home/cc/ros2/src/arm_application/config/handeye_result.yaml", + default_value="/home/cc/ros2/handeye/results/89_eye_in_hand.yaml", ), DeclareLaunchArgument( "ee_frame", diff --git a/src/arm_application/test/test_handeye_lib.py b/src/arm_application/test/test_handeye_lib.py new file mode 100644 index 0000000..d94ecd7 --- /dev/null +++ b/src/arm_application/test/test_handeye_lib.py @@ -0,0 +1,383 @@ +"""handeye_lib 的纯函数测试 —— 不需要机器人、相机或标定板。 + +这里最要紧的一个测试是 `test_roundtrip_recovers_known_transform`： +它用一个人为设定的 `X_true` 反过来生成每个视图的 (gripper→base, target→cam)， +再让求解器去还原。这是唯一能在没有硬件的情况下真正验证"标定算得对不对"的办法 —— +如果求解链路有符号/转置/坐标系方向的错误，它一定会失败。 +""" + +from __future__ import annotations + +import math + +import numpy as np +import pytest + +from arm_application.handeye_lib import ( + BoardConfig, + CameraIntrinsics, + ChessboardStabilizer, + Rt_to_T, + board_object_points, + calibrate_hand_eye, + format_transform_yaml, + invert_Rt, + is_pose_filled, + pose_to_Rt, + quat_xyzw_to_R, +) + + +def _rot(axis: str, deg: float) -> np.ndarray: + a = math.radians(deg) + c, s = math.cos(a), math.sin(a) + if axis == "x": + return np.array([[1, 0, 0], [0, c, -s], [0, s, c]], float) + if axis == "y": + return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]], float) + return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]], float) + + +def _rand_transform(rng: np.random.Generator) -> np.ndarray: + """随机但物理上合法的刚体变换（旋转正交且 det=+1）。""" + R = _rot("x", rng.uniform(-180, 180)) @ _rot("y", rng.uniform(-180, 180)) @ _rot( + "z", rng.uniform(-180, 180) + ) + T = np.eye(4) + T[:3, :3] = R + T[:3, 3] = rng.uniform(-0.6, 0.6, 3) + return T + + +# -------------------------------------------------------------------------- +# 旋转/位姿基础 +# -------------------------------------------------------------------------- + + +def test_quat_identity_is_identity_matrix(): + R = quat_xyzw_to_R([0.0, 0.0, 0.0, 1.0]) + assert np.allclose(R, np.eye(3), atol=1e-12) + + +def test_quat_matches_rotation_matrix_for_known_angles(): + for axis, deg in (("x", 30.0), ("y", -47.0), ("z", 121.0)): + q = _axis_angle_to_quat(axis, deg) + assert np.allclose(quat_xyzw_to_R(q), _rot(axis, deg), atol=1e-9), axis + + +def test_quat_is_normalized_before_use(): + """非单位四元数不该影响结果 —— 有些驱动会给未归一化的值。""" + q = np.array(_axis_angle_to_quat("z", 90.0)) * 3.7 + assert np.allclose(quat_xyzw_to_R(list(q)), _rot("z", 90.0), atol=1e-9) + + +def _axis_angle_to_quat(axis: str, deg: float): + a = math.radians(deg) / 2.0 + s, c = math.sin(a), math.cos(a) + if axis == "x": + return [s, 0.0, 0.0, c] + if axis == "y": + return [0.0, s, 0.0, c] + return [0.0, 0.0, s, c] + + +def test_pose_to_Rt_shapes(): + R, t = pose_to_Rt([1.0, 2.0, 3.0], [0.0, 0.0, 0.0, 1.0]) + assert R.shape == (3, 3) + assert t.shape == (3, 1) + assert np.allclose(t.reshape(3), [1.0, 2.0, 3.0]) + + +def test_Rt_to_T_round_trip(): + rng = np.random.default_rng(0) + R = _rot("z", 33.0) + t = np.array([[0.1], [-0.2], [0.3]]) + T = Rt_to_T(R, t) + assert T.shape == (4, 4) + assert np.allclose(T[3], [0, 0, 0, 1]) + assert np.allclose(T[:3, :3], R) + assert np.allclose(T[:3, 3], t.reshape(3)) + + +def test_invert_Rt_is_true_inverse(): + rng = np.random.default_rng(1) + R = _rot("x", 12.0) @ _rot("y", 88.0) + t = rng.uniform(-1, 1, (3, 1)) + Ri, ti = invert_Rt(R, t) + T = Rt_to_T(R, t) + Ti = Rt_to_T(Ri, ti) + assert np.allclose(T @ Ti, np.eye(4), atol=1e-12) + assert np.allclose(Ti @ T, np.eye(4), atol=1e-12) + + +# -------------------------------------------------------------------------- +# 标定板几何 +# -------------------------------------------------------------------------- + + +def test_board_object_points_count_and_layout(): + cfg = BoardConfig(cols=8, rows=11, square_size=0.015) + pts = board_object_points(cfg) + assert pts.shape == (88, 3) + # z 全为 0（板平面），x/y 是网格 + assert np.allclose(pts[:, 2], 0.0) + assert np.isclose(pts[:, 0].max(), 7 * 0.015) + assert np.isclose(pts[:, 1].max(), 10 * 0.015) + # 第一行是 0..7 个 x + assert np.allclose(pts[:8, 0], np.arange(8) * 0.015) + + +def test_board_square_size_actually_scales_points(): + a = board_object_points(BoardConfig(cols=4, rows=3, square_size=0.01)) + b = board_object_points(BoardConfig(cols=4, rows=3, square_size=0.03)) + assert np.allclose(b, a * 3.0) + + +def test_camera_intrinsics_holds_k_and_dist(): + K = np.array([[600.0, 0.0, 320.0], [0.0, 601.0, 240.0], [0.0, 0.0, 1.0]]) + dist = np.zeros(5) + ci = CameraIntrinsics(K=K, dist=dist) + assert ci.K.shape == (3, 3) + assert ci.K[0, 0] == 600.0 + assert ci.K[1, 2] == 240.0 + assert ci.dist.size == 5 + + +# -------------------------------------------------------------------------- +# 稳定性判据 +# -------------------------------------------------------------------------- + + +def _fake_corners(n: int = 88, jitter: float = 0.0, seed: int = 0): + rng = np.random.default_rng(seed) + base = np.stack( + [np.linspace(0, 100, n), np.linspace(0, 70, n), np.zeros(n)], axis=1 + ).astype(np.float32) + if jitter: + base = base + rng.normal(0, jitter, base.shape).astype(np.float32) + return base + + +def test_stabilizer_not_stable_before_window_is_filled(): + st = ChessboardStabilizer(window=8, max_pixel_std=0.45) + for _ in range(3): + ok, _std = st.push(_fake_corners(jitter=0.01)) + assert not ok, "窗口没填满就不该判稳" + + +def test_stabilizer_declares_stable_for_identical_corners(): + st = ChessboardStabilizer(window=5, max_pixel_std=0.45) + fixed = _fake_corners() + ok = False + for _ in range(6): + ok, std = st.push(fixed) + assert ok + assert std == pytest.approx(0.0, abs=1e-9) + + +def test_stabilizer_rejects_noisy_corners(): + st = ChessboardStabilizer(window=5, max_pixel_std=0.45) + ok = False + for i in range(6): + ok, std = st.push(_fake_corners(jitter=5.0, seed=i)) + assert not ok, "角点抖动 5px 远超 0.45px 阈值，不该判稳" + assert std > 0.45 + + +def test_stabilizer_missing_board_resets_streak(): + st = ChessboardStabilizer(window=3, max_pixel_std=0.45) + fixed = _fake_corners() + st.push(fixed) + st.push(fixed) + ok, _ = st.push(None) # 看不到板 + assert not ok + # 重新开始计数，而不是接着之前的 2 次 + ok, _ = st.push(fixed) + assert not ok + ok, _ = st.push(fixed) + assert not ok + ok, _ = st.push(fixed) + assert ok, "连续 3 帧相同角点后应判稳" + + +def test_stabilizer_reset_clears_history(): + st = ChessboardStabilizer(window=3, max_pixel_std=0.45) + fixed = _fake_corners() + for _ in range(3): + st.push(fixed) + st.reset() + ok, _ = st.push(fixed) + assert not ok + + +# -------------------------------------------------------------------------- +# 核心：合成数据往返（真正验证标定算法） +# -------------------------------------------------------------------------- + + +def _synthesize_views( + X_true: np.ndarray, T_target2base: np.ndarray, n: int, seed: int = 7 +): + """由已知 X_true 生成 n 组视图观测。 + + eye-in-hand 的坐标链：target → camera → gripper → base + T_t2b = T_g2b · X · T_t2c (X = T_cam2gripper) + 所以给定任意的 T_g2b，可反推出该视图下应该观测到的 T_t2c： + T_t2c = inv(X) · inv(T_g2b) · T_t2b + """ + rng = np.random.default_rng(seed) + Rg, tg, Rc, tc = [], [], [], [] + Xinv = np.linalg.inv(X_true) + for _ in range(n): + T_g2b = _rand_transform(rng) + T_t2c = Xinv @ np.linalg.inv(T_g2b) @ T_target2base + Rg.append(T_g2b[:3, :3]) + tg.append(T_g2b[:3, 3].reshape(3, 1)) + Rc.append(T_t2c[:3, :3]) + tc.append(T_t2c[:3, 3].reshape(3, 1)) + return Rg, tg, Rc, tc + + +def test_roundtrip_recovers_known_transform_eye_in_hand(): + rng = np.random.default_rng(42) + X_true = _rand_transform(rng) + T_target2base = _rand_transform(rng) + Rg, tg, Rc, tc = _synthesize_views(X_true, T_target2base, 12) + + R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=False) + X_est = Rt_to_T(R, t) + + assert np.allclose(X_est, X_true, atol=1e-6), ( + "求解结果与已知真值不符 —— 坐标链方向或求逆有问题" + ) + + +def test_roundtrip_translation_is_exact_not_just_rotation(): + """单独盯平移：旋转对但平移错，是最容易漏掉的一类 bug。""" + rng = np.random.default_rng(11) + X_true = _rand_transform(rng) + T_target2base = _rand_transform(rng) + Rg, tg, Rc, tc = _synthesize_views(X_true, T_target2base, 15) + + _R, t = calibrate_hand_eye(Rg, tg, Rc, tc) + assert np.allclose(np.asarray(t).reshape(3), X_true[:3, 3], atol=1e-6) + + +def test_roundtrip_is_rotation_matrix_with_det_one(): + rng = np.random.default_rng(5) + X_true = _rand_transform(rng) + Rg, tg, Rc, tc = _synthesize_views(X_true, _rand_transform(rng), 10) + R, _t = calibrate_hand_eye(Rg, tg, Rc, tc) + R = np.asarray(R, float) + assert np.isclose(np.linalg.det(R), 1.0, atol=1e-8), "det 必须是 +1，不是镜像" + assert np.allclose(R @ R.T, np.eye(3), atol=1e-8), "必须是正交阵" + + +def test_roundtrip_eye_to_hand_recovers_known_transform(): + """eye_to_hand：相机固定在基座，X = cam → base。 + + 此时坐标链变成 target → camera 与 gripper → base 相互独立， + 求解器内部会把 gripper2base 求逆，这条分支容易写错，单独测。 + """ + rng = np.random.default_rng(3) + X_true = _rand_transform(rng) # T_cam2base + Rg, tg, Rc, tc = [], [], [], [] + for _ in range(12): + T_g2b = _rand_transform(rng) # 法兰在基座 + T_t2c = _rand_transform(rng) # 板在相机（固定相机，板随臂动） + # 板在基座 = T_cam2base · T_t2c（板相对基座是固定的物理事实） + _ = X_true @ T_t2c + Rg.append(T_g2b[:3, :3]) + tg.append(T_g2b[:3, 3].reshape(3, 1)) + Rc.append(T_t2c[:3, :3]) + tc.append(T_t2c[:3, 3].reshape(3, 1)) + + # eye_to_hand 下 OpenCV 仍是解 AX=XB，X 语义变为 cam→base； + # 这里只验证"能解出正交且 det=1 的结果"，不比对真值 + # （板随臂动时需要 T_t2b 与 T_g2b 的联动，构造更复杂， + # 真正的语义正确性由 eye_in_hand 那条测试把关坐标链） + R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=True) + R = np.asarray(R, float) + assert R.shape == (3, 3) + assert np.isclose(np.linalg.det(R), 1.0, atol=1e-6) + + +def test_calibration_fails_loudly_on_degenerate_input(): + """全同位姿（没有旋转多样性）时不该悄悄给个"看起来能用"的结果。""" + rng = np.random.default_rng(9) + X_true = _rand_transform(rng) + Rg, tg, Rc, tc = _synthesize_views(X_true, _rand_transform(rng), 6) + same_R, same_t = Rg[0], tg[0] + Rg = [same_R] * 6 + tg = [same_t] * 6 + Rc = [Rc[0]] * 6 + tc = [tc[0]] * 6 + try: + R, _t = calibrate_hand_eye(Rg, tg, Rc, tc) + except Exception: + return # 报错是可以接受的 + # 若没报错，结果必须明显不可信（不该恰好等于真值） + assert not np.allclose(Rt_to_T(np.asarray(R, float), np.zeros(3)), X_true, atol=1e-6) + + +# -------------------------------------------------------------------------- +# 输出格式 +# -------------------------------------------------------------------------- + + +def test_format_transform_yaml_shape_and_parseable(): + import yaml + + R = _rot("z", 90.0) + t = np.array([[0.01], [0.02], [0.03]]) + txt = format_transform_yaml(R, t, "unit test") + + assert "# unit test" in txt + data = yaml.safe_load(txt) + T = np.array(data["T_4x4"], float) + assert T.shape == (4, 4) + assert np.allclose(T, Rt_to_T(R, t), atol=1e-7) + assert np.allclose(data["translation_m"], [0.01, 0.02, 0.03], atol=1e-8) + + +def test_format_transform_yaml_has_bottom_row(): + import yaml + + txt = format_transform_yaml(np.eye(3), np.zeros((3, 1)), "x") + data = yaml.safe_load(txt) + assert np.allclose(data["T_4x4"][3], [0, 0, 0, 1]) + + +def test_roundtrip_write_then_read_back_is_stable(): + """写出去再读回来，变换不该漂移（避免精度不足导致反复重标定）。""" + import yaml + + rng = np.random.default_rng(21) + R = _rot("x", 37.0) @ _rot("z", -14.0) + t = rng.uniform(-0.2, 0.2, (3, 1)) + txt = format_transform_yaml(R, t, "roundtrip") + T = np.array(yaml.safe_load(txt)["T_4x4"], float) + assert np.allclose(T[:3, :3], R, atol=1e-7) + assert np.allclose(T[:3, 3], t.reshape(3), atol=1e-7) + + +# -------------------------------------------------------------------------- +# 位姿占位检测（决定哪些点参与标定） +# -------------------------------------------------------------------------- + + +def test_is_pose_filled_rejects_placeholder_zeros(): + assert not is_pose_filled([0.0] * 7) + + +def test_is_pose_filled_rejects_nan(): + assert not is_pose_filled([float("nan")] * 7) + + +def test_is_pose_filled_rejects_wrong_length(): + assert not is_pose_filled([0.1, 0.2, 0.3]) + assert not is_pose_filled([0.1] * 8) + + +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])