Files

FallenSigh 0db694db85 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

6.7 KiB

Raw Blame History

CRAIC 工具脚本

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

工具列表

1. udp_control.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 → 基坐标系
支持相机安装偏移和旋转
水平安装相机的特化实现

使用：

# 基本变换（相机在 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 图像

使用：

# 指定 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 通信

使用：

python udp_server.py

# 指定端口
python udp_server.py --port 9999

测试：

# 终端 1: 启动服务器
python udp_server.py

# 终端 2: 发送测试
echo 'XYW:100:0:0:XZHY' | nc -u 127.0.0.1 8888

坐标系说明

机械臂基坐标系

    Z (上)
    |
    |
    o----→ X (前)
   /
  /
 Y (左)

原点：J1 滑轨底部
单位：mm

相机坐标系（水平安装）

       Y (下)
        |
        |
        o----→ Z (前，光轴)
       /
      /
    X (右)

原点：相机光心
单位：mm

图像坐标系

o----→ u (列，右)
|
|
↓
v (行，下)

原点：图像左上角
单位：像素

完整工作流示例

1. 测试机械臂连接

# 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. 机械臂运动测试

# 测试关节运动
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. 视觉抓取流程

# 步骤 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

这允许连续运动时从上一个位置平滑插值，而不是从原点跳跃。

禁用缓存：

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

依赖

pip install numpy

可选（用于相机采集）：

pip install opencv-python requests

故障排查

1. 无法连接 ESP32

检查：

# 测试网络连接
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 切换肘部解

开发

这些工具是独立的 Python 脚本，不依赖 ROS。可以直接在任何 Python 3 环境中运行。

测试：

# 测试逆运动学
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

6.7 KiB Raw Blame History Unescape Escape