491 lines
18 KiB
Python
491 lines
18 KiB
Python
# ==================== 五连杆机械臂轨迹控制（直接角度发送版）====================
|
||
# 功能：轨迹生成 + 逆解 + 直接控制 + 动画显示 + 关节角度可视化 + FK 验证
|
||
# ==============================================================================
|
||
|
||
import time
|
||
import numpy as np
|
||
import matplotlib.pyplot as plt
|
||
from matplotlib.animation import FuncAnimation
|
||
from time import perf_counter # 高精度计时
|
||
|
||
# 设置中文字体和负号显示
|
||
plt.rcParams['font.sans-serif'] = ['SimHei', 'WenQuanYi Zen Hei', 'FangSong']
|
||
plt.rcParams['axes.unicode_minus'] = False
|
||
|
||
# ------------------------ 调试开关 ------------------------
|
||
DEBUG_MODE = False # <<< 设为 False 控制真实电机
|
||
|
||
# 导入运动学和轨迹函数（确保路径正确）
|
||
try:
|
||
from calculate.ik import inverseF
|
||
from calculate.fk import forwardF
|
||
from calculate.trajectory import (
|
||
circle_trajectory,
|
||
line_trajectory,
|
||
line_trajectory_fix,
|
||
ellipse_trajectory,
|
||
square_trajectory,
|
||
triangle_trajectory
|
||
)
|
||
except ImportError as e:
|
||
print(f"【警告】无法导入运动学模块: {e}, 使用 DEBUG_MODE")
|
||
DEBUG_MODE = True
|
||
|
||
# -------------------- 非 Debug 模式导入硬件库 --------------------
|
||
if not DEBUG_MODE:
|
||
try:
|
||
from DM_CAN.DM_CAN import Motor, MotorControl, DM_Motor_Type, Control_Type
|
||
import serial
|
||
except ImportError as e:
|
||
print(f"硬件库导入失败: {e}")
|
||
exit(1)
|
||
else:
|
||
print("【DEBUG MODE】: 已启用调试模式，不连接真实硬件。")
|
||
|
||
# ------------------------ 机械臂参数 ------------------------
|
||
L1 = 250 # 左臂长度 (mm)
|
||
L2 = 300
|
||
L3 = 300
|
||
L4 = 250 # 右臂长度
|
||
L0 = 250 # 基座右端偏移
|
||
|
||
# ------------------------ 电机与通信配置 ------------------------
|
||
MOTOR1_ID = 0x01
|
||
MOTOR2_ID = 0x02
|
||
CAN_SERIAL_PORT = '/dev/ttyACM0'
|
||
BAUD_RATE = 921600
|
||
KP = 50.0
|
||
KD = 1.0
|
||
DT = 0.01# 控制周期 10ms（每点延时）
|
||
|
||
# ------------------------ 全局变量 ------------------------
|
||
motor1 = motor2 = motor_control = None
|
||
current_theta1 = current_theta4 = 0.0
|
||
x_list = y_list = [] # 轨迹点
|
||
line = None
|
||
|
||
# ------------------------ 精确延时 ------------------------
|
||
def busy_wait(dt):
|
||
"""高精度延时"""
|
||
start = perf_counter()
|
||
while perf_counter() - start < dt:
|
||
pass
|
||
|
||
# ------------------------ 角度连续性调整 ------------------------
|
||
def adjust_angle_continuity(new_angle, prev_angle):
|
||
"""
|
||
防止角度跳变（如 3.14 → -3.14），保持连续
|
||
"""
|
||
diff = new_angle - prev_angle
|
||
while diff > np.pi:
|
||
diff -= 2 * np.pi
|
||
while diff < -np.pi:
|
||
diff += 2 * np.pi
|
||
return prev_angle + diff
|
||
|
||
# ------------------------ 初始化电机 ------------------------
|
||
def init_motors():
|
||
global motor1, motor2, motor_control
|
||
|
||
if DEBUG_MODE:
|
||
print("【DEBUG】跳过电机初始化")
|
||
motor1 = motor2 = type('Motor', (), {'id': 0})()
|
||
motor_control = type('MotorControl', (), {
|
||
'enable': lambda x: True,
|
||
'disable': lambda x: None,
|
||
'controlMIT': lambda m, kp, kd, pos, vel, torq: None,
|
||
'refresh_motor_status': lambda m: None,
|
||
'switchControlMode': lambda m, mode: None,
|
||
'save_motor_param': lambda m: None
|
||
})()
|
||
return motor1, motor2, motor_control
|
||
|
||
try:
|
||
can_serial = serial.Serial(CAN_SERIAL_PORT, BAUD_RATE, timeout=0.5)
|
||
print(f"CAN 串口 {CAN_SERIAL_PORT} 打开成功")
|
||
except Exception as e:
|
||
print(f"无法打开串口: {e}")
|
||
exit(1)
|
||
|
||
motor_control = MotorControl(can_serial)
|
||
motor1 = Motor(DM_Motor_Type.DM4310, MOTOR1_ID, 0x11)
|
||
motor2 = Motor(DM_Motor_Type.DM4310, MOTOR2_ID, 0x12)
|
||
|
||
motor_control.addMotor(motor1)
|
||
motor_control.addMotor(motor2)
|
||
motor_control.switchControlMode(motor1, Control_Type.MIT)
|
||
motor_control.switchControlMode(motor2, Control_Type.MIT)
|
||
time.sleep(0.1)
|
||
motor_control.save_motor_param(motor1)
|
||
motor_control.save_motor_param(motor2)
|
||
motor_control.enable(motor1)
|
||
motor_control.enable(motor2)
|
||
print("电机已使能。")
|
||
return motor1, motor2, motor_control
|
||
|
||
# 电机1、4安全角度范围（弧度）
|
||
THETA1_MIN = -np.pi/2 # -60°
|
||
THETA1_MAX = np.pi/2 # 60°
|
||
THETA4_MIN = -np.pi/2 # -60°
|
||
THETA4_MAX = np.pi/2 # 60°
|
||
|
||
# ------------------------ MIT 控制函数（带限位 + 回零逻辑） ------------------------
|
||
def control_two_motors_mit(theta1_rad, theta4_rad):
|
||
"""
|
||
发送 MIT 控制指令（角度单位：弧度）
|
||
带限位检测：如果超限，自动触发回零。
|
||
"""
|
||
global current_theta1, current_theta4
|
||
|
||
# --- 限位检测 ---
|
||
if not (THETA1_MIN <= theta1_rad <= THETA1_MAX):
|
||
print(f"θ1 超出限位 [{THETA1_MIN}, {THETA1_MAX}]，执行回零...")
|
||
move_motors_to_zero()
|
||
return
|
||
|
||
if not (THETA4_MIN <= theta4_rad <= THETA4_MAX):
|
||
print(f"θ4 超出限位 [{THETA4_MIN}, {THETA4_MAX}]，执行回零...")
|
||
move_motors_to_zero()
|
||
return
|
||
|
||
# --- 正常控制 ---
|
||
pos1 = theta1_rad
|
||
pos4 = theta4_rad
|
||
vel = 0.1
|
||
torq = 0.0
|
||
|
||
if not DEBUG_MODE:
|
||
motor_control.controlMIT(motor1, KP, KD, pos1, vel, torq)
|
||
motor_control.controlMIT(motor2, KP, KD, pos4, vel, torq)
|
||
else:
|
||
print(f"[DEBUG] 控制 -> θ1={np.degrees(theta1_rad):.2f}°, θ4={np.degrees(theta4_rad):.2f}°")
|
||
|
||
current_theta1 = theta1_rad
|
||
current_theta4 = theta4_rad
|
||
|
||
|
||
# ------------------------ 直接轨迹执行函数（无插值）------------------------
|
||
from time import perf_counter
|
||
import numpy as np
|
||
|
||
|
||
# 假设其他所需函数和全局变量已定义，例如 inverseF, forwardF, adjust_angle_continuity, control_two_motors_mit, 等等.
|
||
|
||
def execute_direct_trajectory(x_list, y_list, dt=DT):
|
||
"""
|
||
直接执行轨迹：对每个 (x,y) 计算逆解并立即发送角度，不做插值或速度规划
|
||
执行前：先把电机从零点平滑移动到轨迹起点
|
||
返回: (time_log, theta1_log, theta4_log, x_fk_log, y_fk_log)
|
||
"""
|
||
global motor_control, motor1, motor2
|
||
|
||
ANGLE_OFFSET_RAD = np.pi / 2 # 90° 偏移量
|
||
|
||
time_log, theta1_log, theta4_log, x_fk_log, y_fk_log = [], [], [], [], []
|
||
l1, l2, l3, l4, l5 = L1, L2, L3, L4, L0
|
||
omega1 = omega4 = 0.0
|
||
alpha1 = alpha4 = 0.0
|
||
|
||
# --- 轨迹起点逆解 ---
|
||
x0, y0 = x_list[0], y_list[0]
|
||
try:
|
||
raw_theta1, raw_theta4 = inverseF(x0, y0, L1, L2, L3, L4, L0)
|
||
theta1_start = float(raw_theta1) - ANGLE_OFFSET_RAD
|
||
theta4_start = float(raw_theta4) - ANGLE_OFFSET_RAD
|
||
print(f"起点逆解成功 -> θ1={np.degrees(theta1_start):.2f}°, θ4={np.degrees(theta4_start):.2f}°")
|
||
except Exception as e:
|
||
print(f"❌ 起点逆解失败: {e}")
|
||
return [], [], [], [], []
|
||
|
||
# --- 电机移动到起点 ---
|
||
print("电机从零点移动到轨迹起点...")
|
||
move_motors_to_target(theta1_start, theta4_start, duration=1.5, dt=dt)
|
||
current_theta1, current_theta4 = theta1_start, theta4_start
|
||
|
||
# --- 轨迹执行 ---
|
||
start_time = perf_counter()
|
||
for i, (x, y) in enumerate(zip(x_list, y_list)):
|
||
try:
|
||
raw_theta1, raw_theta4 = inverseF(x, y, L1, L2, L3, L4, L0)
|
||
target_theta1 = float(raw_theta1) - ANGLE_OFFSET_RAD
|
||
target_theta4 = float(raw_theta4) - ANGLE_OFFSET_RAD
|
||
target_theta1 = adjust_angle_continuity(target_theta1, current_theta1)
|
||
target_theta4 = adjust_angle_continuity(target_theta4, current_theta4)
|
||
except Exception as e:
|
||
print(f"第 {i} 点逆解失败 ({x:.2f}, {y:.2f}): {e}")
|
||
target_theta1, target_theta4 = current_theta1, current_theta4
|
||
else:
|
||
current_theta1, current_theta4 = target_theta1, target_theta4
|
||
|
||
# 控制电机
|
||
control_two_motors_mit(target_theta1, target_theta4)
|
||
|
||
# FK 验证
|
||
try:
|
||
xc, yc, *_ = forwardF(
|
||
u1=target_theta1 + ANGLE_OFFSET_RAD,
|
||
u4=target_theta4 + ANGLE_OFFSET_RAD,
|
||
omega1=omega1, omega4=omega4,
|
||
l1=l1, l2=l2, l3=l3, l4=l4, l5=l5,
|
||
alpha1=alpha1, alpha4=alpha4
|
||
)
|
||
x_fk_log.append(xc)
|
||
y_fk_log.append(yc)
|
||
except:
|
||
x_fk_log.append(np.nan)
|
||
y_fk_log.append(np.nan)
|
||
|
||
# 记录日志
|
||
elapsed = perf_counter() - start_time
|
||
time_log.append(elapsed)
|
||
theta1_log.append(np.degrees(target_theta1))
|
||
theta4_log.append(np.degrees(target_theta4))
|
||
|
||
busy_wait(dt)
|
||
|
||
return time_log, theta1_log, theta4_log, x_fk_log, y_fk_log
|
||
|
||
|
||
# ------------------------ 动画绘制函数 ------------------------
|
||
def draw_frame(i):
|
||
x = x_list[i]
|
||
y = y_list[i]
|
||
try:
|
||
theta1, theta4 = inverseF(x, y, L1, L2, L3, L4, L0)
|
||
x2 = L1 * np.cos(theta1)
|
||
y2 = L1 * np.sin(theta1)
|
||
x4 = L0 + L4 * np.cos(theta4)
|
||
y4 = L4 * np.sin(theta4)
|
||
x_coords = [0, x2, x, x4, L0]
|
||
y_coords = [0, y2, y, y4, 0]
|
||
line.set_data(x_coords, y_coords)
|
||
except:
|
||
line.set_data([], [])
|
||
return line,
|
||
|
||
# ------------------------ 轨迹循环动画与执行 + 可视化 + FK 验证 ------------------------
|
||
def run_trajectory_with_animation_loop(trajectory_func, loop_count=3, **params):
|
||
"""
|
||
循环执行轨迹指定次数，保持关节连续性
|
||
loop_count: 循环次数
|
||
"""
|
||
global x_list, y_list, line, current_theta1, current_theta4
|
||
|
||
print(f"生成轨迹: {trajectory_func.__name__}")
|
||
x_list, y_list = trajectory_func(**params)
|
||
print(f"轨迹点数: {len(x_list)}")
|
||
|
||
# --- 动画初始化 ---
|
||
fig, ax = plt.subplots(figsize=(10, 8))
|
||
ax.set_xlim(-50, L0 + 100)
|
||
ax.set_ylim(0, 500)
|
||
ax.set_aspect('equal')
|
||
ax.grid(True, alpha=0.6)
|
||
ax.set_title(f"五连杆机械臂 - 轨迹循环执行: {trajectory_func.__name__}")
|
||
ax.plot(x_list, y_list, 'b--', alpha=0.5, label='目标轨迹')
|
||
line, = ax.plot([], [], 'r-o', lw=3, ms=6, label='机械臂')
|
||
ax.legend()
|
||
plt.ion()
|
||
plt.show()
|
||
|
||
for count in range(1, loop_count + 1):
|
||
print(f"\n=== 第 {count} 次执行轨迹 ===")
|
||
# --- 执行轨迹 ---
|
||
time_log, theta1_log, theta4_log, x_fk_log, y_fk_log = execute_direct_trajectory(
|
||
x_list, y_list, dt=DT
|
||
)
|
||
|
||
# --- 动画更新 ---
|
||
for i in range(len(x_list)):
|
||
draw_frame(i)
|
||
plt.pause(0.01)
|
||
|
||
# --- 绘制关节角度 ---
|
||
fig2, (ax1, ax2) = plt.subplots(2, 1, figsize=(10, 6), sharex=True)
|
||
ax1.plot(time_log, theta1_log, 'b-o', markersize=3, linewidth=1.5, label='θ₁ (电机1)')
|
||
ax1.set_ylabel('关节角 θ₁ (°)')
|
||
ax1.set_title(f'电机关节角度随时间变化 - 第 {count} 次')
|
||
ax1.grid(True, alpha=0.5)
|
||
ax1.legend()
|
||
|
||
ax2.plot(time_log, theta4_log, 'r-o', markersize=3, linewidth=1.5, label='θ₄ (电机2)')
|
||
ax2.set_xlabel('时间 (秒)')
|
||
ax2.set_ylabel('关节角 θ₄ (°)')
|
||
ax2.grid(True, alpha=0.5)
|
||
ax2.legend()
|
||
plt.tight_layout()
|
||
plt.show()
|
||
|
||
# --- FK 验证 ---
|
||
fig3, ax3 = plt.subplots(figsize=(10, 8))
|
||
ax3.plot(x_list, y_list, 'b--', linewidth=2, label='目标轨迹', alpha=0.8)
|
||
ax3.plot(x_fk_log, y_fk_log, 'g-', linewidth=2, marker='o', markersize=3, label='FK 重建轨迹', alpha=0.7)
|
||
ax3.scatter(x_fk_log[0], y_fk_log[0], c='green', s=100, marker='s', label='起点')
|
||
ax3.scatter(x_fk_log[-1], y_fk_log[-1], c='red', s=100, marker='x', label='终点')
|
||
ax3.set_xlabel('X (mm)')
|
||
ax3.set_ylabel('Y (mm)')
|
||
ax3.set_title(f'FK 验证：目标 vs 实际轨迹 - 第 {count} 次')
|
||
ax3.grid(True, alpha=0.5)
|
||
ax3.legend()
|
||
ax3.set_aspect('equal')
|
||
plt.tight_layout()
|
||
plt.show()
|
||
|
||
# --- 误差统计 ---
|
||
mask = ~np.isnan(x_fk_log) & ~np.isnan(y_fk_log)
|
||
x_fk_valid = np.array(x_fk_log)[mask]
|
||
y_fk_valid = np.array(y_fk_log)[mask]
|
||
if len(x_fk_valid) == 0:
|
||
print("【警告】FK 数据全为 nan，无法计算误差")
|
||
continue
|
||
|
||
t_target = np.linspace(0, 1, len(x_list))
|
||
t_fk = np.linspace(0, 1, len(x_fk_valid))
|
||
from scipy.interpolate import interp1d
|
||
try:
|
||
f_x = interp1d(t_fk, x_fk_valid, kind='linear', fill_value='extrapolate')
|
||
f_y = interp1d(t_fk, y_fk_valid, kind='linear', fill_value='extrapolate')
|
||
x_interp = f_x(t_target)
|
||
y_interp = f_y(t_target)
|
||
errors = np.sqrt((x_interp - x_list)**2 + (y_interp - y_list)**2)
|
||
print("\n=== FK 验证误差统计 ===")
|
||
print(f"平均误差: {np.mean(errors):.3f} mm")
|
||
print(f"最大误差: {np.max(errors):.3f} mm")
|
||
print(f"标准差: {np.std(errors):.3f} mm")
|
||
except Exception as e:
|
||
print(f"误差计算失败: {e}")
|
||
|
||
# --- 关节角度范围 ---
|
||
print("\n=== 关节角度范围统计 ===")
|
||
print(f"θ₁ 范围: {min(theta1_log):.2f}° ~ {max(theta1_log):.2f}°")
|
||
print(f"θ₄ 范围: {min(theta4_log):.2f}° ~ {max(theta4_log):.2f}°")
|
||
|
||
plt.ioff()
|
||
print(f"\n=== 循环执行 {loop_count} 次完成 ===")
|
||
|
||
|
||
# ==================== 获取电机当前位置 =====================
|
||
def get_current_motor_position():
|
||
"""
|
||
获取当前电机角度（电机原始单位，通常是度），并打印。
|
||
返回：theta1_current, theta4_current（度）
|
||
"""
|
||
if not DEBUG_MODE:
|
||
try:
|
||
motor_control.refresh_motor_status(motor1)
|
||
motor_control.refresh_motor_status(motor2)
|
||
time.sleep(0.05)
|
||
theta1_current = motor1.getPosition()
|
||
theta4_current = motor2.getPosition()
|
||
except Exception:
|
||
theta1_current = theta4_current = 0.0
|
||
else:
|
||
theta1_current = motor1.getPosition()
|
||
theta4_current = motor2.getPosition()
|
||
|
||
print(f"当前电机弧度 -> θ1={theta1_current:.2f}, θ4={theta4_current:.2f}")
|
||
return theta1_current, theta4_current
|
||
|
||
# ==================== MIT 模式回零函数 =====================
|
||
def move_motors_to_zero(duration=1.0, dt=DT):
|
||
"""
|
||
使用 MIT 模式将电机从当前位置平滑回到零点
|
||
duration: 总运动时间 (秒)
|
||
dt: 控制周期 (秒)
|
||
"""
|
||
global motor1, motor2, motor_control, current_theta1, current_theta4
|
||
|
||
# 获取当前位置
|
||
theta1_start, theta4_start = get_current_motor_position()
|
||
# 计算步数
|
||
steps = int(duration / dt)
|
||
for i in range(steps + 1):
|
||
t = i / steps
|
||
# 线性插值
|
||
theta1_target = theta1_start * (1 - t)
|
||
theta4_target = theta4_start * (1 - t)
|
||
# 发送 MIT 控制
|
||
control_two_motors_mit(theta1_target, theta4_target)
|
||
busy_wait(dt)
|
||
|
||
# 确保最终位置为零
|
||
control_two_motors_mit(0.0, 0.0)
|
||
current_theta1 = current_theta4 = 0.0
|
||
print("电机已回零完成。")
|
||
|
||
# ==================== MIT 模式移动到指定角度（任意两点） =====================
|
||
def move_motors_to_target(theta1_target_rad, theta4_target_rad, duration=1.0, dt=DT,
|
||
theta1_start_rad=None, theta4_start_rad=None):
|
||
"""
|
||
使用 MIT 模式将电机从起点平滑移动到指定目标角度
|
||
参数:
|
||
theta1_target_rad: 电机1目标角度（弧度）
|
||
theta4_target_rad: 电机4目标角度（弧度）
|
||
duration: 总运动时间 (秒)
|
||
dt: 控制周期 (秒)
|
||
theta1_start_rad: 电机1起始角度（弧度），默认 None 表示从零点
|
||
theta4_start_rad: 电机4起始角度（弧度），默认 None 表示从零点
|
||
"""
|
||
global motor1, motor2, motor_control, current_theta1, current_theta4
|
||
|
||
# 默认从零点出发
|
||
if theta1_start_rad is None:
|
||
theta1_start_rad = 0.0
|
||
if theta4_start_rad is None:
|
||
theta4_start_rad = 0.0
|
||
|
||
steps = int(duration / dt)
|
||
for i in range(steps + 1):
|
||
t = i / steps
|
||
# 线性插值
|
||
theta1_now = theta1_start_rad + (theta1_target_rad - theta1_start_rad) * t
|
||
theta4_now = theta4_start_rad + (theta4_target_rad - theta4_start_rad) * t
|
||
# 发送 MIT 控制
|
||
control_two_motors_mit(theta1_now, theta4_now)
|
||
busy_wait(dt)
|
||
|
||
# 确保最终位置精确到目标
|
||
control_two_motors_mit(theta1_target_rad, theta4_target_rad)
|
||
current_theta1 = theta1_target_rad
|
||
current_theta4 = theta4_target_rad
|
||
print(f"电机已移动到目标角度: θ1={np.degrees(theta1_target_rad):.2f}°, θ4={np.degrees(theta4_target_rad):.2f}°")
|
||
|
||
|
||
if __name__ == "__main__":
|
||
try:
|
||
# ---------------- 初始化电机 ----------------
|
||
init_motors()
|
||
|
||
# --- 获取当前位置测试 ---
|
||
print("\n=== 测试：获取电机当前位置 ===")
|
||
theta1, theta4 = get_current_motor_position()
|
||
|
||
# --- MIT 模式回零 ---
|
||
print("\n=== MIT 模式回零 ===")
|
||
move_motors_to_zero(duration=1.0) # 主函数里执行回零
|
||
|
||
# ---------------- 选择轨迹 ----------------
|
||
#trajectory_config = {
|
||
# 'func': line_trajectory_fix,
|
||
# 'params': {'start': (25, 300), 'end': (200, 300), 'num_points': 100}
|
||
#}
|
||
trajectory_config = {
|
||
'func': circle_trajectory,
|
||
'params': {'center': (100, 300), 'radius': 100}
|
||
}
|
||
run_trajectory_with_animation_loop(trajectory_config['func'], loop_count=2, **trajectory_config['params'])
|
||
|
||
|
||
except KeyboardInterrupt:
|
||
print("\n【用户中断】")
|
||
except Exception as e:
|
||
print(f"程序异常: {e}")
|
||
finally:
|
||
if not DEBUG_MODE:
|
||
motor_control.disable(motor1)
|
||
motor_control.disable(motor2)
|
||
print("电机已停机。")
|
||
else:
|
||
print("【DEBUG】程序结束")
|
||
|