如何在Python中使用 Plot 畫出一個簡單的模型

在下面的程序中，首先要知道機器人的DH參數，然后計算出每一個關節的位置，最后利用 plot 函數畫出關節之間的連桿就可以了，最后利用 animation 庫來實現一個動畫效果。

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 函數中，可以在每一幀中去更新關節角度，然后呈現出一種動畫的效果。

		# 關節角度固定不變self.q_list = [0, 0, 0, 0, 0, 0]# 關節角度每一幀都在更新，呈現出一種動畫效果# self.q_list = [frame, frame - 90, 0, 0, 0, 0]