FallenSigh
0db694db85
- 新增 camera_to_base.py: 相机坐标系到基坐标系的完整变换 - 支持相机到 TCP 的变换（平移+旋转） - 支持 TCP 到基坐标系的变换 - 水平安装相机的特化实现 - 更新 tools/README.md: 完整的工具使用指南 - 所有工具的详细说明 - 坐标系定义和图示 - 完整工作流示例 - 故障排查指南 - 删除 .udp_control_state.json: 不应提交到版本控制 （运行时生成的状态缓存文件） - 更新 udp_control.py: 代码优化和注释改进
330 lines
11 KiB
Python
330 lines
11 KiB
Python
#!/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())
|