在上一篇文章中如何在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 = robot
self.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.ax
self.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_end
T = 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_list
rotation_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 = axis
self.current_direction = direction
self.timer.start(100) # 每 100 毫秒触发一次
def stop_motion(self):
"""停止运动"""
self.timer.stop()
self.current_axis = None
self.current_direction = None
self.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.0
elif direction == '负向':
self.show_robot.q_list[axis] -= 1.0
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_end
T = 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_list
rotation_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 = 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)
ex = RobotControlApp(robot, show_robot)
ex.show()
sys.exit(app.exec_())
在界面中，按下 J1+， 关节1轴就会一直正向转动，松开 J1+按钮之后关节1轴就会停止运动，其他关节轴也是一样的。
然后在界面的下方中还是实时的显示出机器人当前的关节角度和笛卡尔末端位姿。
最终实现的效果如下：
