Jukebox/jukebox-software
10
0
Fork 0
Code Issues 3 Pull Requests Packages Projects Releases 2 Wiki Activity

Created functions for routines

Browse Source
This commit is contained in:
BlueOceanWave 2024-04-24 14:40:38 -05:00
parent ae97ed1a14
commit f4e43f33d2
1 changed files with 31 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
ur5_control.py
View File

@ -493,6 +493,10 @@ def holder_routine(robot, holder_index, pick_up, verbose=False):
robot = connect(robot)
rob = robot.robot
# Don't attempt to place a tube in the camera slot
if holder_index == 49:
return
if verbose:
fprint('Pickup routine for index' + str(holder_index))
@ -532,6 +536,10 @@ def tray_routine(robot, slot=0, pick_up=True):
robot = connect(robot)
rob = robot.robot
# Default to 0 if invalid value
if slot not in [0, 1, 2, 3]:
slot = 0
slot_prepositions = [(-9.93, -112.67, 144.02, -116.69, -54.13, -10.29),
(-12.35, -124.95, 148.61, -107.27, -54.36, -13.26),
(-16.45, -96.97, 137.85, 58.39, -305.08, 161.75),
@ -633,49 +641,29 @@ def return_routine(robot, slot, holder_index=None, verbose=False):
return True
def to_camera(robot, holder_index, verbose=False):
def goto_camera(robot):
robot = connect(robot)
goto_holder_index(robot, 49, 0.2)
def tray_to_camera(robot, slot):
pick_up_tray(robot, slot)
goto_camera(robot)
def holder_to_tray(robot, holder_index, slot):
pick_up_holder(robot, holder_index)
drop_off_tray(robot, slot)
def holder_to_camera(robot, holder_index, verbose=False):
robot = connect(robot)
fprint("Bringing tube at " + str(holder_index) + " to camera")
rob = robot.robot
goto_holder_index(robot, holder_index, 0.05, use_closest_path=False)
open_gripper()
curr_pos = rob.getl()
new_pos = curr_pos
new_pos[2] = 0.005
rob.movel(new_pos, vel=0.1, acc=1)
# goto_holder_index(robot, holder_index, 0.0)
close_gripper()
new_pos[2] = 0.2
rob.movel(new_pos, vel=2, acc=1)
was_flipped = is_flipped(robot)
# start inwards
rob.movej(offset_gripper_angle(robot, 0.15, -0.1, 0.25, flip=was_flipped, use_closest_path=False), vel=2, acc=2)
# go to camera
rob.movej(offset_gripper_angle(robot, 0.3, -0.2, 0.25, flip=was_flipped, use_closest_path=False), vel=2, acc=2)
return True
def return_camera(robot, holder_index, verbose=False):
pick_up_holder(robot, holder_index)
goto_camera(robot)
def camera_to_holder(robot, holder_index, verbose=False):
robot = connect(robot)
rob = robot.robot
# move inwards first
rob.movej(offset_gripper_angle(robot, 0.15, -0.1, 0.25, flip=is_flipped(robot), use_closest_path=False), vel=2, acc=2)
goto_holder_index(robot, holder_index, 0.2, use_closest_path=False)
curr_pos = rob.getl()
new_pos = curr_pos
new_pos[2] = 0.015
rob.movel(new_pos, vel=0.1, acc=1)
open_gripper()
new_pos[2] = 0.1
rob.movel(new_pos, vel=2, acc=1)
drop_off_holder(robot, holder_index)
def open_gripper():
@ -724,17 +712,14 @@ if __name__ == "__main__":
#move_to_packup(robot)
pick_up_holder(robot, 0)
drop_off_tray(robot, 0)
pick_up_holder(robot, 1)
drop_off_tray(robot, 3)
pick_up_tray(robot, 0)
drop_off_holder(robot, 0)
pick_up_tray(robot, 3)
drop_off_holder(robot, 1)
# for i in range(3):
# pick_up_holder(robot, i)
# print('Drop off', i+1)
# drop_off_holder(robot, i+1)
holder_to_tray(robot, 0, 1)
tray_to_camera(robot, 1)
print("Current tool pose is: ", rob.getl())
curr_pos = rob.getl()