feat(tools): 添加相机坐标变换工具和完善文档
- 新增 camera_to_base.py: 相机坐标系到基坐标系的完整变换 - 支持相机到 TCP 的变换（平移+旋转） - 支持 TCP 到基坐标系的变换 - 水平安装相机的特化实现 - 更新 tools/README.md: 完整的工具使用指南 - 所有工具的详细说明 - 坐标系定义和图示 - 完整工作流示例 - 故障排查指南 - 删除 .udp_control_state.json: 不应提交到版本控制 （运行时生成的状态缓存文件） - 更新 udp_control.py: 代码优化和注释改进
This commit is contained in:
@@ -1,8 +0,0 @@
|
||||
{
|
||||
"height": -280,
|
||||
"j2": -6,
|
||||
"j3": 122,
|
||||
"j4": -80,
|
||||
"j5": -100,
|
||||
"j6": 0
|
||||
}
|
||||
336
tools/README.md
336
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
|
||||
```
|
||||
|
||||
329
tools/camera_to_base.py
Normal file
329
tools/camera_to_base.py
Normal file
@@ -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())
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user