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 1/3] =?UTF-8?q?=E6=9C=BA=E6=A2=B0=E8=87=82=E7=9A=84?= =?UTF-8?q?=E5=90=84=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 From 172097d535d2e04caaae33fd221d6c16192a12a1 Mon Sep 17 00:00:00 2001 From: lijinghan12 <15894570343@163.com> Date: Fri, 19 Dec 2025 16:52:42 +0800 Subject: [PATCH 2/3] =?UTF-8?q?=E6=9C=BA=E6=A2=B0=E8=87=82=E7=9A=84?= =?UTF-8?q?=E8=BF=90=E5=8A=A8=E8=8C=83=E5=9B=B4=E5=8F=8A=E8=87=82=E9=95=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/mechanical_arm/arm_test.py | 220 +++++++++++++++++++++++++++++++++ 1 file changed, 220 insertions(+) create mode 100644 src/mechanical_arm/arm_test.py diff --git a/src/mechanical_arm/arm_test.py b/src/mechanical_arm/arm_test.py new file mode 100644 index 0000000000..b98ef08b9f --- /dev/null +++ b/src/mechanical_arm/arm_test.py @@ -0,0 +1,220 @@ +import matplotlib.pyplot as plt +import numpy as np +import os +import matplotlib.patches as patches + +# ==================== PyCharm环境配置 ==================== +plt.switch_backend('TkAgg') # 显示交互窗口(如需静默保存改'Agg') +plt.rcParams['font.sans-serif'] = ['SimHei'] # 解决中文显示 +plt.rcParams['axes.unicode_minus'] = False +plt.rcParams['figure.dpi'] = 200 # 高清渲染 +plt.rcParams['lines.antialiased'] = True # 抗锯齿 + +# 保存路径(桌面,可自定义) +SAVE_PATH = os.path.join(os.path.expanduser("~"), "Desktop", "精细机械臂可视化.png") + + +class PrecisionArmVisualizer: + def __init__(self): + """初始化精细机械臂参数""" + # 工业级机械臂参数(模拟三菱RV-2F) + self.link_params = { + 'link1': {'length': 120, 'width': 15, 'color': '#1f77b4', 'max_angle': 180, 'min_angle': -180}, + 'link2': {'length': 100, 'width': 12, 'color': '#ff7f0e', 'max_angle': 150, 'min_angle': -90}, + 'link3': {'length': 80, 'width': 10, 'color': '#2ca02c', 'max_angle': 120, 'min_angle': -120}, + 'link4': {'length': 60, 'width': 8, 'color': '#d62728', 'max_angle': 180, 'min_angle': -180} + } + # 关节参数 + self.joint_params = { + 'base': {'radius': 25, 'color': '#000000', 'bearing_width': 5}, + 'joint': {'radius': 20, 'color': '#7f7f7f', 'bearing_width': 4}, + 'end_effector': {'radius': 22, 'color': '#9467bd', 'gripper_length': 30, 'gripper_width': 15} + } + # 工作空间参数 + self.workspace = {'radius': 350, 'color': '#e377c2', 'alpha': 0.1} + + def calculate_joint_coords(self, angles): + """高精度计算关节坐标(毫米单位)""" + # 角度转换(输入为角度,内部转弧度) + angles_rad = np.radians(angles) + coords = np.array([[0.0, 0.0]]) # 基座原点 + + current_angle = 0.0 + for i, angle in enumerate(angles_rad): + link_key = f'link{i + 1}' + link_len = self.link_params[link_key]['length'] + + current_angle += angle + dx = link_len * np.cos(current_angle) + dy = link_len * np.sin(current_angle) + + next_coord = coords[-1] + np.array([dx, dy]) + coords = np.vstack([coords, next_coord]) + + return coords + + def draw_precision_arm(self, angles=[30, -45, 60, -15]): + """绘制精细机械臂""" + # 1. 计算坐标 + coords = self.calculate_joint_coords(angles) + num_joints = len(coords) + + # 2. 创建画布(工业图纸风格) + fig, ax = plt.subplots(figsize=(12, 12)) + ax.set_xlim(-400, 400) + ax.set_ylim(-400, 400) + ax.set_aspect('equal', adjustable='box') + + # 3. 绘制工作空间 + workspace_circle = patches.Circle((0, 0), self.workspace['radius'], + facecolor=self.workspace['color'], + edgecolor=self.workspace['color'], + alpha=self.workspace['alpha'], + label='工作空间范围') + ax.add_patch(workspace_circle) + + # 4. 绘制连杆(精细分层) + for i in range(num_joints - 1): + link_key = f'link{i + 1}' + link = self.link_params[link_key] + + # 计算连杆中心点和方向 + start = coords[i] + end = coords[i + 1] + dx = end[0] - start[0] + dy = end[1] - start[1] + length = np.hypot(dx, dy) + angle = np.arctan2(dy, dx) + + # 绘制连杆主体(填充) + link_rect = patches.Rectangle( + (start[0], start[1] - link['width'] / 2), + length, link['width'], + angle=np.degrees(angle), + facecolor=link['color'], + edgecolor='black', + linewidth=1.5, + alpha=0.8 + ) + ax.add_patch(link_rect) + + # 绘制连杆轮廓(增强立体感) + link_outline = patches.Rectangle( + (start[0], start[1] - link['width'] / 2), + length, link['width'], + angle=np.degrees(angle), + facecolor='none', + edgecolor='white', + linewidth=0.8, + linestyle='-' + ) + ax.add_patch(link_outline) + + # 连杆参数标注 + mid_x = (start[0] + end[0]) / 2 + mid_y = (start[1] + end[1]) / 2 + ax.text(mid_x, mid_y, + f'L{i + 1}\n{link["length"]}mm\n{angles[i]}°', + fontsize=8, ha='center', va='center', + bbox=dict(boxstyle='round,pad=0.3', facecolor='white', alpha=0.9), + rotation=np.degrees(angle)) + + # 5. 绘制关节(带轴承细节) + # 基座关节 + base = self.joint_params['base'] + base_circle = patches.Circle(coords[0], base['radius'], + facecolor=base['color'], edgecolor='white', linewidth=2) + ax.add_patch(base_circle) + # 基座轴承 + base_bearing = patches.Circle(coords[0], base['radius'] - base['bearing_width'], + facecolor='#333333', edgecolor='white', linewidth=1) + ax.add_patch(base_bearing) + + # 中间关节 + joint = self.joint_params['joint'] + for i in range(1, num_joints - 1): + # 关节主体 + joint_circle = patches.Circle(coords[i], joint['radius'], + facecolor=joint['color'], edgecolor='white', linewidth=1.5) + ax.add_patch(joint_circle) + # 关节轴承 + joint_bearing = patches.Circle(coords[i], joint['radius'] - joint['bearing_width'], + facecolor='#555555', edgecolor='white', linewidth=0.8) + ax.add_patch(joint_bearing) + + # 关节限位标注 + link_key = f'link{i}' + link = self.link_params[link_key] + ax.text(coords[i][0] + 30, coords[i][1] + 30, + f'限位:{link["min_angle"]}°~{link["max_angle"]}°', + fontsize=7, bbox=dict(boxstyle='round,pad=0.2', facecolor='lightyellow', alpha=0.8)) + + # 6. 绘制末端执行器(夹爪细节) + end_eff = self.joint_params['end_effector'] + end_coord = coords[-1] + # 末端主体 + end_circle = patches.Circle(end_coord, end_eff['radius'], + facecolor=end_eff['color'], edgecolor='white', linewidth=2) + ax.add_patch(end_circle) + + # 夹爪 + gripper_len = end_eff['gripper_length'] + gripper_wid = end_eff['gripper_width'] + # 左夹爪 + left_gripper = patches.Rectangle( + (end_coord[0] + 10, end_coord[1] - gripper_wid / 2), + gripper_len, gripper_wid / 2, + facecolor='#888888', edgecolor='black', linewidth=1 + ) + ax.add_patch(left_gripper) + # 右夹爪 + right_gripper = patches.Rectangle( + (end_coord[0] + 10, end_coord[1] + gripper_wid / 4), + gripper_len, gripper_wid / 2, + facecolor='#888888', edgecolor='black', linewidth=1 + ) + ax.add_patch(right_gripper) + + # 7. 绘制辅助信息 + # 坐标网格(精细刻度) + ax.grid(True, which='both', alpha=0.2, linestyle='-', linewidth=0.5) + ax.set_xticks(np.arange(-400, 401, 50)) + ax.set_yticks(np.arange(-400, 401, 50)) + ax.tick_params(labelsize=9) + + # 末端坐标高精度标注 + end_x, end_y = end_coord + ax.text(end_x + 50, end_y - 50, + f'末端执行器参数\n坐标:({end_x:.2f}, {end_y:.2f}) mm\n姿态角:{sum(angles):.1f}°\n理论负载:2.0 kg', + fontsize=9, bbox=dict(boxstyle='round,pad=0.5', facecolor='lightgreen', alpha=0.9)) + + # 标题和图例 + ax.set_title('高精度工业机械臂2D可视化(1:10比例)', fontsize=14, pad=20) + ax.legend(loc='upper left', fontsize=10, framealpha=0.9) + + # 8. 保存高清图片 + try: + plt.tight_layout() + fig.savefig(SAVE_PATH, dpi=300, bbox_inches='tight', facecolor='white') + print(f"\n✅ 精细机械臂图片已保存至:") + print(f" {SAVE_PATH}") + print(f" 图片分辨率:{fig.get_size_inches()[0] * 300}x{fig.get_size_inches()[1] * 300} 像素") + except Exception as e: + print(f"\n❌ 保存失败:{str(e)}") + + # 显示交互窗口(PyCharm中可缩放/平移) + plt.show() + + +# ==================== 运行入口 ==================== +if __name__ == "__main__": + # 创建可视化器 + visualizer = PrecisionArmVisualizer() + + # 设置关节角度(可自定义,单位:度) + # 顺序:link1角度, link2角度, link3角度, link4角度 + arm_angles = [45, -60, 75, -30] + + # 绘制精细机械臂 + visualizer.draw_precision_arm(angles=arm_angles) + From ed3f6fc5ad522dee7c2d8860e8b57f4aa171e037 Mon Sep 17 00:00:00 2001 From: lijinghan12 <15894570343@163.com> Date: Fri, 19 Dec 2025 17:35:03 +0800 Subject: [PATCH 3/3] =?UTF-8?q?=E6=8E=A7=E5=88=B6=E6=9C=BA=E6=A2=B0?= =?UTF-8?q?=E8=87=82=E8=BF=9B=E8=A1=8C=E7=AE=80=E5=8D=95=E8=BF=90=E5=8A=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/mechanical_arm/test_five.py | 305 ++++++++++++++++++++++++++++++++ 1 file changed, 305 insertions(+) create mode 100644 src/mechanical_arm/test_five.py diff --git a/src/mechanical_arm/test_five.py b/src/mechanical_arm/test_five.py new file mode 100644 index 0000000000..acf6ba89f7 --- /dev/null +++ b/src/mechanical_arm/test_five.py @@ -0,0 +1,305 @@ +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) + + # 添加控制滑块 + self.setup_sliders() + + # 初始化绘图 + 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 + + +def main(): + """主函数""" + print("正在初始化3D机械臂可视化系统...") + print("=" * 50) + print("操作说明:") + print("1. 使用滑块调整各个关节的角度") + print("2. 点击'重置'按钮将机械臂恢复初始位置") + print("3. 关闭第一个窗口后,将演示轨迹动画") + print("=" * 50) + + # 创建机械臂实例 + arm = RoboticArm3D() + + # 显示交互式界面 + plt.show() + + # 询问是否显示动画演示 + response = input("\n是否显示轨迹动画演示?(y/n): ") + if response.lower() == 'y': + print("正在生成轨迹动画...") + arm.animate_trajectory() + + +if __name__ == "__main__": + main() \ No newline at end of file