feat(tools): 添加相机坐标变换工具和完善文档

- 新增 camera_to_base.py: 相机坐标系到基坐标系的完整变换 - 支持相机到 TCP 的变换（平移+旋转） - 支持 TCP 到基坐标系的变换 - 水平安装相机的特化实现 - 更新 tools/README.md: 完整的工具使用指南 - 所有工具的详细说明 - 坐标系定义和图示 - 完整工作流示例 - 故障排查指南 - 删除 .udp_control_state.json: 不应提交到版本控制（运行时生成的状态缓存文件） - 更新 udp_control.py: 代码优化和注释改进
2026-06-16 19:08:33 +08:00
parent d716ed5e67
commit 0db694db85
4 changed files with 640 additions and 35 deletions
--- a/tools/README.md
+++ b/tools/README.md
@@ -1,43 +1,327 @@
-# tools — 调试 & 测试工具
+# CRAIC 工具脚本

-## 文件
+独立的命令行工具，用于机械臂控制、相机采集和坐标变换。

-| 文件 | 说明 |
-|------|------|
-| `udp_server.py` | UDP 回显服务器，监听 `0.0.0.0:8888`，用于测试 UDP 通讯协议 |
+## 工具列表

-## 使用
+### 1. udp_control.py
+机械臂命令行控制工具（带完整逆运动学）。

-### UDP 回显服务器
-
-启动服务器，接收来自 ESP32 或 ROS 2 键盘遥控节点的 UDP 指令并打印：
+**功能**：
+- 关节空间运动控制
+- 笛卡尔空间运动控制（自动逆运动学）
+- 轨迹插值（平滑运动）
+- 状态缓存（连续运动）

+**使用**：
 ```bash
-python tools/udp_server.py
+# 关节空间控制
+python udp_control.py joints \
+    --height -100 --j2 10 --j3 20 --j4 30 \
+    --duration 2.0 --rate 20
+
+# 笛卡尔空间控制
+python udp_control.py pose \
+    --x 200 --y 100 --z -100 --phi 45 \
+    --duration 2.0
+
+# 抓取动作
+python udp_control.py pose \
+    --x 200 --y 100 --z -150 --phi 45 \
+    --grip --duration 1.0
+
+# 释放动作
+python udp_control.py pose \
+    --x 200 --y 100 --z -50 --phi 45 \
+    --release --duration 1.0
+
+# 干运行（查看命令但不发送）
+python udp_control.py pose --x 200 --y 0 --z -100 --phi 0 --dry-run
+
+# 显示正运动学验证
+python udp_control.py pose --x 200 --y 0 --z -100 --phi 0 --show-fk
 ```

-发送测试指令验证协议格式：
+**参数**：
+- `--ip` - ESP32 IP 地址（默认 192.168.4.1）
+- `--port` - UDP 端口（默认 8888）
+- `--duration` - 运动时长（秒，默认 1.0）
+- `--rate` - 插值频率（Hz，默认 20.0）
+- `--elbow-up` - 肘部朝上解（默认朝下）
+- `--no-state-cache` - 禁用状态缓存

