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.")