diff --git a/ur5_control.py b/ur5_control.py index 7f9ec8a..faec1a1 100755 --- a/ur5_control.py +++ b/ur5_control.py @@ -83,6 +83,7 @@ def connect(robot): # Set weight robot.robot.set_payload(2, (0, 0, 0.1)) return robot + def init_arm(robot): robot = connect(robot) @@ -95,7 +96,7 @@ def init_arm(robot): # setup - in case of fail. open gripper, move up, then go home. rob = robot.robot - open_gripper() + # open_gripper() curr_pos = rob.getl() new_pos = curr_pos new_pos[2] += 0.025 @@ -248,6 +249,9 @@ def move_to_polar(robot, start_pos, end_pos): def move_to_home(robot, speed=2): rob = robot.robot + if is_flipped(robot): + flip(robot) + # Home position in degrees home_pos = [0.10421807948612624, -2.206111555015423, @@ -423,7 +427,7 @@ def offset_gripper_angle(robot, x, y, z, gripperangle=30, gripperlength=0.20+0.0 gripperx -= (1-math.cos(gripperangle)) * limb3 rz = math.pi / 2 # flip the whole wrist - return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=math.pi/2, rz=rz + rzoffset, use_closest_path=use_closest_path) + return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=-3*math.pi/2, rz=rz + rzoffset, use_closest_path=use_closest_path) else: gripperangle = math.radians(gripperangle) @@ -451,7 +455,7 @@ def is_flipped(robot): rob = robot.robot wrist2 = rob.getj()[4] - if wrist2>0: + if wrist2<-math.pi: return True else: return False @@ -462,6 +466,7 @@ def flip(robot): # A list of safe positions to flip safe_positions = [(-0.18, -0.108, 0.35), (0.18, -0.108, 0.35)] + # Find the closest safe position curr_pos = rob.getl()[:3] @@ -474,8 +479,9 @@ def flip(robot): safe_pos = min(pos_dist_pairs, key=lambda x:x[1])[0] # Flip at safe position - rob.movej(offset_gripper_angle(robot, *safe_pos, flip=is_flipped(robot)), vel=4, acc=3) # Move to safe position - rob.movej(offset_gripper_angle(robot, *safe_pos, flip=(not is_flipped(robot))), vel=4, acc=3) # Flip gripper + rob.movej(offset_gripper_angle(robot, *safe_pos, flip=is_flipped(robot)), vel=2, acc=2) # Move to safe position + rob.movej(offset_gripper_angle(robot, *safe_pos, flip=(not is_flipped(robot))), vel=2, acc=2) # Flip gripper + # print('flip?: ', is_flipped(robot)) return True def safe_move(robot, x, y, z, use_closest_path=True): @@ -490,7 +496,7 @@ 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, slot=None, verbose=False): +def pick_up_routine(robot, holder_index, verbose=False): robot = connect(robot) rob = robot.robot @@ -507,69 +513,73 @@ def pick_up_routine(robot, holder_index, slot=None, verbose=False): # 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) + goto_holder_index(robot, 25, z=0.2) - #move_to_home(robot, speed=4) - # starting pos - angles = (-47.04,-115.81,138.33,-58.11,-56.55,-68.03) - angles = [x*math.pi/180 for x in angles] - rob.movej(angles,vel=1,acc=0.5) +def place_tube(robot, slot=0): + robot = connect(robot) + rob = robot.robot - angles = (-41.57,-120.35,155.43,-75.38,-47.98,-60.39) - angles = [x*math.pi/180 for x in angles] - rob.movej(angles,vel=1,acc=0.5) + 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), + (-16.66, -97.28, 138.16, 58.54, -305.05, 161.50)] - if slot is None: - slot = 0 - if slot == 0: - angles = (7.09,-64.83,147.87,-188.00,-29.84,17.15) - angles = [x*math.pi/180 for x in angles] - if slot == 1: - angles = (8.43,-64.63,154.74,-197.30,-30.21,19.72) - angles = [x*math.pi/180 for x in angles] - if slot == 2: - angles = (10.39,-70.86,133.95,7.83,30.91,-159.00) - angles = [x*math.pi/180 for x in angles] - if slot == 3: - angles = (13.31,-76.23,140.78,1.90,31.98,-153.76) - angles = [x*math.pi/180 for x in angles] + # Initial position + print('goto holder') + goto_holder_index(robot, 25, z=0.3) - rob.movej(angles,vel=1,acc=0.5) + 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), + (-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) - open_gripper() + # Positions for each slot + slot_distance = .052 + slot_height = -.015-.0095 + first_slot = -0.3084+0.02 + slot_position = [ + [first_slot, -0.3426, slot_height, 1.5899, 1.5526, -0.9411], + [first_slot+slot_distance, -0.3426, slot_height, 1.5899, 1.5526, -0.9411], + [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) + time.sleep(2) + + # Move back + tilt = 0.3 curr_pos = rob.getl() new_pos = curr_pos - new_pos[2] += 0.04 + if slot==3: + new_pos[0] -= 0.05 #x + new_pos[1] += 0.15 #y + new_pos[2] = 0.09 #z + new_pos[3] += tilt + new_pos[4] += tilt + new_pos[5] += tilt rob.movel(new_pos, vel=0.1, acc=1) - - angles = (-41.57,-120.35,155.43,-75.38,-47.98,-60.39) - angles = [x*math.pi/180 for x in angles] - rob.movej(angles,vel=1,acc=0.5) - move_to_home(robot, speed=3) - # # Tray position 1 - # if slot is None: - # rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=was_flipped, use_closest_path=False), vel=4, acc=3) - # rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=4, acc=3) - # open_gripper() - # else: - # xoffset = 0.051 * slot - # rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=was_flipped, use_closest_path=False), vel=4, acc=3) - # rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, gripperangle=60, flip=was_flipped, use_closest_path=False,rzoffset=math.pi), vel=4, acc=3) - # #rob.movej(get_joints_from_xyz_abs(robot, -0.35+xoffset, -0.15, 0, math.pi/2, 0.1), vel=4, acc=3) - # open_gripper() - # Back to safe position - #time.sleep(10) - #rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=4, acc=3) - return True + # Go home to safe position + move_to_home(robot, speed=1) + + # return True def return_routine(robot, slot, holder_index=None, verbose=False): robot = connect(robot) @@ -652,13 +662,13 @@ def return_camera(robot, holder_index, verbose=False): def open_gripper(): fprint("Opening gripper") - c = ModbusClient(host="192.168.1.20", port=502, auto_open=True, auto_close=False) + c = ModbusClient(host="192.168.1.21", port=502, auto_open=True, auto_close=False) - c.write_single_register(0, 0b0) - c.write_single_register(435, 0b1) + c.write_single_register(112, 0b0) + c.write_single_register(435, 0b10000000) - c.write_single_register(0, 0b0) - c.write_single_register(435, 0b1) + c.write_single_register(112, 0b0) + c.write_single_register(435, 0b10000000) time.sleep(0.5) c.close() @@ -666,12 +676,12 @@ def open_gripper(): def close_gripper(): fprint("Closing gripper") - c = ModbusClient(host="192.168.1.20", port=502, auto_open=True, auto_close=False) - c.write_single_register(435, 0b0) - c.write_single_register(0, 0b1) + c = ModbusClient(host="192.168.1.21", port=502, auto_open=True, auto_close=False) + c.write_single_register(435, 0b00000000) + c.write_single_register(112, 0b1) - c.write_single_register(435, 0b0) - c.write_single_register(0, 0b1) + c.write_single_register(435, 0b00000000) + c.write_single_register(112, 0b1) time.sleep(0.5) c.close() # @@ -697,20 +707,36 @@ 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) + # move_to_home(robot, speed=4) #time.sleep(3) #move_to_packup(robot) - - pick_up_routine(robot, 0, 0, True) + + # 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) - - time.sleep(100) - print("Current tool pose is: ", rob.getl()) @@ -718,9 +744,6 @@ if __name__ == "__main__": config = None - joints = [] - for i in np.linspace(-0.2, -0.7, 10): - joints.append(get_joints_from_xyz_abs(robot, i, 0, 0)) # 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)