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 的正交阵(排除镜像解)
- 退化输入(全同位姿)不得悄悄给出"看起来能用"的结果
- 稳定性判据:窗口未满不判稳、丢帧后重新计数、抖动超阈不判稳
- 写盘再读回不漂移
This commit is contained in:
arm_dev
2026-09-23 11:46:03 +08:00
parent 52c108de07
commit a3a589b9e3
18 changed files with 735 additions and 85 deletions
+11 -7
View File
@@ -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)),
+11 -7
View File
@@ -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}"