80 lines
1.6 KiB
Python
80 lines
1.6 KiB
Python
import numpy as np
|
|
from calculate.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
|
|
|
|
hd = np.pi / 180
|
|
u1 = 120 * hd
|
|
u4 = 60 * hd
|
|
|
|
omega1 = 500
|
|
omega4 = 500
|
|
alpha1 = 0
|
|
alpha4 = 0
|
|
|
|
# 计算正向运动学
|
|
xc, yc, u2, u3, omega2, omega3, alpha2, alpha3 = forwardF(u1, u4, omega1, omega4, l1, l2, l3, l4, l5, alpha1, alpha4)
|
|
|
|
# 存储中间变量
|
|
theta2 = [u2]
|
|
theta3 = [u3]
|
|
omega2_list = [omega2]
|
|
omega3_list = [omega3]
|
|
alpha2_list = [alpha2]
|
|
alpha3_list = [alpha3]
|
|
xcd = [xc]
|
|
ycd = [yc]
|
|
|
|
# 绘图部分
|
|
fig = plt.figure(figsize=(8, 8))
|
|
ax = fig.add_subplot(111)
|
|
ax.set_title('并联SCARA')
|
|
ax.set_xlabel('mm')
|
|
ax.set_ylabel('mm')
|
|
ax.set_xlim(-200, 600)
|
|
ax.set_ylim(-200, 600)
|
|
ax.grid(True)
|
|
|
|
x = [0] * 5
|
|
y = [0] * 5
|
|
|
|
# 基础点
|
|
x[0] = 0
|
|
y[0] = 0
|
|
|
|
# 第一个连杆末端
|
|
x[1] = l1 * np.cos(u1)
|
|
y[1] = l1 * np.sin(u1)
|
|
|
|
# 末端执行器位置
|
|
x[2] = xcd[0]
|
|
y[2] = ycd[0]
|
|
|
|
# 第四个连杆末端
|
|
x[3] = l4 * np.cos(u4) + l5
|
|
y[3] = l4 * np.sin(u4)
|
|
|
|
# 第五个点（第二个电机位置）
|
|
x[4] = l5
|
|
y[4] = 0
|
|
|
|
# 绘制结构线和关键点
|
|
ax.plot(x, y, 'k-', linewidth=2)
|
|
ax.plot(x[0], y[0], 'or') # 基础点
|
|
ax.plot(x[1], y[1], 'or') # 第一连杆末端
|
|
ax.plot(x[2], y[2], 'og') # 末端执行器
|
|
ax.plot(x[3], y[3], 'om') # 第四连杆末端
|
|
ax.plot(x[4], y[4], 'oc') # 第二个电机
|
|
|
|
plt.show() |