From ae97ed1a14aad8d1eb867fe01e60af80d77ccc6e Mon Sep 17 00:00:00 2001 From: BlueOceanWave <97416032+BlueOceanWave@users.noreply.github.com> Date: Tue, 23 Apr 2024 18:56:11 -0500 Subject: [PATCH] Made pickup and dropoff routines for tray and holder. Adjusted home position --- tempCodeRunnerFile.py | 2 + ur5_control.py | 195 +++++++++++++++--------------------------- 2 files changed, 71 insertions(+), 126 deletions(-) create mode 100644 tempCodeRunnerFile.py diff --git a/tempCodeRunnerFile.py b/tempCodeRunnerFile.py new file mode 100644 index 0000000..3bc2adc --- /dev/null +++ b/tempCodeRunnerFile.py @@ -0,0 +1,2 @@ + + drop_off_tray(robot, 0) \ No newline at end of file diff --git a/ur5_control.py b/ur5_control.py index faec1a1..b25fc97 100755 --- a/ur5_control.py +++ b/ur5_control.py @@ -187,7 +187,6 @@ def polar_to_cartesian(r, theta): y = r * np.sin(theta) return x, y - def move_to_polar(robot, start_pos, end_pos): rob = robot.robot @@ -246,22 +245,16 @@ def move_to_polar(robot, start_pos, end_pos): return rx_intermediate -def move_to_home(robot, speed=2): + +def move_to_home(robot, keep_flip=False, speed=2): rob = robot.robot - if is_flipped(robot): + if is_flipped(robot) and not keep_flip: flip(robot) - # Home position in degrees - home_pos = [0.10421807948612624, - -2.206111555015423, - 1.710679229503537, - -1.075834511928354, - -1.569301366430687, - 1.675098295930943] + # Move robot to home position + rob.movej(offset_gripper_angle(robot, *(-0.18, -0.108, 0.35), flip=is_flipped(robot)), vel=2, acc=2) # Move to safe position - # Move robot - rob.movej(home_pos, acc=2, vel=speed) return True def move_to_packup(robot, speed=0.25): @@ -453,9 +446,9 @@ def goto_holder_index(robot, idx, z=0.05, gripperangle=30, flip=False, use_close def is_flipped(robot): rob = robot.robot - wrist2 = rob.getj()[4] - - if wrist2<-math.pi: + wrist1 = rob.getj()[3] + + if wrist1 > 0: return True else: return False @@ -496,31 +489,46 @@ def safe_move(robot, x, y, z, use_closest_path=True): rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot), use_closest_path=use_closest_path), vel=4, acc=3) return True -def pick_up_routine(robot, holder_index, verbose=False): +def holder_routine(robot, holder_index, pick_up, verbose=False): robot = connect(robot) rob = robot.robot if verbose: fprint('Pickup routine for index' + str(holder_index)) - goto_holder_index(robot, holder_index, 0.05, use_closest_path=False) - open_gripper() + # Go to the correct holder + if pick_up: + goto_holder_index(robot, holder_index, 0.05, use_closest_path=False) + else: + goto_holder_index(robot, holder_index, 0.2, use_closest_path=False) + + if pick_up: + open_gripper() + # Move down 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() + # Pick up or drop off + if pick_up: + close_gripper() + else: + open_gripper() + + # Move up new_pos[2] = 0.2 rob.movel(new_pos, vel=2, acc=1) was_flipped = is_flipped(robot) - goto_holder_index(robot, 25, z=0.2) + # goto_holder_index(robot, 25, z=0.2) +def pick_up_holder(robot, holder_index, verbose=False): + holder_routine(robot, holder_index, True, verbose=verbose) +def drop_off_holder(robot, holder_index, verbose=False): + holder_routine(robot, holder_index, False, verbose=verbose) - -def place_tube(robot, slot=0): +def tray_routine(robot, slot=0, pick_up=True): robot = connect(robot) rob = robot.robot @@ -529,24 +537,26 @@ def place_tube(robot, slot=0): (-16.45, -96.97, 137.85, 58.39, -305.08, 161.75), (-16.66, -97.28, 138.16, 58.54, -305.05, 161.50)] - # Initial position - print('goto holder') - goto_holder_index(robot, 25, z=0.3) + # Initial position depending on slot and robot orientation + if slot in [0, 1]: + if is_flipped(robot): + flip(robot) + else: + move_to_home(robot, keep_flip=True) + else: + goto_holder_index(robot, 25, z=0.3) - print('aligning slot') # Align robot to the slot if slot in [2,3]: angles = [(-2.77, -99.64, 131.02, 67.67, 70.04-360, 153.03), slot_prepositions[slot]] - # (-16.44, -94.17, 139.46, 54, 54.92, 161.75),] else: - flip(robot) - angles = [(-34.1, -117.51, 129.81, -70.05, -52.30, 315.25), + angles = [(-58, -114.45, 100.52, -45.24, -96.95, 120), (-39.98, -124.92, 132.28, -61.56, -55.60, -50.77), slot_prepositions[slot]] angles = [[x*math.pi/180 for x in move] for move in angles] - rob.movejs(angles,vel=1,acc=0.5) + rob.movejs(angles,vel=2,acc=1) # Positions for each slot slot_distance = .052 @@ -558,9 +568,14 @@ def place_tube(robot, slot=0): [first_slot+2*slot_distance, -0.3426, slot_height, 1.5899, 1.5526, -0.9411], [first_slot+3*slot_distance, -0.3426, slot_height, 1.5899, 1.5526, -0.9411], ] - rob.movel(slot_position[slot],vel=0.2, acc=0.2) + rob.movel(slot_position[slot],vel=0.2, acc=1) + + # Place/Grab the tube + if pick_up: + close_gripper() + else: + open_gripper() - time.sleep(2) # Move back tilt = 0.3 @@ -573,13 +588,16 @@ def place_tube(robot, slot=0): new_pos[3] += tilt new_pos[4] += tilt new_pos[5] += tilt - rob.movel(new_pos, vel=0.1, acc=1) + rob.movel(new_pos, vel=0.2, acc=1) # Go home to safe position - move_to_home(robot, speed=1) - - # return True + move_to_home(robot, speed=1, keep_flip=True) +def pick_up_tray(robot, slot=0): + tray_routine(robot, slot, True) +def drop_off_tray(robot, slot=0): + tray_routine(robot, slot, False) + def return_routine(robot, slot, holder_index=None, verbose=False): robot = connect(robot) @@ -688,16 +706,11 @@ def close_gripper(): if __name__ == "__main__": - #rob.movej((0, 0, 0, 0, 0, 0), 0.1, 0.2) - #rob.movel((x, y, z, rx, ry, rz), a, v) #init("192.168.1.145") with open('config.yml', 'r') as fileread: #global config config = yaml.safe_load(fileread) - #open_gripper() - #time.sleep(1) - #close_gripper() robot = Rob(config) # robot of type Rob is the custom class above @@ -707,96 +720,26 @@ if __name__ == "__main__": rob = robot.robot # rob is robot.robot is the urx robot class, what we've been using previously - # move_to_home(robot, speed=4) - #time.sleep(3) #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) + + - # pick_up_routine(robot, 0) - # place_tube(robot, 0) - # time.sleep(0.5) - # place_tube(robot, 1) - # time.sleep(0.5) - # place_tube(robot, 2) - # time.sleep(0.5) - place_tube(robot, 2) - time.sleep(0.5) - - - - # pick_up_routine(robot, 3, True) - # place_tube(robot, 3) - # angles = (7.09,-64.83,147.87,-188.00,-29.84,17.15) - # angles = (8.43,-64.63,154.74,-197.30,-30.21,19.72) - # angles = (10.39,-70.86,133.95,7.83,30.91,-159.00) - # angles = (13.31,-76.23,140.78,1.90,31.98,-153.76) - - - - #move_to_home(robot) - - #return_routine(robot, 0, 0, True) - print("Current tool pose is: ", rob.getl()) - - curr_pos = rob.getl() - - config = None - # rob.movejs(joints, acc=2, vel=2, radius=0.1) - - # rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, gripperangle=90, flip=is_flipped(robot)), vel=2, acc=2) - - # move_arc(0, 0.3, 0.1) - # move_arc(0, -0.3, 0.3) - - # rob.movej(get_joints_from_xyz_abs(robot, 0.2, 0, 0.05), vel=0.5, acc=2) - # goto_holder_index(robot, 27, 0.05) - - #for i in [6,7,8]: - #pick_up_routine(robot, i, verbose=True) - - - - # goto_holder_index(robot, 7, 0.0) - - # pick_up_routine(robot, 8) - - - # rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=2, acc=2) - - - #rob.movej(goto_holder_index(24, 0.2, 0), acc=2, vel=2). - #joints = [] - - - # angle = 30 - # goto_holder_index(robot, 26, 0.1, angle) - # time.sleep(1) - # goto_holder_index(robot, 25, 0.1, angle) - # time.sleep(1) - # goto_holder_index(robot, 24, 0.1, angle) - - - - - #rob.movej(angles, acc=2, vel=2) - #time.sleep(10) - #rob.movejs(joints, acc=2, vel=2) - # joints = [] - # for i in np.linspace(-0.3, -0.7, 50): - # joints.append(get_joints_from_xyz_abs(i, 0, 0)) - # rob.movejs(joints, acc=2, vel=2) - - # time.sleep(5) - - - - # print("Current tool pose is: ", rob.getl()) - # print("getj(): ", rob.getj()) - - # move_to_home() + rob.stop() os.kill(os.getpid(), 9) # dirty kill of self