64 lines
1.4 KiB
Python
64 lines
1.4 KiB
Python
|
import math
|
|||
|
|
from ik import inverseF
|
|||
|
|
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 = 250
|
|||
|
|
l2 = 300
|
|||
|
|
l3 = 300
|
|||
|
|
l4 = 250
|
|||
|
|
l5 = 250
|
|||
|
|
x = 125
|
|||
|
|
#y = 382.338
|
|||
|
|
y = 300
|
|||
|
|
omega1 = omega4 = 500
|
|||
|
|
alpha1 = alpha4 = 0
|
|||
|
|
|
|||
|
|
# 逆解
|
|||
|
|
u1, u4 = inverseF(x, y, l1, l2, l3, l4, l5)
|
|||
|
|
|
|||
|
|
# 正解验证
|
|||
|
|
xc, yc, u2, u3, omega, alpha, _, _ = forwardF(u1, u4, omega1, omega4, l1, l2, l3, l4, l5, alpha1, alpha4)
|
|||
|
|
|
|||
|
|
# 左侧链路
|
|||
|
|
x1, y1 = 0, 0
|
|||
|
|
x2 = l1 * math.cos(u1)
|
|||
|
|
y2 = l1 * math.sin(u1)
|
|||
|
|
|
|||
|
|
# 右侧链路
|
|||
|
|
x5, y5 = l5, 0
|
|||
|
|
x4 = l4 * math.cos(u4)+l5 # 注意方向
|
|||
|
|
y4 = l4 * math.sin(u4)
|
|||
|
|
|
|||
|
|
# 绘图
|
|||
|
|
plt.figure(figsize=(8, 8))
|
|||
|
|
|
|||
|
|
# 左侧链路
|
|||
|
|
plt.plot([x1, x2, xc], [y1, y2, yc], 'b-o', label='左侧链路')
|
|||
|
|
|
|||
|
|
# 右侧链路
|
|||
|
|
plt.plot([x5, x4, xc], [y5, y4, yc], 'r-o', label='右侧链路')
|
|||
|
|
|
|||
|
|
# 标记关键点
|
|||
|
|
plt.plot(x1, y1, 'ro') # O1
|
|||
|
|
plt.plot(x5, y5, 'go') # O2
|
|||
|
|
plt.plot(x2, y2, 'yo') # B
|
|||
|
|
plt.plot(x4, y4, 'mo') # D
|
|||
|
|
plt.plot(xc, yc, 'ko', markersize=10) # C（末端）
|
|||
|
|
|
|||
|
|
# 设置图形
|
|||
|
|
plt.grid(True)
|
|||
|
|
plt.axis('equal')
|
|||
|
|
plt.xlim([-200, l5 + 200])
|
|||
|
|
plt.ylim([-200, 600])
|
|||
|
|
plt.title('SCARA 五连杆逆解结构图')
|
|||
|
|
plt.xlabel('X (mm)')
|
|||
|
|
plt.ylabel('Y (mm)')
|
|||
|
|
plt.legend()
|
|||
|
|
plt.show()
|