from robodk import robolink # RoboDK API from robodk import robomath # Robot toolbox import tkinter as tk from tkinter import simpledialog import time RDK = robolink.Robolink("localhost", 20500) robots = { 'robot_1': RDK.Item('FANUC_1 LR Mate 200iC 5L', robolink.ITEM_TYPE_ROBOT), 'robot_2': RDK.Item('Fanuc_2 M-710iC/45M', robolink.ITEM_TYPE_ROBOT), } tools = {f'tool_{i+1}': robot.Childs() for i, robot in enumerate(robots.values())} current_tool = 0 # Setting the starting tool to the robot flange (0) targets = { 'home_1': RDK.Item('FANUC_1_HOME_pozicija'), 'welding_set': RDK.Item('FANUC_1_WELDING_SET_pozicija'), 'grinding_set': RDK.Item('FANUC_1_GRINDING_SET_pozicija'), 'sanding_set': RDK.Item('FANUC_1_SANDING_SET_pozicija'), 'home_2': RDK.Item('FANUC_2_HOME_pozicija'), 'tocka_1_1': RDK.Item('Tocka_prilaska_WG_alatu'), 'tocka_1_2': RDK.Item('Tocka_uzimanja_WG_alata'), 'tocka_1_3': RDK.Item('Tocka_odlaska_s_WG_alatom'), 'tocka_1_4': RDK.Item('Tocka_izlaza_s_WG_alatom'), 'tocka_1_5': RDK.Item('Tocka_dolaska_do_WG_alata'), 'tocka_2_1': RDK.Item('Tocka_prilaska_GR_alatu'), 'tocka_2_2': RDK.Item('Tocka_uzimanja_GR_alata'), 'tocka_2_3': RDK.Item('Tocka_odlaska_s_GR_alatom'), 'tocka_2_4': RDK.Item('Tocka_izlaza_s_GR_alatom'), 'tocka_2_5': RDK.Item('Tocka_dolaska_do_GR_alata'), 'tocka_3_1': RDK.Item('Tocka_prilaska_SAN_alatu'), 'tocka_3_2': RDK.Item('Tocka_uzimanja_SAN_alata'), 'tocka_3_3': RDK.Item('Tocka_odlaska_sa_SAN_alatom'), 'tocka_3_4': RDK.Item('Tocka_izlaza_sa_SAN_alatom'), 'tocka_3_5': RDK.Item('Tocka_dolaska_do_SAN_alata'), 'tocka_prilaska_prilasku': RDK.Item('Tocka_prilaska_prilasku'), 'tocka_prilaska_uzimanju': RDK.Item('Tocka_prilaska_uzimanju'), 'tocka_uzimanja': RDK.Item('Tocka_uzimanja'), 'tocka_prilaska_zavaru_1': RDK.Item('Tocka_prilaska_zavaru_1'), 'tocka_prilaska_zavaru_2': RDK.Item('Tocka_prilaska_zavaru_2'), 'tocka_prilaska_zavaru_3': RDK.Item('Tocka_prilaska_zavaru_3'), 'zavar_1': RDK.Item('Zavar_1'), 'zavar_2': RDK.Item('Zavar_2'), 'zavar_3': RDK.Item('Zavar_3'), 'tocka_prilaska_zavaru_4': RDK.Item('Tocka_prilaska_zavaru_4'), 'tocka_prilaska_zavaru_5': RDK.Item('Tocka_prilaska_zavaru_5'), 'tocka_prilaska_zavaru_6': RDK.Item('Tocka_prilaska_zavaru_6'), 'zavar_4': RDK.Item('Zavar_4'), 'zavar_5': RDK.Item('Zavar_5'), 'zavar_6': RDK.Item('Zavar_6'), 'tocka_odlaska': RDK.Item('Tocka_odlaska'), 'START_pozicija_zavarivanja_robota': RDK.Item('START_pozicija_zavarivanja_robota'), 'START_pozicija_brusenja_robota': RDK.Item('START_pozicija_brusenja_robota'), 'START_pozicija_poliranja_robota': RDK.Item('START_pozicija_poliranja_robota'), 'START_pozicija_obrade_1': RDK.Item('START_pozicija_obrade_1'), 'START_pozicija_obrade_2':RDK.Item('START_pozicija_obrade_2'), 'END_promjena_prihvata': RDK.Item('END_promjena_prihvata'), 'tocka_prilaska_promjeni_prihvata': RDK.Item('Tocka_prilaska_promjeni_prihvata'), 'tocka_promjene_prihvata': RDK.Item('Tocka_promjene_prihvata'), 'tocka_prilaska_drugom_prihvatu': RDK.Item('Tocka_prilaska_drugom_prihvatu'), 'medutocka_promjene_prihvata': RDK.Item('Medutocka_promjene_prihvata'), 'tocka_drugog_prihvata': RDK.Item('Tocka_drugog_prihvata'), 'tocka_izlaza_s_nosaca_prihvata': RDK.Item('Tocka_izlaza_s_nosaca_prihvata'), 'tocka_prilaska_paleti': RDK.Item('Tocka_prilaska_paleti'), 'tocka_prilaska_ostavljanju': RDK.Item('Tocka_prilaska_ostavljanju'), 'tocka_ostavljanja': RDK.Item('Tocka_ostavljanja'), 'tocka_izlaza_s_palete': RDK.Item('Tocka_izlaza_s_palete'), 'tocka_prilaska_izlazu': RDK.Item('Tocka_prilaska_izlazu'), } frames = { 'komad_ref_frame': RDK.Item('Ref_koo_sustav_komada', robolink.ITEM_TYPE_FRAME), 'stol_ref_frame': RDK.Item('Ref_koo_sustav_stola', robolink.ITEM_TYPE_FRAME), 'homing_ref_frame': RDK.Item('Ref_koo_sustav_hominga', robolink.ITEM_TYPE_FRAME), 'nosac_ref_frame': RDK.Item('Ref_koo_sustav_nosaca', robolink.ITEM_TYPE_FRAME), 'zamjena_alata_ref_frame': RDK.Item('Ref_koo_sustav_zamjene_alata_2', robolink.ITEM_TYPE_FRAME), 'prihvat_ref_frame': RDK.Item('Ref_koo_sustav_prihvata', robolink.ITEM_TYPE_FRAME), 'paleta_ref_frame': RDK.Item('Ref_koo_sustav_palete', robolink.ITEM_TYPE_FRAME) } tool_objects = { '1': RDK.Item('Objekt_WG', robolink.ITEM_TYPE_OBJECT), '2': RDK.Item('Objekt_GR', robolink.ITEM_TYPE_OBJECT), '3': RDK.Item('Objekt_SAN', robolink.ITEM_TYPE_OBJECT), } poses = {} for target_id, target_item in targets.items(): if target_item.Valid(): poses[target_id] = target_item.Pose() else: print(f"Warning: Target '{target_id}' not found in RoboDK station.") if not all(tools.values()): print("Error: No tools found attached to the robots.") exit() weld_path_1 = RDK.Item('Putanja_zavarivanja_1') weld_path_2 = RDK.Item('Putanja_zavarivanja_2') grinding_path_1 = RDK.Item('Putanja_brusenja_1') grinding_path_2 = RDK.Item('Putanja_brusenja_2') grinding_path_3 = RDK.Item('Putanja_brusenja_3') grinding_path_4 = RDK.Item('Putanja_brusenja_4') curve_1 = RDK.Item('Krivulja_poliranja', robolink.ITEM_TYPE_CURVE) curve_2 = RDK.Item('Krivulja_poliranja_2', robolink.ITEM_TYPE_CURVE) curve_3 = RDK.Item('Krivulja_poliranja_3', robolink.ITEM_TYPE_CURVE) curve_4 = RDK.Item('Krivulja_poliranja_4', robolink.ITEM_TYPE_CURVE) polishing_path_1 = RDK.Item('Putanja_poliranja') polishing_path_2 = RDK.Item('Putanja_poliranja_2') polishing_path_3 = RDK.Item('Putanja_poliranja_3') polishing_path_4 = RDK.Item('Putanja_poliranja_4') # repetitions_input = int(input("\nUnesite broj komada koji želite paletizirati(max. 8 komada): ")) # Unos broja komada za paletizaciju pomoću terminala root = tk.Tk() ## Unos broja komada root.withdraw() ## za paletizaciju repetitions_input = simpledialog.askinteger("Input", "Unesite broj komada koji želite paletizirati (max. 8 komada): ") ## pomoću TKINTER libraryja # repetitions_input = easygui.integerbox("Unesite broj komada koji želite paletizirati (max. 8 komada): ") ### Unos broja komada za paletizaciju pomoću EASYGUI libraryja repetitions = min(repetitions_input, 8) print(f"Komadi koji će se paletizirati: {repetitions}") RDK.ShowMessage(f"Komadi koji će se paletizirati: {repetitions}", True) original_object = RDK.Item('Koljeno trodijelno fi 150-90 (245x245)_centered_zlijebovi', robolink.ITEM_TYPE_OBJECT) for i in range(7): RDK.Item(f'Koljeno_{i+1}', robolink.ITEM_TYPE_OBJECT).setVisible(False) if not original_object.Valid(): raise ValueError("Object not found in RoboDK station.") slow_linear_weld_poses = {"zavar_1", "zavar_2", "zavar_3", "zavar_4", "zavar_5", "zavar_6"} joint_poses = {"tocka_1_4", "tocka_2_4", "tocka_3_4", "tocka_prilaska_prilasku", "START_pozicija_obrade_1", "medutocka_promjene_prihvata"} circular_poses = {"START_pozicija_obrade_1", "tocka_prilaska_ostavljanju"} custom_poses = { "home_1": [0.0, -57.180466, 29.7657, 0.0, -74.7657, 0.0, 0.0, -50.0, -10.0, -0.0, 20.0, 90.0], "home_2": [0.0, -50.0, -10.0, 0.0, 20.0, 90.0], "welding_set": [0.000000, -55.000000, 70.000000, 0.000000, -80.000000, 0.000000], "grinding_set": [0.000000, -60.000000, 45.000000, 0.000000, -100.000000, 0.000000], "sanding_set": [0.000000, -60.000000, 55.000000, -0.000000, -60.000000, 0.000000], "START_pozicija_obrade_1": [-4.388814, 9.053399, -36.339812, 92.603954, 93.533991, -53.579778], "START_pozicija_obrade_2": [-4.388814, 9.053399, -36.339812, 92.603954, 93.533991, 126.420222] } def move_sequence(robot, pose_ids, active_frame=frames['homing_ref_frame'], lin_velocity=250, joint_velocity=100, set_object = False, move_C = False, move_J = False): if not robot.Valid(): raise ValueError("Invalid robot specified.") robot.setFrame(active_frame) robot.setSpeed(lin_velocity, joint_velocity) for pose_id in pose_ids: if pose_id in slow_linear_weld_poses: robot.setSpeed(150, joint_velocity) robot.MoveL(poses[pose_id]) time.sleep(1.0) elif pose_id in circular_poses and move_C: if pose_id == "tocka_prilaska_ostavljanju": robot.MoveC(poses["tocka_prilaska_paleti"], poses[pose_id]) else: robot.MoveC(poses["END_promjena_prihvata"], poses[pose_id]) elif pose_id in custom_poses: joints_custom = custom_poses[pose_id] if pose_id.endswith("_set"): if set_object: if "welding" in pose_id or "grinding" in pose_id or "sanding" in pose_id: joints_robot_2 = custom_poses["START_pozicija_obrade_1"] else: joints_robot_2 = robots["robot_2"].Joints().tolist() else: joints_robot_2 = robots["robot_2"].Joints().tolist() joints_combined = joints_custom + joints_robot_2 robot.MoveJ(joints_combined) else: robot.MoveJ(joints_custom) elif pose_id in joint_poses or move_J: robot.MoveJ(poses[pose_id]) elif pose_id in poses: robot.MoveL(poses[pose_id]) else: print(f"Warning: Pose '{pose_id}' not found.") def weld_sequence_1(): move_sequence(robots['robot_1'], ["welding_set"]) move_sequence(robots['robot_2'], pose_sequence_3, frames['nosac_ref_frame']) weld_path_1.RunProgram() weld_path_1.WaitFinished() move_sequence(robots['robot_1'], ["welding_set"]) def weld_sequence_2(): move_sequence(robots['robot_1'], ["welding_set"], set_object=True) weld_path_2.RunProgram() weld_path_2.WaitFinished() move_sequence(robots['robot_1'], ["welding_set"]) def grinding_sequence(): move_sequence(robots['robot_1'], ["grinding_set"], set_object=True) grinding_path_1.RunProgram() grinding_path_1.WaitFinished() grinding_path_2.RunProgram() grinding_path_2.WaitFinished() grinding_path_3.RunProgram() grinding_path_3.WaitFinished() move_sequence(robots['robot_1'], ["grinding_set"]) def polishing_sequence(): move_sequence(robots['robot_1'], ["sanding_set"], set_object=True) polishing_path_1.RunProgram() polishing_path_1.WaitFinished() polishing_path_2.RunProgram() polishing_path_2.WaitFinished() polishing_path_3.RunProgram() polishing_path_3.WaitFinished() polishing_path_4.RunProgram() polishing_path_4.WaitFinished() move_sequence(robots['robot_1'], ["sanding_set"]) def tool_visualization_activation(tool, visible=True, active=False): tool.setVisible(visible) if active: robots['robot_1'].WaitMove() robots['robot_2'].WaitMove() robots['robot_1'].setTool(tool) robots['robot_1'].setPoseTool(tool) GRIPPER_OPEN_DO = 2 # DO[1] GRIPPER_CLOSE_DO = 3 # DO[2] LOCK_TOOL_EXCHANGER_DO_1 = 81 # DO[81] --> RO[3] LOCK_TOOL_EXCHANGER_DO_2 = 82 # DO[82] --> RO[4] def open_gripper(): robots['robot_2'].setDO(GRIPPER_OPEN_DO, 1) robots['robot_2'].setDO(GRIPPER_CLOSE_DO, 0) # RDK.ShowMessage("GRIPPER OPEN!") time.sleep(0.25) def close_gripper(): robots['robot_2'].setDO(GRIPPER_OPEN_DO, 0) robots['robot_2'].setDO(GRIPPER_CLOSE_DO, 1) # RDK.ShowMessage("GRIPPER CLOSED!") time.sleep(0.25) def exchange_tool_lock(): robots['robot_1'].setDO(LOCK_TOOL_EXCHANGER_DO_1, 0) robots['robot_1'].setDO(LOCK_TOOL_EXCHANGER_DO_2, 1) # RDK.ShowMessage("TOOL LOCKED!") time.sleep(0.25) def exchange_tool_unlock(): robots['robot_1'].setDO(LOCK_TOOL_EXCHANGER_DO_1, 1) robots['robot_1'].setDO(LOCK_TOOL_EXCHANGER_DO_2, 0) # RDK.ShowMessage("TOOL UNLOCKED!") time.sleep(0.25) def tool_replacement(previous_tool, new_tool): replace_frame = frames['zamjena_alata_ref_frame'] tool_visualization_activation(tools['tool_1'][0], True, True) ## OSTAVLJANJE ALATA if previous_tool == 0: # TOOL 0 is only the ROBOT FLANGE move_sequence(robots['robot_1'], [f"tocka_{new_tool}_5", f"tocka_{new_tool}_1"], replace_frame, move_J=True) exchange_tool_unlock() move_sequence(robots['robot_1'], [f"tocka_{new_tool}_2"], replace_frame) exchange_tool_lock() tool_visualization_activation(tools['tool_1'][previous_tool], True) else: move_sequence(robots['robot_1'], [f"tocka_{previous_tool}_4", f"tocka_{previous_tool}_3", f"tocka_{previous_tool}_1", f"tocka_{previous_tool}_2"], replace_frame, 200, 50) exchange_tool_unlock() tool_visualization_activation(tools['tool_1'][previous_tool], False) tool_objects[f'{previous_tool}'].setVisible(True) move_sequence(robots['robot_1'], [f"tocka_{previous_tool}_1", f"tocka_{previous_tool}_5"], replace_frame) if new_tool != 0: exchange_tool_unlock() move_sequence(robots['robot_1'], [f"tocka_{new_tool}_5", f"tocka_{new_tool}_1"], replace_frame) move_sequence(robots['robot_1'], [f"tocka_{new_tool}_1"], replace_frame, 50, 25) exchange_tool_unlock() move_sequence(robots['robot_1'], [f"tocka_{new_tool}_2"], replace_frame, 50, 25) exchange_tool_lock() ### UZIMANJE NOVOG ALATA if new_tool == 0: # move_sequence(robots['robot_1'], [f"tocka_{previous_tool}_1", f"tocka_{previous_tool}_5"], replace_frame) tool_visualization_activation(tools['tool_1'][0], True, True) move_sequence(robots['robot_1'], ["home_1"]) else: tool_objects[f'{new_tool}'].setVisible(False) tool_visualization_activation(tools['tool_1'][new_tool], True) move_sequence(robots['robot_1'], [f"tocka_{new_tool}_1",f"tocka_{new_tool}_3", f"tocka_{new_tool}_4"], replace_frame, 200, 100) tool_visualization_activation(tools['tool_1'][0], True) tool_visualization_activation(tools['tool_1'][new_tool], True, True) return new_tool def original_object_initialization(): x, y, z, rx, ry, rz = 0, 0, 0, 0, 0, 0 pose_rel = robomath.transl(x, y, z) * \ robomath.rotx(rx * robomath.pi/180) * \ robomath.roty(ry * robomath.pi/180) * \ robomath.rotz(rz * robomath.pi/180) pose_abs = frames['stol_ref_frame'].Pose() * pose_rel original_object.setParentStatic(frames['komad_ref_frame']) original_object.setPose(pose_rel) original_object.setVisible(True) def grip_change_sequence(): move_sequence(robots['robot_2'], pose_sequence_6, frames['nosac_ref_frame']) move_sequence(robots['robot_2'], pose_sequence_7, frames['prihvat_ref_frame']) original_object.setParentStatic(frames['prihvat_ref_frame']) close_gripper() move_sequence(robots['robot_2'], pose_sequence_8, frames['prihvat_ref_frame']) original_object.setParentStatic(tools['tool_2'][1]) open_gripper() move_sequence(robots['robot_2'], pose_sequence_9, frames['prihvat_ref_frame']) # move_sequence(robots['robot_2'], pose_sequence_4, frames['nosac_ref_frame'], 100, 50, move_C = False) move_sequence(robots['robot_2'], pose_sequence_5, frames['nosac_ref_frame'], 100, 50, move_C = True) def palletization_sequence(object_placement_seq, object_released_seq, counter): x_offset, y_offset = -200, 280 palette_x, palette_y = 4, 2 poses_keys = ["tocka_prilaska_ostavljanju", "tocka_ostavljanja", "tocka_izlaza_s_palete", "tocka_prilaska_izlazu"] if counter != palette_x: x_offsetter = counter*x_offset if counter == 0 else x_offset y_offsetter = 0 else: x_offsetter = -(palette_x - 1) * x_offset y_offsetter = y_offset for pose_key in poses_keys: position = poses[pose_key].Pos() position[0] += x_offsetter position[1] += y_offsetter poses[pose_key].setPos(position) move_sequence(robots['robot_2'], pose_sequence_6, frames['nosac_ref_frame']) move_sequence(robots['robot_2'], object_placement_seq, frames['paleta_ref_frame'], 200, 150, move_C = True) original_object.setParentStatic(frames['paleta_ref_frame']) relative_pose = original_object.Pose() close_gripper() move_sequence(robots['robot_2'], object_released_seq, frames['paleta_ref_frame']) if counter < repetitions - 1: current_working_object = RDK.Item(f'Koljeno_{counter + 1}', robolink.ITEM_TYPE_OBJECT) current_working_object.setParentStatic(frames['paleta_ref_frame']) current_working_object.setPose(relative_pose) original_object.setVisible(False) current_working_object.setVisible(True) ## SEQUENCES USING RECORDED POINTS FROM ROBODK STATION pose_sequence_1 = ["tocka_prilaska_prilasku", "tocka_prilaska_uzimanju", "tocka_uzimanja"] pose_sequence_2 = ["tocka_prilaska_zavaru_1", "zavar_1", "tocka_prilaska_zavaru_1", "tocka_prilaska_zavaru_2", "zavar_2", "tocka_prilaska_zavaru_2", "tocka_prilaska_zavaru_3", "zavar_3", "tocka_prilaska_zavaru_3", "tocka_prilaska_zavaru_6", "zavar_6", "tocka_prilaska_zavaru_6", "tocka_prilaska_zavaru_5", "zavar_5", "tocka_prilaska_zavaru_5", "tocka_prilaska_zavaru_4", "zavar_4", "tocka_prilaska_zavaru_4", "tocka_prilaska_zavaru_5"] pose_sequence_3 = ["tocka_odlaska", "START_pozicija_obrade_1"] # pose_sequence_4 = ["END_promjena_prihvata", "START_pozicija_obrade_1"] # Move_C = False pose_sequence_5 = ["START_pozicija_obrade_1"] # Move_C = True pose_sequence_6 = ["START_pozicija_obrade_1"] pose_sequence_7 = ["tocka_prilaska_promjeni_prihvata", "tocka_promjene_prihvata"] pose_sequence_8 = ["tocka_prilaska_promjeni_prihvata", "medutocka_promjene_prihvata", "tocka_prilaska_drugom_prihvatu", "tocka_drugog_prihvata"] pose_sequence_9 = ["tocka_izlaza_s_nosaca_prihvata"] pose_sequence_10 = ["tocka_prilaska_ostavljanju", "tocka_ostavljanja"] pose_sequence_11 = ["tocka_izlaza_s_palete", "tocka_prilaska_izlazu"] ### MAIN SEQUENCE STARTS HERE ### def main(): exchange_tool_unlock() close_gripper() current_tool = 0 tool_objects['1'].setVisible(True) tool_objects['2'].setVisible(True) tool_objects['3'].setVisible(True) original_object_initialization() # INICIJALIZACIJA RADNOG KOMADA tool_visualization_activation(tools['tool_1'][0], False) tool_visualization_activation(tools['tool_1'][1], False) tool_visualization_activation(tools['tool_1'][2], False) tool_visualization_activation(tools['tool_1'][3], False) tool_visualization_activation(tools['tool_1'][current_tool], True, True) move_sequence(robots['robot_1'], ["home_1"]) # HOMING ROBOTA current_tool = tool_replacement(current_tool, 1) # POSTAVLJANJE ALATA ZA ZAVARIVANJE for path_count in range(repetitions): move_sequence(robots['robot_1'], ["home_1"]) move_sequence(robots['robot_2'], pose_sequence_1, frames['nosac_ref_frame'], 200, 50) # POSTAVLJANJE NA NOSAC ZA ZAVARIVANJE move_sequence(robots['robot_1'], pose_sequence_2, frames['nosac_ref_frame']) # ZAVARIVANJE U TRI TOCKE open_gripper() original_object.setParentStatic(tools['tool_2'][1]) weld_sequence_1() # ZAVARIVANJE_1 grip_change_sequence() # PROMJENA PRIHVATA ZA DRUGI ZAVAR weld_sequence_2() # ZAVARIVANJE_2 current_tool = tool_replacement(current_tool, 2) # PROMJENA ALATA ZA BRUSENJE grinding_sequence() # BRUSENJE grip_change_sequence() # PROMJENA PRIHVATA ZA DRUGI PROLAZ grinding_sequence() # BRUSENJE_2 # current_tool = tool_replacement(current_tool, 3) # PROMJENA ALATA ZA POLIRANJE # polishing_sequence() # POCETAK POLIRANJA # grip_change_sequence(current_tool) # PROMJENA PRIHVATA ZA DRUGI PROLAZ # polishing_sequence() # KRAJ POLIRANJA palletization_sequence(pose_sequence_10, pose_sequence_11, path_count) # PALETIZACIJA if path_count < repetitions - 1: current_tool = tool_replacement(current_tool, 1) original_object_initialization() # VRACANJE RADNOG KOMADA U INICIJALNU POZICIJU else: current_tool = tool_replacement(current_tool, 0) if __name__ == "__main__": main()