进行机器人操作的研究学习,需要测试机械臂的能力,已经对机械臂进行轨迹规划,对机械臂细粒度的运动轨迹一行一行的写显然是不现实的。根据起始位置和目标位置通过算法进行细粒度的轨迹,通过算法进行轨迹规划涉及轨迹点的密度和位置,具有一定的不确定性。人工拖拽机械臂记录动作然后回放则是相对简单直观的。代码通过python socket 实现。

record.py

import socket
import struct
import time

# UR3 机器人 IP
HOST = "192.168.1.101"
PORT = 30003  # 30003 端口读取实时数据
RECORD_DURATION = 10  # 录制时间(秒)
FREQUENCY = 125  # 采样频率(125Hz)
OUTPUT_FILE = "ur3_recorded_trajectory.txt"
JOINT_ANGLES_OFFSET = 252  # 可能需要调整(尝试 248 或 256)

def recv_exactly(sock, size):
    """ 确保接收 `size` 字节的数据 """
    data = b""
    while len(data) < size:
        packet = sock.recv(size - len(data))
        if not packet:
            return None  # 连接丢失
        data += packet
    return data

def clear_socket_buffer(sock):
    """ 清空 socket 缓冲区,避免读取旧数据 """
    sock.setblocking(False)
    try:
        while sock.recv(1108):
            pass
    except BlockingIOError:
        pass
    sock.setblocking(True)

def get_joint_angles(s):
    """
    通过 UR3 socket 读取当前 6 个关节角度
    """
    clear_socket_buffer(s)  # 清空缓冲区,防止旧数据干扰

    data = recv_exactly(s, 1108)
    if not data:
        return None  # 连接丢失

    try:
        # 解析 6 个 `double` 关节角度
        joint_angles = [struct.unpack('!d', data[JOINT_ANGLES_OFFSET + i * 8: JOINT_ANGLES_OFFSET + (i + 1) * 8])[0] for i in range(6)]
        
        # 允许更大的关节角度范围(UR 机械臂可能返回 -360° ~ +360° 之间的值)
        if not all(-62.8 <= angle <= 62.8 for angle in joint_angles):
            raise ValueError("Invalid joint angles detected!")

        return joint_angles
    except Exception as e:
        print(f"❌ 数据解析错误: {e}")
        return None

def record_trajectory():
    """
    记录 UR3 机械臂的关节轨迹
    """
    try:
        with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
            s.connect((HOST, PORT))
            print("🔴 Connected to UR3. Recording joint angles...")

            start_time = time.perf_counter()
            next_time = start_time  # 控制均匀采样

            with open(OUTPUT_FILE, "w") as file:
                while time.perf_counter() - start_time < RECORD_DURATION:
                    joint_angles = get_joint_angles(s)
                    if joint_angles:
                        timestamp = time.perf_counter() - start_time  # 记录相对时间
                        file.write(f"{timestamp:.6f}, {', '.join(map(str, joint_angles))}\n")
                        print(f"🔹 Recorded: {timestamp:.6f} s - {joint_angles}")

                    # 精准控制采样时间
                    next_time += 1 / FREQUENCY
                    sleep_time = max(0, next_time - time.perf_counter())
                    time.sleep(sleep_time)

            print(f"✅ Recording complete. Data saved in '{OUTPUT_FILE}'.")

    except Exception as e:
        print(f"❌ Error during recording: {e}")

if __name__ == "__main__":
    record_trajectory()

replay.py

import socket
import time
import struct
import math

# UR3 机器人 IP
HOST = "192.168.1.101"
PORT = 30003  # 30003 端口用于实时发送 servoj
FREQUENCY = 125  # 轨迹回放频率(125Hz)
LOOKAHEAD_TIME = 0.2  # 增加 lookahead_time,使运动更平滑
GAIN = 100  # 降低增益,减少振动
MAX_JOINT_SPEED = 1.0  # 关节最大速度(弧度/秒)
INPUT_FILE = "ur3_recorded_trajectory.txt"

def recv_exactly(sock, size):
    """ 确保接收 `size` 字节的数据 """
    data = b""
    while len(data) < size:
        packet = sock.recv(size - len(data))
        if not packet:
            return None  # 连接丢失
        data += packet
    return data

def get_current_joint_angles(s):
    """
    通过 UR3 socket 读取当前 6 个关节角度
    """
    data = recv_exactly(s, 1108)
    if not data:
        return None  # 连接丢失

    try:
        # 解析 6 个 `double` 关节角度(252 字节偏移,每个占 8 字节)
        joint_angles = [struct.unpack('!d', data[252 + i * 8:260 + i * 8])[0] for i in range(6)]
        return joint_angles
    except Exception as e:
        print(f"❌ 解析当前关节角度失败: {e}")
        return None

def parse_trajectory(file_path):
    """
    解析轨迹文件,返回 (时间戳, [6 关节角度]) 列表
    """
    trajectory = []
    try:
        with open(file_path, "r") as file:
            for line in file:
                parts = line.strip().split(", ")
                timestamp = float(parts[0])
                joint_angles = list(map(float, parts[1:]))
                trajectory.append((timestamp, joint_angles))
    except Exception as e:
        print(f"❌ 读取轨迹文件失败: {e}")
    return trajectory

