采用多线程实现SpeedL控制UR5e机器人

控制UR5e机器人运动的同时接收传感器数据，您可以使用Python的多线程功能来实现并行处理。

import threading
import numpy as np
from rtde_control import RTDEControlInterface as RTDEControl
from rtde_receive import RTDEReceiveInterface as RTDEReceive
import time

# 定义机器人IP地址
UR5E_IP_ADDRESS = "192.168.1.102"

# 初始化控制和接收接口
rtde_c = RTDEControl(UR5E_IP_ADDRESS)
rtde_r = RTDEReceive(UR5E_IP_ADDRESS)

# 设置工具中心点（TCP）位置和方向
TCP_POSITION = [0, 0, 0, 0, 0, 0]  # 根据实际工具的偏移进行调整
rtde_c.setTcp(TCP_POSITION)
rtde_c.zeroFtSensor()

# 运动参数
ACC = 0.5  # 加速度（m/s^2）
TIME_STEP = 0.002  # 时间步长（秒）

# 定义全局变量用于线程间通信
stop_event = threading.Event()
current_pose = None
current_force = None

def control_thread():
    """控制机器人运动的线程。"""
    global stop_event
    while not stop_event.is_set():
        # 定义速度向量（在基坐标系下）
        velocity = [0.1, 0, 0, 0, 0, 0]  # 以0.1 m/s的速度沿X轴移动
        rtde_c.speedL(velocity, ACC, TIME_STEP)
        time.sleep(TIME_STEP)

def receive_thread():
    """接收机器人状态和传感器数据的线程。"""
    global current_pose, current_force, stop_event
    while not stop_event.is_set():
        current_pose = rtde_r.getActualTCPPose()
        current_force = rtde_r.getActualTCPForce()
        print(f"当前位姿: {current_pose}")
        print(f"当前力: {current_force}")
        time.sleep(0.1)  # 每0.1秒获取一次数据

# 创建并启动线程
control_thread_instance = threading.Thread(target=control_thread)
receive_thread_instance = threading.Thread(target=receive_thread)
control_thread_instance.start()
receive_thread_instance.start()

# 运行一段时间后停止
try:
    time.sleep(10)  # 运行10秒
finally:
    stop_event.set()
    control_thread_instance.join()
    receive_thread_instance.join()
    rtde_c.speedStop()  # 停止机器人运动
    rtde_c.stopScript()