132 lines
3.8 KiB
Python
132 lines
3.8 KiB
Python
# main1.py
|
||
|
||
import numpy as np
|
||
import matplotlib.pyplot as plt
|
||
from matplotlib.animation import FuncAnimation
|
||
from caculate.fkik import TwoLinkArm
|
||
|
||
# -------------------------
|
||
# 参数设置
|
||
# -------------------------
|
||
L1 = 100.0 # 连杆1长度
|
||
L2 = 100.0 # 连杆2长度
|
||
|
||
KP, KD = 60.0, 1.0
|
||
DEBUG_MODE = True # 调试模式
|
||
|
||
# ✅ 创建机械臂实例（关键！）
|
||
arm = TwoLinkArm(l1=L1, l2=L2)
|
||
|
||
|
||
# -------------------------
|
||
# 控制两个电机（调试用）
|
||
# -------------------------
|
||
def control_two_motors_mit(theta1_rad, theta2_rad):
|
||
if not DEBUG_MODE:
|
||
# 这里可以接入真实电机控制
|
||
# motor_control.controlMIT(motor1, KP, KD, theta1_rad, 0.1, 0.0)
|
||
# motor_control.controlMIT(motor2, KP, KD, theta2_rad, 0.1, 0.0)
|
||
pass
|
||
else:
|
||
print(f"[DEBUG] 控制 -> θ1={theta1_rad:+.2f}rad, θ2={theta2_rad:+.2f}rad")
|
||
|
||
|
||
# -------------------------
|
||
# 轨迹生成
|
||
# -------------------------
|
||
def circle_trajectory(r=60, steps=100):
|
||
"""生成圆形轨迹"""
|
||
angles = np.linspace(0, 2 * np.pi, steps)
|
||
x = r * np.cos(angles) + 100
|
||
y = r * np.sin(angles) + 50
|
||
return x, y
|
||
|
||
|
||
def line_trajectory(x0, y0, x1, y1, steps=100):
|
||
"""生成直线轨迹"""
|
||
x = np.linspace(x0, x1, steps)
|
||
y = np.linspace(y0, y1, steps)
|
||
return x, y
|
||
|
||
|
||
# -------------------------
|
||
# 执行轨迹
|
||
# -------------------------
|
||
def execute_trajectory(x_list, y_list):
|
||
print("开始执行轨迹...")
|
||
for i, (x, y) in enumerate(zip(x_list, y_list)):
|
||
try:
|
||
theta1, theta2 = arm.ik(x, y) # ✅ 使用实例调用 ik
|
||
control_two_motors_mit(theta1, theta2)
|
||
|
||
_, (x_fk, y_fk) = arm.fk(theta1, theta2) # 注意：用 _ 忽略第一个返回值 (x1,y1)
|
||
print(f"点 {i:3d}: 目标({x:6.1f},{y:6.1f}) -> FK验证({x_fk:6.1f},{y_fk:6.1f})") # ✅ 使用实例调用 fk
|
||
except ValueError as e:
|
||
print(f"点 {i:3d}: 跳过不可达点 ({x:.1f}, {y:.1f}) -> {e}")
|
||
print("轨迹执行完成。\n")
|
||
|
||
|
||
# -------------------------
|
||
# 动画演示
|
||
# -------------------------
|
||
def run_animation(x_list, y_list):
|
||
print("启动动画...")
|
||
fig, ax = plt.subplots(figsize=(8, 8))
|
||
ax.set_xlim(-200, 200)
|
||
ax.set_ylim(-200, 200)
|
||
ax.set_aspect('equal')
|
||
ax.grid(True, linestyle='--', alpha=0.6)
|
||
ax.set_title("2R 机械臂轨迹跟踪动画")
|
||
ax.set_xlabel("X (mm)")
|
||
ax.set_ylabel("Y (mm)")
|
||
|
||
# 绘图元素
|
||
line_arm, = ax.plot([], [], 'o-', lw=2, color='blue', label='机械臂')
|
||
target_point, = ax.plot([], [], 'r*', markersize=10, label='目标点')
|
||
ax.legend()
|
||
|
||
def draw_frame(i):
|
||
x_target, y_target = x_list[i], y_list[i]
|
||
try:
|
||
# ✅ 使用实例 arm 调用 ik
|
||
theta1, theta2 = arm.ik(x_target, y_target)
|
||
x1 = L1 * np.cos(theta1)
|
||
y1 = L1 * np.sin(theta1)
|
||
(x1, y1), (x_fk, y_fk) = arm.fk(theta1, theta2) # 实际末端位置
|
||
|
||
# 更新机械臂：基座 -> 关节1 -> 末端
|
||
line_arm.set_data([0, x1, x_fk], [0, y1, y_fk])
|
||
except Exception as e:
|
||
# IK失败，只显示目标
|
||
line_arm.set_data([], [])
|
||
|
||
# 始终显示目标点
|
||
target_point.set_data([x_target], [y_target])
|
||
return line_arm, target_point
|
||
|
||
# 创建动画
|
||
ani = FuncAnimation(
|
||
fig, draw_frame,
|
||
frames=len(x_list),
|
||
interval=100,
|
||
blit=True,
|
||
repeat=True
|
||
)
|
||
|
||
plt.show()
|
||
return ani # 防止动画被回收
|
||
|
||
|
||
# -------------------------
|
||
# 主程序
|
||
# -------------------------
|
||
if __name__ == "__main__":
|
||
# 选择轨迹
|
||
x_list, y_list = circle_trajectory(r=40, steps=100)
|
||
# x_list, y_list = line_trajectory(50, 50, 150, 100, steps=100)
|
||
|
||
# 执行轨迹控制
|
||
execute_trajectory(x_list, y_list)
|
||
|
||
# 播放动画
|
||
run_animation(x_list, y_list) |