def main(): # start the server server = Server(server_address, server_port) server.start() server.client_ips.update({"UR": ur_ip}) # create client wrappers, that wrap the underlying communication to the sockets ur = ClientWrapper("UR") # wait for the clients to be connected ur.wait_for_connected() # now enter fabrication loop counter = 0 while True: # and ur and gh connected # send test cmds ur.send_command_digital_out(2, True) # open tool ur.wait_for_ready() ur.send_command_digital_out(2, False) # close tool ur.wait_for_ready() """ ur.send_command_airpick(True) #send vac grip on ur.wait_for_ready() ur.send_command_airpick(False) #send vac grip off ur.wait_for_ready() """ counter += 0.01 #ur.send_command_movel([-0.78012+counter, 0.1214, 0.13928, 0.97972, -1.06629, 1.18038], v=0.1, a=1.2) # move to a location ur.send_command_movej([-1.765, -1.268, 2.373, 3.608, -1.571, -3.336], v=.1, a=.1) ur.wait_for_ready() # read out joint values from the rcv_queue current_pose_joints_queue = ur.rcv_queues[MSG_CURRENT_POSE_JOINT] current_pose_joint = current_pose_joints_queue.get_nowait() print("current pose joint: ", current_pose_joint) # read out pose cartesian from the rcv_queue current_pose_cartesian_queue = ur.rcv_queues[ MSG_CURRENT_POSE_CARTESIAN] current_pose_cartesian = current_pose_cartesian_queue.get_nowait() print("current pose cartesian: ", current_pose_cartesian) print("============================================================") ur.quit() server.close() print("Please press a key to terminate the program.") junk = sys.stdin.readline() print("Done.")
def main(): print("hi") # start the server server = Server(server_address, server_port) # print("Server"), server server.start() server.client_ips.update({"UR": ur_ip}) # create client wrappers, that wrap the underlying communication to the sockets gh = ClientWrapper("GH") ur = ClientWrapper("UR") # wait for the clients to be connected gh.wait_for_connected() ur.wait_for_connected() number_of_completed_poses = 0 # now enter fabrication loop while True: # and ur and gh connected # 1. let gh control if we should continue # continue_fabrication = gh.wait_for_int() # print("continue_fabrication: %i" % continue_fabrication) # if not continue_fabrication: # break # 2. set the tool tcp tool_pose = gh.wait_for_float_list( ) # client.send(MSG_FLOAT_LIST, tool_pose) print("2: set tool TCP") ur.send_command_tcp(tool_pose) # 3. receive command length print('3: send length of message') len_command = gh.wait_for_int() print("length", len_command) # 4. receive commands flattened, and format according to the sent length commands_flattened = gh.wait_for_float_list() print("commands", commands_flattened) commands = format_commands(commands_flattened, len_command) print("We received %i commands." % len(commands)) # 5. receive safety commands flattened, and format according to the sent length # savety_commands_flattened = gh.wait_for_float_list() # savety_commands = format_commands(savety_commands_flattened, len_command) # print("savety_commands_flattened: %s" % str(savety_commands_flattened)) for i, cmd in enumerate(commands): # x, y, z, ax, ay, az, acceleration, speed, wait_time = cmd if cmd[0] == 0.0: print((len(cmd) - 1) / 9) for j in range((len(cmd) - 1) / 9): print("inside_loop") ind_s = j + 1 ind_e = j + 1 + 6 pose = cmd[ind_s:ind_e] print("pose", pose) acceleration, speed, wait_time = cmd[ind_e:ind_e + 3] print("speed_data", cmd[ind_e:ind_e + 3]) ##ur.send_command_digital_out(number, True) ur.send_command_movel(pose, a=acceleration, v=speed) ## ur.send_command_digital_out(number, False) ur.wait_for_ready() number_of_completed_poses += 1 else: for j in range((len(cmd) - 4) / 9): if j == cmd[1] or j == cmd[3]: ur.send_command_digital_out(0, False) ur.send_command_digital_out(1, True) elif j == cmd[2]: ur.send_command_digital_out(1, False) ur.send_command_digital_out(0, True) ind_s = j * 9 + 4 ind_e = j * 9 + 4 + 6 pose_joints = cmd[ind_s:ind_e] print("pose_joints", pose_joints) acceleration, speed, wait_time = cmd[ind_e:ind_e + 3] print("speed_data", cmd[ind_e:ind_e + 3]) ##ur.send_command_digital_out(number, True) ur.send_command_movej(pose_joints, a=acceleration, v=speed) ## ur.send_command_digital_out(number, False) # x, y, z, ax, ay, az = cmd[-9:-3] # print("position", cmd[-9:-3]) # acceleration, speed, wait_time = cmd[-3:] # print("speed data", cmd[-3:]) # ur.send_command_movel([x, y, z, ax, ay, az], a=acceleration, v=speed) ur.wait_for_ready() number_of_completed_poses += 1 # current_pose_joint = ur.wait_for_current_pose_joint() # if i == len(commands) - 1: # send savety commands # for cmd in savety_commands: # x, y, z, ax, ay, az, acceleration, speed, wait_time = cmd # ur.send_command_movel([x, y, z, ax, ay, az], a=acceleration, v=speed) # ur.wait_for_ready() # gh.send_float_list(current_pose_joint) # ur.wait_for_ready() # # update time for deposition constantly # deposition_time = gh.wait_for_float_list()[0] # # if the value is not zero, overwrite, otherwise take the list value # if deposition_time == 0: # deposition_time = wait_time # # send cmd to place the material # if i != len(commands) - 1: # send_cmd_place_sand(ur, deposition_time) # check if the routine is paused # pause = gh.wait_for_int() # print("pause: %i" % pause) # if pause: # stop_pause = gh.wait_for_int() print("number of completed robot poses: %i" % number_of_completed_poses) # print("sand_deposition_time: %f" % deposition_time) # ur.wait_for_ready() print("============================================================") """ ur.wait_for_ready() # wait for sensor value digital_in = ur.wait_for_digital_in(number) current_pose_joint = ur.wait_for_current_pose_joint() current_pose_cartesian = ur.get_current_pose_cartesian() # send further to gh gh.send_float_list(digital_in) gh.send_float_list(current_pose_joint) gh.send_float_list(current_pose_cartesian) """ ur.quit() gh.quit() server.close() print("Please press a key to terminate the program.") junk = sys.stdin.readline() print("Done.")