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() |