Files
5dof/calculate/traj_fk.py

62 lines
1.8 KiB
Python
Raw Permalink Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

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