UR3 机械臂动作的record和replay
·
进行机器人操作的研究学习,需要测试机械臂的能力,已经对机械臂进行轨迹规划,对机械臂细粒度的运动轨迹一行一行的写显然是不现实的。根据起始位置和目标位置通过算法进行细粒度的轨迹,通过算法进行轨迹规划涉及轨迹点的密度和位置,具有一定的不确定性。人工拖拽机械臂记录动作然后回放则是相对简单直观的。代码通过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()
更多推荐
所有评论(0)