record.py

from rtde_control import RTDEControlInterface
from rtde_receive import RTDEReceiveInterface
import time
import math
import os
from datetime import datetime

# ====== 配置信息 ======
ROBOT_HOST = "192.168.1.14"   # UR机械臂的IP地址
RTDE_PORT = 8000             # 默认RTDE端口
DATA_DIR = "video2"            # 数据保存目录

# 创建数据目录（如果不存在）
os.makedirs(DATA_DIR, exist_ok=True)

# 生成包含当前时间的文件名（年月日_时分秒）
current_time_str = datetime.now().strftime("%Y%m%d_%H%M%S")
OUTPUT_FILE = f"{DATA_DIR}/robot_pose_data_{current_time_str}.txt"

print("连接中 ")

# ====== 初始化接口 ======
rtde_r = RTDEReceiveInterface(ROBOT_HOST, RTDE_PORT)

# 打开文件并写入列名（包含时间）
with open(OUTPUT_FILE, 'w') as f:
    f.write("time,x,y,z,rx,ry,rz\n")  # 写入包含时间的列名

print("连接成功，开始实时读取末端位姿数据并保存到文件...")
print(f"数据将保存到: {OUTPUT_FILE}")

try:
    while True:
        # 获取当前时间（格式：年-月-日 时:分:秒.毫秒）
        current_time = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
        
        # 获取当前末端位姿 [x, y, z, Rx, Ry, Rz]
        tcp_pose = rtde_r.getActualTCPPose()

        # 转换单位和计算（根据你的校准公式）
        x = tcp_pose[0] * 1000  # 转换为毫米
        y = tcp_pose[1] * 1000
        z =  tcp_pose[2] * 1000
        rx =  tcp_pose[3]
        ry =  tcp_pose[4]
        rz =  tcp_pose[5]

        # 输出格式化结果
        print(f"[{current_time}] Position: x={x:.2f}mm, y={y:.2f}mm, z={z:.2f}mm")
        print(f"[{current_time}] Orientation (Rx, Ry, Rz): {rx:.3f}, {ry:.3f}, {rz:.3f}")
        print("-" * 80)

        # 将数据写入文件（包含时间戳）
        with open(OUTPUT_FILE, 'a') as f:
            f.write(f"{current_time},{x:.4f},{y:.4f},{z:.4f},{rx:.4f},{ry:.4f},{rz:.4f}\n")

        time.sleep(0.1)  # 每100ms刷新一次

except KeyboardInterrupt:
    print("读取已停止。")
    print(f"数据已保存到 {OUTPUT_FILE}")