def send_urscript_command(s, command):
    """
    发送 URScript 指令到 UR3
    """
    try:
        s.sendall(command.encode('utf-8'))
    except Exception as e:
        print(f"❌ 发送 URScript 失败: {e}")

def send_servoj_command(s, joint_angles):
    """
    发送 `servoj` 进行高精度轨迹插补
    """
    command = f"servoj([{', '.join(map(str, joint_angles))}], t={1/FREQUENCY}, lookahead_time={LOOKAHEAD_TIME}, gain={GAIN})\n"
    send_urscript_command(s, command)

def move_to_start_position(s, start_angles):
    """
    使用 `servoj` 平滑过渡到 `record` 时的起始位置
    采用 **500Hz 采样 + 七次多项式插值**,避免振动
    """
    current_angles = get_current_joint_angles(s)
    if current_angles is None:
        print("❌ 无法获取当前关节角度")
        return

    # 计算每个关节的偏差
    joint_differences = [abs(curr - start) for curr, start in zip(current_angles, start_angles)]
    
    # 计算最大关节偏差
    max_joint_difference = max(joint_differences)
    
    # **限制最大关节速度**,确保运动更加柔和
    MAX_JOINT_SPEED = 0.3  # **进一步降低最大关节速度,减少晃动**
    
    # **动态计算平滑过渡时间**
    transition_duration = max(3.0, min(max_joint_difference / MAX_JOINT_SPEED, 10.0))  # 限制范围 [3s, 10s]
    
    print(f"🔹 细腻平滑移动到起始位置,过渡时间: {transition_duration:.2f} 秒")

    # **计算步数,确保最小角度步长 ≤ 0.001 rad**
    HIGH_FREQUENCY = 500  # 500Hz 控制频率
    min_steps = max_joint_difference / 0.001  # 确保最小步长 ≤ 0.001 rad
    steps = int(max(min_steps, transition_duration * HIGH_FREQUENCY))  

    # **七次多项式插值(Septic Polynomial Interpolation)**
    for i in range(steps):
        t = i / steps
        smooth_t = t**4 * (35 - 84*t + 70*t**2 - 20*t**3)  # **七次多项式插值**
        interpolated_angles = [(1 - smooth_t) * curr + smooth_t * start for curr, start in zip(current_angles, start_angles)]
        send_servoj_command(s, interpolated_angles)
        time.sleep(1 / HIGH_FREQUENCY)  # **500Hz 控制周期**

    print("✅ 已细腻平滑到达轨迹起始位置。")

def replay_trajectory():
    """
    读取轨迹数据并进行回放
    """
    trajectory_data = parse_trajectory(INPUT_FILE)
    if not trajectory_data:
        print("❌ 无法找到有效的轨迹数据。")
        return

    try:
        with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
            s.connect((HOST, PORT))
            print("🟢 已连接到 UR3,准备回放轨迹...")

            # 1️⃣ 获取录制时的初始位置,并平滑过渡
            start_angles = trajectory_data[0][1]  # 轨迹的第一个关节角度
            move_to_start_position(s, start_angles)  # 使用 servoj 平滑移动到轨迹起始位置

            # 2️⃣ 开始 `servoj` 轨迹回放
            start_time = time.perf_counter()
            for i in range(len(trajectory_data) - 1):
                current_timestamp, joint_angles = trajectory_data[i]
                next_timestamp, _ = trajectory_data[i + 1]

                send_servoj_command(s, joint_angles)

                elapsed = time.perf_counter() - start_time
                sleep_time = max(0, next_timestamp - elapsed)
                time.sleep(sleep_time)

            print("✅ 轨迹回放完成。")

    except Exception as e:
        print(f"❌ 回放轨迹时发生错误: {e}")

if __name__ == "__main__":
    replay_trajectory()

replay部分改为了两段

1. move_to_start

import socket
import time
import struct

# UR3 机器人 IP
HOST = "192.168.1.101"
PORT = 30002  # 30002 端口用于发送 URScript 指令
INPUT_FILE = "ur3_recorded_trajectory.txt"

def recv_exactly(sock, size):
    """ 确保接收 `size` 字节的数据 """
    data = b""
    while len(data) < size:
        packet = sock.recv(size - len(data))
        if not packet:
            return None  # 连接丢失
        data += packet
    return data

def get_current_joint_angles(s):
    """
    通过 UR3 socket 读取当前 6 个关节角度
    """
    data = recv_exactly(s, 1108)
    if not data:
        print("❌ 读取关节角度数据失败")
        return None

    try:
        joint_angles = [struct.unpack('!d', data[252 + i * 8:260 + i * 8])[0] for i in range(6)]
        print(f"🔹 当前关节角度: {joint_angles}")  # **调试信息**
        return joint_angles
    except Exception as e:
        print(f"❌ 解析当前关节角度失败: {e}")
        return None

