如何在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_listself.a_list = a_listself.d_list = d_listself.theta_list = theta_listdef 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 * Treturn T6class ShowRobot(object):def __init__(self):self.ax = Noneself.robot = Noneself.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) * 60self.s200 = max(fig_width / 50, fig_height / 50) * 200self.fontsize = max(fig_width / 50, fig_height / 50) * 80self.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_endT = 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 = 388L2 = 50L3 = 330L4 = 50L5 = 332L6 = 96alpha_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 = axshow_robot.robot = robotroot = 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]