def init_movements(i, base, base_cyclic, return_val):
# Initial Movements
success = True
#Move into transport position (home)
home_pose = [184.19, 291.7, 171.7, 213.3, 181.8, 45.8, 266.6]
#success &= highlevel_movements.angular_action_movement(base,joint_positions=home_pose,speed=5)
#sys.exit()
# home_pose=[-0.154,0,0.34,-55.9,173.3,86.6]
# success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,waypoint=home_pose,speed=0.1)
#Exectue Sequence
#success &= highlevel_movements.call_kinova_web_sequence(base, base_cyclic,seq_name="Grab_Beer")
#sys.exit()
#move to surveillance position
success &= highlevel_movements.move_to_waypoint_linear(
base,
base_cyclic, [-0.06, -0.096, 0.8, -90.823, 171.8, 113.73],
speed=0.05)
return_val = success
type=float,
default=0.1,
help='Score threshold for detected objects.')
#parser.add_argument('--camera_idx', type=int, help='Index of which video source to use. ', default = 1)
args = parser.parse_args()
labels = load_labels(args.labels) if args.labels else {}
interpreter = make_interpreter(args.model)
interpreter.allocate_tensors()
# Initial Movements
success = True
#Move into transport position (home)
home_pose = [184.19, 291.7, 171.7, 213.3, 181.8, 45.8, 266.6]
#success &= highlevel_movements.angular_action_movement(base,joint_positions=home_pose,speed=5)
success &= highlevel_movements.move_to_waypoint_linear(
base,
base_cyclic, [-0.06, -0.096, 0.8, -90.823, 171.8, 113.73],
speed=0.1)
#sys.exit()
# home_pose=[-0.154,0,0.34,-55.9,173.3,86.6]
# success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,waypoint=home_pose,speed=0.1)
#Exectue Sequence
#success &= highlevel_movements.call_kinova_web_sequence(base, base_cyclic,seq_name="grab_beer_v3")
#sys.exit()
#Execute Sequence V2
"""
speedj=5
speedl=0.1
success &= highlevel_movements.angular_action_movement(base,joint_positions=[89.695, 336.743, 176.642, 232.288, 180.629, 70.981, 272.165],speed=speedj)
success &= highlevel_movements.angular_action_movement(base,joint_positions=[91.583, 23.663, 174.547, 265.846, 180.949, 35.446, 272.106],speed=speedj)
success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.01796681061387062, -0.6095998287200928, 0.2607220709323883, -89.98168182373047, -176.41896057128906, 178.88327026367188],speed=speedl)
success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.015759950503706932, -0.6483935713768005, 0.2543827295303345, -91.34257507324219, 178.12986755371094, 178.2921905517578],speed=speedl)
#Main
#Move Robot into position
# Import the utilities helper module
#sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
from Subfunctions import utilities
# Parse arguments
args = utilities.parseConnectionArguments()
# Create connection to the device and get the router
with utilities.DeviceConnection.createTcpConnection(args) as router:
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
# Example core
success = True
success &= highlevel_movements.move_to_waypoint_linear(
base, base_cyclic, [0.02, -0.08, 0.78, 90, 3, 3])
#Start
cap = cv2.VideoCapture('rtsp://192.168.1.10/color')
t0 = time.time()
while (True):
ret, frame = cap.read()
#crop the frame so the grippertip is gone
(h, w) = frame.shape[:2]
frame = frame[0:int(h * 0.88)]
#Place the middle circle
(h, w) = frame.shape[:2]
vision_middle = (int(w / 2), int(h / 2))
cv2.circle(frame, vision_middle, target_circle_size * 2, (255, 0, 255),
1)
#get the target circle
image, target_middle, success_flag = functions_.detect_rectangle_middle(
return cv2_im
from Subfunctions import utilities
# Parse arguments
args = utilities.parseConnectionArguments()
# Create connection to the device and get the router
with utilities.DeviceConnection.createTcpConnection(args) as router:
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
# Example core
success = True
#success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.02,-0.08,0.78,90,3,3])
success &= highlevel_movements.move_to_waypoint_linear(
base,
base_cyclic, [-0.06, -0.096, 0.8, -90.823, 171.8, 113.73],
speed=0.1)
##TESTING AREA
"""## Just for fun: Reading Robot torque values
torque_tool=functions_.TorqueTool()
print(torque_tool.read_torque_values(base_cyclic))
#Function tap_toggle - Robot is waiting for a touch
torque_threshold=3 #the torque that has to be applied as a tap
torque_tool.tap_toggle(base_cyclic,torque_threshold)
sys.exit()"""
##
"""#Testing POSETRANS
feedback=base_cyclic.RefreshFeedback()
#print(feedback.base.tool_pose_x)
#print(feedback.actuators[0].torque)
startpose=[
# time.sleep(1) # success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.016505524516105652, -0.65036940574646, 0.38649141788482666, -92.8912353515625, 178.2748565673828, 179.34559631347656],speed=speedl) # success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.07478067278862, -0.1246325895190239, 0.8614432215690613, 90.16726684570312, 9.310687065124512, 5.854083061218262],speed=speedl) # # move to surveillance position #success &= highlevel_movements.angular_action_movement(base,joint_positions=[179.62, 307.879, 172.652, 289.174, 180.408, 69.243, 272.359],speed=speedj) #move to surveillance position #success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[-0.06,-0.096,0.8,-90.823,171.8,113.73],speed=0.1) #Dog Sequence success &= highlevel_movements.angular_action_movement(base,joint_positions=[268, 240, 178, 213, 186, 56, 270],speed=4) #Dog Pose success &= highlevel_movements.send_gripper_command(base,value=0) success &= highlevel_movements.angular_action_movement(base,joint_positions=[267.55, 265.4, 177.448, 213.575, 186.333, 34.982, 269.921],speed=1.5) #sys.exit() #success &= highlevel_movements.angular_action_movement(base,joint_positions=[258.146, 263.791, 79.082, 233.233, 173.541, 63.965, 178.723],speed=2) success &= highlevel_movements.angular_action_movement(base,joint_positions=[235.447, 265.103, 86.313, 213.584, 170.83, 239.68, 178.675],speed=3) success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.008, -0.555, 0.253, 88.106, 2.153, 8.451],speed=speedl) #GRIPPER_ACTION success &= highlevel_movements.send_gripper_command(base,value=0.4) time.sleep(1.5) success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.003, -0.555, 0.377, 88.107, 2.155, 8.496],speed=speedl) success &= highlevel_movements.angular_action_movement(base,joint_positions=[217.115, 287.757, 83.895, 268.331, 209.21, 38.784, 171.332],speed=4) enable_detection=False success &= highlevel_movements.angular_action_movement(base,joint_positions=[92.74, 293.44, 175.53, 276.7, 184.22, 69.3, 271.8],speed=5,watch_for_flag=enable_detection,success_flag_shared=success_flag_shared.value) #sys.exit() #search for a face and interrupt when found #vision_action.autofocus_action(vision_config, vision_device_id,action_id=4) success &= highlevel_movements.example_send_joint_speeds(base,speeds=[-18, 0, 0, 0, 0, 0, 0],timer=20,success_flag_shared=success_flag_shared) #Find Face
def main():
target_middle_shared=multiprocessing.Array("i",[0,0])
delta_shared=multiprocessing.Array("i",[0,0])
success_flag_shared=multiprocessing.Value("i",0)
vision_middle_shared=multiprocessing.Array("i",[0,0])
h_shared=multiprocessing.Value("i",0)
w_shared=multiprocessing.Value("i",0)
elapsed_time=multiprocessing.Value("d",0.0)
args = utilities.parseConnectionArguments()
# Create connection to the device and get the router
e = multiprocessing.Event()
p = multiprocessing.Process(target=cam, args=('rtsp://192.168.1.10/color',e,target_middle_shared,delta_shared,success_flag_shared,vision_middle_shared,h_shared,w_shared,elapsed_time,beer_time))
p.start()
with utilities.DeviceConnection.createTcpConnection(args) as router:
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
device_manager = DeviceManagerClient(router)
#Activate the continuous auto-focus of the camera
vision_config = VisionConfigClient(router)
vision_device_id = vision_action.example_vision_get_device_id(device_manager)
# if vision_device_id != 0:
# vision_action.autofocus_action(vision_config, vision_device_id,action_id=2)
# Initial Movements
success = True
#Move into transport position (home)
home_pose=[184.19,291.7,171.7,213.3,181.8,45.8,266.6]
#success &= highlevel_movements.angular_action_movement(base,joint_positions=home_pose,speed=5)
#success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[-0.06,-0.096,0.8,-90.823,171.8,113.73],speed=speedl)
# Grab the beer
#Desk Sequence
# highlevel_movements.send_gripper_command(base,value=0.0)
# #success &= highlevel_movements.angular_action_movement(base,joint_positions=[89.695, 336.743, 176.642, 232.288, 180.629, 70.981, 272.165],speed=speedj)
# success &= highlevel_movements.angular_action_movement(base,joint_positions=[91.583, 23.663, 174.547, 265.846, 180.949, 35.446, 272.106],speed=speedj)
# success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.01796681061387062, -0.6095998287200928, 0.2607220709323883, -89.98168182373047, -176.41896057128906, 178.88327026367188],speed=speedl)
# success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.015759950503706932, -0.6483935713768005, 0.2543827295303345, -91.34257507324219, 178.12986755371094, 178.2921905517578],speed=speedl)
# # GRIPPER_ACTION
# success &= highlevel_movements.send_gripper_command(base,value=0.7)
# time.sleep(1)
# success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.016505524516105652, -0.65036940574646, 0.38649141788482666, -92.8912353515625, 178.2748565673828, 179.34559631347656],speed=speedl)
# success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.07478067278862, -0.1246325895190239, 0.8614432215690613, 90.16726684570312, 9.310687065124512, 5.854083061218262],speed=speedl)
# # move to surveillance position
#success &= highlevel_movements.angular_action_movement(base,joint_positions=[179.62, 307.879, 172.652, 289.174, 180.408, 69.243, 272.359],speed=speedj)
#move to surveillance position
#success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[-0.06,-0.096,0.8,-90.823,171.8,113.73],speed=0.1)
#Dog Sequence Desired Pose : [249.37, 239.26, 223.77, 133.81, 217.24, 259.69, 307.95]
#TODO Startposition [249.37, 239.26, 223.77, 133.81, 217.24, 259.69, 307.95] check
#TODO Schwenk ins Publikum nach vorne (extra funktion, muss aufgerufen werden) Tkinter
#TODO Winken nur mit oberen zwei Gelenken Tkinter
#TODO Theta_y korrigieren
#TODO Sicht korrigieren (Fokus und ggf. heller/Dunkler)
# success &= highlevel_movements.angular_action_movement(base,joint_positions=[249.37, 239.26, 223.77, 133.81, 217.24, 259.69, 307.95],speed=10) #Dog Pose
# sys.exit()
success &= highlevel_movements.send_gripper_command(base,value=0) #open gripper
success &= highlevel_movements.angular_action_movement(base,joint_positions=[233.73, 265.96, 71.5, 212.18, 349.19, 70.11, 8.31],speed=speedj) #pre snake pose
success &= highlevel_movements.angular_action_movement(base,joint_positions=[257.64, 246.34, 31.6, 218.63, 214.63, 98.69, 129.78],speed=3) #desired dog pose
#success &= highlevel_movements.angular_action_movement(base,joint_positions=[233.73, 265.96, 71.5, 212.18, 349.19, 70.11, 8.31],speed=speedj) #pre snake pose
success &= highlevel_movements.angular_action_movement(base,joint_positions=[238.78, 264.98, 70.21, 217.99, 349.19, 120.32, 8.3],speed=3) #snake
success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.006, -0.536, 0.191, 88.99, 1.239, 7.97],speed=speedl)
#GRIPPER_ACTION
success &= highlevel_movements.send_gripper_command(base,value=0.4)
time.sleep(1.5)
success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.008, -0.535, 0.275, 88.988, 1.245, 7.968],speed=speedl) #move a little up
#success &= highlevel_movements.angular_action_movement(base,joint_positions=[235.7, 344.71, 64.58, 219.35, 49.72, 80.94, 54.84],speed=speedj) # upright1
#success &= highlevel_movements.angular_action_movement(base,joint_positions=[231.32, 353.1, 32.7, 304.08, 37.1, 321.32, 58.52],speed=speedj) #upright 2
success &= highlevel_movements.angular_action_movement(base,joint_positions=[270.94, 42.99, 348.31, 305.15, 0.84, 286.28, 91.49],speed=speedj) #final
#success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,[0.117, -0.058, 0.914, 86.046, 9.61, 6.732],speed=speedl) #final
#sys.exit()
#search for a face and interrupt when found
#vision_action.autofocus_action(vision_config, vision_device_id,action_id=4)
success &= highlevel_movements.example_send_joint_speeds(base,speeds=[18, 0, 0, 0, 0, 0, 0],timer=20,success_flag_shared=success_flag_shared)
#sys.exit()
#Find Face
t0=time.time()
token=False
target_reached_flag=False
beer_mode=False
print("searching for person")
while True:
if time.time()-t0>action_time_interval and success_flag_shared.value==True and beer_mode==False and p.is_alive():
#calculate the pose increment and move robot into target
target_reached_flag=functions_.rotate_to_target(
delta_shared[:],target_middle_shared[:],vision_middle_shared[:],w_shared.value,h_shared.value,max_speed,min_speed,
target_circle_size,base,base_cyclic)
t0=time.time()
elif time.time()-t0>action_time_interval and success_flag_shared.value==False and beer_mode==False:
base.Stop()
# if time.time()-t0>2:
# vision_action.autofocus_action(vision_config, vision_device_id,action_id=6,focus_point=target_middle_shared[:])
elif p.is_alive()==False:
base.Stop()
print("Process aborded")
sys.exit()
#after robot is Xs in target, hand over beer
if (target_reached_flag==True and token==True and success_flag_shared.value==True) or beer_mode==True:
elapsed_time.value=time.time()-beer_timer
if elapsed_time.value>beer_time:
if beer_mode==False:
reach_beer_twist_timer=time.time()
beer_mode=True
#the target was long enough in scope
tool_twist_duration=6
if time.time()-reach_beer_twist_timer<tool_twist_duration:
highlevel_movements.tool_twist_time(base,tool_twist_duration,pose_distance=[0,-0.35,0.4,0,0,0])
else:
base.Stop()
time.sleep(2) #swing out
#wait until force applied to robot
torque_tool=functions_.TorqueTool()
torque_tool.tap_toggle(base_cyclic,torque_threshold=5)
#open gripper
highlevel_movements.send_gripper_command(base,value=0.0)
time.sleep(2)
home_pose=[184.19,291.7,171.7,213.3,181.8,45.8,266.6] #Desk
#success &= highlevel_movements.angular_action_movement(base,joint_positions=[267.038, 239.612, 177.453, 212.171, 185.765, 56.095, 269.943],speed=speedj)
highlevel_movements.tool_twist_time(base,tool_twist_duration,pose_distance=[0,-0.35,0.4,0,0,0])
#Wave Sequence
wave_start_pose=[180, 0, 0, 0, 82, 90, 270]
wave_left_pose=list(map(add,wave_start_pose,[0,0,0,30,0,0,-30]))
wave_time=2 #seconds
#wave_left_pose=list(map(add,wave_start_pose,[0,-30,0,-30,0,0,0]))
highlevel_movements.angular_action_movement(base,joint_positions=wave_start_pose,speed=4)
#while True:
highlevel_movements.angular_action_movement(base,joint_positions=wave_left_pose,speed=2.5) #okay
highlevel_movements.angular_action_movement(base,joint_positions=[180, 0, 0, 330, 82, 90, 300],speed=4.5)
highlevel_movements.angular_action_movement(base,joint_positions=wave_left_pose,speed=4.5)
highlevel_movements.angular_action_movement(base,joint_positions=[180, 0, 0, 330, 82, 90, 300],speed=4.5)
highlevel_movements.angular_action_movement(base,joint_positions=wave_start_pose,speed=2.5)
#sys.exit()
#Moving back to init Pose
success &= highlevel_movements.angular_action_movement(base,joint_positions=[233.73, 265.96, 71.5, 212.18, 349.19, 70.11, 8.31],speed=speedj) #pre snake pose
success &= highlevel_movements.angular_action_movement(base,joint_positions=[257.64, 246.34, 31.6, 218.63, 214.63, 98.69, 129.78],speed=3) #desired dog pose
#success &= highlevel_movements.angular_action_movement(base,joint_positions=home_pose,speed=speedj)
e.set() #kill the camera process
break
elif target_reached_flag==True and success_flag_shared.value==True:
beer_timer=time.time()
token=True
else:
token=False
elapsed_time.value=0
type=float,
default=0.1,
help='Score threshold for detected objects.')
#parser.add_argument('--camera_idx', type=int, help='Index of which video source to use. ', default = 1)
args = parser.parse_args()
labels = load_labels(args.labels) if args.labels else {}
interpreter = make_interpreter(args.model)
interpreter.allocate_tensors()
# Initial Movements
success = True
#Move into transport position (home)
home_pose = [184.19, 291.7, 171.7, 213.3, 181.8, 45.8, 266.6]
#success &= highlevel_movements.angular_action_movement(base,joint_positions=home_pose,speed=5)
success &= highlevel_movements.move_to_waypoint_linear(
base,
base_cyclic, [-0.06, -0.096, 0.8, -90.823, 171.8, 113.73],
speed=speedl)
#sys.exit()
# home_pose=[-0.154,0,0.34,-55.9,173.3,86.6]
# success &= highlevel_movements.move_to_waypoint_linear(base, base_cyclic,waypoint=home_pose,speed=0.1)
#Exectue Sequence
#success &= highlevel_movements.call_kinova_web_sequence(base, base_cyclic,seq_name="grab_beer_v3")
#sys.exit()
#Execute Sequence V2
highlevel_movements.send_gripper_command(base, value=0.0)
success &= highlevel_movements.angular_action_movement(
base,
joint_positions=[
89.695, 336.743, 176.642, 232.288, 180.629, 70.981, 272.165
],
def main():
# Create connection to the device and get the router
global args
global e
if not args:
args = utilities.parseConnectionArguments()
with utilities.DeviceConnection.createTcpConnection(
args) as router: #kann das "with" weg? Nein
# Create required services
base = BaseClient(router)
base_cyclic = BaseCyclicClient(router)
device_manager = DeviceManagerClient(router)
#Activate the continuous auto-focus of the camera
vision_config = VisionConfigClient(router)
vision_device_id = vision_action.example_vision_get_device_id(
device_manager)
# if vision_device_id != 0:
# vision_action.autofocus_action(vision_config, vision_device_id,action_id=2)
# Initial Movements
success = True
success &= highlevel_movements.send_gripper_command(
base, value=0) #open gripper
if functions_.pose_comparison(
transport_pos, base_cyclic,
"joint") == True: #Robot already in position?
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=pre_transport_pos,
speed=speedj) #pre transport pose
temp_speed = 3
else:
temp_speed = 10 #robot further away so give more time
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=[
231.4, 266.21, 67.14, 216.41, 346.65, 114.31, 10.59
],
speed=speedj) #snake
time.sleep(0.4)
success &= highlevel_movements.move_to_waypoint_linear(
e,
base,
base_cyclic, [0.02, -0.511, 0.184, 90.689, 3.832, 4.054],
speed=3) #pre bottle
success &= highlevel_movements.move_to_waypoint_linear(
e,
base,
base_cyclic, [0.023, -0.539, 0.184, 90.687, 3.828, 4.049],
speed=3) #bottle
success &= highlevel_movements.send_gripper_command(base, value=0.8)
#time.sleep(2)
success &= highlevel_movements.move_to_waypoint_linear(
e,
base,
base_cyclic, [0.032, -0.553, 0.335, 90.691, 3.831, 4.048],
speed=3) #move bottle up
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=[
270.94, 42.99, 348.31, 305.15, 0.84, 286.28, 91.49
],
speed=speedj) #final
time.sleep(0.5)
#INSERT Focus on target_pose
#search for a face and interrupt when found
success &= highlevel_movements.example_send_joint_speeds(
e, base, speeds=[12, 0, 0, 0, 0, 0,
0], timer=0) #robot rotates until face found
while success_flag_shared.value == False and e.is_set(
) == False: #success_flag_shared gets True when face was detected
time.sleep(0.05)
base.Stop()
#Follow Face
t0 = time.time()
token = False # plays a role when beer has to be handed over after target was in lock for some time
target_reached_flag = False
beer_mode = False #hand over beer
print("searching for person")
while e.is_set() == False:
if time.time(
) - t0 > action_time_interval and success_flag_shared.value == True and beer_mode == False:
#calculate the pose increment and rotate robot into target
target_reached_flag = functions_.rotate_to_target(
delta_shared[:], w_shared.value, h_shared.value, max_speed,
min_speed, target_circle_size, base, base_cyclic)
t0 = time.time()
elif time.time(
) - t0 > action_time_interval and success_flag_shared.value == False and beer_mode == False:
#no target in sight
base.Stop()
# if no target was detected, search with base rotation
if time.time() - t0 > 3:
#search for a face and interrupt when found
highlevel_movements.example_send_joint_speeds(
e, base, speeds=[12, 0, 0, 0, 0, 0, 0],
timer=0) #robot rotates until face found
while success_flag_shared.value == False and e.is_set(
) == False: #success_flag_shared gets True when face was detected
time.sleep(0.05)
base.Stop()
t0 = time.time()
#after robot is Xs in target, hand over beer
if (target_reached_flag == True and token == True and
success_flag_shared.value == True) or beer_mode == True:
elapsed_time.value = time.time() - beer_timer
if elapsed_time.value > beer_time:
if beer_mode == False:
reach_beer_twist_timer = time.time()
beer_mode = True
#the target was long enough in scope
tool_twist_duration = 6
if time.time(
) - reach_beer_twist_timer < tool_twist_duration:
highlevel_movements.tool_twist_time(
e,
base,
tool_twist_duration,
pose_distance=beer_pass_dist)
else:
base.Stop()
time.sleep(2) #swing out
#wait until force applied to robot
torque_tool = functions_.TorqueTool()
torque_tool.tap_toggle(base_cyclic, torque_threshold=5)
#open gripper
highlevel_movements.send_gripper_command(base,
value=0.0)
time.sleep(2)
#move into idle upright joint_position
#success &= highlevel_movements.angular_action_movement(e,base,base_cyclic,joint_positions=[87.58, 78.68, 353.68, 216.49, 4.0, 336.91, 89.14],speed=speedj)
print("beer was passed")
break
elif target_reached_flag == True and success_flag_shared.value == True:
beer_timer = time.time()
token = True
else:
token = False
elapsed_time.value = 0
if e.is_set() == True or no_high_five == True:
return
else:
# move robot up so he is on face eight again
rev_beer_pass_dist = [i * (-1) for i in beer_pass_dist]
tool_twist_duration = 6
# highlevel_movements.tool_twist_time(e,base,tool_twist_duration,pose_distance=beer_pass_dist)
highlevel_movements.tool_twist_time(
e, base, tool_twist_duration, pose_distance=rev_beer_pass_dist)
# print("Done")
#sys.exit()
# follow a little then high five to person
t0 = time.time()
t0_wave = time.time()
time_to_wave = False
while e.is_set() == False:
if time.time(
) - t0 > action_time_interval and success_flag_shared.value == True and time_to_wave == False:
target_reached_flag = functions_.rotate_to_target(
delta_shared[:], target_middle_shared[:],
vision_middle_shared[:], w_shared.value,
h_shared.value, max_speed, min_speed,
target_circle_size, base, base_cyclic)
t0 = time.time()
elif time.time(
) - t0 > action_time_interval and success_flag_shared.value == False and time_to_wave == False:
base.Stop()
if time.time() - t0_wave > 5:
time_to_wave = True
base.Stop()
time.sleep(0.5)
break
if time.time() - t0_wave > 5:
elapsed_time.value = -1
print("time to high five!")
#read the joint angles to get the base angle
pose = highlevel_movements.get_joint_angles(base_cyclic)
base_angle = pose[0]
#enter high five position with base rotation
# high_five(base_angle=base_angle)
high_five_pos[0] = base_angle
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=high_five_pos,
speed=speedj)
torque_tool = functions_.TorqueTool()
torque_tool.tap_toggle(base_cyclic, torque_threshold=12)
time.sleep(0.1)
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=[
base_angle, 352.44, 355.16, 329.09, 1.48, 38.79, 96.13
],
speed=speedj)
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=high_five_pos,
speed=speedj)
elapsed_time.value = -2
# transport_position()
success = True
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=pre_transport_pos,
speed=speedj) #pre transport safety pose
success &= highlevel_movements.angular_action_movement(
e,
base,
base_cyclic,
joint_positions=transport_pos,
speed=speedj) #transport pose
#process finished
base.Stop()
# speed=6,watch_for_flag=True) #final
# while True:
current_pose = highlevel_movements.get_tcp_pose(base_cyclic)
print("current_pose: ", current_pose)
joint_angles = highlevel_movements.get_joint_angles(base_cyclic)
pose_tait_bryan = functions_.forward_kin(joint_angles)
print("pose_tait_bryan: ", pose_tait_bryan)
delta_angles = np.array(current_pose[0:6]) - np.array(pose_tait_bryan[0:6])
print("delta_angles: ", delta_angles)
delta_angles = np.linalg.norm(
np.array(current_pose[0:3]) - np.array(pose_tait_bryan[0:3]))
print("Magnitude: ", delta_angles)
highlevel_movements.move_to_waypoint_linear(e,
base,
base_cyclic,
pose_tait_bryan,
speed=0.05)
# time.sleep(1)
# current_pose=highlevel_movements.get_tcp_pose(base_cyclic)
# print("current_pose: ",current_pose)
# new_pose=CAMERA_Process.posetrans(current_pose,[0,0,0,0,0,0])
# print("new_pose: ",new_pose)
# highlevel_movements.move_to_waypoint_linear(e, base, base_cyclic, new_pose,speed=0.1)
# sys.exit()
#current_pose[3:6]=[math.radians(num) for num in current_pose[3:6]]
print("--------------------")
current_pose = highlevel_movements.get_tcp_pose(base_cyclic)
print("current_pose: ", current_pose)
new_pose = functions_.posetrans(current_pose, [0, 0, 0.05, 0, 0, 0])
new_pose = [round(num, 3) for num in new_pose]
