diff --git a/tools/.udp_control_state.json b/tools/.udp_control_state.json deleted file mode 100644 index c42a9cb..0000000 --- a/tools/.udp_control_state.json +++ /dev/null @@ -1,8 +0,0 @@ -{ - "height": -280, - "j2": -6, - "j3": 122, - "j4": -80, - "j5": -100, - "j6": 0 -} \ No newline at end of file diff --git a/tools/README.md b/tools/README.md index fe59ed3..4311b4e 100644 --- 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:::::::0:0:EZHY\n +``` + +- 所有角度已包含零点偏移 +- 高度单位：mm +- 角度单位：度 + +**底盘命令格式**： +``` +XYW::::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 ``` diff --git a/tools/camera_to_base.py b/tools/camera_to_base.py new file mode 100644 index 0000000..ccf3831 --- /dev/null +++ b/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()) diff --git a/tools/udp_control.py b/tools/udp_control.py index 972de9e..73b9c57 100644 --- a/tools/udp_control.py +++ b/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