import matplotlib.pyplot as plt from mpl_toolkits.mplot3d.art3d import Poly3DCollection import numpy as np import trimesh def generate_picture_wp(all_group_vars, picture_path, DEBUG=False): # try: vars = all_group_vars['wp'] WPR_X, WPR_Y, WPR_Z, WPF_X, WPF_Y, WPF_Z, WP_CLP_OFFSET_X, WP_GRP_OFFSET_Z = (float(vars[k]) for k in ['WPR_X', 'WPR_Y', 'WPR_Z', 'WPF_X', 'WPF_Y', 'WPF_Z', 'WP_CLP_OFFSET_X', 'WP_GRP_OFFSET_Z']) if min(WPR_X, WPR_Y, WPR_Z, WPF_X, WPF_Y, WPF_Z) <= 0: raise ValueError("Dimensions must be positive.") fig, ax = plt.subplots(subplot_kw={'projection': '3d'}) # -------------------- wpraw ------------------------------------------------------------------------------------- x_cube, y_cube, z_cube = 2 * WPR_X, 2 * WPR_Y, WPR_Z vertices_raw = np.array([[0, 0, 0], [x_cube, 0, 0], [x_cube, y_cube, 0], [0, y_cube, 0], [0, 0, WPR_Z], [x_cube, 0, WPR_Z], [x_cube, y_cube, WPR_Z], [0, y_cube, WPR_Z]]) faces_raw = [vertices_raw[[0, 1, 2, 3]], vertices_raw[[4, 5, 6, 7]], vertices_raw[[0, 3, 7, 4]], vertices_raw[[1, 2, 6, 5]], vertices_raw[[0, 1, 5, 4]], vertices_raw[[2, 3, 7, 6]]] # Achsen-Einstellungen zusammenfassen ax.set_box_aspect([x_cube, y_cube, z_cube]) ax.set(xticks=[], yticks=[], zticks=[]) # KOS WP zeichnen axis_len = max(x_cube, y_cube, z_cube) * 0.2 colors = ['r', 'g', 'b'] origin_x = 0 origin_y = 0 origin_z = 0 for i, vec in enumerate(np.eye(3)): ax.quiver(origin_x, origin_y, origin_z, *(axis_len * vec), color=colors[i], linewidth=3) ax.text(origin_x, origin_y, origin_z-axis_len*0.3, 'WP', color='black', fontsize=9, weight='bold') # WP ax.add_collection3d(Poly3DCollection(faces_raw, facecolors='k', linewidths=1, edgecolors='k', alpha=.15)) # GRIFFPUNKT u, v = np.mgrid[0:2 * np.pi:20j, 0:np.pi:20j] ball_radius = min(WPR_X, WPR_Y, WPR_Z) * 0.1 ax.plot_surface( WPR_X + ball_radius * np.cos(u) * np.sin(v), WPR_Y + ball_radius * np.sin(u) * np.sin(v), WPR_Z + ball_radius * np.cos(v), color='r' ) # GREIFER gripper_thickness = max(x_cube, y_cube, z_cube) * 0.1 gripper_wide = WPR_X * 0.4 GRP_ALPHA = .40 bars = [ ((x_cube - gripper_wide) * 0.5, -gripper_thickness, WPR_Z - WP_GRP_OFFSET_Z), ((x_cube - gripper_wide) * 0.5, y_cube, WPR_Z - WP_GRP_OFFSET_Z), ] for (x, y, z) in bars: ax.bar3d( x, y, z, gripper_wide, gripper_thickness, 5 * WPR_Z + WP_GRP_OFFSET_Z, color='r', alpha=GRP_ALPHA, edgecolor='k', linewidth=0.5 ) # -------------------- wpfin--------------------------------------------------------------------------------- x_cube2, y_cube2, z_cube2 = 2 * WPF_X, 2 * WPF_Y, WPF_Z # GRIFFPUNKT u, v = np.mgrid[0:2 * np.pi:20j, 0:np.pi:20j] ball_radius = min(WPF_X, WPF_Y, WPF_Z) * 0.08 ax.plot_surface( WPF_X + ball_radius * np.cos(u) * np.sin(v), WPF_Y + ball_radius * np.sin(u) * np.sin(v), WPF_Z + ball_radius * np.cos(v), color='g' ) # GREIFER gripper_thickness2 = max(x_cube, y_cube, z_cube) * 0.05 gripper_wide2 = WPR_X * 0.4 GRP_ALPHA2 = .25 # wpf_y_offs = (np.sign(WPF_Y-WPR_Y)/2+.5) * (WPF_Y-WPR_Y) yyy2 = [-gripper_thickness2, y_cube2] if WPF_Y < WPR_Y else [-gripper_thickness2+(WPF_Y-WPR_Y)*2, y_cube] bars = [ ((x_cube2 - gripper_wide2) * 0.5, yyy2[0], WPF_Z - WP_GRP_OFFSET_Z), ((x_cube2 - gripper_wide2) * 0.5, yyy2[1], WPF_Z - WP_GRP_OFFSET_Z), ] for (x, y, z) in bars: ax.bar3d( x, y, z, gripper_wide2, gripper_thickness2, 5 * WPF_Z + WP_GRP_OFFSET_Z, color='g', alpha=GRP_ALPHA2, edgecolor='k', linewidth=0.5 ) # -------------------- clp ------------------------------------------------------------------------------------- # KOS CLP zeichnen axis_len = max(x_cube, y_cube, z_cube) * 0.15 colors = ['r', 'g', 'b'] origin_x = WPR_X - WP_CLP_OFFSET_X origin_y = WPR_Y origin_z = 0 for i, vec in enumerate(np.eye(3)): ax.quiver(origin_x, origin_y, origin_z, *(axis_len * vec), color=colors[i], linewidth=3) ax.text(origin_x, origin_y, origin_z-axis_len*0.3, 'CLP', color='blue', fontsize=9, weight='bold') # Add two black blocks on the Y-faces in the middle of the cube height clp_height = z_cube * 0.2 clp_width = 0.6 * x_cube clp_deep = y_cube * 0.3 # Scale the arrows relative to the smallest dimension clp_supportdeep = 3 # Scale the arrows relative to the smallest dimension base_x = WPR_X - clp_width/2 alpha = 0.35 color = 'blue' linewidth = 0 # Replace the bar3d calls with plot_merged_cubes_trimesh x = base_x - WP_CLP_OFFSET_X dx = clp_width y = 0 dy1 = clp_deep dy2 = clp_supportdeep z = 0 dz = clp_height plot_clamping(ax, x, y, z, dx, dy1, dy2, dz, color=color, alpha=alpha, linewidth=linewidth) y = y_cube plot_clamping(ax, x, y, z, dx, dy1, dy2, dz, color=color, alpha=alpha, linewidth=linewidth, inverty=True) # -------------------- view ------------------------------------------------------------------------------------- ax.view_init(elev=18, azim=-130) # ax.view_init(elev=45, azim=-160) ax.set_xlim([0, x_cube]); ax.set_ylim([0, y_cube]); ax.set_zlim([0, WPR_Z]) ax.set_aspect('auto') if not DEBUG: fig.savefig(picture_path, bbox_inches='tight', dpi=300, transparent=True) else: plt.show() plt.close() # except Exception as e: # print(f"An error occurred: {e}") def plot_clamping(ax, x, y, z, dx, dy1, dy2, dz, color='blue', alpha=0.5, linewidth=1, inverty=False): """Plots merged cubes using trimesh, hiding mesh edges and drawing specific edges manually.""" dyy1 = y - dy1 / 2 if not inverty else y + dy1 / 2 dyy2 = y - (dy1-dy2) / 2 if not inverty else y + (dy1-dy2)/2 # Create and translate Cube 1 cube1 = trimesh.creation.box(extents=[dx, dy1, dz]) cube1.apply_translation([x + dx / 2, dyy1, z + dz / 2]) # Create and translate Cube 2 cube2 = trimesh.creation.box(extents=[dx, dy2+dy1, dz]) cube2.apply_translation([x + dx / 2, dyy2, z - dz / 2]) try: # Perform boolean union merged = trimesh.boolean.union([cube1, cube2]) # Create Poly3DCollection with NO edges ax.add_collection3d(Poly3DCollection( merged.vertices[merged.faces], facecolors=color, alpha=alpha, edgecolors=color, linewidth=linewidth )) except Exception as e: print(f"Trimesh union failed: {e}") if __name__ == "__main__": vars = {'WPR_X': 20, 'WPR_Y': 15, 'WPR_Z': 20, 'WPF_X': 10, 'WPF_Y': 18, 'WPF_Z': 10, 'WP_CLP_OFFSET_X': 3, 'WP_GRP_OFFSET_Z': 5} generate_picture_wp({'wp': vars}, "cfg_picture/TEST_wpraw.jpg", DEBUG=True) # vars = {'x_wpfin': 10, 'y_wpfin': 20, 'z_wpfin': 20, 'l_wpfin': 10} # generate_picture_wpfin({'wp_fin': vars}, "cfg_picture/TEST_wpfin.jpg", DEBUG=True)