# CRAIC 工具脚本

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

## 工具列表

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

**功能**：
- 关节空间运动控制
- 笛卡尔空间运动控制（自动逆运动学）
- 轨迹插值（平滑运动）
- 状态缓存（连续运动）

**使用**：
```bash
# 关节空间控制
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
```

## 坐标系说明

### 机械臂基坐标系

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

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

### 相机坐标系（水平安装）

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

- 原点：相机光心
- 单位：mm

### 图像坐标系

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

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

## 完整工作流示例

### 1. 测试机械臂连接

```bash
# 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
```