如何在Python用Plot画出一个简单的机器人模型

如何在Python中使用 Plot 画出一个简单的模型

在下面的程序中,首先要知道机器人的DH参数,然后计算出每一个关节的位置,最后利用 plot 函数画出关节之间的连杆就可以了,最后利用 animation 库来实现一个动画效果。

python 复制代码
import matplotlib.pyplot as plt
import numpy as np
import matplotlib.pyplot as plt
import numpy as np
from IPython import embed
import matplotlib.animation as animation
from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg
import tkinter as tk


# 取消科学计数法,保留6位小数
np.set_printoptions(precision=6, suppress=True)

class Robot(object):
	def __init__(self):
		self.alpha_list = []
		self.a_list = []
		self.d_list = []
		self.theta_list = []

	def SetDHParamList(self, alpha_list, a_list, d_list, theta_list):
		self.alpha_list = alpha_list
		self.a_list = a_list
		self.d_list = d_list
		self.theta_list = theta_list

	def DH(self, index, theta):
		theta = theta + self.theta_list[index]
		T = np.zeros([4, 4])
		c_t = np.cos(theta * np.pi/180)
		s_t = np.sin(theta * np.pi/180)
		c_a = np.cos(self.alpha_list[index] * np.pi/180)
		s_a = np.sin(self.alpha_list[index] * np.pi/180)
	
		T[0] = [c_t, - s_t * c_a,     s_t * s_a,  self.a_list[index] * c_t]
		T[1] = [s_t,   c_t * c_a,   - c_t * s_a,  self.a_list[index] * s_t]
		T[2] = [  0,         s_a,           c_a,        self.d_list[index]]
		T[3] = [  0,           0,             0,                    1]

		# print(T)

		return np.mat(T)
	
	def GetRobotTool(self, q_list):
		T6 = np.identity(4, dtype= float)
		for i in range(6):
			T = self.DH(i, q_list[i])
			T6 = T6 * T
		
		return T6


class ShowRobot(object):
	def __init__(self):
		self.ax = None
		self.robot = None
		self.q_list = [0, 0, 0, 0, 0, 0]

		fig_width, fig_height = plt.gcf().get_size_inches()
		# 根据画布大小自动调整直线的粗细, 这里乘上系数3只是为了更好地显示效果
		self.line_thicknes = max(fig_width / 50, fig_height / 50) * 60
		self.s200 = max(fig_width / 50, fig_height / 50) * 200
		self.fontsize = max(fig_width / 50, fig_height / 50) * 80
		self.alpha = 0.7 #透明度


	def ShowFrame(self, T, length = 1, width = 2):
		# 使用quiver绘制坐标轴
		# 参数说明:
		# origin[0], origin[1], origin[2] 是箭头的起点
		# x_axis[0], x_axis[1], x_axis[2] 是箭头的方向
		# length 是箭头的长度
		# arrow_length_ratio 是箭头头部与箭杆的比例
		# linewidth 是箭杆的宽度
		self.ax.quiver(T[0, 3], T[1, 3], T[2, 3], T[0, 0], T[1, 0], T[2, 0], color='r', length=length, arrow_length_ratio=0.1, linewidth=width)
		self.ax.quiver(T[0, 3], T[1, 3], T[2, 3], T[0, 1], T[1, 1], T[2, 1], color='g', length=length, arrow_length_ratio=0.1, linewidth=width)
		self.ax.quiver(T[0, 3], T[1, 3], T[2, 3], T[0, 2], T[1, 2], T[2, 2], color='b', length=length, arrow_length_ratio=0.1, linewidth=width)

	def ShowLink(self, joint_index, T_start, T_end):
		mx2, my2, mz2 = np.array([T_start[0, 3],T_end[0, 3]]), np.array([T_start[1, 3], T_end[1, 3]]), np.array([T_start[2, 3], T_end[2, 3]])
		self.ax.plot(mx2, my2, mz2, solid_capstyle='round', color='blue', linewidth=self.line_thicknes, alpha=self.alpha)
		self.ax.text(T_start[0, 3], T_start[1, 3], T_start[2, 3], "J" + str(joint_index), color='r', fontsize=self.fontsize, zorder=1, ha='center')
		self.ax.scatter(T_start[0, 3], T_start[1, 3], T_start[2, 3], c='orange', marker='.', s=self.s200)


	# 更新函数,用于每一帧的更新
	def update(self, frame):
		self.ax.cla()  # 清除所有轴

		# 设置坐标轴标签
		self.ax.set_xlabel('X')
		self.ax.set_ylabel('Y')
		self.ax.set_zlabel('Z')

		# # 设置图形显示范围
		self.ax.set_xlim([-1000, 1000])
		self.ax.set_ylim([-1000, 1000])
		self.ax.set_zlim([-1000, 1000])

		T_start = np.identity(4, dtype= float)
		T_end = np.identity(4, dtype= float)
		self.ShowFrame(T_start, length=300)

		# 关节角度固定不变
		self.q_list = [0, 0, 0, 0, 0, 0]
		# 关节角度每一帧都在更新,呈现出一种动画效果
		# self.q_list = [frame, frame - 90, 0, 0, 0, 0]
		for joint_index in range(6):
			T_start = T_end
			T = self.robot.DH(joint_index, self.q_list[joint_index])
			T_end = T_end * T
			# print(T_end)
			self.ShowLink(joint_index, T_start, T_end)

		self.ShowFrame(T_end, length=300)


