Publish standalone A7 hand-eye calibration workflow and tutorial
This commit is contained in:
@@ -0,0 +1,9 @@
|
||||
.venv/
|
||||
__pycache__/
|
||||
*.pyc
|
||||
config/local.env
|
||||
current_session.json
|
||||
.session.lock
|
||||
data/*
|
||||
!data/.gitkeep
|
||||
logs/
|
||||
@@ -0,0 +1,53 @@
|
||||
# A7 固定相机手眼标定
|
||||
|
||||
固定式 Intel RealSense D405 相机(eye-to-hand):拍摄棋盘 → 操作者用同一刚性触点对准格子中心 → 只读记录机械臂末端位姿 → 联合求解触点偏移和相机外参 → 独立验证。
|
||||
|
||||
支持A7左臂和右臂。控制器固定地址为 **192.168.10.21:9090**,通过ROSBridge订阅所选臂的 `/left/arm_state` 或 `/right/arm_state`。工具不发送机械臂或灵巧手动作,不切换使能,不自动部署结果。`capture` 会按需启动相机驱动。
|
||||
|
||||
配套抓取项目:[a7l20-cup-grasp](https://gitea.robotquan.com/liyang/a7l20-cup-grasp)。详细步骤见 **[完整标定教程](WORKFLOW.md)**。
|
||||
|
||||
## 安装
|
||||
|
||||
Ubuntu 24.04、系统Python 3.12、ROS 2 Jazzy、D405。先按[ROS官方说明](https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debs.html)配置ROS软件源,再安装:
|
||||
|
||||
```bash
|
||||
sudo apt update
|
||||
sudo apt install python3.12-venv python3-numpy python3-opencv util-linux \
|
||||
ros-jazzy-ros-base ros-jazzy-sensor-msgs ros-jazzy-realsense2-camera
|
||||
git clone https://gitea.robotquan.com/liyang/a7-handeye-calibration.git
|
||||
cd a7-handeye-calibration
|
||||
bash scripts/setup.sh
|
||||
bash scripts/check.sh
|
||||
```
|
||||
|
||||
相机驱动参考[realsense-ros安装说明](https://github.com/realsenseai/realsense-ros#installation-on-ubuntu),USB权限与内核支持参考[librealsense Linux指南](https://github.com/realsenseai/librealsense/blob/master/doc/distribution_linux.md)。确认D405接入可靠USB3接口,关闭占用它的Viewer/其他采集程序。
|
||||
|
||||
相机与棋盘检测使用系统Python的ROS、OpenCV和NumPy;触点读取和联合求解使用本仓库venv中的numpy/scipy/roslibpy。两者分开,避免系统ROS视觉包与pip NumPy ABI混用。无需A7 SDK动态库,也无需灵巧手CAN或大模型权重。需要覆盖解释器或ROS域时复制 `config/local.env.example` 为 `config/local.env` 后修改。
|
||||
|
||||
## 流程
|
||||
|
||||
棋盘默认为 **8列×5行内角点、格长27 mm**,使用前核对实物。相机、棋盘和基座在整轮采集期间保持固定。
|
||||
|
||||
```bash
|
||||
bash run.sh capture --arm left
|
||||
bash run.sh status
|
||||
bash run.sh record --arm left
|
||||
bash run.sh solve --arm left
|
||||
bash run.sh validate --arm left
|
||||
```
|
||||
|
||||
右臂将 `--arm left` 改为 `--arm right`。分阶段操作,不要直接串行执行整组命令。采集与验证由操作者摆位,软件只读反馈;新消息不等于真实关节角已更新。
|
||||
|
||||
每次capture创建独立 `data/handeye_时间戳/`,会话指针为 `current_session.json`。样本与外参覆盖前自动存入会话内history,命令输出保存在logs。文件锁防止两轮同时写入。发布仓库不包含旧工位的真实采样或可直接套用的外参。
|
||||
|
||||
## 输出与接入抓取
|
||||
|
||||
验证后使用本轮 `camera.json`:`T_base_cam_opencv` 把相机OpenCV坐标转换到共同基座 `base_link`,单位米。`fingertip_offset_m` 是本轮所选臂固定触点相对于末端的偏移,不能迁移给另一只手。
|
||||
|
||||
抓取项目的默认入口使用**新RGB-D特征直接视觉伺服,不要求掌心Tag**。手眼外参提供相机与基座坐标关系,不能替代新视觉观测,也不等于末段实测局部运动映射。按[教程的部署步骤](WORKFLOW.md#8-接入杯子抓取项目)备份并替换抓取项目的外参文件;工具不会自动操作另一个项目。
|
||||
|
||||
## 验证范围
|
||||
|
||||
`bash scripts/check.sh` 只检查依赖、语法、纯离线合成数据求解和左右臂数据隔离,不连接相机或控制器。求解、采点和验证入口已做离线检查;重新打包后未采集真机数据,不宣称新的实物标定精度。
|
||||
|
||||
当前PnP沿用无畸变求解;使用前必须确认图像已校正或畸变可忽略。非零畸变需先适配求解器。训练拟合残差和图像重投影误差不能替代独立触点验证。代码来源、局限与验收记录见[VALIDATION.md](VALIDATION.md)。
|
||||
@@ -0,0 +1,15 @@
|
||||
# 发布验证
|
||||
|
||||
2026-10-29,从既有A7固定相机标定工具提取8个Python脚本、独立环境入口和操作教程,未复制旧标定会话或真实外参。
|
||||
|
||||
发布整理只调整解释器/资源路径、相对会话指针、新目录创建和说明;触点联合求解与棋盘PnP算法沿用源实现。源码来源哈希见source_manifest.json。
|
||||
|
||||
离线检查从仓库外目录启动,使用已有匹配版本Python依赖与宿主ROS/OpenCV:
|
||||
|
||||
- Python/Bash语法和ROS相机消息导入通过,没有创建ROS节点。
|
||||
- 合成棋盘检测与左右臂对准图生成通过。
|
||||
- 左右臂各12组已知真值的合成触点,求解恢复相机变换和触点偏移,通过1e-6数值比较;这是数值测试,不代表实物精度。
|
||||
- 不同臂的样本或触点偏移拒绝混用。
|
||||
- 工作文件原子替换及覆盖前历史备份通过。
|
||||
|
||||
未进行本轮真机采点、相机采集或新工位精度验收。未在空白机器重新安装全部系统和Python依赖。PnP目前不使用畸变参数;腕姿态覆盖不足时求解可能病态,训练残差不能作为独立验证。完整操作边界见WORKFLOW.md。
|
||||
+118
@@ -0,0 +1,118 @@
|
||||
# A7 + D405 固定相机手眼标定流程
|
||||
|
||||
## 1. 本次方案及已知限制
|
||||
|
||||
相机固定在双臂中间,属于 eye-to-hand。用相机识别棋盘,再用**左臂上的固定法兰参考点**对准棋盘上的已知位置,联合求出法兰参考点偏移和棋盘相对基座的位置,最后得到相机到基座的变换。默认左臂,也可在record/solve/validate中显式使用 `--arm right`。
|
||||
|
||||
本工具沿用固定触点求解方法,现已支持左臂。此前左臂掉使能时曾出现:操作者扭动,但状态固定在约±0.011°。因此不能用“收到新消息/时间戳变化”代替真实编码器更新。采样前必须确认所选臂有效反馈,不能默认掉使能可读。
|
||||
|
||||
**操作循环:掉使能手动摆位 → 操作者使能 → 确认角度恢复、姿态静止且参考点仍对准 → 记录。** 掉使能时支撑好手臂;使能导致接触位置变化时,该点不能记录。软件不代替操作者切换使能。
|
||||
|
||||
## 2. 固定设备和触点
|
||||
|
||||
- 相机、棋盘、机器人基座在整个采集过程中不能移动;相机线缆不能拉动支架。
|
||||
- 本棋盘为8列×5行内角点,格长27mm,正式开始前核对实物尺寸。
|
||||
- 选定同一个可重复定位的法兰参考点。只用侧边但每次接触位置不同,不满足固定偏移假设。
|
||||
- 不混用手指尖、法兰边和不同接触点。如使用手指尖,它相对法兰必须保持刚性固定;灵巧手关节变化会破坏假设。
|
||||
- 本套脚本默认读取**左臂**;record/solve/validate必须选择同一臂。样本和求解结果均保存臂标记,跨臂混用会拒绝。旧右臂数据保留,左臂在新会话中重新采点和求解固定触点偏移。
|
||||
|
||||
## 3. 新建采集并拍板
|
||||
|
||||
在README所示目录运行 `bash run.sh capture`。它自动检查并按需启动域29的D405驱动,保存五帧RGB和内参,再生成棋盘编号图。已有相机驱动会复用,不重复启动;不会启动机械臂驱动。
|
||||
|
||||
检查完整棋盘、角点与图像交点一致,无遮挡或明显模糊。平均重投影误差约0.5px以内可作为图像检测检查目标,不代表机器人绝对定位精度。此旧PnP脚本未使用畸变参数;当前相机内参/图像是否已校正应核对,有非零畸变时需先适配求解器,不能套用误差结论。
|
||||
|
||||
新一轮运行capture自动创建北京时间微秒时间戳目录,成功后自动设为当前会话;record/solve/validate自动跟随,无需指定路径。status可查看实际路径。已保存且有效的当前拍板数据可以续用,无需重复capture;相机/棋盘若移动则必须开始新一轮。不要复制旧touch_samples.json进来,也不要对旧数据直接使用 `--fresh`。
|
||||
|
||||
## 4. 编号:必须对准格子中心,不是内角点
|
||||
|
||||
`corners_labeled.png`给40个内角点编号0–39。左臂使用`align_lines_left.png`,黄线连接格子的k与k+9;右臂使用`align_lines_right.png`,黄线连接k+1与k+8。两条线属于同一个格子的两条对角线,实际触点均为锚点k的**右下方格中心**。`align_lines.png`为capture所选臂的图。编号取决于本次识别方向,始终以本次图为准。
|
||||
|
||||
有效锚点:
|
||||
|
||||
```
|
||||
0 1 2 3 4 5 6
|
||||
8 9 10 11 12 13 14
|
||||
16 17 18 19 20 21 22
|
||||
24 25 26 27 28 29 30
|
||||
```
|
||||
|
||||
7、15、23、31及32–39没有对应的右下内部格子,不能用。输入0表示0号锚点右下的**格子中心**,不是触碰0号角点。旧采集脚本把它叫“角点”,容易误解;整理版提示已纠正,求解和验证均采用格子中心。
|
||||
|
||||
## 5. 记录样本
|
||||
|
||||
运行 `bash run.sh record`。
|
||||
|
||||
1. 操作者掉使能,在可安全手动移动的前提下摆位。
|
||||
2. 用同一法兰参考点对准本次图中目标格子中心。
|
||||
3. 操作者使能,确认实际位置没有变化,角度已恢复可信反馈。
|
||||
4. 输入 `s`查看反馈。新数据包不保证真实更新,应结合刚才姿态变化检查。
|
||||
5. 输入锚点编号并回车保存;改姿态后重复。`u`撤销最近一条,`done`结束。
|
||||
|
||||
整理版每条样本立即存盘;拒绝无效编号和已观察到的全零附近占位角度。该检查不是通用使能状态检测:非零缓存也可能无效,仍需操作者确认。与上一点位置差<3mm会询问是否保存;同一点换朝向的有效样本可以保存,但应先确认确实不是旧反馈。
|
||||
|
||||
建议12–20组训练样本,覆盖板的不同区域。在可达、无碰撞的前提下绕不同方向改变腕部朝向;只有平移或始终同一朝向会让触点偏移难以确定。15°/40°都不是精度保证,需结合解的可观测性和独立误差评估。旧脚本接受至少4个不同点只代表可以尝试求解,不代表采集充分。
|
||||
|
||||
同时预留3–5个未参与求解的位置/朝向用于验证。不能把验证点混入训练后仍称独立验证。
|
||||
|
||||
## 6. 联合求解
|
||||
|
||||
运行 `bash run.sh solve`,入口自动备份该目录的旧camera.json。
|
||||
|
||||
对第i次采样:
|
||||
|
||||
```
|
||||
t_i + R_i * d = R_base_board * b_i + t_base_board
|
||||
T_base_camera = T_base_board * inverse(T_camera_board)
|
||||
```
|
||||
|
||||
其中R_i、t_i来自控制器报告的末端pose;d是相对于该pose所代表末端坐标系的固定触点偏移。不要未经核对就认定该pose一定是裸法兰;采集期间工具/工作坐标系不能改变。b_i是格子中心的棋盘坐标,单位米。
|
||||
|
||||
输出包括T_base_board、T_base_camera、d以及训练残差。相机文件的T_base_cam_opencv和历史字段T_world_cam_opencv均表示相机在基座系中的位姿,可将相机坐标点转换到base_link;不是已经抬高后的RViz world坐标。
|
||||
|
||||
本旧求解器没有完整的病态性拒绝或自动验收门控。即使打印calibrated,也只是求解结果,不能直接用于运动。大触点偏移、少量样本造成结果大变、朝向覆盖小或误差集中在某区域,均需重新检查样本,而不是增加显示平移掩盖。
|
||||
|
||||
## 7. 独立验证
|
||||
|
||||
运行 `bash run.sh validate`。使用未参与训练的格子中心及不同腕部朝向,仍由操作者摆位、使能、确认实际反馈后输入编号。此脚本仅读取与计算,不会自动移动过去。
|
||||
|
||||
记录各点误差、平均值和最大值;终端输出自动保存在当前目录logs/下的时间戳文件。软件的“<10mm可用”只是旧经验提示,不是抓取放行标准。验收阈值应按后续任务所需精度确定。低训练残差、低图像重投影误差都不能替代独立验证。
|
||||
|
||||
再看左右腕部、固定手掌与**原始点云**是否同时重合。不要用过滤后的点云评估整体重合,也不要用未接入实际关节角的手指作为标定参照。
|
||||
|
||||
## 8. 接入杯子抓取项目
|
||||
|
||||
只有独立验证完成、相机安装保持不变时才接入。`T_base_cam_opencv` 将相机OpenCV系的点映射到共同base_link;`T_world_cam_opencv` 是本工具保留的同值历史字段,不能据字段名当成已转换到其他world系。
|
||||
|
||||
1. 在本工具目录运行 `bash run.sh status`,确认当前标定会话。
|
||||
2. 将本轮camera.json复制到杯子项目,在复制前备份旧文件。以下路径由操作者填写;不填写历史示例会话:
|
||||
|
||||
```bash
|
||||
# 在标定仓库根目录执行
|
||||
cup_project="../a7l20-cup-grasp"
|
||||
calibration_session="data/handeye_本轮时间戳"
|
||||
cp "$cup_project/config/camera_extrinsics_current.json" \
|
||||
"$cup_project/config/camera_extrinsics_current.backup.$(date +%Y%m%d_%H%M%S).json"
|
||||
cp "$calibration_session/camera.json" "$cup_project/config/camera_extrinsics_current.json"
|
||||
bash "$cup_project/run.sh" check
|
||||
```
|
||||
|
||||
3. 确认文件包含 `coordinate_frame=base_link` 和本轮 `calibration_arm`。左、右臂共享基座时可使用同一相机外参;固定触点偏移只属于标定时的那只臂,不能拿右臂触点偏移充当左手TCP。
|
||||
4. 杯子默认流程在观察位采新RGB-D、配准手与杯并进行相对视觉伺服,不需要掌心标签。替换外参不会重新生成手部角度标定、HUG模板、示教路线或实测局部运动映射;这些工位条件变化时需单独核对。
|
||||
5. 只有明确请求执行时才运行抓取。离线check通过不代表新外参已经完成真机验证,不自动续跑旧停位。
|
||||
|
||||
如需RViz显示,另按实际 `T_world_base` 计算 `T_world_camera = T_world_base × T_base_camera`,并确保只有一个TF发布者;本仓库不提供自动部署RViz或自动规划运动。
|
||||
|
||||
## 9. 常见失败
|
||||
|
||||
| 现象 | 处理 |
|
||||
|---|---|
|
||||
| 消息一直刷新,角度不变或全在±0.011°附近 | 拒绝采点;确认使能后实际反馈恢复 |
|
||||
| 编号和实物对不上 | 以本次align_lines图为准,检查格子中心/角点是否混淆 |
|
||||
| 训练误差小,换朝向就偏 | 核查固定触点、腕姿态覆盖和偏移可观测性 |
|
||||
| 左右臂只有一侧对齐 | 检查两臂反馈、坐标链、深度表面质量;不要仅凭一侧强行平移相机 |
|
||||
| 点云消失但模型仍红 | 查询具体碰撞对、累积占据体素和关节限位,不能靠隐藏点云解决 |
|
||||
| 相机或棋盘采集中移动 | 本轮几何假设失效,重新采集 |
|
||||
|
||||
全流程不需要启动会自动使能的原生Bridge;只读工具通过控制器rosbridge获取所选臂状态,默认左臂。使能与掉使能由操作者明确操作。
|
||||
|
||||
@@ -0,0 +1,5 @@
|
||||
# Optional runtime overrides; copy to local.env.
|
||||
# HANDEYE_PYTHON=/path/to/python3.12
|
||||
HANDEYE_ROS_PYTHON=/usr/bin/python3
|
||||
HANDEYE_ROS_SETUP=/opt/ros/jazzy/setup.bash
|
||||
ROS_DOMAIN_ID=29
|
||||
@@ -0,0 +1,3 @@
|
||||
numpy==2.5.3
|
||||
scipy==1.18.1
|
||||
roslibpy==2.1.0
|
||||
@@ -0,0 +1,70 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
kit_dir="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" && pwd)"
|
||||
source "$kit_dir/scripts/environment.sh"
|
||||
cd -- "$kit_dir"
|
||||
export TZ=Asia/Shanghai
|
||||
action="${1:---help}"
|
||||
if [[ $# -gt 0 ]]; then shift; fi
|
||||
arm=left
|
||||
if [[ $# -gt 0 ]]; then
|
||||
if [[ $# != 2 || "$1" != --arm || ( "$2" != left && "$2" != right ) ]]; then
|
||||
echo '用法:bash run.sh capture|record|solve|validate|status [--arm left|right]' >&2; exit 2
|
||||
fi
|
||||
arm="$2"
|
||||
fi
|
||||
case "$action" in
|
||||
capture|record|solve|validate|status) ;;
|
||||
*) echo '用法:bash run.sh capture|record|solve|validate|status [--arm left|right](默认left)'
|
||||
echo 'capture 自动新建时间戳目录,其余命令自动使用最近一次成功采集目录。'
|
||||
echo '仅采集与计算,不自动使能或运动,不自动部署。'; exit 0 ;;
|
||||
esac
|
||||
exec 9>"$kit_dir/.session.lock"
|
||||
flock -n 9 || { echo '另一个采集/求解任务正在运行,请先结束它。' >&2; exit 2; }
|
||||
if [[ "$action" == capture ]]; then
|
||||
export ROS_AUTOMATIC_DISCOVERY_RANGE="${ROS_AUTOMATIC_DISCOVERY_RANGE:-LOCALHOST}"
|
||||
set +u
|
||||
source "$HANDEYE_ROS_SETUP"
|
||||
set -u
|
||||
"$HANDEYE_ROS_PYTHON" scripts/ensure_camera.py
|
||||
capture_dir="$("$HANDEYE_ROS_PYTHON" scripts/capture_board.py)"
|
||||
"$HANDEYE_ROS_PYTHON" - "$capture_dir" <<'PY'
|
||||
import sys,json
|
||||
from pathlib import Path
|
||||
p=Path(sys.argv[1]);m=json.loads((p/'camera.json').read_text());k=m['K'];(p/'intrinsics.txt').write_text(f'{k[0][0]} {k[1][1]} {k[0][2]} {k[1][2]}\n')
|
||||
PY
|
||||
mkdir -p "$capture_dir/logs"
|
||||
"$HANDEYE_ROS_PYTHON" scripts/handeye_solve.py --capture "$capture_dir" --square .027 --cols 8 --rows 5 --arm "$arm" 2>&1 | tee "$capture_dir/logs/capture_$(date +%Y%m%d_%H%M%S_%N).log"
|
||||
"$HANDEYE_ROS_PYTHON" - "$capture_dir" <<'PY'
|
||||
import sys,json
|
||||
from pathlib import Path
|
||||
sys.path.insert(0,'scripts')
|
||||
from storage import save_json
|
||||
p=Path(sys.argv[1]);cam=json.loads((p/'camera.json').read_text())
|
||||
if cam['board_reproj_px']>.5:raise SystemExit('重投影误差>0.5px,本次数据保留,但不切换当前标定目录。')
|
||||
save_json(Path('current_session.json'),{'capture_dir':str(p.resolve().relative_to(Path.cwd())),'timezone':'Asia/Shanghai'})
|
||||
PY
|
||||
echo "当前标定目录:$capture_dir"
|
||||
echo '下一步:bash run.sh record(不需要路径)'
|
||||
exit 0
|
||||
fi
|
||||
capture_dir="$("$HANDEYE_ROS_PYTHON" - <<'PY'
|
||||
from pathlib import Path
|
||||
import json
|
||||
p=Path('current_session.json')
|
||||
if not p.exists():raise SystemExit('还没有新标定目录,请先运行 bash run.sh capture')
|
||||
c=Path(json.loads(p.read_text())['capture_dir']).resolve()
|
||||
if not c.is_relative_to(Path('data').resolve()) or not (c/'camera.json').is_file():raise SystemExit('当前标定目录无效,请重新capture')
|
||||
print(c)
|
||||
PY
|
||||
)"
|
||||
echo "当前标定目录:$capture_dir"
|
||||
if [[ "$action" == status ]]; then exit 0; fi
|
||||
echo "标定臂:$arm(只读订阅 /$arm/arm_state)"
|
||||
case "$action" in
|
||||
record) script=record_touch.py ;;
|
||||
solve) script=solve_base_board.py ;;
|
||||
validate) script=validate_touch.py ;;
|
||||
esac
|
||||
mkdir -p "$capture_dir/logs"
|
||||
"$HANDEYE_PYTHON" -u "scripts/$script" --capture "$capture_dir" --arm "$arm" 2>&1 | tee "$capture_dir/logs/${action}_$(date +%Y%m%d_%H%M%S_%N).log"
|
||||
@@ -0,0 +1,17 @@
|
||||
"""Keep arm-specific touch samples and fitted tool offsets separate."""
|
||||
|
||||
|
||||
def check_sample_arm(data, arm):
|
||||
recorded = data.get('arm')
|
||||
if recorded != arm:
|
||||
raise SystemExit(f'样本属于 {recorded or "未标注的"} 臂,当前选择 {arm}。'
|
||||
'不能混用;请使用匹配的 --arm,或 capture 新建标定会话。')
|
||||
if any(sample.get('arm', recorded) != arm for sample in data.get('samples', [])):
|
||||
raise SystemExit('样本中混入不同机械臂的数据,拒绝采集/求解/验证。')
|
||||
|
||||
|
||||
def check_calibration_arm(camera, arm):
|
||||
recorded = camera.get('calibration_arm')
|
||||
if recorded != arm:
|
||||
raise SystemExit(f'触点偏移标定属于 {recorded or "未标注的"} 臂,当前选择 {arm}。'
|
||||
'请用当前臂重新 solve,不能复用另一臂的触点偏移。')
|
||||
@@ -0,0 +1,36 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Capture calibration-board images from the running ROS camera; no device commands."""
|
||||
import json,time,os
|
||||
from storage import stamp as session_timestamp
|
||||
from datetime import datetime,timezone
|
||||
from pathlib import Path
|
||||
import numpy as np,cv2,rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import Image,CameraInfo
|
||||
ROOT=Path(__file__).resolve().parents[1]
|
||||
def main():
|
||||
rclpy.init();n=Node('capture_board_readonly');frames=[];infos=[];last=[0.]
|
||||
def cb(msg):
|
||||
if len(frames)>=5 or time.monotonic()-last[0]<.35:return
|
||||
if msg.encoding not in ('rgb8','bgr8'):return
|
||||
image=np.frombuffer(msg.data,dtype=np.uint8).reshape(msg.height,msg.step)[:,:msg.width*3].reshape(msg.height,msg.width,3).copy()
|
||||
if msg.encoding=='rgb8':image=cv2.cvtColor(image,cv2.COLOR_RGB2BGR)
|
||||
frames.append((image,{'sec':msg.header.stamp.sec,'nanosec':msg.header.stamp.nanosec,'frame_id':msg.header.frame_id}));last[0]=time.monotonic()
|
||||
n.create_subscription(Image,'/camera/camera/color/image_raw',cb,qos_profile_sensor_data)
|
||||
n.create_subscription(CameraInfo,'/camera/camera/color/camera_info',lambda m:infos.append(m),qos_profile_sensor_data)
|
||||
deadline=time.monotonic()+20
|
||||
while (len(frames)<5 or not infos) and time.monotonic()<deadline:rclpy.spin_once(n,timeout_sec=.1)
|
||||
if not frames or not infos:
|
||||
missing=[]
|
||||
if not frames:missing.append('/camera/camera/color/image_raw')
|
||||
if not infos:missing.append('/camera/camera/color/camera_info')
|
||||
n.destroy_node();rclpy.shutdown()
|
||||
raise SystemExit('20秒内未收到相机消息:'+', '.join(missing)+
|
||||
'\n当前ROS_DOMAIN_ID='+os.environ.get('ROS_DOMAIN_ID','0')+
|
||||
'。capture只订阅已运行的相机,请先启动同一ROS域内的D405驱动;也需核对话题名。')
|
||||
out=ROOT/'data'/('handeye_'+session_timestamp());out.mkdir(parents=True)
|
||||
for i,(im,frame_stamp) in enumerate(frames):cv2.imwrite(str(out/('rgb.png' if i==0 else f'rgb_{i:02d}.png')),im)
|
||||
info=infos[-1];meta={'source':'live_ROS_D405','K':np.array(info.k).reshape(3,3).tolist(),'D':list(info.d),'distortion_model':info.distortion_model,'width':info.width,'height':info.height,'frames':[s for im,s in frames],'board_fixed_user_confirmed':True,'motion_sent':False}
|
||||
(out/'camera.json').write_text(json.dumps(meta,indent=2));print(out,flush=True);n.destroy_node();rclpy.shutdown()
|
||||
if __name__=='__main__':main()
|
||||
@@ -0,0 +1,15 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
source "$(dirname -- "${BASH_SOURCE[0]}")/environment.sh"
|
||||
set +u
|
||||
source "$HANDEYE_ROS_SETUP"
|
||||
set -u
|
||||
"$HANDEYE_ROS_PYTHON" -c 'import cv2, numpy, rclpy; from sensor_msgs.msg import Image, CameraInfo; print("ROS camera imports passed; no node created")'
|
||||
"$HANDEYE_PYTHON" - "$HANDEYE_ROOT" <<'PY'
|
||||
import ast,pathlib,subprocess,sys
|
||||
root=pathlib.Path(sys.argv[1])
|
||||
for path in (root/'scripts').glob('*.py'): ast.parse(path.read_text(),filename=str(path))
|
||||
for path in [root/'run.sh',*(root/'scripts').glob('*.sh')]:subprocess.run(['bash','-n',str(path)],check=True)
|
||||
print('Syntax checks passed')
|
||||
PY
|
||||
"$HANDEYE_PYTHON" -B -m unittest discover -s "$HANDEYE_ROOT/tests" -v
|
||||
@@ -0,0 +1,88 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Start only the camera driver if needed; wait for fresh image and intrinsics."""
|
||||
import os
|
||||
from pathlib import Path
|
||||
import signal
|
||||
import subprocess
|
||||
import sys
|
||||
import time
|
||||
|
||||
import rclpy
|
||||
from rclpy.qos import qos_profile_sensor_data
|
||||
from sensor_msgs.msg import CameraInfo, Image
|
||||
from storage import stamp
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
IMAGE = '/camera/camera/color/image_raw'
|
||||
INFO = '/camera/camera/color/camera_info'
|
||||
|
||||
|
||||
def main():
|
||||
rclpy.init()
|
||||
node = rclpy.create_node('handeye_ensure_camera')
|
||||
received = set()
|
||||
node.create_subscription(Image, IMAGE, lambda m: received.add('image')
|
||||
if m.data and m.encoding in ('rgb8', 'bgr8') else None,
|
||||
qos_profile_sensor_data)
|
||||
node.create_subscription(CameraInfo, INFO, lambda m: received.add('info')
|
||||
if m.k[0] > 0 and m.k[4] > 0 else None,
|
||||
qos_profile_sensor_data)
|
||||
|
||||
def wait_ready(seconds, process=None):
|
||||
deadline = time.monotonic() + seconds
|
||||
while time.monotonic() < deadline:
|
||||
rclpy.spin_once(node, timeout_sec=.1)
|
||||
if len(received) == 2:
|
||||
return True
|
||||
if process is not None and process.poll() is not None:
|
||||
return False
|
||||
return False
|
||||
|
||||
owned = None
|
||||
ready = False
|
||||
log_path = None
|
||||
try:
|
||||
if wait_ready(4):
|
||||
ready = True
|
||||
print('相机图像和内参已就绪,复用已有驱动。', file=sys.stderr)
|
||||
return
|
||||
running = (node.count_publishers(IMAGE) or node.count_publishers(INFO)
|
||||
or ('camera', '/camera') in node.get_node_names_and_namespaces())
|
||||
if running:
|
||||
print('已有相机节点,等待消息,不重复启动。', file=sys.stderr)
|
||||
else:
|
||||
logs = ROOT / 'logs'
|
||||
logs.mkdir(exist_ok=True)
|
||||
log_path = logs / f'camera_driver_{stamp()}.log'
|
||||
with log_path.open('w') as output:
|
||||
# A separate session and closed FDs let the driver survive
|
||||
# capture and prevent inheritance of run.sh's session lock.
|
||||
owned = subprocess.Popen([
|
||||
'ros2', 'launch', 'realsense2_camera', 'rs_launch.py',
|
||||
'rgb_camera.color_profile:=848x480x30',
|
||||
'depth_module.depth_profile:=848x480x30',
|
||||
'align_depth.enable:=true',
|
||||
], stdout=output, stderr=subprocess.STDOUT,
|
||||
stdin=subprocess.DEVNULL, start_new_session=True, close_fds=True)
|
||||
print(f'正在启动D405驱动,PID={owned.pid},日志:{log_path}', file=sys.stderr)
|
||||
if not wait_ready(30, owned):
|
||||
raise RuntimeError('相机图像/内参未就绪。检查USB连接、设备占用和相机驱动日志。'
|
||||
+ (f' 日志:{log_path}' if log_path else ''))
|
||||
ready = True
|
||||
print('相机图像和内参已就绪,开始标定采集;驱动在采集结束后保持运行。', file=sys.stderr)
|
||||
finally:
|
||||
if owned is not None and not ready and owned.poll() is None:
|
||||
os.killpg(owned.pid, signal.SIGINT)
|
||||
try:
|
||||
owned.wait(timeout=5)
|
||||
except subprocess.TimeoutExpired:
|
||||
os.killpg(owned.pid, signal.SIGTERM)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
try:
|
||||
main()
|
||||
except (RuntimeError, OSError) as error:
|
||||
raise SystemExit(str(error))
|
||||
@@ -0,0 +1,8 @@
|
||||
#!/usr/bin/env bash
|
||||
HANDEYE_ROOT="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
if [[ -f "$HANDEYE_ROOT/config/local.env" ]]; then source "$HANDEYE_ROOT/config/local.env"; fi
|
||||
export HANDEYE_PYTHON="${HANDEYE_PYTHON:-$HANDEYE_ROOT/.venv/bin/python}"
|
||||
export HANDEYE_ROS_PYTHON="${HANDEYE_ROS_PYTHON:-/usr/bin/python3}"
|
||||
export HANDEYE_ROS_SETUP="${HANDEYE_ROS_SETUP:-/opt/ros/jazzy/setup.bash}"
|
||||
export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-29}"
|
||||
export PYTHONNOUSERSITE=1 PYTHONDONTWRITEBYTECODE=1
|
||||
@@ -0,0 +1,131 @@
|
||||
"""手眼标定(方法 A:板平放桌上)求解器。
|
||||
① 检测棋盘格 → solvePnP → T_cam_board(板在相机系位姿,OpenCV 约定)。
|
||||
② 若给 --base-board(T_base_board 4x4,板在机器人基座系位姿):
|
||||
T_base_cam = T_base_board @ inv(T_cam_board) → 写进 <capture>/camera.json 的外参,供 perceive_object 用。
|
||||
|
||||
棋盘格朝向消歧:板平放桌上、法线朝相机——强制 T_cam_board 的 z 轴(板法线)指向相机(cam→board 连线反向)。
|
||||
|
||||
用法:.venv/bin/python scripts/handeye_solve.py --capture data/real_calib --square 0.027 [--cols 9 --rows 6] \
|
||||
[--base-board "r00,r01,...,1"] # 16 个数行主序
|
||||
"""
|
||||
import argparse, json
|
||||
from storage import save_json
|
||||
from pathlib import Path
|
||||
import numpy as np, cv2
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
|
||||
|
||||
def draw_alignment(img, cp, cols, rows, arm):
|
||||
result = img.copy()
|
||||
for k in range(cols * rows):
|
||||
if k % cols == cols - 1 or k // cols == rows - 1:
|
||||
continue
|
||||
# Left arm uses the other diagonal of the same cell. The physical
|
||||
# target remains its centre, independent of the alignment direction.
|
||||
pa, pb = ((cp[k], cp[k + cols + 1]) if arm == 'left'
|
||||
else (cp[k + 1], cp[k + cols]))
|
||||
mid = (cp[k + 1] + cp[k + cols]) / 2
|
||||
cv2.line(result, tuple(pa.astype(int)), tuple(pb.astype(int)),
|
||||
(0, 200, 255), 2, cv2.LINE_AA)
|
||||
cv2.circle(result, tuple(mid.astype(int)), 3, (0, 255, 0), -1)
|
||||
cv2.putText(result, str(k), (int(mid[0]) - 6, int(mid[1]) + 5),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, .5, (255, 0, 0), 2, cv2.LINE_AA)
|
||||
cv2.putText(result, f'{arm.upper()} ARM', (15, 30),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, .8, (0, 200, 255), 2, cv2.LINE_AA)
|
||||
return result
|
||||
|
||||
|
||||
def load_K(cap):
|
||||
fx, fy, cx, cy = map(float, (cap / "intrinsics.txt").read_text().split())
|
||||
return np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1.0]])
|
||||
|
||||
|
||||
def _pnp_err(objp, cor, K):
|
||||
ok, rv, tv = cv2.solvePnP(objp, cor, K, None, flags=cv2.SOLVEPNP_ITERATIVE)
|
||||
pr, _ = cv2.projectPoints(objp, rv, tv, K, None)
|
||||
e = float(np.linalg.norm(pr.reshape(-1, 2) - cor.reshape(-1, 2), axis=1).mean())
|
||||
R, _ = cv2.Rodrigues(rv); T = np.eye(4); T[:3, :3] = R; T[:3, 3] = tv.ravel()
|
||||
return e, T
|
||||
|
||||
|
||||
def detect_grid(gray, K, square, cols, rows):
|
||||
"""扫描候选网格,对每个跑 solvePnP,取重投影误差最小者(自动拒绝误检/转置)。"""
|
||||
flags = cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
|
||||
cand = [(cols, rows)] if (cols and rows) else [(c, r) for c in range(4, 14) for r in range(3, 12) if c > r]
|
||||
best = None
|
||||
for (c, r) in cand:
|
||||
ok, cor = cv2.findChessboardCorners(gray, (c, r), flags=flags)
|
||||
if not ok:
|
||||
continue
|
||||
cor = cv2.cornerSubPix(gray, cor, (11, 11), (-1, -1),
|
||||
(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-3))
|
||||
objp = np.zeros((c * r, 3), np.float32); objp[:, :2] = np.mgrid[0:c, 0:r].T.reshape(-1, 2) * square
|
||||
e, T = _pnp_err(objp, cor, K)
|
||||
if best is None or e < best[0]:
|
||||
best = (e, (c, r), cor, T)
|
||||
return best # (err, grid, corners, T_cam_board) 或 None
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument("--capture", required=True); ap.add_argument("--square", type=float, required=True, help="方格边长(m)")
|
||||
ap.add_argument("--cols", type=int, default=None); ap.add_argument("--rows", type=int, default=None)
|
||||
ap.add_argument("--base-board", default=None, help="T_base_board 4x4,16 个数逗号分隔(行主序)")
|
||||
ap.add_argument("--arm", choices=["left", "right"], default="left")
|
||||
ap.add_argument("--alignment-only", action="store_true", help="仅重绘对准线,不修改标定数据")
|
||||
a = ap.parse_args()
|
||||
cap = Path(a.capture).expanduser().resolve()
|
||||
K = load_K(cap)
|
||||
img = cv2.imread(str(cap / "rgb.png")); gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
|
||||
best = detect_grid(gray, K, a.square, a.cols, a.rows)
|
||||
if best is None:
|
||||
raise SystemExit("未检测到完整棋盘格。确认整块板在画面内、够清晰;或用 --cols/--rows 指定内角点网格。")
|
||||
err, grid, corners, T_cam_board = best
|
||||
c, r = grid
|
||||
cp = corners.reshape(-1, 2)
|
||||
for side in ('left', 'right'):
|
||||
guide = draw_alignment(img, cp, c, r, side)
|
||||
cv2.imwrite(str(cap / f'align_lines_{side}.png'), guide)
|
||||
if side == a.arm:
|
||||
cv2.imwrite(str(cap / 'align_lines.png'), guide)
|
||||
print(f"{a.arm}臂对准线 -> {cap}/align_lines_{a.arm}.png")
|
||||
if a.alignment_only:
|
||||
return
|
||||
if err > 2.0:
|
||||
print(f"⚠ 最佳网格 {c}x{r} 重投影误差仍达 {err:.1f}px,可能板太斜/模糊/被遮,结果不可信——建议重摆再存帧")
|
||||
# 每个角点的板系坐标(与 solvePnP 的 objp 同序):objp[k] ↔ corners[k]
|
||||
objp = np.zeros((c * r, 3), np.float32); objp[:, :2] = np.mgrid[0:c, 0:r].T.reshape(-1, 2) * a.square
|
||||
# 编号叠加图:触碰时按编号 k 碰角点,record_touch 用 k 取 objp[k]
|
||||
lab = img.copy()
|
||||
for k, px in enumerate(cp):
|
||||
cv2.circle(lab, (int(px[0]), int(px[1])), 3, (0, 255, 0), -1)
|
||||
cv2.putText(lab, str(k), (int(px[0]) + 4, int(px[1]) - 4), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 0, 255), 1, cv2.LINE_AA)
|
||||
cv2.imwrite(str(cap / "corners_labeled.png"), lab)
|
||||
vis = img.copy(); cv2.drawChessboardCorners(vis, grid, corners, True)
|
||||
cv2.imwrite(str(cap / "handeye_detect.png"), vis)
|
||||
print(f"检测到棋盘格 {c}x{r} 内角点,方格 {a.square*1000:.0f}mm;重投影误差 = {err:.2f} px(<0.5 好)")
|
||||
print(f"T_cam_board(相机系, m):\n{np.array2string(T_cam_board, precision=4, suppress_small=True)}")
|
||||
print(f"板中心在相机系: {(T_cam_board[:3,3]).round(3).tolist()} 距离 {np.linalg.norm(T_cam_board[:3,3])*100:.1f}cm")
|
||||
print(f"叠加图 -> {cap}/handeye_detect.png")
|
||||
# 持久化 T_cam_board + 板规格,供下一步 solve_base_board 用
|
||||
meta = json.loads((cap / "camera.json").read_text())
|
||||
meta["T_cam_board"] = T_cam_board.tolist(); meta["board_grid"] = [c, r]
|
||||
meta["board_square_m"] = a.square; meta["board_reproj_px"] = round(err, 3)
|
||||
meta["board_objp"] = objp.tolist() # 角点编号 k → 板系坐标(m),供触碰求解用
|
||||
save_json(cap / "camera.json", meta)
|
||||
print(f"角点编号叠加图 -> {cap}/corners_labeled.png(触碰时按编号 0~{c*r-1} 碰角点)")
|
||||
|
||||
if a.base_board:
|
||||
Tbb = np.array([float(x) for x in a.base_board.replace("[", "").replace("]", "").split(",")]).reshape(4, 4)
|
||||
T_base_cam = Tbb @ np.linalg.inv(T_cam_board)
|
||||
meta = json.loads((cap / "camera.json").read_text())
|
||||
meta["T_world_cam_opencv"] = T_base_cam.tolist(); meta["cam_pos_world"] = T_base_cam[:3, 3].tolist()
|
||||
meta["extrinsic_status"] = "calibrated(handeye method A)"
|
||||
save_json(cap / "camera.json", meta)
|
||||
print(f"\nT_base_cam(基座系, m):\n{np.array2string(T_base_cam, precision=4, suppress_small=True)}")
|
||||
print(f"相机在基座系位置: {T_base_cam[:3,3].round(3).tolist()} → 已写入 {cap/'camera.json'} 外参")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,119 @@
|
||||
"""手眼标定·触碰记录器(只读)。掉使能手动摆位,上使能且确认反馈有效后记录;固定法兰参考点对准锚点右下格子中心,本脚本 snapshot 每次的末端位姿。
|
||||
连控制器 rosbridge 读所选臂的 arm_state(默认 /left/arm_state,末端 pose 在 base_link 系)。全程只订阅、不发任何指令。
|
||||
|
||||
健壮性:每次记录都【强制等一条新消息】再 snapshot(避免 input() 阻塞期间读到陈旧位姿);
|
||||
并检测与上一条是否雷同(臂没动/位姿流卡住会立刻报警)。
|
||||
|
||||
用法:.venv/bin/python scripts/record_touch.py --capture data/real_calib
|
||||
交互:固定法兰参考点对准第 k 号格子中心(见 out/align_lines.png)、手腕朝向每次都变一变 → 输入 k 回车记一条;
|
||||
s=看实时位姿, u=撤销上一条, done=结束存盘。要 ≥5 个角点、朝向各不相同。
|
||||
"""
|
||||
import argparse, json, time
|
||||
from storage import save_json, stamp
|
||||
from arm_contract import check_sample_arm
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
import roslibpy
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument("--capture", default="data/real_calib")
|
||||
ap.add_argument("--host", default="192.168.10.21"); ap.add_argument("--port", type=int, default=9090)
|
||||
ap.add_argument("--fresh", action="store_true", help="清空已有样本重新开始")
|
||||
ap.add_argument("--arm", choices=["left", "right"], default="left")
|
||||
a = ap.parse_args()
|
||||
cap = ROOT / a.capture
|
||||
cam = json.loads((cap / 'camera.json').read_text()); cols, rows = cam['board_grid']
|
||||
guide = cap / f'align_lines_{a.arm}.png'
|
||||
if not guide.exists():
|
||||
raise SystemExit('缺少所选臂对准图,请先重绘:'+str(guide))
|
||||
print(f"请使用{a.arm}臂对准图:{guide}")
|
||||
samples = []
|
||||
f = cap / "touch_samples.json"
|
||||
if f.exists() and not a.fresh:
|
||||
data = json.loads(f.read_text())
|
||||
check_sample_arm(data, a.arm)
|
||||
samples = data.get("samples", [])
|
||||
print(f"已有 {len(samples)} 条 {a.arm} 臂样本,将在其后追加")
|
||||
ros = roslibpy.Ros(host=a.host, port=a.port); ros.run(timeout=8)
|
||||
print("connected:", ros.is_connected)
|
||||
|
||||
st = {"m": None, "n": 0}
|
||||
topic = f"/{a.arm}/arm_state"
|
||||
print(f"采集 {a.arm} 臂:{topic}")
|
||||
tp = roslibpy.Topic(ros, topic, "lbot_arm_interfaces/msg/ArmState")
|
||||
tp.subscribe(lambda m: (st.__setitem__("m", m), st.__setitem__("n", st["n"] + 1)))
|
||||
|
||||
def fresh(timeout=2.0):
|
||||
"""等一条【新】消息再返回,保证不是陈旧帧。返回 m 或 None(流卡住)。"""
|
||||
n0 = st["n"]; t0 = time.time()
|
||||
while st["n"] == n0 and time.time() - t0 < timeout:
|
||||
time.sleep(0.02)
|
||||
return st["m"] if st["n"] != n0 else None
|
||||
|
||||
if fresh() is None:
|
||||
tp.unsubscribe(); ros.terminate()
|
||||
raise SystemExit(f"收不到 {topic} 新消息——确认 web 控制器在线、rosbridge 通")
|
||||
|
||||
def pose_of(m):
|
||||
return np.array([m["pose"]["position"][k] for k in "xyz"])
|
||||
|
||||
print("== 固定法兰参考点对准锚点右下格子中心、上使能确认反馈后,每次换个手腕朝向 → 输入 k 回车。s=实时位姿, u=撤销, done=结束 ==")
|
||||
while True:
|
||||
s = input(f"[{len(samples)} 条] 角点编号 k / s / u / done: ").strip().lower()
|
||||
if s == "done":
|
||||
break
|
||||
if s == "s":
|
||||
m = fresh()
|
||||
if m is None: print(" ⚠ 位姿流卡住,没收到新消息"); continue
|
||||
p = m["pose"]["position"]; q = m["pose"]["orientation"]
|
||||
print(f" 实时末端 pos=({p['x']:.4f},{p['y']:.4f},{p['z']:.4f}) quat=({q['x']:.3f},{q['y']:.3f},{q['z']:.3f},{q['w']:.3f})")
|
||||
continue
|
||||
if s == "u":
|
||||
if samples:
|
||||
print(f" 撤销角点 {samples.pop()['corner']}")
|
||||
save_json(f, {'samples':samples,'host':a.host,'arm':a.arm,'target_definition':'anchor_lower_right_cell_center'})
|
||||
continue
|
||||
try:
|
||||
k = int(s)
|
||||
except ValueError:
|
||||
print(" 请输入角点编号(整数) / s / u / done"); continue
|
||||
if not (0 <= k < cols*rows and k % cols < cols-1 and k // cols < rows-1):
|
||||
print('编号无右下格子,拒绝记录'); continue
|
||||
m = fresh()
|
||||
if m is None:
|
||||
print(" ⚠ 2 秒内没收到新位姿消息——位姿流卡住了,没记录。检查 web 控制器/网络"); continue
|
||||
p = m["pose"]["position"]; q = m["pose"]["orientation"]
|
||||
angles=np.array(m['joint_state']['position'],float)
|
||||
if len(angles)!=7 or not np.isfinite(angles).all() or np.max(np.abs(angles)) < np.deg2rad(.02):
|
||||
print('拒绝记录:全零附近固定值或无效角度,请确认实际反馈恢复;收到新消息不代表编码器更新。'); continue
|
||||
pos = pose_of(m)
|
||||
# 雷同检测:与上一条几乎相同 → 忘了上使能/位姿没刷新
|
||||
if samples and np.linalg.norm(pos - np.array(samples[-1]["pos"])) < 0.003:
|
||||
c = input(f" ⚠ 与上一条角点 {samples[-1]['corner']} 位姿几乎相同(<3mm)——移动后【上使能】了吗?仍要记录输 y: ").strip().lower()
|
||||
if c != "y":
|
||||
print(" 已跳过"); continue
|
||||
samples.append({"arm":a.arm,"recorded_at_local":stamp(),"corner": k, "pos": [p["x"], p["y"], p["z"]],
|
||||
"quat_xyzw": [q["x"], q["y"], q["z"], q["w"]],
|
||||
"joints": list(m["joint_state"]["position"])})
|
||||
save_json(f, {'samples':samples,'host':a.host,'arm':a.arm,'target_definition':'anchor_lower_right_cell_center'})
|
||||
print(f" 记下角点 {k}: pos=({p['x']:.4f},{p['y']:.4f},{p['z']:.4f}) (共 {len(samples)} 条)")
|
||||
|
||||
tp.unsubscribe(); ros.terminate()
|
||||
cap.mkdir(parents=True, exist_ok=True)
|
||||
save_json(f, {"samples": samples, "host": a.host,"arm":a.arm,"target_definition":"anchor_lower_right_cell_center"})
|
||||
nc = len({s["corner"] for s in samples})
|
||||
# 检查位姿/朝向是否真的有变化
|
||||
if samples:
|
||||
P = np.array([s["pos"] for s in samples]); Q = np.array([s["quat_xyzw"] for s in samples])
|
||||
print(f"\n存盘 {f}:{len(samples)} 条 / {nc} 个角点;位置跨度 {(np.ptp(P,0)*1000).round(0).tolist()} mm;四元数跨度 {np.ptp(Q,0).round(3).tolist()}")
|
||||
print(("" if nc >= 4 else "⚠ 不同角点 <4,无法求解。") +
|
||||
f" 下一步(在标定工具目录):.venv/bin/python scripts/solve_base_board.py --capture {cap.resolve()} --arm {a.arm}")
|
||||
print(f"当前会话也可运行:bash run.sh solve --arm {a.arm}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,5 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)"
|
||||
python3.12 -m venv "$root/.venv"
|
||||
"$root/.venv/bin/python" -m pip install -r "$root/requirements.txt"
|
||||
@@ -0,0 +1,92 @@
|
||||
"""手眼标定·联合求解:从触碰样本同时解 指尖偏移 d 和 标定板位姿 T_base_board,再算 T_base_cam 写回 camera.json。
|
||||
|
||||
模型:食指尖碰第 k 号角点时,指尖基座坐标 = 该角点基座坐标:
|
||||
t_i + R_i·d = R_bb·b_i + t_bb (R_i,t_i = arm_state.pose;b_i = 角点板系坐标 objp[k])
|
||||
未知:R_bb(3) t_bb(3) d(3) = 9。≥4 个不同角点、手腕朝向有变化即可解(朝向不变则 d 不可观)。
|
||||
然后 T_base_cam = T_base_board · inv(T_cam_board),写进 camera.json 外参。
|
||||
|
||||
用法:.venv/bin/python scripts/solve_base_board.py --capture data/real_calib
|
||||
"""
|
||||
import argparse, json
|
||||
from storage import save_json
|
||||
from arm_contract import check_sample_arm
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.optimize import least_squares
|
||||
from scipy.spatial.transform import Rotation as Rot
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
|
||||
|
||||
def residual(x, Rs, ts, bs):
|
||||
R_bb = Rot.from_rotvec(x[0:3]).as_matrix(); t_bb = x[3:6]; d = x[6:9]
|
||||
return np.concatenate([ti + Ri @ d - (R_bb @ bi + t_bb) for Ri, ti, bi in zip(Rs, ts, bs)])
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument("--capture", default="data/real_calib")
|
||||
ap.add_argument("--arm", choices=["left", "right"], default="left")
|
||||
a = ap.parse_args()
|
||||
cap = ROOT / a.capture
|
||||
cam = json.loads((cap / "camera.json").read_text())
|
||||
assert "board_objp" in cam and "T_cam_board" in cam, "camera.json 缺 board_objp/T_cam_board,先跑 handeye_solve.py"
|
||||
objp = np.array(cam["board_objp"], float); T_cb = np.array(cam["T_cam_board"], float)
|
||||
C, R = cam["board_grid"] # 列, 行(8, 5)
|
||||
data = json.loads((cap / "touch_samples.json").read_text())
|
||||
check_sample_arm(data, a.arm)
|
||||
samples = data["samples"]
|
||||
ncor = len({s["corner"] for s in samples})
|
||||
assert ncor >= 4, f"只有 {ncor} 个不同角点,至少要 4 个"
|
||||
|
||||
def target(k):
|
||||
"""锚点 k 的目标板系点 = 其右下方格中心 = (角点 k+1 + 角点 k+C)/2(法兰侧边对准 k+1~k+C 对角线中点)。"""
|
||||
assert k % C < C - 1 and k // C < R - 1, f"角点 {k} 在最右列/最底行、没有右下方格,不能作锚点(用 0~6/8~14/16~22/24~30)"
|
||||
return (objp[k + 1] + objp[k + C]) / 2
|
||||
|
||||
Rs = [Rot.from_quat(s["quat_xyzw"]).as_matrix() for s in samples]
|
||||
ts = [np.array(s["pos"], float) for s in samples]
|
||||
bs = [target(s["corner"]) for s in samples]
|
||||
|
||||
# 朝向变化幅度(d 可观性):样本旋转两两最大夹角
|
||||
spread = 0.0
|
||||
for i in range(len(Rs)):
|
||||
for j in range(i + 1, len(Rs)):
|
||||
ang = np.rad2deg(np.linalg.norm(Rot.from_matrix(Rs[i].T @ Rs[j]).as_rotvec()))
|
||||
spread = max(spread, ang)
|
||||
print(f"样本 {len(samples)} 条 / {ncor} 个角点;手腕朝向最大变化 {spread:.0f}°" +
|
||||
("" if spread > 15 else " ⚠ 朝向变化过小,d 可能解不准——多换几个朝向再碰"))
|
||||
|
||||
# 多 yaw 初值防局部最优
|
||||
best = None
|
||||
for yaw in np.deg2rad([0, 90, 180, 270]):
|
||||
R0 = Rot.from_euler("z", yaw)
|
||||
t0 = np.mean([ti - R0.as_matrix() @ bi for ti, bi in zip(ts, bs)], axis=0)
|
||||
x0 = np.concatenate([R0.as_rotvec(), t0, np.zeros(3)])
|
||||
sol = least_squares(residual, x0, args=(Rs, ts, bs), method="lm")
|
||||
if best is None or sol.cost < best.cost:
|
||||
best = sol
|
||||
x = best.x
|
||||
R_bb = Rot.from_rotvec(x[0:3]).as_matrix(); t_bb = x[3:6]; d = x[6:9]
|
||||
res = residual(x, Rs, ts, bs).reshape(-1, 3)
|
||||
err = np.linalg.norm(res, axis=1) * 1000
|
||||
print(f"拟合残差(每次触碰): 均值 {err.mean():.2f}mm 最大 {err.max():.2f}mm (<3mm 好)")
|
||||
print(f"解出的食指尖偏移 d(Arm_Tip 系, mm) = {(d*1000).round(1).tolist()}")
|
||||
|
||||
T_bb = np.eye(4); T_bb[:3, :3] = R_bb; T_bb[:3, 3] = t_bb
|
||||
T_base_cam = T_bb @ np.linalg.inv(T_cb)
|
||||
cam["T_world_cam_opencv"] = T_base_cam.tolist(); cam["cam_pos_world"] = T_base_cam[:3, 3].tolist()
|
||||
cam["T_base_cam_opencv"] = T_base_cam.tolist()
|
||||
cam["calibration_arm"] = a.arm
|
||||
cam["calibration_pose_topic"] = f"/{a.arm}/arm_state"
|
||||
cam["coordinate_frame"] = "base_link"
|
||||
cam["T_base_board"] = T_bb.tolist(); cam["fingertip_offset_m"] = d.tolist()
|
||||
cam["extrinsic_status"] = f"calibrated(touch joint-solve, {ncor}pts, res {err.mean():.1f}mm)"
|
||||
save_json(cap / "camera.json", cam)
|
||||
print(f"\nT_base_cam(相机在基座系, m):\n{np.array2string(T_base_cam, precision=4, suppress_small=True)}")
|
||||
print(f"相机在基座系位置: {T_base_cam[:3,3].round(3).tolist()} → 已写入 {cap/'camera.json'} 外参(T_world_cam_opencv)")
|
||||
print(f"下一步:bash run.sh validate --arm {a.arm},用同一固定参考点做独立验证(只读,不自动运动)。")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,19 @@
|
||||
"""Timestamped archives plus atomic working files. No hardware access."""
|
||||
from datetime import datetime
|
||||
from zoneinfo import ZoneInfo
|
||||
from pathlib import Path
|
||||
import json,uuid,os
|
||||
|
||||
def stamp():
|
||||
return datetime.now(ZoneInfo('Asia/Shanghai')).strftime('%Y%m%d_%H%M%S_%f')
|
||||
|
||||
def save_json(path,data):
|
||||
path=Path(path);path.parent.mkdir(parents=True,exist_ok=True)
|
||||
if path.exists():
|
||||
archive=path.parent/'history';archive.mkdir(exist_ok=True)
|
||||
archived=archive/f'{path.stem}_{stamp()}_{uuid.uuid4().hex[:6]}{path.suffix}'
|
||||
with archived.open('xb') as f:f.write(path.read_bytes())
|
||||
temp=path.with_name(path.name+'.'+uuid.uuid4().hex+'.tmp')
|
||||
with temp.open('x') as f:
|
||||
json.dump(data,f,indent=2,ensure_ascii=False);f.flush();os.fsync(f.fileno())
|
||||
temp.replace(path)
|
||||
@@ -0,0 +1,79 @@
|
||||
"""标定验证(只读):用已标定的 T_base_board + 法兰偏移 d,预测某锚点目标点的基座坐标,
|
||||
和你实际把法兰对准该锚点后读到的位姿对比,看误差。臂掉使能移动、上使能后运行。
|
||||
|
||||
关键:验证时【手腕朝向换成和标定时不同的姿态】——若 d 解对了,换朝向仍能对上(<1cm);若 d 错了,会露馅。
|
||||
|
||||
用法:.venv/bin/python scripts/validate_touch.py --capture data/real_align
|
||||
交互:法兰对准锚点 k 的黄线(align_lines.png)、上使能后 → 输入 k → 打印预测/实际/误差;done 结束。
|
||||
"""
|
||||
import argparse, json, time
|
||||
from arm_contract import check_calibration_arm
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
import roslibpy
|
||||
from scipy.spatial.transform import Rotation as Rot
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument("--capture", default="data/real_align")
|
||||
ap.add_argument("--arm", choices=["left", "right"], default="left")
|
||||
ap.add_argument("--host", default="192.168.10.21"); ap.add_argument("--port", type=int, default=9090)
|
||||
a = ap.parse_args()
|
||||
cam = json.loads((ROOT / a.capture / "camera.json").read_text())
|
||||
check_calibration_arm(cam, a.arm)
|
||||
print(f"请使用对准图:{ROOT / a.capture / ('align_lines_' + a.arm + '.png')}")
|
||||
T_bb = np.array(cam["T_base_board"], float); d = np.array(cam["fingertip_offset_m"], float)
|
||||
objp = np.array(cam["board_objp"], float); C, R_ = cam["board_grid"]
|
||||
print(f"用标定: 相机基座位置 {np.round(cam['cam_pos_world'],3).tolist()}, 法兰偏移 d(mm) {np.round(d*1000,1).tolist()}")
|
||||
|
||||
def target(k):
|
||||
assert k % C < C - 1 and k // C < R_ - 1, "该角点无右下方格,用 0~6/8~14/16~22/24~30"
|
||||
return (objp[k + 1] + objp[k + C]) / 2
|
||||
|
||||
ros = roslibpy.Ros(host=a.host, port=a.port); ros.run(timeout=8)
|
||||
st = {"m": None, "n": 0}
|
||||
topic = f"/{a.arm}/arm_state"
|
||||
print(f"验证 {a.arm} 臂:{topic}")
|
||||
tp = roslibpy.Topic(ros, topic, "lbot_arm_interfaces/msg/ArmState")
|
||||
tp.subscribe(lambda m: (st.__setitem__("m", m), st.__setitem__("n", st["n"] + 1)))
|
||||
|
||||
def fresh(t=2.0):
|
||||
n0 = st["n"]; t0 = time.time()
|
||||
while st["n"] == n0 and time.time() - t0 < t:
|
||||
time.sleep(0.02)
|
||||
return st["m"] if st["n"] != n0 else None
|
||||
|
||||
if fresh() is None:
|
||||
tp.unsubscribe(); ros.terminate()
|
||||
raise SystemExit(f"收不到 {topic}")
|
||||
print("== 法兰对准锚点 k 的黄线、上使能后输入 k(朝向请换成和标定不同的姿态)。done 结束 ==")
|
||||
errs = []
|
||||
while True:
|
||||
s = input("锚点 k / done: ").strip().lower()
|
||||
if s == "done":
|
||||
break
|
||||
try:
|
||||
k = int(s)
|
||||
except ValueError:
|
||||
continue
|
||||
m = fresh()
|
||||
if m is None:
|
||||
print(" 位姿流卡住,重试"); continue
|
||||
p = m["pose"]["position"]; q = m["pose"]["orientation"]
|
||||
Rm = Rot.from_quat([q["x"], q["y"], q["z"], q["w"]]).as_matrix()
|
||||
actual = np.array([p["x"], p["y"], p["z"]]) + Rm @ d # 实际法兰参考点(基座系)
|
||||
pred = T_bb[:3, :3] @ target(k) + T_bb[:3, 3] # 标定预测该锚点目标点(基座系)
|
||||
e = np.linalg.norm(actual - pred) * 1000
|
||||
errs.append(e)
|
||||
print(f" 角点{k}: 预测={pred.round(4).tolist()} 实际={actual.round(4).tolist()} 误差={e:.1f}mm")
|
||||
tp.unsubscribe(); ros.terminate()
|
||||
if errs:
|
||||
print(f"\n验证 {len(errs)} 点:误差 均值 {np.mean(errs):.1f}mm 最大 {np.max(errs):.1f}mm")
|
||||
print("<10mm 可用;>20mm 说明标定(尤其 d)不准,需重标(大朝向变化)。")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,38 @@
|
||||
{
|
||||
"source": "A7 fixed-camera touch calibration toolkit",
|
||||
"packaged_on": "2026-10-29",
|
||||
"files": [
|
||||
{
|
||||
"file": "scripts/arm_contract.py",
|
||||
"source_sha256": "9f1183cffe71dc4868aed2fcc1dbbfc1d611008540ed1ab463ac1e7c74d8e8f1"
|
||||
},
|
||||
{
|
||||
"file": "scripts/capture_board.py",
|
||||
"source_sha256": "28b7c51f007130cf97771b499e67060d422ca9f608653e1401587ec02b2071a2"
|
||||
},
|
||||
{
|
||||
"file": "scripts/ensure_camera.py",
|
||||
"source_sha256": "fbe490744bff8bd9f4a63667d52304e3915c81a235bb434a225bb35b5546bf4c"
|
||||
},
|
||||
{
|
||||
"file": "scripts/handeye_solve.py",
|
||||
"source_sha256": "62a44a539f45958ff7244c4bc0178d19ffcddaef9bfd219b448b636f8ee18e51"
|
||||
},
|
||||
{
|
||||
"file": "scripts/record_touch.py",
|
||||
"source_sha256": "22f9e04c5c468f74c2d77adc984d71cd99c323c0186eeef75a1a22d84d2289b5"
|
||||
},
|
||||
{
|
||||
"file": "scripts/solve_base_board.py",
|
||||
"source_sha256": "5b9873c1c6e891637f79002b2c97373ceee9988ba0bc5b3745ecb7c739acd815"
|
||||
},
|
||||
{
|
||||
"file": "scripts/storage.py",
|
||||
"source_sha256": "63fe99a68095b572edd14a8c5ff47b116755ebe11760038b092e8714a22a3107"
|
||||
},
|
||||
{
|
||||
"file": "scripts/validate_touch.py",
|
||||
"source_sha256": "e2d5700cd3bc171d53547e4dc7b26c3643b6f762127b9d1e355549c15fbd9826"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,68 @@
|
||||
"""Synthetic geometry checks only; no hardware or ROS nodes."""
|
||||
import json, os, subprocess, sys, tempfile, unittest
|
||||
from pathlib import Path
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation as Rot
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
sys.path.insert(0, str(ROOT/'scripts'))
|
||||
from arm_contract import check_sample_arm, check_calibration_arm
|
||||
from storage import save_json
|
||||
|
||||
class OfflineTests(unittest.TestCase):
|
||||
def test_left_and_right_joint_solve(self):
|
||||
for arm in ('left', 'right'):
|
||||
with self.subTest(arm=arm), tempfile.TemporaryDirectory(prefix='handeye solve ') as td:
|
||||
cap = Path(td)
|
||||
points = np.zeros((40, 3)); points[:, :2] = np.mgrid[0:8, 0:5].T.reshape(-1, 2)*.027
|
||||
Tbb = np.eye(4); Tbb[:3,:3] = Rot.from_euler('xyz', [.2,-.1,.3]).as_matrix(); Tbb[:3,3] = [.4,.2,.05]
|
||||
Tcb = np.eye(4); Tcb[:3,:3] = Rot.from_euler('xyz', [-.1,.2,.3]).as_matrix(); Tcb[:3,3] = [.1,-.2,.5]
|
||||
d = np.array([.02,.01,.11])
|
||||
camera = dict(board_objp=points.tolist(), board_grid=[8,5], T_cam_board=Tcb.tolist())
|
||||
(cap/'camera.json').write_text(json.dumps(camera))
|
||||
samples=[]
|
||||
for i,k in enumerate([0,3,6,8,12,14,16,20,22,24,27,30]):
|
||||
R = Rot.from_euler('xyz', [.13*i, -.07*(i%4), .09*(i%5)])
|
||||
b = (points[k+1]+points[k+8])/2
|
||||
t = Tbb[:3,:3]@b + Tbb[:3,3] - R.as_matrix()@d
|
||||
samples.append(dict(arm=arm,corner=k,pos=t.tolist(),quat_xyzw=R.as_quat().tolist()))
|
||||
(cap/'touch_samples.json').write_text(json.dumps(dict(arm=arm,samples=samples)))
|
||||
subprocess.run([sys.executable,str(ROOT/'scripts/solve_base_board.py'),'--capture',str(cap),'--arm',arm],check=True,capture_output=True,text=True)
|
||||
actual=json.loads((cap/'camera.json').read_text())
|
||||
np.testing.assert_allclose(actual['T_base_cam_opencv'],Tbb@np.linalg.inv(Tcb),atol=1e-6)
|
||||
np.testing.assert_allclose(actual['fingertip_offset_m'],d,atol=1e-6)
|
||||
self.assertEqual(actual['coordinate_frame'],'base_link')
|
||||
self.assertEqual(actual['calibration_arm'],arm)
|
||||
self.assertTrue(list((cap/'history').glob('camera_*.json')))
|
||||
|
||||
def test_cross_arm_data_rejected(self):
|
||||
with self.assertRaises(SystemExit): check_sample_arm({'arm':'right','samples':[]},'left')
|
||||
with self.assertRaises(SystemExit): check_sample_arm({'arm':'left','samples':[{'arm':'right'}]},'left')
|
||||
with self.assertRaises(SystemExit): check_calibration_arm({'calibration_arm':'right'},'left')
|
||||
|
||||
def test_atomic_backup(self):
|
||||
with tempfile.TemporaryDirectory() as td:
|
||||
target=Path(td)/'sample.json'; save_json(target,{'v':1}); save_json(target,{'v':2})
|
||||
self.assertEqual(json.loads(target.read_text()),{'v':2})
|
||||
archive=list((target.parent/'history').glob('sample_*.json'))
|
||||
self.assertEqual(len(archive),1)
|
||||
self.assertEqual(json.loads(archive[0].read_text()),{'v':1})
|
||||
|
||||
def test_board_detection_and_guides(self):
|
||||
with tempfile.TemporaryDirectory(prefix='handeye board ') as td:
|
||||
cap=Path(td)
|
||||
image=np.full((420,600,3),255,np.uint8)
|
||||
for row in range(6):
|
||||
for col in range(9):
|
||||
if (row+col)%2==0:image[30+row*60:90+row*60,30+col*60:90+col*60]=0
|
||||
# PPM is written without importing a second OpenCV into the solve environment.
|
||||
(cap/'rgb.png').write_bytes(b'P6'+bytes([10])+b'600 420'+bytes([10])+b'255'+bytes([10])+image.tobytes())
|
||||
(cap/'intrinsics.txt').write_text('500 500 300 210')
|
||||
(cap/'camera.json').write_text(json.dumps({'D':[0,0,0,0,0]}))
|
||||
command=[os.environ.get('HANDEYE_ROS_PYTHON','/usr/bin/python3'),str(ROOT/'scripts/handeye_solve.py'),'--capture',str(cap),'--cols','8','--rows','5','--square','.027','--arm','right']
|
||||
subprocess.run(command,check=True,capture_output=True,text=True)
|
||||
camera=json.loads((cap/'camera.json').read_text())
|
||||
self.assertEqual(camera['board_grid'],[8,5]); self.assertLess(camera['board_reproj_px'],.5)
|
||||
self.assertTrue((cap/'align_lines_left.png').is_file());self.assertTrue((cap/'align_lines_right.png').is_file())
|
||||
|
||||
if __name__=='__main__':unittest.main()
|
||||
Reference in New Issue
Block a user