From 59843dfd1e9769d556b157e845eca11fb0c01e3d Mon Sep 17 00:00:00 2001 From: lijinghan12 <15894570343@163.com> Date: Fri, 19 Dec 2025 16:11:14 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9C=BA=E6=A2=B0=E8=87=82=E7=9A=84=E5=90=84?= =?UTF-8?q?=E9=83=A8=E5=88=86=E8=BF=90=E5=8A=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/mechanical_arm/main.py | 405 ++++++++++++++++++++++++++----------- 1 file changed, 292 insertions(+), 113 deletions(-) diff --git a/src/mechanical_arm/main.py b/src/mechanical_arm/main.py index 97170c30ef..acf6ba89f7 100644 --- a/src/mechanical_arm/main.py +++ b/src/mechanical_arm/main.py @@ -1,126 +1,305 @@ -import matplotlib.pyplot as plt import numpy as np +import matplotlib.pyplot as plt +from matplotlib.widgets import Slider, Button +from mpl_toolkits.mplot3d import Axes3D +import matplotlib.animation as animation + + +class RoboticArm3D: + def __init__(self): + # 机械臂参数:每个关节的长度 + self.link_lengths = [2.0, 1.5, 1.0, 0.5] # 四个连杆长度 + self.joint_angles = [0.0, 0.0, 0.0, 0.0] # 四个关节角度(弧度) + + # DH参数:每个连杆的参数 [a, alpha, d, theta] + # a: 连杆长度, alpha: 连杆扭角, d: 连杆偏移, theta: 关节角度 + self.dh_params = [ + [self.link_lengths[0], 0, 0, self.joint_angles[0]], + [self.link_lengths[1], np.pi / 2, 0, self.joint_angles[1]], + [self.link_lengths[2], 0, 0, self.joint_angles[2]], + [self.link_lengths[3], 0, 0, self.joint_angles[3]] + ] + + # 初始化图形 + self.fig = plt.figure(figsize=(14, 8)) + self.setup_plot() + + def dh_matrix(self, a, alpha, d, theta): + """计算DH变换矩阵""" + cos_t = np.cos(theta) + sin_t = np.sin(theta) + cos_a = np.cos(alpha) + sin_a = np.sin(alpha) + + return np.array([ + [cos_t, -sin_t * cos_a, sin_t * sin_a, a * cos_t], + [sin_t, cos_t * cos_a, -cos_t * sin_a, a * sin_t], + [0, sin_a, cos_a, d], + [0, 0, 0, 1] + ]) + + def forward_kinematics(self): + """正向运动学:计算每个关节的位置""" + T = np.eye(4) # 单位矩阵 + positions = [np.array([0, 0, 0])] # 基座位置 + + for params in self.dh_params: + a, alpha, d, theta = params + T = T @ self.dh_matrix(a, alpha, d, theta) + position = T[:3, 3] + positions.append(position) + + return positions + + def setup_plot(self): + """设置3D图形""" + # 3D主视图 + self.ax1 = self.fig.add_subplot(121, projection='3d') + self.ax1.set_title('3D机械臂模型') + self.ax1.set_xlabel('X轴') + self.ax1.set_ylabel('Y轴') + self.ax1.set_zlabel('Z轴') + self.ax1.set_xlim(-3, 3) + self.ax1.set_ylim(-3, 3) + self.ax1.set_zlim(0, 4) + + # XY平面视图 + self.ax2 = self.fig.add_subplot(222) + self.ax2.set_title('XY平面视图') + self.ax2.set_xlabel('X轴') + self.ax2.set_ylabel('Y轴') + self.ax2.set_xlim(-3, 3) + self.ax2.set_ylim(-3, 3) + self.ax2.grid(True) + # XZ平面视图 + self.ax3 = self.fig.add_subplot(224) + self.ax3.set_title('XZ平面视图') + self.ax3.set_xlabel('X轴') + self.ax3.set_ylabel('Z轴') + self.ax3.set_xlim(-3, 3) + self.ax3.set_ylim(0, 4) + self.ax3.grid(True) -class ArmVisualizer: - def __init__(self, link_lengths=None): - """ - 初始化机械臂可视化器 - :param link_lengths: 各连杆长度列表,默认[5, 4, 3] - """ - # 默认连杆长度 - self.link_lengths = link_lengths if link_lengths else [5, 4, 3] - self.num_joints = len(self.link_lengths) + 1 # 关节数 = 连杆数 + 1 + # 添加控制滑块 + self.setup_sliders() # 初始化绘图 - self.fig, self.ax = plt.subplots(figsize=(8, 8)) - self.ax.set_xlim(-12, 12) - self.ax.set_ylim(-12, 12) - self.ax.set_aspect('equal') - self.ax.grid(True, alpha=0.3) - self.ax.set_xlabel('X (cm)') - self.ax.set_ylabel('Y (cm)') - self.ax.set_title('3自由度平面机械臂可视化') - - def calculate_joint_positions(self, angles): - """ - 根据关节角度计算各关节坐标 - :param angles: 各关节角度列表(弧度),长度等于连杆数 - :return: 关节坐标列表 [(x0,y0), (x1,y1), ..., (xn,yn)] - """ - if len(angles) != len(self.link_lengths): - raise ValueError(f"角度数量应等于连杆数量({len(self.link_lengths)})") - - # 初始关节(基座)在原点 - joints = [(0.0, 0.0)] - current_x, current_y = 0.0, 0.0 - current_angle = 0.0 - - # 计算每个关节的位置 - for i, (angle, length) in enumerate(zip(angles, self.link_lengths)): - current_angle += angle - current_x += length * np.cos(current_angle) - current_y += length * np.sin(current_angle) - joints.append((current_x, current_y)) - - return joints - - def draw_arm(self, angles, save_path=None): - """ - 绘制机械臂并可选保存图片 - :param angles: 关节角度列表(弧度) - :param save_path: 保存路径,如'robot_arm.png' - """ - # 清空之前的绘图 - self.ax.clear() - self.ax.set_xlim(-12, 12) - self.ax.set_ylim(-12, 12) - self.ax.set_aspect('equal') - self.ax.grid(True, alpha=0.3) - self.ax.set_xlabel('X (cm)') - self.ax.set_ylabel('Y (cm)') - self.ax.set_title('3自由度平面机械臂可视化') - - # 计算关节位置 - joints = self.calculate_joint_positions(angles) - - # 提取x和y坐标 - x_coords = [joint[0] for joint in joints] - y_coords = [joint[1] for joint in joints] - - # 绘制连杆 - self.ax.plot(x_coords, y_coords, 'b-', linewidth=4, label='连杆') - - # 绘制关节(基座用红色,其他关节用蓝色) - self.ax.plot(x_coords[0], y_coords[0], 'ro', markersize=10, label='基座') - self.ax.plot(x_coords[1:], y_coords[1:], 'bo', markersize=8, label='关节') - - # 绘制末端执行器 - self.ax.plot(x_coords[-1], y_coords[-1], 'go', markersize=12, label='末端执行器') - - # 添加角度标注 - for i, (x, y) in enumerate(joints[:-1]): - angle_deg = np.degrees(sum(angles[:i + 1])) - self.ax.text(x + 0.2, y + 0.2, f'θ{i + 1}={angle_deg:.0f}°', - fontsize=9, bbox=dict(facecolor='white', alpha=0.7)) - - # 添加连杆长度标注 - for i in range(len(self.link_lengths)): - mid_x = (x_coords[i] + x_coords[i + 1]) / 2 - mid_y = (y_coords[i] + y_coords[i + 1]) / 2 - self.ax.text(mid_x, mid_y, f'L{i + 1}={self.link_lengths[i]}', - fontsize=8, color='darkred') - - # 添加末端坐标标注 - end_x, end_y = joints[-1] - self.ax.text(end_x + 0.3, end_y - 0.3, f'末端坐标:\n({end_x:.2f}, {end_y:.2f})', - fontsize=9, bbox=dict(facecolor='lightgreen', alpha=0.7)) - - self.ax.legend(loc='upper right') - - # 保存图片(如果指定路径) - if save_path: - plt.tight_layout() - plt.savefig(save_path, dpi=300, bbox_inches='tight') - print(f"机械臂图片已保存至: {save_path}") + self.update_plot() + + def setup_sliders(self): + """设置角度控制滑块""" + # 为滑块留出空间 + plt.subplots_adjust(left=0.1, right=0.9, top=0.9, bottom=0.25) + + # 创建滑块轴 + ax_slider1 = plt.axes([0.1, 0.15, 0.8, 0.03]) + ax_slider2 = plt.axes([0.1, 0.10, 0.8, 0.03]) + ax_slider3 = plt.axes([0.1, 0.05, 0.8, 0.03]) + ax_slider4 = plt.axes([0.1, 0.00, 0.8, 0.03]) + + # 创建滑块(角度范围:-π 到 π) + self.slider1 = Slider(ax_slider1, '关节1', -np.pi, np.pi, + valinit=self.joint_angles[0], valfmt='%.2f rad') + self.slider2 = Slider(ax_slider2, '关节2', -np.pi / 2, np.pi / 2, + valinit=self.joint_angles[1], valfmt='%.2f rad') + self.slider3 = Slider(ax_slider3, '关节3', -np.pi / 2, np.pi / 2, + valinit=self.joint_angles[2], valfmt='%.2f rad') + self.slider4 = Slider(ax_slider4, '关节4', -np.pi, np.pi, + valinit=self.joint_angles[3], valfmt='%.2f rad') + + # 滑块回调函数 + self.slider1.on_changed(self.update_angles) + self.slider2.on_changed(self.update_angles) + self.slider3.on_changed(self.update_angles) + self.slider4.on_changed(self.update_angles) + + # 添加重置按钮 + ax_reset = plt.axes([0.8, 0.9, 0.1, 0.04]) + self.reset_button = Button(ax_reset, '重置') + self.reset_button.on_clicked(self.reset_angles) + + def update_angles(self, val): + """更新关节角度""" + self.joint_angles = [ + self.slider1.val, + self.slider2.val, + self.slider3.val, + self.slider4.val + ] + + # 更新DH参数 + for i in range(4): + self.dh_params[i][3] = self.joint_angles[i] + + self.update_plot() + + def reset_angles(self, event): + """重置所有角度为0""" + self.slider1.set_val(0) + self.slider2.set_val(0) + self.slider3.set_val(0) + self.slider4.set_val(0) + + def update_plot(self): + """更新所有图形""" + # 清除所有轴 + self.ax1.clear() + self.ax2.clear() + self.ax3.clear() + + # 重新设置轴属性 + self.ax1.set_title('3D机械臂模型') + self.ax1.set_xlabel('X轴') + self.ax1.set_ylabel('Y轴') + self.ax1.set_zlabel('Z轴') + self.ax1.set_xlim(-3, 3) + self.ax1.set_ylim(-3, 3) + self.ax1.set_zlim(0, 4) + + self.ax2.set_title('XY平面视图') + self.ax2.set_xlabel('X轴') + self.ax2.set_ylabel('Y轴') + self.ax2.set_xlim(-3, 3) + self.ax2.set_ylim(-3, 3) + self.ax2.grid(True) + + self.ax3.set_title('XZ平面视图') + self.ax3.set_xlabel('X轴') + self.ax3.set_ylabel('Z轴') + self.ax3.set_xlim(-3, 3) + self.ax3.set_ylim(0, 4) + self.ax3.grid(True) + # 计算正向运动学 + positions = self.forward_kinematics() + + # 提取坐标 + x_coords = [p[0] for p in positions] + y_coords = [p[1] for p in positions] + z_coords = [p[2] for p in positions] + + # 3D视图 + self.ax1.plot(x_coords, y_coords, z_coords, 'o-', linewidth=3, + markersize=8, color='blue', markerfacecolor='red') + + # 添加基座 + self.ax1.plot([0, 0], [0, 0], [0, -0.5], 'k-', linewidth=5) + + # XY平面视图 + self.ax2.plot(x_coords, y_coords, 'o-', linewidth=2, + markersize=6, color='blue', markerfacecolor='red') + self.ax2.plot(0, 0, 'ks', markersize=10) # 基座 + + # XZ平面视图 + self.ax3.plot(x_coords, z_coords, 'o-', linewidth=2, + markersize=6, color='green', markerfacecolor='red') + self.ax3.plot(0, 0, 'ks', markersize=10) # 基座 + + # 显示末端位置 + end_effector = positions[-1] + self.ax1.text(end_effector[0], end_effector[1], end_effector[2], + f'末端: ({end_effector[0]:.2f}, {end_effector[1]:.2f}, {end_effector[2]:.2f})', + fontsize=10, color='red') + + # 显示角度信息 + angle_text = f'关节角度:\n' + '\n'.join([f'关节{i + 1}: {angle:.2f} rad ({np.degrees(angle):.1f}°)' + for i, angle in enumerate(self.joint_angles)]) + self.ax2.text(-2.8, 2.5, angle_text, fontsize=9, + bbox=dict(boxstyle="round,pad=0.3", facecolor="lightyellow")) + + self.fig.canvas.draw_idle() + + def animate_trajectory(self): + """演示轨迹动画""" + fig_anim = plt.figure(figsize=(10, 8)) + ax_anim = fig_anim.add_subplot(111, projection='3d') + ax_anim.set_title('机械臂轨迹演示') + ax_anim.set_xlabel('X轴') + ax_anim.set_ylabel('Y轴') + ax_anim.set_zlabel('Z轴') + ax_anim.set_xlim(-3, 3) + ax_anim.set_ylim(-3, 3) + ax_anim.set_zlim(0, 4) + + # 生成轨迹点 + t = np.linspace(0, 2 * np.pi, 100) + x_traj = 2 * np.cos(t) + y_traj = 2 * np.sin(t) + z_traj = 2 + 0.5 * np.sin(2 * t) + + # 绘制目标轨迹 + ax_anim.plot(x_traj, y_traj, z_traj, 'r--', alpha=0.5, label='目标轨迹') + + # 动画初始化 + line, = ax_anim.plot([], [], [], 'o-', linewidth=3, markersize=8) + + def init(): + line.set_data([], []) + line.set_3d_properties([]) + return line, + + def animate(i): + # 简单的逆运动学(简化版) + theta1 = np.arctan2(y_traj[i], x_traj[i]) + theta2 = 0.5 + theta3 = -0.3 + theta4 = 0.2 + + # 更新关节角度 + temp_angles = [theta1, theta2, theta3, theta4] + temp_dh = self.dh_params.copy() + for j in range(4): + temp_dh[j][3] = temp_angles[j] + + # 计算位置 + T = np.eye(4) + positions = [np.array([0, 0, 0])] + for params in temp_dh: + a, alpha, d, theta = params + T = T @ self.dh_matrix(a, alpha, d, theta) + positions.append(T[:3, 3]) + + x_coords = [p[0] for p in positions] + y_coords = [p[1] for p in positions] + z_coords = [p[2] for p in positions] + + line.set_data(x_coords, y_coords) + line.set_3d_properties(z_coords) + + return line, + + anim = animation.FuncAnimation(fig_anim, animate, init_func=init, + frames=len(t), interval=50, blit=True) + + ax_anim.legend() plt.show() + return anim -# 示例使用 -if __name__ == "__main__": - # 创建机械臂可视化器(可自定义连杆长度) - arm_visualizer = ArmVisualizer(link_lengths=[6, 4, 3]) - # 设置关节角度(弧度),可以修改这些值查看不同姿态 - # 示例1:初始姿态(伸展) - # angles = [0, 0, 0] +def main(): + """主函数""" + print("正在初始化3D机械臂可视化系统...") + print("=" * 50) + print("操作说明:") + print("1. 使用滑块调整各个关节的角度") + print("2. 点击'重置'按钮将机械臂恢复初始位置") + print("3. 关闭第一个窗口后,将演示轨迹动画") + print("=" * 50) - # 示例2:弯曲姿态 - angles = [np.pi / 4, np.pi / 6, -np.pi / 3] + # 创建机械臂实例 + arm = RoboticArm3D() - # 示例3:更复杂的姿态 - # angles = [np.pi/3, -np.pi/4, np.pi/2] + # 显示交互式界面 + plt.show() - # 绘制并保存机械臂图片 - arm_visualizer.draw_arm(angles, save_path='robot_arm_visualization.png') \ No newline at end of file + # 询问是否显示动画演示 + response = input("\n是否显示轨迹动画演示?(y/n): ") + if response.lower() == 'y': + print("正在生成轨迹动画...") + arm.animate_trajectory() + + +if __name__ == "__main__": + main() \ No newline at end of file