def parse_trajectory(file_path):
    """
    解析轨迹文件,返回 `record` 轨迹的起始位置
    """
    try:
        with open(file_path, "r") as file:
            first_line = file.readline().strip()
            if not first_line:
                print("❌ 轨迹文件为空!")
                return None
            parts = first_line.split(", ")
            start_angles = list(map(float, parts[1:]))
            print(f"🔹 轨迹起始角度: {start_angles}")  # **调试信息**
            return start_angles
    except Exception as e:
        print(f"❌ 读取轨迹文件失败: {e}")
    return None

def send_urscript_command(s, command):
    """
    发送 URScript 指令到 UR3
    """
    try:
        s.sendall(command.encode('utf-8'))
        print(f"📤 发送 URScript 指令: {command.strip()}")  # **调试信息**
    except Exception as e:
        print(f"❌ 发送 URScript 失败: {e}")

def move_to_start_position():
    """
    让 UR3 **使用 movej** 直接移动到 `record` 轨迹的起始位置
    """
    start_angles = parse_trajectory(INPUT_FILE)
    if start_angles is None:
        print("❌ 无法找到轨迹起始位置")
        return

    try:
        with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
            s.connect((HOST, PORT))
            print("🟢 已连接到 UR3,准备使用 movej 移动到起始位置...")

            movej_command = f"movej([{', '.join(map(str, start_angles))}], a=0.5, v=0.2)\n"
            send_urscript_command(s, movej_command)

            print("⏳ 机械臂正在移动,请等待...")
            time.sleep(5)  # 等待机械臂移动到位(时间可调整)

            print("✅ 机械臂已到达起始位置。")

    except Exception as e:
        print(f"❌ 连接或移动时发生错误: {e}")

if __name__ == "__main__":
    move_to_start_position()

2. replay_trajectory

import socket
import time
import struct

# UR3 机器人 IP
HOST = "192.168.1.101"
# HOST = "172.17.0.2"
PORT = 30003  # 30003 端口用于实时发送 servoj
FREQUENCY = 125  # 轨迹回放频率(125Hz)
LOOKAHEAD_TIME = 0.2  # 增加 lookahead_time,使运动更平滑
GAIN = 100  # 降低增益,减少振动
INPUT_FILE = "ur3_recorded_trajectory.txt"
# INPUT_FILE = "ur3_recorded_trajectory_1.txt"
# INPUT_FILE = "ur3_recorded_trajectory_2.txt"
# INPUT_FILE = "ur3_recorded_trajectory_3.txt"

def recv_exactly(sock, size):
    """ 确保接收 `size` 字节的数据 """
    data = b""
    while len(data) < size:
        packet = sock.recv(size - len(data))
        if not packet:
            return None  # 连接丢失
        data += packet
    return data

def parse_trajectory(file_path):
    """
    解析轨迹文件,返回 (时间戳, [6 关节角度]) 列表
    """
    trajectory = []
    try:
        with open(file_path, "r") as file:
            for line in file:
                parts = line.strip().split(", ")
                timestamp = float(parts[0])
                joint_angles = list(map(float, parts[1:]))
                trajectory.append((timestamp, joint_angles))
    except Exception as e:
        print(f"❌ 读取轨迹文件失败: {e}")
    return trajectory

def send_servoj_command(s, joint_angles):
    """
    发送 `servoj` 进行高精度轨迹插补
    """
    command = f"servoj([{', '.join(map(str, joint_angles))}], t={1/FREQUENCY}, lookahead_time={LOOKAHEAD_TIME}, gain={GAIN})\n"
    s.sendall(command.encode('utf-8'))

def replay_trajectory():
    """
    读取轨迹数据并进行回放
    """
    trajectory_data = parse_trajectory(INPUT_FILE)
    if not trajectory_data:
        print("❌ 无法找到有效的轨迹数据。")
        return

    cycle_time = 0.008  # 8ms per cycle = 125Hz
    next_time = time.time()

    try:
        with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
            s.connect((HOST, PORT))
            print("🟢 已连接到 UR3,开始轨迹回放...")

            # start_time = time.perf_counter()
            for i in range(len(trajectory_data) - 1):
                current_timestamp, joint_angles = trajectory_data[i]
                next_timestamp, _ = trajectory_data[i + 1]

                send_servoj_command(s, joint_angles)

                # elapsed = time.perf_counter() - start_time
                # sleep_time = max(0, next_timestamp - elapsed)
                # time.sleep(sleep_time)
                next_time += cycle_time
                sleep_time = next_time - time.time()
                if sleep_time > 0:
                    time.sleep(sleep_time)
                else:
                    print("Warning: overrun!", sleep_time)

            print("✅ 轨迹回放完成。")

    except Exception as e:
        print(f"❌ 回放轨迹时发生错误: {e}")

if __name__ == "__main__":
    replay_trajectory()

Logo

聚焦RDK与具身智能,汇集机器人开发教程、开源案例和技术交流

更多推荐