def main():
# start the server
server = Server(server_address, server_port)
server.start()
# create client wrappers, that wrap the underlying communication to the sockets
gh = ClientWrapper("GH")
# wait for the clients to be connected
gh.wait_for_connected()
# now enter fabrication loop
while True: # and ur and gh connected
# let gh control if we should continue
continue_fabrication = gh.wait_for_int()
print("continue_fabrication: %i" % continue_fabrication)
if not continue_fabrication:
break
float_list = gh.wait_for_float_list()
# do some calculation
x, y = float_list
sum = x + y
print("%f + %f = %f" % (x, y, sum))
gh.send_float_list([sum])
gh.quit()
server.close()
time.sleep(2)
print("Done.")
def main():
# start the server
server = Server(server_address, server_port)
server.start()
server.client_ips.update({"UR": ur_ip})
ur = ClientWrapper("UR")
ur.wait_for_connected()
for i in range(100):
analog_in = ur.wait_for_current_analog_in(1)
print("analog_in", analog_in)
# also working:
#digital_in = ur.wait_for_current_digital_in(1)
#print("digital_in", digital_in)
#current_pose_joint = ur.wait_for_current_pose_joint()
#print("current_pose_joint", current_pose_joint)
#current_pose_cartesian = ur.wait_for_current_pose_cartesian()
#print("current_pose_cartesian", current_pose_cartesian)
time.sleep(0.5)
print("============================================================")
ur.quit()
server.close()
print("Please press a key to terminate the program.")
junk = sys.stdin.readline()
print("Done.")
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
gh = ClientWrapper("GH")
ur = ClientWrapper("UR")
# wait for the clients to be connected
gh.wait_for_connected()
ur.wait_for_connected()
# now enter fabrication loop
while True: # and ur and gh connected
# let gh control if we should continue
continue_fabrication = gh.wait_for_int(
) #1 client.send(MSG_INT, int(continue_fabrication))
print("1: continue_fabrication: %i" % continue_fabrication)
print("start fabrication")
ur.send_command_digital_out(0, True) # Turn ON
"""
if not continue_fabrication:
break
"""
tool_string = gh.wait_for_float_list(
) #2 client.send(MSG_FLOAT_LIST,tool_string_axis)
print("2: set tool TCP")
ur.send_command_tcp(tool_string)
in_pose_cmd = gh.wait_for_float_list(
) #3 client.send(MSG_FLOAT_LIST, safe_in_pose_cmd)
print("3: safe pose")
x, y, z, ax, ay, az, speed, acc = in_pose_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, a=acc)
ur.send_command_wait(0.5)
waiting_times = gh.wait_for_float_list(
) #3 client.send(MSG_FLOAT_LIST, safe_in_pose_cmd)
print("NEW: waiting_times")
len_command = gh.wait_for_int() #4 client.send(MSG_INT, len_command)
print("4: len command list")
commands_flattened = gh.wait_for_float_list(
) #5 client.send(MSG_FLOAT_LIST, commands_flattened)
print("5: command list")
# the commands are formatted according to the sent length
commands = format_commands(commands_flattened, len_command)
print("We received %i commands." % len(commands))
# placing path
for i in range(0, len(commands), 1):
placing_cmd = commands[i]
wait = waiting_times[i]
print(wait)
if i == 0:
x, y, z, ax, ay, az, speed, radius = placing_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
else:
# turn sand on
ur.send_command_digital_out(0, False) # Turn ON extruder
# wait
ur.send_command_wait(1.0)
# turn sand off
ur.send_command_digital_out(0, True) # Turn OFF extruder
# wait again
ur.send_command_wait(2.0)
# move to next point
x, y, z, ax, ay, az, speed, radius = placing_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
#ur.send_command_movel([x, y, z, ax, ay, az], r=radius, t=2)
ur.send_command_digital_out(0, True) # Turn OFF extruder
ur.send_command_wait(0.5)
ur.wait_for_ready()
### Out Pose ###
safe_out_pose_cmd = gh.wait_for_float_list(
) #6 client.send(MSG_FLOAT_LIST, safe_out_pose_cmd)
print("6: safe out pose")
x, y, z, ax, ay, az, speed, acc = safe_out_pose_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, a=acc)
ur.send_command_wait(0.5)
ur.wait_for_ready()
#ur.wait_for_ready()
print("============================================================")
ur.quit()
gh.quit()
server.close()
print("Please press a key to terminate the program.")
junk = sys.stdin.readline()
print("Done.")
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
gh = ClientWrapper("GH")
ur = ClientWrapper("UR")
# wait for the clients to be connected
gh.wait_for_connected()
ur.wait_for_connected()
# now enter fabrication loop
while True: # and ur and gh connected
# let gh control if we should continue
continue_fabrication = gh.wait_for_int()
print("continue_fabrication: %i" % continue_fabrication)
if not continue_fabrication:
break
safe_pt_toggle = gh.wait_for_int()
len_command = gh.wait_for_int()
commands_flattened = gh.wait_for_float_list()
# the commands are formatted according to the sent length
commands = format_commands(commands_flattened, len_command)
print("We received %i commands." % len(commands))
if safe_pt_toggle:
print("Moving to safe point")
for i, cmd in enumerate(commands):
if i == 0:
# Move to first point, toggle extruder and wait
x, y, z, ax, ay, az, speed, radius = cmd
ur.send_command_movel([x, y, z, ax, ay, az],
v=speed,
r=radius)
print("Toggling extruder")
ur.send_command_digital_out(0, True)
print("Waiting for 30 seconds")
ur.send_command_wait(30)
# And move axis
# p = s.SiemensPortal(1)
# p.set_x(650)
# p.set_z(850)
else:
x, y, z, ax, ay, az, speed, radius = cmd
ur.send_command_movel([x, y, z, ax, ay, az],
v=speed,
r=radius)
else:
print("Skipping safe points")
for i, cmd in enumerate(commands[1:-1]):
x, y, z, ax, ay, az, speed, radius = cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
# for i, cmd in enumerate(commands):
# ur.wait_for_command_executed(i)
# print("Executed command", i+1, "of", len(commands), "[", (i+1)*100/(len(commands)), "%]")
# current_pose_cartesian = ur.get_current_pose_cartesian()
# print(current_pose_cartesian)
ur.wait_for_ready()
ur.send_command_digital_out(0, False)
gh.send_float_list(commands[0])
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.")
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
gh = ClientWrapper("GH")
ur = ClientWrapper("UR")
# wait for the clients to be connected
gh.wait_for_connected()
ur.wait_for_connected()
# now enter fabrication loop
while True: # and ur and gh connected
# let gh control if we should continue
continue_fabrication = gh.wait_for_int(
) #1 client.send(MSG_INT, int(continue_fabrication))
print("1: continue_fabrication: %i" % continue_fabrication)
print("start fabrication")
if not continue_fabrication:
break
tool_string = gh.wait_for_float_list(
) #2 client.send(MSG_FLOAT_LIST,tool_string_axis)
print("2: set string tool TCP")
ur.send_command_tcp(tool_string)
in_pose_cmd = gh.wait_for_float_list(
) #3 client.send(MSG_FLOAT_LIST, safe_in_pose_cmd)
print("3: safe string pose")
x, y, z, ax, ay, az, speed, acc = in_pose_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, a=acc)
ur.send_command_wait(0.5)
len_command = gh.wait_for_int() #4 client.send(MSG_INT, len_command)
print("4: len command list")
commands_flattened = gh.wait_for_float_list(
) #5 client.send(MSG_FLOAT_LIST, commands_flattened)
print("5: command list")
# the commands are formatted according to the sent length
commands = format_commands(commands_flattened, len_command)
print("We received %i commands." % len(commands))
# placing path
for i in range(0, len(commands), 1):
placing_cmd = commands[i]
if i == 0:
x, y, z, ax, ay, az, speed, radius = placing_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, a=acc)
if i == 1:
### Turn on string tool ###
ur.send_command_digital_out(0, False)
x, y, z, ax, ay, az, speed, radius = placing_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
else:
### Move smoothly to next plane ###
x, y, z, ax, ay, az, speed, radius = placing_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
ur.send_command_digital_out(0, True) # turn off string tool
ur.send_command_wait(0.5)
ur.wait_for_ready()
### Out Pose ###
safe_out_pose_cmd = gh.wait_for_float_list(
) #6 client.send(MSG_FLOAT_LIST, safe_out_pose_cmd)
print("6: safe out pose")
x, y, z, ax, ay, az, speed, acc = safe_out_pose_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, a=acc)
ur.send_command_wait(0.5)
ur.wait_for_ready()
#ur.wait_for_ready()
### Gravel Placment ###
tool_gravel = gh.wait_for_float_list(
) #7 client.send(MSG_FLOAT_LIST,tool_gravel_axis)
ur.send_command_tcp(tool_gravel)
ur.wait_for_ready()
print("7: Set TCP Done")
safe_in_pose_gravel_cmd = gh.wait_for_float_list(
) #8 client.send(MSG_FLOAT_LIST, safe_in_pose_gravel)
x, y, z, ax, ay, az, speed, acc = safe_in_pose_gravel_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, a=acc)
ur.send_command_wait(0.5)
ur.wait_for_ready()
print("8: Safe gravel pose Done")
pose_gravel_cmd = gh.wait_for_float_list(
) #9 client.send(MSG_FLOAT_LIST, pose_gravel)
x, y, z, ax, ay, az, speed, acc = pose_gravel_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, a=acc)
ur.send_command_wait(5)
ur.wait_for_ready()
print("9: gravel pose done")
safe_out_pose_gravel_cmd = gh.wait_for_float_list(
) #10 client.send(MSG_FLOAT_LIST, safe_out_pose_gravel)
x, y, z, ax, ay, az, speed, acc = safe_out_pose_gravel_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, a=acc)
ur.send_command_wait(0.5)
ur.wait_for_ready()
print("10: Safe out gravel pose Done")
print("============================================================")
ur.quit()
gh.quit()
server.close()
print("Please press a key to terminate the program.")
junk = sys.stdin.readline()
print("Done.")
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
gh = ClientWrapper("GH")
ur = ClientWrapper("UR")
# wait for the clients to be connected
gh.wait_for_connected()
ur.wait_for_connected()
# now enter fabrication loop
while True: # and ur and gh connected
# let gh control if we should continue
continue_fabrication = gh.wait_for_int()
print("continue_fabrication: %i" % continue_fabrication)
if not continue_fabrication:
break
len_command = gh.wait_for_int()
commands_flattened = gh.wait_for_float_list()
# the commands are formatted according to the sent length
commands = format_commands(commands_flattened, len_command)
print("We received %i commands." % len(commands))
for cmd in commands:
x, y, z, ax, ay, az, speed, radius = cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
#ur.send_command_movel([x, y, z, ax, ay, az], v=speed, a=acceleration)
ur.wait_for_ready()
print("============================================================")
ur.quit()
gh.quit()
server.close()
print("Please press a key to terminate the program.")
junk = sys.stdin.readline()
print("Done.")
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
gh = ClientWrapper("GH")
ur = ClientWrapper("UR")
# wait for the clients to be connected
gh.wait_for_connected()
ur.wait_for_connected()
# now enter fabrication loop
while True: # and ur and gh connected
# let gh control if we should continue
continue_fabrication = gh.wait_for_int() #1 client.send(MSG_INT, int(continue_fabrication))
print("1: continue_fabrication: %i" % continue_fabrication)
print ("start fabrication")
tool_string = gh.wait_for_float_list() #2 client.send(MSG_FLOAT_LIST,tool_string_axis)
print("2: set tool TCP")
ur.send_command_tcp(tool_string)
### Safe Pose ###
safety_pose_cmd = gh.wait_for_float_list() #3 client.send(MSG_FLOAT_LIST, safe_in_pose_cmd)
print("3: safe in pose")
x, y, z, ax, ay, az, speed, acc = safety_pose_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, a=acc)
ur.send_command_wait(1.0)
len_command = gh.wait_for_int() #4 client.send(MSG_INT, len_command)
print ("4: len command list")
commands_flattened = gh.wait_for_float_list() #5 client.send(MSG_FLOAT_LIST, commands_flattened)
print ("5: command list")
# the commands are formatted according to the sent length
commands = format_commands(commands_flattened, len_command)
print("We received %i commands." % len(commands))
# ur.wait_for_ready()
# print (time.time())
# printing path
for i in range(0, len(commands)):
printing_cmd = commands[i]
x, y, z, ax, ay, az, speed, radius = printing_cmd
# print tcp_WCS
# joint_angles = ur.get
# ur.send_command_movej(joint_angles, v=speed_set, r=radius)
# ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
# ur_frame = Frame([x, y, z], [])
ur.wait_for_ready()
print (time.time())
ur.wait_for_ready()
#ur.wait_for_ready()
print("============================================================")
ur.quit()
gh.quit()
server.close()
print("Please press a key to terminate the program.")
junk = sys.stdin.readline()
print("Done.")
def main():
logger.info("\n\nlog started ,main is running\n")
# start the server
server = Server(server_address, server_port)
server.start()
server.client_ips.update({"UR": ur_ip})
logger.info("server started")
# create client wrappers, that wrap the underlying communication to the sockets
gh = ClientWrapper("GH")
ur = ClientWrapper("UR")
logger.info("client wrappers created")
# wait for the clients to be connected
gh.wait_for_connected()
ur.wait_for_connected()
logger.info("gh and ur are connected")
continue_fabrication = gh.wait_for_int()
print("continue_fabrication: %i" % continue_fabrication)
move_filament_loading_pt = gh.wait_for_int()
print("move_filament_loading_pt: %i" % move_filament_loading_pt)
linear_axis_toggle = gh.wait_for_int()
print("linear_axis_toggle: %i" % linear_axis_toggle)
#if linear_axis_toggle:
axis_moving_pts_indices = gh.wait_for_float_list()
print("We received %i linear axis_moving_pts_indices" % len(axis_moving_pts_indices))
logger.info("{} float list of axis_moving_pts_indices received".format(len(axis_moving_pts_indices)))
len_command = gh.wait_for_int()
print("len_command: %i" % len_command)
no_batches = gh.wait_for_int()
print("no_batches: %i" % no_batches)
commands_to_wait_flattened = []
for j in range(no_batches):
commands_flattened = gh.wait_for_float_list()
# create mew list for commands executed
commands_to_wait_flattened.extend(commands_flattened)
# the commands are formatted according to the sent length
commands = format_commands(commands_flattened, len_command)
print("We received %i commands." % len(commands))
logger.info("{} float list of commands_flattened received".format(len(commands)))
if move_filament_loading_pt:
commands_to_send = commands
else:
commands_to_send = commands[1:-1]
for i, cmd in enumerate(commands_to_send):
x, y, z, ax, ay, az, speed, radius = cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
logger.info("all commands in batch number {} were sent".format(j))
break
print("=>>>>>>>>>>>>>>>>>>>>> wait for the 3rd")
ur.wait_for_command_executed(3)
ur.purge_commands()
# ur.quit()
# time.sleep(3)
# ur.wait_for_connected()
print("01_waiting for ready")
# # time.sleep(1.5)
ur.wait_for_ready()
print("02 send new commands")
# toggle extruder, turn off motor
# move away robot problem: it still sends it at end
x1, y1, z1, ax1, ay1, az1, speed1, radius1 = commands_to_send[0]
ur.send_command_movel([x1, y1, z1, ax1, ay1, az1], v=speed1, r=radius1)
print("03_Moving robot to a safe point")
# print("Waiting for 10 seconds")
# time.sleep(10)
print("04_waiting for ready")
ur.wait_for_ready()
print("======================")
ur.quit()
gh.quit()
# time.sleep(3)
server.close()
logger.info("siemens protal, gh, ur and server are closed")
print("Please press a key to terminate the program.")
junk = sys.stdin.readline()
print("Done.")
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
gh = ClientWrapper("GH")
ur = ClientWrapper("UR")
# wait for the clients to be connected
gh.wait_for_connected()
ur.wait_for_connected()
# now enter fabrication loop
while True: # and ur and gh connected
# let gh control if we should continue
continue_fabrication = gh.wait_for_int()
print("continue_fabrication: %i" % continue_fabrication)
if not continue_fabrication:
break
# receive further information from gh
# e.g. send number of commands:
# number = gh.wait_for_int()
# commands = []
# for i in range(number):
# cmd = gh.wait_for_float_list()
# commands.append(cmd)
picking_pose_cmd = gh.wait_for_float_list()
savety_pose_cmd = gh.wait_for_float_list()
len_command = gh.wait_for_int()
commands_flattened = gh.wait_for_float_list()
# the commands are formatted according to the sent length
commands = format_commands(commands_flattened, len_command)
print("We received %i commands." % len(commands))
# 1. move to savety pose
x, y, z, ax, ay, az, acc, vel = savety_pose_cmd
ur.send_command_movel([x, y, z, ax, ay, az], a=acc, v=vel)
# 2. open gripper
ur.send_command_digital_out(2, True) # open tool
# 3. move to picking pose
x, y, z, ax, ay, az, acc, vel = picking_pose_cmd
ur.send_command_movel([x, y, z, ax, ay, az], a=acc, v=vel)
# 4. Close gripper
ur.send_command_digital_out(2, False) # close tool
# 5. move to savety pose
x, y, z, ax, ay, az, acc, vel = savety_pose_cmd
ur.send_command_movel([x, y, z, ax, ay, az], a=acc, v=vel)
ur.wait_for_message(self, msg_id)
# placing path
for i in range(0, len(commands), 3):
savety_cmd1 = commands[i]
placing_cmd = commands[i+1]
savety_cmd2 = commands[i+2]
# 5. move to savety pose 1
x, y, z, ax, ay, az, speed, radius = savety_cmd1
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
# 6. move to placing pose
x, y, z, ax, ay, az, speed, radius = placing_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
# 7. wait
ur.send_command_wait(2.)
# 8. open gripper
ur.send_command_digital_out(2, True) # open tool
# 7. wait
ur.send_command_wait(1.)
# 9. move to savety pose 2
x, y, z, ax, ay, az, speed, radius = savety_cmd2
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
# 1. move to savety pose
x, y, z, ax, ay, az, acc, vel = savety_pose_cmd
ur.send_command_movel([x, y, z, ax, ay, az], a=acc, v=vel)
# 2. open gripper
ur.send_command_digital_out(2, True) # open tool
# 3. move to picking pose
x, y, z, ax, ay, az, acc, vel = picking_pose_cmd
ur.send_command_movel([x, y, z, ax, ay, az], a=acc, v=vel)
# 4. Close gripper
ur.send_command_digital_out(2, False) # close tool
# 5. move to savety pose
x, y, z, ax, ay, az, acc, vel = savety_pose_cmd
ur.send_command_movel([x, y, z, ax, ay, az], a=acc, v=vel)
ur.wait_for_ready()
gh.send_float_list(commands[0])
print("============================================================")
break
"""
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.")
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.")
args = parser.parse_args()
print('Starting UR control server...')
print('Press CTRL+C to abort')
print(
'NOTE: if CTRL+C does not abort cleanly, set the env variable to FOR_DISABLE_CONSOLE_CTRL_HANDLER=1'
)
print('UR Server starting...', end='\r')
server = Server(args.server_address, args.server_port)
server.start()
server.client_ips.update({'UR': args.ur_address})
print('UR Server started. ')
print('Waiting for UR client...', end='\r')
ur = ClientWrapper("UR")
ur.wait_for_connected()
print('UR client connected. Ready to process tasks.')
print('ROS Nodes status:')
print(' [ ] ROS Services', end='\r')
client = RosClient(host=args.ros_host, port=int(args.ros_port))
storage_service = roslibpy.Service(client, '/store_assembly_task',
'workshop_tum_msgs/AssemblyTask')
storage_service.advertise(storage_handler)
execute_service = roslibpy.Service(client, '/execute_assembly_task',
'workshop_tum_msgs/AssemblyTask')
execute_service.advertise(functools.partial(execution_handler, ur=ur))
print(' [X] ROS Services')
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.")
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
gh = ClientWrapper("GH")
ur = ClientWrapper("UR")
# wait for the clients to be connected
gh.wait_for_connected()
ur.wait_for_connected()
# now enter fabrication loop
while True: # and ur and gh connected
# let gh control if we should continue
continue_fabrication = gh.wait_for_int(
) #1 client.send(MSG_INT, int(continue_fabrication))
print("1: continue_fabrication: %i" % continue_fabrication)
print("start fabrication")
"""
if not continue_fabrication:
break
"""
tool_string = gh.wait_for_float_list(
) #2 client.send(MSG_FLOAT_LIST,tool_string_axis)
print("2: set tool TCP")
ur.send_command_tcp(tool_string)
len_command_picking = gh.wait_for_int(
) #3 client.send(MSG_INT, len_command)
print("3: len command list picking")
commands_flattened_picking = gh.wait_for_float_list(
) #4 client.send(MSG_FLOAT_LIST, commands_flattened)
print("4: command list picking")
len_command_placing = gh.wait_for_int(
) #3 client.send(MSG_INT, len_command)
print("5: len command list placing")
commands_flattened_placing = gh.wait_for_float_list(
) #4 client.send(MSG_FLOAT_LIST, commands_flattened)
print("6: command list placing")
# the commands are formatted according to the sent length
commands_picking = format_commands(commands_flattened_picking,
len_command_picking)
print("We received %i picking commands." % len(commands_picking))
commands_placing = format_commands(commands_flattened_placing,
len_command_placing)
print("We received %i placing commands." % len(commands_placing))
# placing path
picking_cnt = 0
for i in range(0, len(commands_flattened_placing), 3):
ur.send_command_wait(0.3)
ur.send_command_digital_out(0, False)
ur.send_command_wait(0.3)
#above picking
placing_cmd = commands_picking[picking_cnt]
x, y, z, ax, ay, az, speed, radius = placing_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
#picking
placing_cmd = commands_picking[picking_cnt + 1]
x, y, z, ax, ay, az, speed, radius = placing_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
ur.send_command_wait(1.0)
#rotate
placing_cmd = commands_picking[picking_cnt + 2]
x, y, z, ax, ay, az, speed, radius = placing_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
ur.send_command_wait(1.0)
#above picking
placing_cmd = commands_picking[picking_cnt + 3]
x, y, z, ax, ay, az, speed, radius = placing_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
#above placing
placing_cmd = commands_placing[i]
x, y, z, ax, ay, az, speed, radius = placing_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
#placing
placing_cmd = commands_placing[i + 1]
x, y, z, ax, ay, az, speed, radius = placing_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
ur.send_command_wait(0.3)
ur.send_command_digital_out(0, True)
ur.send_command_wait(0.3)
#above placing
placing_cmd = commands_placing[i + 2]
x, y, z, ax, ay, az, speed, radius = placing_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
picking_cnt += 4
ur.send_command_popup()
ur.send_command_digital_out(0, True) # Turn ON piston
ur.send_command_wait(0.5)
ur.wait_for_ready()
### Out Pose ###
#safe_out_pose_cmd = gh.wait_for_float_list() #6 client.send(MSG_FLOAT_LIST, safe_out_pose_cmd)
#print ("6: safe out pose")
#x, y, z, ax, ay, az, speed, acc = safe_out_pose_cmd
#ur.send_command_movel([x, y, z, ax, ay, az], v=speed, a=acc)
ur.send_command_wait(0.5)
ur.wait_for_ready()
#ur.wait_for_ready()
print("============================================================")
ur.quit()
gh.quit()
server.close()
print("Please press a key to terminate the program.")
junk = sys.stdin.readline()
print("Done.")
def main():
logger.info("\n\nlog started ,main is running\n")
# start the server
server = Server(server_address, server_port)
server.start()
server.client_ips.update({"UR": ur_ip})
logger.info("server started")
ur = ClientWrapper("UR")
time.sleep(0.5)
ur_driver.send()
ur.wait_for_connected()
move_filament_loading_pt = data['move_filament_loading_pt']
linear_axis_toggle = data['move_linear_axis']
axis_moving_pts_indices = data['axis_moving_pts_indices']
len_command = data['len_command']
batches = data['batches']
print("Batch length:", len(batches))
print("move_filament_loading_pt: %i" % move_filament_loading_pt)
logger.info(
"move_filament_loading_pt: {}".format(move_filament_loading_pt))
print("linear_axis_toggle: %i" % linear_axis_toggle)
logger.info("linear_axis_toggle: {}".format(linear_axis_toggle))
print("We received %i linear axis_moving_pts_indices" %
len(axis_moving_pts_indices))
logger.info("{} float list of axis_moving_pts_indices received".format(
len(axis_moving_pts_indices)))
#if the ur required to start extruding always from the same start base
print("============================================================")
linear_axis_x = 500
linear_axis_z = 800
if linear_axis_toggle:
# And move axis
p = s.SiemensPortal(2)
logger.info("siemens portal connected")
currentPosX = p.get_x()
currentPosZ = p.get_z()
if currentPosZ != linear_axis_z or currentPosX != linear_axis_x:
p.set_x(linear_axis_x)
p.set_z(linear_axis_z)
print("Linear axis is set to default x and z value")
p.wait_ext_axis()
p.close()
print("============================================================")
print("len_command: %i" % len_command)
# move linear axis except start and end (at filament loading and unloading pos)
linear_axis_move_z = linear_axis_z
linear_axis_failed = False
print("============================================================")
p1 = s.SiemensPortal(2)
# loop through all commands batches
command_counter = 0
for j, batch in enumerate(batches):
commands = format_commands(batch, len_command)
print("For batch nr %d, we received %i commands." %
(j + 1, len(commands)))
logger.info("{} float list of commands_flattened received".format(
len(commands)))
if move_filament_loading_pt:
commands = commands[1:-1]
difference_to_prev_command_before_sending_next = 10
# ===>>> start sending commands
for i, cmd in enumerate(commands):
x, y, z, ax, ay, az, speed, radius = cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
command_counter += 1
if move_filament_loading_pt and i == 0 and j == 0:
# after Moving to first point, toggle extruder and wait
print("Moved to safe point")
print("Toggling extruder")
ur.send_command_digital_out(0, True)
command_counter += 1
print("Waiting for 2 seconds")
ur.send_command_wait(2)
command_counter += 1
print("all commands in batch number %i were sent." % j)
logger.info("all commands in batch number {} were sent".format(j))
wait_for_counter = command_counter - difference_to_prev_command_before_sending_next
print("Waiting for command counter %d" % wait_for_counter)
ur.wait_for_command_executed(wait_for_counter)
print("Executed command {} of {}".format(wait_for_counter,
len(commands) * len(batches)))
if linear_axis_toggle:
if i in axis_moving_pts_indices:
linear_axis_move_z += 1
p1.set_z(linear_axis_move_z)
print("Linear axis is supposed to move to %d mm" %
linear_axis_move_z)
#sleeping time depends on linear axis override speed
#for ex. 0.5 sec sleep is not enough to move 16mm in z
time.sleep(.7)
linear_axis_currentPosZ = p1.get_z()
if linear_axis_move_z == linear_axis_currentPosZ:
print("SUCCESS")
print("Linear axis moved to %d mm" %
linear_axis_currentPosZ)
print("Linear axis moved at point index %d" % i)
logger.info("SUCCESS")
logger.info("Linear axis moved to {} mm".format(
linear_axis_currentPosZ))
logger.info("Executed command {} of {} [{}%]".format(
i + 1, len(commands_to_wait),
(i + 1) * 100 / (len(commands_to_wait))))
else:
print(
"============================================================"
)
print("FAILED")
print("Linear axis still at %d mm" %
linear_axis_currentPosZ)
print("Linear axis failed to move at point index %d" % i)
logger.info("FAILED")
logger.info("Linear axis still at {} mm".format(
linear_axis_currentPosZ))
logger.info("Executed command {} of {} [{}%]".format(
i + 1, len(commands_to_wait),
(i + 1) * 100 / (len(commands_to_wait))))
ur.purge_commands()
print("00_purge done")
print("waiting for ready ......... ")
ur.wait_for_ready()
print("01_send new commands")
# toggle extruder, turn off motor
ur.send_command_digital_out(0, False)
logger.info("02_Nozzle Motor Stopped")
print("02_Nozzle Motor Stopped")
# move away robot problem: it still sends it at end
x1, y1, z1, ax1, ay1, az1, speed1, radius1 = commands_to_wait[
0]
ur.send_command_movel([x1, y1, z1, ax1, ay1, az1],
v=speed1,
r=radius1)
print("03_Moving robot to a safe point")
logger.info("03_Moving robot to a safe point")
print("waiting for ready ......... ")
ur.wait_for_ready()
linear_axis_failed = True
ur.quit()
print("04_ur quit")
logger.info("04_ur quit")
if not linear_axis_failed:
print("============================================================")
ur.wait_for_ready()
print("waiting for ready")
ur.send_command_digital_out(0, False)
logger.info("nozzle motor stopped")
print("Nozzle Motor Stopped")
ur.wait_for_ready()
logger.info("all commands were successfully executed")
print("all commands were successfully executed")
print("============================================================")
ur.quit()
p1.close()
time.sleep(1)
server.close()
print("siemens protal, gh, ur and server are closed")
logger.info("siemens protal, gh, ur and server are closed")
print("Please press a key to terminate the program.")
junk = sys.stdin.readline()
print("Done.")
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
gh = ClientWrapper("GH")
ur = ClientWrapper("UR")
# wait for the clients to be connected
gh.wait_for_connected()
ur.wait_for_connected()
# now enter fabrication loop
while True: # and ur and gh connected
# let gh control if we should continue
continue_fabrication = gh.wait_for_int(
) #1 client.send(MSG_INT, int(continue_fabrication))
print("1: continue_fabrication: %i" % continue_fabrication)
print("start fabrication")
tool_string = gh.wait_for_float_list(
) #2 client.send(MSG_FLOAT_LIST,tool_string_axis)
print("2: set tool TCP")
ur.send_command_tcp(tool_string)
### Safe Pose ###
safety_pose_cmd = gh.wait_for_float_list(
) #3 client.send(MSG_FLOAT_LIST, safe_in_pose_cmd)
print("3: safe in pose")
x, y, z, ax, ay, az, speed, acc = safety_pose_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, a=acc)
ur.send_command_wait(1.0)
#turn on air and off motor
ur.send_command_digital_out(4, False)
ur.send_command_digital_out(5, True)
ur.wait_for_ready()
ur.send_command_popup(title='hello!',
message='Press ok when you are ready!',
blocking=True)
ur.wait_for_ready()
#turn on motor
ur.send_command_digital_out(4, True)
ur.wait_for_ready()
ur.send_command_popup(title='hello!',
message='Press ok when you are ready!',
blocking=True)
ur.wait_for_ready()
len_command = gh.wait_for_int() #4 client.send(MSG_INT, len_command)
print("4: len command list")
commands_flattened = gh.wait_for_float_list(
) #5 client.send(MSG_FLOAT_LIST, commands_flattened)
print("5: command list")
# the commands are formatted according to the sent length
commands = format_commands(commands_flattened, len_command)
print("We received %i commands." % len(commands))
# printing path
count = 0
for i in range(0, len(commands)):
printing_cmd = commands[i]
x, y, z, ax, ay, az, speed, radius, extrude_bool = printing_cmd
if extrude_bool == 0:
count += 1
ur.send_command_digital_out(4, False)
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
if count > 1:
ur.send_command_digital_out(4, True)
ur.send_command_wait(0.5)
else:
count = 0
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
for i, cmd in enumerate(commands[:-2]):
ur.wait_for_command_executed(i)
print("Executed command", i + 1)
print(cmd)
### Safe Pose ###
safety_pose_cmd = gh.wait_for_float_list(
) #6 client.send(MSG_FLOAT_LIST, safe_out_pose_cmd)
print("6: safe out pose")
x, y, z, ax, ay, az, speed, acc = safety_pose_cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, a=acc)
ur.send_command_wait(5.0)
# turn off motor and extruder
ur.send_command_digital_out(4, False)
ur.send_command_wait(0.5)
ur.wait_for_ready()
#ur.wait_for_ready()
print("============================================================")
ur.quit()
gh.quit()
server.close()
print("Please press a key to terminate the program.")
junk = sys.stdin.readline()
print("Done.")
def main():
logger.info("\n\nlog started ,main is running\n")
# start the server
server = Server(server_address, server_port)
server.start()
server.client_ips.update({"UR": ur_ip})
logger.info("server started")
# create client wrappers, that wrap the underlying communication to the sockets
ur = ClientWrapper("UR")
logger.info("client wrappers created")
# wait for the clients to be connected
ur.wait_for_connected()
logger.info("ur is connected")
# now enter fabrication loop
while True: # and ur and gh connected
commands_to_send = [[i] * 8 for i in range(20)]
for i, cmd in enumerate(commands_to_send):
x, y, z, ax, ay, az, speed, radius = cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
time.sleep(4)
ur.purge_commands()
print("waiting for ready")
ur.wait_for_ready()
print("send new commands")
for i, cmd in enumerate(commands_to_send):
x, y, z, ax, ay, az, speed, radius = cmd
ur.send_command_movel([x, y, z, ax, ay, az], v=speed, r=radius)
print("============================================================")
ur.quit()
server.close()
logger.info("siemens protal, gh, ur and server are closed")
print("Please press a key to terminate the program.")
junk = sys.stdin.readline()
print("Done.")