def APP():

	L1 = 388
	L2 = 50
	L3 = 330
	L4 = 50
	L5 = 332
	L6 = 96
	alpha_list = [90, 0, 90, -90, 90, 0]
	a_list     = [L2, L3, L4, 0, 0, 0]
	d_list     = [L1, 0, 0, L5, 0, L6]
	theta_list = [0, 90, 0, 0, 0, 0]


	robot = Robot()
	show_robot = ShowRobot()
	robot.SetDHParamList(alpha_list, a_list, d_list, theta_list)

	# 创建一个3D图形
	fig = plt.figure()
	ax = fig.add_subplot(111, projection='3d')
	show_robot.ax = ax
	show_robot.robot = robot

	root = tk.Tk()
	canvas = FigureCanvasTkAgg(fig, master = root)
	canvas.get_tk_widget().pack()
	ani = animation.FuncAnimation(fig, show_robot.update, interval = 50)
	canvas.draw()
	tk.mainloop()

	# 显示动画
	plt.show()

if __name__ == "__main__":
	APP()

下面是实际显示出来的3D机器人模型,其中也画出了机器人的基坐标系和末端工具坐标系。

·

在 Update 函数中,可以在每一帧中去更新关节角度,然后呈现出一种动画的效果。

python 复制代码
		# 关节角度固定不变
		self.q_list = [0, 0, 0, 0, 0, 0]
		# 关节角度每一帧都在更新,呈现出一种动画效果
		# self.q_list = [frame, frame - 90, 0, 0, 0, 0]
相关推荐
爬虫程序猿1 小时前
用 Python 给京东商品详情做“全身 CT”——可量产、可扩展的爬虫实战
开发语言·爬虫·python
诗句藏于尽头1 小时前
自动签到之实现掘金模拟签到
python·1024程序员节
徐同保2 小时前
tailwindcss暗色主题切换
开发语言·前端·javascript
蓝纹绿茶3 小时前
bash:**:pip:***python: 错误的解释器: 没有那个文件或目录
开发语言·python·pip
云知谷3 小时前
【经典书籍】C++ Primer 第15章类虚函数与多态 “友元、异常和其他高级特性” 精华讲解
c语言·开发语言·c++·软件工程·团队开发
START_GAME4 小时前
深度学习Diffusers:用 DiffusionPipeline 实现图像生成
开发语言·python·深度学习
不爱编程的小九九4 小时前
小九源码-springboot088-宾馆客房管理系统
java·开发语言·spring boot
Deamon Tree4 小时前
后端开发常用Linux命令
linux·运维·python
Evand J5 小时前
【MATLAB例程】到达角度定位(AOA),平面环境多锚点定位(自适应基站数量),动态轨迹使用EKF滤波优化。附代码下载链接
开发语言·matlab·平面·滤波·aoa·到达角度
细节控菜鸡5 小时前
【2025最新】ArcGIS for JS 实现随着时间变化而变化的热力图
开发语言·javascript·arcgis