62 lines
1.8 KiB
Python
62 lines
1.8 KiB
Python
import numpy as np
|
||
|
||
from fk import forwardF
|
||
import matplotlib.pyplot as plt
|
||
|
||
# 设置中文字体和解决负号显示问题
|
||
plt.rcParams['font.sans-serif'] = ['SimHei', 'WenQuanYi Zen Hei', 'FangSong'] # 按优先级选择字体
|
||
plt.rcParams['axes.unicode_minus'] = False # 显示负号 -
|
||
|
||
|
||
# 杆长参数
|
||
l1 = 50
|
||
l2 = 50
|
||
l3 = 50
|
||
l4 = 50
|
||
l5 = 50
|
||
|
||
# 初始角度（弧度）
|
||
u1_base = np.deg2rad(120) # 左侧电机初始角
|
||
u4_base = np.deg2rad(120) # 右侧电机初始角
|
||
|
||
# 设置绘图区域
|
||
plt.figure(figsize=(8, 8))
|
||
ax = plt.gca()
|
||
ax.set_xlim(-100, l5 + 100)
|
||
ax.set_ylim(-100, 100)
|
||
ax.set_aspect('equal')
|
||
ax.grid(True)
|
||
ax.set_title("五连杆机构运动仿真（调用FK函数）")
|
||
|
||
for i in range(1, 61):
|
||
# 更新两个驱动臂的角度
|
||
angle1 = u1_base - np.deg2rad(1.75 * i) # 左侧角度变化
|
||
angle4 = u4_base + np.deg2rad(1.75 * i) # 右侧角度变化
|
||
|
||
#正向运动学函数获取末端位置和中间角度
|
||
result = forwardF(angle1, angle4, 0, 0, l1, l2, l3, l4, l5, 0, 0)
|
||
xc, yc, u2, u3 = result[:4]
|
||
|
||
# 构建各点坐标
|
||
x = [0, l1*np.cos(angle1), xc, l4*np.cos(angle4)+l5, l5]
|
||
y = [0, l1*np.sin(angle1), yc, l4*np.sin(angle4), 0]
|
||
|
||
# 清除上一帧并绘制新图形
|
||
ax.cla()
|
||
ax.set_xlim(-100, l5 + 100)
|
||
ax.set_ylim(-100, 100)
|
||
ax.set_aspect('equal')
|
||
ax.grid(True)
|
||
ax.set_title("五连杆机构运动仿真（调用FK函数）")
|
||
|
||
# 绘制结构线和关键点
|
||
ax.plot(x, y, 'r-o', linewidth=2, markersize=6, markerfacecolor='red')
|
||
ax.plot(x[0], y[0], 'go') # 原点
|
||
ax.plot(x[1], y[1], 'bo') # 第二个点
|
||
ax.plot(x[2], y[2], 'mo') # 中间点
|
||
ax.plot(x[3], y[3], 'co') # 第四个点
|
||
ax.plot(x[4], y[4], 'yo') # 最后一个点
|
||
|
||
plt.pause(0.1)
|
||
|
||
plt.close() |