+### 2. camera_to_base.py
+相机坐标系到机械臂基坐标系的变换工具。
+
+**功能**：
+- 完整的坐标变换链：相机 → TCP → 基坐标系
+- 支持相机安装偏移和旋转
+- 水平安装相机的特化实现
+
+**使用**：
 ```bash
-# 底盘指令
+# 基本变换（相机在 TCP 中心）
+python camera_to_base.py \
+    --camera 10 -5 250 \
+    --tcp 200 0 -120 --phi 0
+
+# 考虑相机偏移（相机不在 TCP 中心）
+python camera_to_base.py \
+    --camera 10 -5 250 \
+    --tcp 200 0 -120 --phi 0 \
+    --cam-offset 0 10 50 \
+    --cam-rotation 0 15 0
+```
+
+**参数**：
+- `--camera X Y Z` - 相机坐标系坐标
+- `--tcp X Y Z` - 当前 TCP 位置
+- `--phi` - 当前 TCP 偏航角（度）
+- `--cam-offset TX TY TZ` - 相机到 TCP 的平移
+- `--cam-rotation ROLL PITCH YAW` - 相机到 TCP 的旋转（度）
+
+### 3. camera_capture.py
+ESP32 MJPEG 流采集工具。
+
+**功能**：
+- 自动扫描局域网中的 ESP32 相机
+- 解析 MJPEG 流并提取单帧
+- 保存 JPEG 图像
+
+**使用**：
+```bash
+# 指定 IP
+python camera_capture.py --ip 192.168.4.1
+
+# 自动扫描
+python camera_capture.py --scan
+
+# 指定输出文件
+python camera_capture.py --ip 192.168.4.1 --output frame.jpg
+```
+
+### 4. udp_server.py
+UDP 回显测试服务器。
+
+**功能**：
+- 监听 UDP 端口
+- 打印接收到的所有消息
+- 用于调试 UDP 通信
+
+**使用**：
+```bash
+python udp_server.py
+
+# 指定端口
+python udp_server.py --port 9999
+```
+
+**测试**：
+```bash
+# 终端 1: 启动服务器
+python udp_server.py
+
+# 终端 2: 发送测试
 echo 'XYW:100:0:0:XZHY' | nc -u 127.0.0.1 8888
-
-# 机械臂指令
-echo 'JXB:-10:90:0:0:45:0:0:0:EZHY' | nc -u 127.0.0.1 8888
-
-# 激光指令
-echo 'LASERON' | nc -u 127.0.0.1 8888
 ```

-### 协议测试流程
+## 坐标系说明

