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

- 新增 camera_to_base.py: 相机坐标系到基坐标系的完整变换
  - 支持相机到 TCP 的变换（平移+旋转）
  - 支持 TCP 到基坐标系的变换
  - 水平安装相机的特化实现

- 更新 tools/README.md: 完整的工具使用指南
  - 所有工具的详细说明
  - 坐标系定义和图示
  - 完整工作流示例
  - 故障排查指南

- 删除 .udp_control_state.json: 不应提交到版本控制
  （运行时生成的状态缓存文件）

- 更新 udp_control.py: 代码优化和注释改进

tools/.udp_control_state.json

@@ -1,8 +0,0 @@
-{
-  "height": -280,
-  "j2": -6,
-  "j3": 122,
-  "j4": -80,
-  "j5": -100,
-  "j6": 0
-}

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

tools/camera_to_base.py

@@ -0,0 +1,329 @@
+#!/usr/bin/env python3
+"""相机坐标系到机械臂基坐标系的直接变换工具。
+
+使用场景：
+1. 已经通过其他方式获得了相机坐标系坐标
+2. 测试和验证坐标变换算法
+3. 调试机械臂控制逻辑
+
+输入：
+    - 相机坐标系坐标 (xc, yc, zc)，单位 mm
+    - 当前 TCP 位姿 (x, y, z, phi)
+
+输出：
+    - 基坐标系坐标 (xb, yb, zb)，单位 mm
+"""
+
+import argparse
+import math
+import sys
+import json
+from pathlib import Path
+from typing import Tuple
+
+import numpy as np
+
+
+def euler_to_rotation_matrix(roll_deg: float, pitch_deg: float, yaw_deg: float) -> np.ndarray:
+    """欧拉角转旋转矩阵（ZYX顺序）"""
+    roll = math.radians(roll_deg)
+    pitch = math.radians(pitch_deg)
+    yaw = math.radians(yaw_deg)
+
+    Rx = np.array([
+        [1, 0, 0],
+        [0, math.cos(roll), -math.sin(roll)],
+        [0, math.sin(roll), math.cos(roll)]
+    ])
+
+    Ry = np.array([
+        [math.cos(pitch), 0, math.sin(pitch)],
+        [0, 1, 0],
+        [-math.sin(pitch), 0, math.cos(pitch)]
+    ])
+
+    Rz = np.array([
+        [math.cos(yaw), -math.sin(yaw), 0],
+        [math.sin(yaw), math.cos(yaw), 0],
+        [0, 0, 1]
+    ])
+
+    return Rz @ Ry @ Rx
+
+
+def camera_to_tcp(
+    xc: float, yc: float, zc: float,
+    tx: float = 0.0, ty: float = 0.0, tz: float = 0.0,
+    roll: float = 0.0, pitch: float = 0.0, yaw: float = 0.0
+) -> Tuple[float, float, float]:
+    """相机坐标系 → TCP 坐标系
+
+    Args:
+        xc, yc, zc: 相机坐标系坐标 (mm)
+        tx, ty, tz: 相机到 TCP 的平移 (mm)
+        roll, pitch, yaw: 相机到 TCP 的旋转 (度)
+
+    Returns:
+        (xt, yt, zt): TCP 坐标系坐标 (mm)
+    """
+    R = euler_to_rotation_matrix(roll, pitch, yaw)
+    T = np.array([tx, ty, tz])
+    P_cam = np.array([xc, yc, zc])
+    P_tcp = R @ P_cam + T
+    return float(P_tcp[0]), float(P_tcp[1]), float(P_tcp[2])
+
+
+def tcp_to_base(
+    xt: float, yt: float, zt: float,
+    tcp_x: float, tcp_y: float, tcp_z: float, tcp_phi_deg: float
+) -> Tuple[float, float, float]:
+    """TCP 坐标系 → 机械臂基坐标系（水平相机版本）
+
+    坐标映射：
+        - TCP X (相机右) → 基坐标系 垂直于 phi 方向
+        - TCP Y (相机下) → 基坐标系 -Z 方向（向下）
+        - TCP Z (相机前) → 基坐标系 phi 方向
+
+    Args:
+        xt, yt, zt: TCP 坐标系坐标 (mm)
+        tcp_x, tcp_y, tcp_z: TCP 当前位置 (mm)
+        tcp_phi_deg: TCP 当前朝向角 (度)
+
+    Returns:
+        (xb, yb, zb): 基坐标系坐标 (mm)
+    """
+    phi = math.radians(tcp_phi_deg)
+
+    # 旋转矩阵：TCP → 基坐标系
+    R_tcp_to_base = np.array([
+        [-math.sin(phi), 0, math.cos(phi)],
+        [ math.cos(phi), 0, math.sin(phi)],
+        [0,             -1, 0]
+    ])
+
+    P_tcp = np.array([xt, yt, zt])
+    P_base_relative = R_tcp_to_base @ P_tcp
+
+    P_base = P_base_relative + np.array([tcp_x, tcp_y, tcp_z])
+
+    return float(P_base[0]), float(P_base[1]), float(P_base[2])
+
+
+def camera_to_base(
+    xc: float, yc: float, zc: float,
+    tcp_x: float, tcp_y: float, tcp_z: float, tcp_phi_deg: float,
+    cam_tx: float = 0.0, cam_ty: float = 0.0, cam_tz: float = 0.0,
+    cam_roll: float = 0.0, cam_pitch: float = 0.0, cam_yaw: float = 0.0
+) -> Tuple[float, float, float]:
+    """完整变换：相机坐标系 → 基坐标系
+
+    Args:
+        xc, yc, zc: 相机坐标系坐标 (mm)
+        tcp_x, tcp_y, tcp_z, tcp_phi_deg: TCP 当前位姿
+        cam_tx, cam_ty, cam_tz: 相机到 TCP 的平移
+        cam_roll, cam_pitch, cam_yaw: 相机到 TCP 的旋转
+
+    Returns:
+        (xb, yb, zb): 基坐标系坐标 (mm)
+    """
+    # 步骤 1: 相机 → TCP
+    xt, yt, zt = camera_to_tcp(xc, yc, zc, cam_tx, cam_ty, cam_tz,
+                                cam_roll, cam_pitch, cam_yaw)
+
+    # 步骤 2: TCP → 基坐标系
+    xb, yb, zb = tcp_to_base(xt, yt, zt, tcp_x, tcp_y, tcp_z, tcp_phi_deg)
+
+    return xb, yb, zb
+
+
+def load_tcp_pose_from_state() -> Tuple[float, float, float, float]:
+    """从状态缓存文件读取当前 TCP 位姿"""
+    state_file = Path("tools/.udp_control_state.json")
+
+    if not state_file.exists():
+        raise FileNotFoundError(
+            f"状态文件不存在: {state_file}\n"
+            "请先运行一次 udp_control.py 以初始化状态"
+        )
+
+    state = json.loads(state_file.read_text())
+
+    # 导入必要的模块计算正运动学
+    sys.path.insert(0, str(Path(__file__).parent))
+    from udp_control import (
+        forward_kinematics,
+        ArmGeometry,
+        ArmMathState,
+        DEFAULT_L1, DEFAULT_L2, DEFAULT_X4, DEFAULT_Z4,
+        DEFAULT_ZERO_J2, DEFAULT_ZERO_J3, DEFAULT_ZERO_J4
+    )
+
+    geometry = ArmGeometry(l1=DEFAULT_L1, l2=DEFAULT_L2, x4=DEFAULT_X4, z4=DEFAULT_Z4)
+    math_state = ArmMathState(
+        d1=state["height"],
+        theta2_deg=state["j2"] - DEFAULT_ZERO_J2,
+        theta3_deg=state["j3"] - DEFAULT_ZERO_J3,
+        theta4_deg=state["j4"] - DEFAULT_ZERO_J4,
+    )
+
+    pose = forward_kinematics(geometry, math_state)
+    return pose.x, pose.y, pose.z, pose.phi_deg
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser(
+        description="相机坐标系到机械臂基坐标系的直接变换",
+        formatter_class=argparse.RawDescriptionHelpFormatter,
+        epilog="""
+示例用法:
+
+  1. 基本用法（手动指定 TCP 位姿）:
+     python %(prog)s --xc 10 --yc 5 --zc 250 \\
+         --tcp-x 150 --tcp-y 50 --tcp-z -100 --tcp-phi 45
+
+  2. 自动读取 TCP 位姿（从状态缓存）:
+     python %(prog)s --xc 10 --yc 5 --zc 250 --auto-tcp
+
+  3. 包含相机到 TCP 的变换:
+     python %(prog)s --xc 10 --yc 5 --zc 250 --auto-tcp \\
+         --cam-tx 20 --cam-ty 10 --cam-tz 5
+
+相机坐标系定义（水平安装）:
+  Xc: 向右
+  Yc: 向下
+  Zc: 向前（水平，光轴方向）
+
+机械臂基坐标系定义:
+  X, Y: 水平面
+  Z: 向上
+        """
+    )
+
+    # 相机坐标系输入
+    parser.add_argument("--xc", type=float, required=True,
+                       help="相机坐标系 X 坐标（向右，mm）")
+    parser.add_argument("--yc", type=float, required=True,
+                       help="相机坐标系 Y 坐标（向下，mm）")
+    parser.add_argument("--zc", type=float, required=True,
+                       help="相机坐标系 Z 坐标（向前，mm）")
+
+    # TCP 位姿
+    tcp_group = parser.add_mutually_exclusive_group(required=True)
+    tcp_group.add_argument("--auto-tcp", action="store_true",
+                          help="自动从状态缓存读取当前 TCP 位姿")
+    tcp_group.add_argument("--tcp-manual", action="store_true",
+                          help="手动指定 TCP 位姿（需要 --tcp-x/y/z/phi）")
+
+    parser.add_argument("--tcp-x", type=float,
+                       help="TCP X 坐标（mm）")
+    parser.add_argument("--tcp-y", type=float,
+                       help="TCP Y 坐标（mm）")
+    parser.add_argument("--tcp-z", type=float,
+                       help="TCP Z 坐标（mm）")
+    parser.add_argument("--tcp-phi", type=float,
+                       help="TCP 朝向角（度）")
+
+    # 相机到 TCP 的变换（可选）
+    parser.add_argument("--cam-tx", type=float, default=0.0,
+                       help="相机到 TCP 平移 X（mm，默认 0）")
+    parser.add_argument("--cam-ty", type=float, default=0.0,
+                       help="相机到 TCP 平移 Y（mm，默认 0）")
+    parser.add_argument("--cam-tz", type=float, default=0.0,
+                       help="相机到 TCP 平移 Z（mm，默认 0）")
+    parser.add_argument("--cam-roll", type=float, default=0.0,
+                       help="相机到 TCP 旋转 roll（度，默认 0）")
+    parser.add_argument("--cam-pitch", type=float, default=0.0,
+                       help="相机到 TCP 旋转 pitch（度，默认 0）")
+    parser.add_argument("--cam-yaw", type=float, default=0.0,
+                       help="相机到 TCP 旋转 yaw（度，默认 0）")
+
+    # 输出格式
+    parser.add_argument("--json", action="store_true",
+                       help="以 JSON 格式输出")
+    parser.add_argument("--quiet", action="store_true",
+                       help="只输出坐标，不输出说明信息")
+
+    args = parser.parse_args()
+
+    # 获取 TCP 位姿
+    if args.auto_tcp:
+        try:
+            tcp_x, tcp_y, tcp_z, tcp_phi = load_tcp_pose_from_state()
+            if not args.quiet:
+                print(f"从状态缓存读取 TCP 位姿: "
+                      f"({tcp_x:.3f}, {tcp_y:.3f}, {tcp_z:.3f}), phi={tcp_phi:.3f}°")
+        except Exception as e:
+            print(f"错误: 无法读取 TCP 位姿: {e}", file=sys.stderr)
+            return 1
+    else:
+        if not all([args.tcp_x is not None, args.tcp_y is not None,
+                   args.tcp_z is not None, args.tcp_phi is not None]):
+            print("错误: 使用 --tcp-manual 时必须指定 --tcp-x, --tcp-y, --tcp-z, --tcp-phi",
+                  file=sys.stderr)
+            return 1
+        tcp_x, tcp_y, tcp_z, tcp_phi = args.tcp_x, args.tcp_y, args.tcp_z, args.tcp_phi
+
+    # 执行变换
+    try:
+        xb, yb, zb = camera_to_base(
+            args.xc, args.yc, args.zc,
+            tcp_x, tcp_y, tcp_z, tcp_phi,
+            args.cam_tx, args.cam_ty, args.cam_tz,
+            args.cam_roll, args.cam_pitch, args.cam_yaw
+        )
+
+        # 输出结果
+        if args.json:
+            result = {
+                "input": {
+                    "camera": {"x": args.xc, "y": args.yc, "z": args.zc},
+                    "tcp": {"x": tcp_x, "y": tcp_y, "z": tcp_z, "phi": tcp_phi}
+                },
+                "output": {
+                    "base": {"x": xb, "y": yb, "z": zb}
+                }
+            }
+            print(json.dumps(result, indent=2))
+        elif args.quiet:
+            print(f"{xb:.3f} {yb:.3f} {zb:.3f}")
+        else:
+            print("=" * 70)
+            print("相机坐标系到基坐标系变换")
+            print("=" * 70)
+            print(f"\n输入（相机坐标系）:")
+            print(f"  X_cam = {args.xc:8.3f} mm  (向右)")
+            print(f"  Y_cam = {args.yc:8.3f} mm  (向下)")
+            print(f"  Z_cam = {args.zc:8.3f} mm  (向前，光轴)")
+
+            print(f"\nTCP 位姿:")
+            print(f"  X_tcp = {tcp_x:8.3f} mm")
+            print(f"  Y_tcp = {tcp_y:8.3f} mm")
+            print(f"  Z_tcp = {tcp_z:8.3f} mm")
+            print(f"  φ_tcp = {tcp_phi:8.3f} °")
+
+            if any([args.cam_tx, args.cam_ty, args.cam_tz,
+                   args.cam_roll, args.cam_pitch, args.cam_yaw]):
+                print(f"\n相机到 TCP 变换:")
+                print(f"  平移: ({args.cam_tx:.3f}, {args.cam_ty:.3f}, {args.cam_tz:.3f}) mm")
+                print(f"  旋转: roll={args.cam_roll:.3f}°, pitch={args.cam_pitch:.3f}°, yaw={args.cam_yaw:.3f}°")
+
+            print(f"\n输出（机械臂基坐标系）:")
+            print(f"  X_base = {xb:8.3f} mm")
+            print(f"  Y_base = {yb:8.3f} mm")
+            print(f"  Z_base = {zb:8.3f} mm")
+            print("=" * 70)
+
+            print(f"\n使用此坐标控制机械臂:")
+            print(f"  python tools/udp_control.py pose \\")
+            print(f"    --x {xb:.1f} --y {yb:.1f} --z {zb:.1f} \\")
+            print(f"    --phi {tcp_phi:.1f}")
+
+        return 0
+
+    except Exception as e:
+        print(f"错误: {e}", file=sys.stderr)
+        return 1
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

tools/udp_control.py

@@ -45,7 +45,7 @@ DEFAULT_FIXED_J5 = J5_OPEN
 # ==== 抓取/释放（由 J6 控制）====
 # 请填写实际角度值：
 GRIP_ANGLE = -5      # TODO: 填写抓取时 J6 的角度
-RELEASE_ANGLE = 30   # TODO: 填写释放时 J6 的角度
+RELEASE_ANGLE = 80   # TODO: 填写释放时 J6 的角度
 DEFAULT_FIXED_J6 = RELEASE_ANGLE
 DEFAULT_ZERO_J2 = 3
 DEFAULT_ZERO_J3 = 7