利用PyQt簡單的實現一個機器人的關節JOG界面

在上一篇文章中如何在Python用Plot畫出一個簡單的機器人模型，我們介紹了如何在Python中畫出一個簡單的機器人3D模型，但是有的時候我們需要通過界面去控制機器人每一個軸的轉動，并實時的顯示出當前機器人的關節位置和末端笛卡爾位姿。
那么要實現上述功能的話，那么我就可以采用 Pyqt5 來實現，具體代碼如下：


import sys
import numpy as np
from PyQt5.QtWidgets import (QApplication, QWidget, QPushButton, QVBoxLayout, QHBoxLayout, QLabel, QFileDialog, QLineEdit, QFormLayout, QGridLayout, QDialog, QSpinBox, QDialogButtonBox
)
from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas
from matplotlib.figure import Figure
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
from PyQt5.QtCore import QTimer
from ShowRobot import Robot, ShowRobot
from scipy.spatial.transform import Rotation as R
from functools import partial# 設置全局字體以支持中文
plt.rcParams['font.sans-serif'] = ['SimHei']  # 設置默認字體為黑體
plt.rcParams['axes.unicode_minus'] = False  # 解決負號顯示問題class SettingsDialog(QDialog):"""設置對話框，用于配置數據格式化規則"""def __init__(self, parent=None):super().__init__(parent)self.setWindowTitle("數據格式化設置")self.initUI()def initUI(self):layout = QFormLayout(self)# 添加小數點位數設置self.decimal_spinbox = QSpinBox(self)self.decimal_spinbox.setRange(0, 6)  # 小數點位數范圍self.decimal_spinbox.setValue(2)  # 默認值layout.addRow("小數點位數:", self.decimal_spinbox)# 添加單位設置self.unit_edit = QLineEdit(self)self.unit_edit.setPlaceholderText("例如：° 或 m")layout.addRow("單位:", self.unit_edit)# 添加確認和取消按鈕buttons = QDialogButtonBox(QDialogButtonBox.Ok | QDialogButtonBox.Cancel, self)buttons.accepted.connect(self.accept)buttons.rejected.connect(self.reject)layout.addRow(buttons)def get_settings(self):"""獲取用戶設置的格式化規則"""return {"decimal_places": self.decimal_spinbox.value(),"unit": self.unit_edit.text().strip(),}class RobotControlApp(QWidget):def __init__(self, robot : Robot, show_robot : ShowRobot):super().__init__()self.data_format = {"decimal_places": 3, "unit": "°"}  # 默認數據格式self.robot = robotself.show_robot = show_robot# 初始化定時器self.timer = QTimer(self)self.timer.timeout.connect(self.on_timer_timeout)self.current_axis = None  # 當前運動的軸self.current_direction = None  # 當前運動的方向self.initUI()def initUI(self):# 設置窗口標題和大小self.setWindowTitle('6軸機器人JOG控制')self.setGeometry(100, 100, 1000, 600)# 創建主布局main_layout = QHBoxLayout()# 左側布局：控制面板control_layout = QVBoxLayout()# 創建標簽self.status_label = QLabel('當前狀態: 停止', self)control_layout.addWidget(self.status_label)# 創建6個軸的JOG控制按鈕self.axis_buttons = []for i in range(6):# axis_label = QLabel(f'軸 {i+1}', self)# control_layout.addWidget(axis_label)button_layout = QHBoxLayout()positive_button = QPushButton(f'J{i+1} +', self)negative_button = QPushButton(f'J{i+1} -', self)# 連接按鈕點擊事件,點擊一次運動一次positive_button.clicked.connect(lambda _, axis=i: self.move_axis(axis, '正向'))negative_button.clicked.connect(lambda _, axis=i: self.move_axis(axis, '負向'))# 連接按鈕按下和松開事件, 使用 partial 綁定參數positive_button.pressed.connect(partial(self.start_motion, axis=i, direction='正向'))positive_button.released.connect(self.stop_motion)negative_button.pressed.connect(partial(self.start_motion, axis=i, direction='負向'))negative_button.released.connect(self.stop_motion)button_layout.addWidget(positive_button)button_layout.addWidget(negative_button)control_layout.addLayout(button_layout)# 添加保存按鈕save_button = QPushButton('保存圖像', self)save_button.clicked.connect(self.save_plot)control_layout.addWidget(save_button)# 添加設置按鈕settings_button = QPushButton('數據格式化設置', self)settings_button.clicked.connect(self.open_settings_dialog)control_layout.addWidget(settings_button)# 添加關節角度顯示控件（一行顯示）joint_layout = QHBoxLayout()self.joint_angle_edits = []for i in range(6):edit = QLineEdit(self)edit.setReadOnly(True)  # 設置為只讀edit.setPlaceholderText(f'關節 {i + 1} 角度')joint_layout.addWidget(edit)self.joint_angle_edits.append(edit)# 添加笛卡爾位置顯示控件（一行顯示）cartesian_layout = QHBoxLayout()self.cartesian_position_edits = []for label in ['X', 'Y', 'Z', 'Rx', 'Ry', 'Rz']:edit = QLineEdit(self)edit.setReadOnly(True)  # 設置為只讀edit.setPlaceholderText(f'{label} 位置')cartesian_layout.addWidget(edit)self.cartesian_position_edits.append(edit)# 將關節角度和笛卡爾位置添加到控制面板control_layout.addLayout(joint_layout)control_layout.addLayout(cartesian_layout)# 將控制面板添加到主布局main_layout.addLayout(control_layout)# 右側布局：3D 繪圖窗口self.figure = Figure()self.canvas = FigureCanvas(self.figure)self.ax = self.figure.add_subplot(111, projection='3d')self.ax.set_title("機器人運動軌跡 (3D)")self.ax.set_xlabel("X 軸")self.ax.set_ylabel("Y 軸")self.ax.set_zlabel("Z 軸")self.ax.grid(True)self.show_robot.ax = self.axself.show_robot.robot = self.robot# 初始化繪圖數據T_start = np.identity(4, dtype= float)T_end = np.identity(4, dtype= float)self.show_robot.ShowFrame(T_start, length=500)for joint_index in range(6):T_start = T_endT = self.robot.DH(joint_index, self.show_robot.q_list[joint_index])T_end = T_end * T# print(T_end)self.show_robot.ShowLink(joint_index, T_start, T_end)self.show_robot.ShowFrame(T_end, length=500)joint_angles = self.show_robot.q_listrotation_matrix = T_end[:3, :3]r = R.from_matrix(rotation_matrix)# 將旋轉矩陣轉換為 XYZ 固定角（Roll-Pitch-Yaw 角）roll, pitch, yaw = r.as_euler('xyz', degrees=True)cartesian_positions = [T_end[0, 3], T_end[1, 3], T_end[2, 3], roll, pitch, yaw]# 更新關節角度顯示for i, edit in enumerate(self.joint_angle_edits):edit.setText(f"{joint_angles[i]:.{self.data_format['decimal_places']}f}{self.data_format['unit']}")# 更新笛卡爾位置顯示for i, edit in enumerate(self.cartesian_position_edits):edit.setText(f"{cartesian_positions[i]:.{self.data_format['decimal_places']}f}")self.ax.set_xlim([-1000, 1000])self.ax.set_ylim([-1000, 1000])self.ax.set_zlim([-1000, 1000])# 將繪圖窗口添加到主布局main_layout.addWidget(self.canvas)# 設置主布局self.setLayout(main_layout)def start_motion(self, axis, direction):"""開始運動"""self.current_axis = axisself.current_direction = directionself.timer.start(100)  # 每 100 毫秒觸發一次def stop_motion(self):"""停止運動"""self.timer.stop()self.current_axis = Noneself.current_direction = Noneself.status_label.setText('當前狀態: 停止')def on_timer_timeout(self):"""定時器觸發時的邏輯"""if self.current_axis is not None and self.current_direction is not None:self.move_axis(self.current_axis, self.current_direction)def move_axis(self, axis, direction):# 這里是控制機器人軸運動的邏輯self.status_label.setText(f'軸 {axis+1} {direction}運動')# 模擬機器人運動并更新繪圖self.update_plot(axis, direction)def update_plot(self, axis, direction):self.ax.cla()  # 清除所有軸# 模擬機器人運動數據if direction == '正向':self.show_robot.q_list[axis] += 1.0elif direction == '負向':self.show_robot.q_list[axis] -= 1.0T_start = np.identity(4, dtype= float)T_end = np.identity(4, dtype= float)self.show_robot.ShowFrame(T_start, length=500)for joint_index in range(6):T_start = T_endT = self.robot.DH(joint_index, self.show_robot.q_list[joint_index])T_end = T_end * T# print(T_end)self.show_robot.ShowLink(joint_index, T_start, T_end)self.show_robot.ShowFrame(T_end, length=500)joint_angles = self.show_robot.q_listrotation_matrix = T_end[:3, :3]r = R.from_matrix(rotation_matrix)# 將旋轉矩陣轉換為 XYZ 固定角（Roll-Pitch-Yaw 角）roll, pitch, yaw = r.as_euler('xyz', degrees=True)cartesian_positions = [T_end[0, 3], T_end[1, 3], T_end[2, 3], roll, pitch, yaw]# 更新關節角度顯示for i, edit in enumerate(self.joint_angle_edits):edit.setText(f"{joint_angles[i]:.{self.data_format['decimal_places']}f}{self.data_format['unit']}")# 更新笛卡爾位置顯示for i, edit in enumerate(self.cartesian_position_edits):edit.setText(f"{cartesian_positions[i]:.{self.data_format['decimal_places']}f}")self.ax.set_xlim([-1000, 1000])self.ax.set_ylim([-1000, 1000])self.ax.set_zlim([-1000, 1000])# 重繪圖self.canvas.draw()def save_plot(self):# 打開文件對話框，讓用戶選擇保存路徑和文件格式options = QFileDialog.Options()file_name, selected_filter = QFileDialog.getSaveFileName(self, "保存圖像", "", "PNG 文件 (*.png);;JPG 文件 (*.jpg);;PDF 文件 (*.pdf)", options=options)if file_name:# 根據用戶選擇的文件擴展名保存圖像if selected_filter == "PNG 文件 (*.png)":self.figure.savefig(file_name, format='png')elif selected_filter == "JPG 文件 (*.jpg)":self.figure.savefig(file_name, format='jpg')elif selected_filter == "PDF 文件 (*.pdf)":self.figure.savefig(file_name, format='pdf')# 更新狀態標簽self.status_label.setText(f'圖像已保存為 {file_name}')def open_settings_dialog(self):"""打開數據格式化設置對話框"""dialog = SettingsDialog(self)if dialog.exec_() == QDialog.Accepted:self.data_format = dialog.get_settings()  # 更新數據格式self.update_joint_and_cartesian_display()  # 更新顯示if __name__ == '__main__':app = QApplication(sys.argv)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)ex = RobotControlApp(robot, show_robot)ex.show()sys.exit(app.exec_())