-1. 启动 `python tools/udp_server.py`
-2. 修改 ROS 2 节点参数指向本机：`udp_ip:=127.0.0.1`
-3. 运行 ROS 2 键盘遥控节点，观察服务器收到的指令
-4. 确认协议格式正确后，将目标 IP 改为实际设备地址
+### 机械臂基坐标系
+
+```
+    Z (上)
+    |
+    |
+    o----→ X (前)
+   /
+  /
+ Y (左)
+```
+
+- 原点：J1 滑轨底部
+- 单位：mm
+
+### 相机坐标系（水平安装）
+
+```
+       Y (下)
+        |
+        |
+        o----→ Z (前，光轴)
+       /
+      /
+    X (右)
+```
+
+- 原点：相机光心
+- 单位：mm
+
+### 图像坐标系
+
+```
+o----→ u (列，右)
+|
+|
+↓
+v (行，下)
+```
+
+- 原点：图像左上角
+- 单位：像素
+
+## 完整工作流示例
+
+### 1. 测试机械臂连接

 ```bash
-# 示例：ROS 2 键盘遥控 → 本地回显
-ros2 run udp_teleop keyboard_control \
-    --ros-args -p udp_ip:=127.0.0.1 -p udp_port:=8888
+# 1. 启动回显服务器
+python udp_server.py
+
+# 2. 测试 UDP（新终端）
+echo 'JXB:0:0:0:0:81:30:0:0:EZHY' | nc -u 127.0.0.1 8888
+
+# 3. 验证服务器收到消息
+```
+
+### 2. 机械臂运动测试
+
+```bash
+# 测试关节运动
+python udp_control.py joints --height -50 --j2 10 --j3 20 --j4 30 --duration 2.0
+
+# 测试笛卡尔运动
+python udp_control.py pose --x 200 --y 100 --z -100 --phi 45 --duration 2.0
+
+# 测试抓取
+python udp_control.py pose --x 200 --y 100 --z -150 --phi 45 --grip --duration 1.0
+```
+
+### 3. 视觉抓取流程
+
+```bash
+# 步骤 1: 采集图像
+python camera_capture.py --ip 192.168.4.1 --output target.jpg
+
+# 步骤 2: 检测物体（假设得到像素坐标和宽度）
+# u=320, v=240, pixel_width=50
+# 使用相似三角形计算深度：depth = (real_width * focal_length) / pixel_width
+# 转换到相机坐标：x_cam, y_cam, z_cam
+
+# 步骤 3: 获取当前 TCP 位姿（从 ROS 或状态文件）
+# tcp_x=200, tcp_y=0, tcp_z=-120, phi=0
+
+# 步骤 4: 坐标变换
+python camera_to_base.py \
+    --camera 10 -5 250 \
+    --tcp 200 0 -120 --phi 0
+
+# 步骤 5: 移动到目标
+python udp_control.py pose --x 323.5 --y 229.6 --z -108.6 --phi 0 --duration 3.0
+
+# 步骤 6: 抓取
+python udp_control.py pose --x 323.5 --y 229.6 --z -158.6 --phi 0 --grip --duration 1.0
+
+# 步骤 7: 回收
+python udp_control.py pose --x 200 --y 0 --z -158.6 --phi 0 --duration 2.0
+```
+
+## 状态缓存
+
+`udp_control.py` 会将最后发送的关节状态保存到：
+```
+~/.ros/udp_control_state.json
+```
+
+这允许连续运动时从上一个位置平滑插值，而不是从原点跳跃。
+
+**禁用缓存**：
+```bash
+python udp_control.py pose --x 200 --y 0 --z -100 --phi 0 --no-state-cache
+```
+
+## UDP 协议
+
+所有工具通过 UDP 端口 8888 与 ESP32 通信。
+
+**机械臂命令格式**：
+```
+JXB:<height>:<J2>:<J3>:<J4>:<J5>:<J6>:0:0:EZHY\n
+```
+
+- 所有角度已包含零点偏移
+- 高度单位：mm
+- 角度单位：度
+
+**底盘命令格式**：
+```
+XYW:<X>:<Y>:<W>:XZHY\n
+```
+
+## 依赖
+
+```bash
+pip install numpy
+```
+
+可选（用于相机采集）：
+```bash
+pip install opencv-python requests
+```
+
+## 故障排查
+
+### 1. 无法连接 ESP32
+
+**检查**：
+```bash
+# 测试网络连接
+ping 192.168.4.1
+
+# 测试 UDP 端口
+echo 'test' | nc -u 192.168.4.1 8888
+```
+
+### 2. 逆运动学失败
+
+**错误**：`目标超出工作空间`
+
+**原因**：目标位置超出机械臂可达范围
+
+**解决**：
+- 检查目标是否在圆环内（半径 110-360mm）
+- 检查高度是否在范围内（-370 到 -80mm）
+
+### 3. 运动不平滑
+
+**解决**：
+- 增加 `--duration`（运动时间）
+- 增加 `--rate`（插值频率）
+- 确保状态缓存已启用
+
+### 4. 关节角度超限
+
+**错误**：`J2=-150 超出范围 [-110, 115]`
+
+**解决**：
+- 检查目标位姿是否合理
+- 尝试 `--elbow-up` 切换肘部解
+
+## 相关文档
+
+- **运动学推导**：`docs/arm.md` - 完整数学推导
+- **ROS 节点**：`ros2/README.md` - ROS 2 集成
+- **视觉标定**：`docs/vision_calibration_horizontal.md` - 相机标定
+
+## 开发
+
+这些工具是独立的 Python 脚本，不依赖 ROS。可以直接在任何 Python 3 环境中运行。
+
+**测试**：
+```bash
+# 测试逆运动学
+python udp_control.py pose --x 200 --y 0 --z -100 --phi 0 --dry-run --show-fk
+
+# 测试坐标变换
+python camera_to_base.py --camera 0 0 300 --tcp 200 0 -120 --phi 0
 ```