diff --git a/ur5_control.py b/ur5_control.py index 00c2b12..1424e6d 100755 --- a/ur5_control.py +++ b/ur5_control.py @@ -20,6 +20,7 @@ class Rob(): #offset_x, offset_y, offset_z = (0, 0, 0.14) # Tool offset # def __init__(self, config): + self.config = config armc = config["arm"] self.ip = armc["ip"] tool = armc["tool"] @@ -54,7 +55,6 @@ class Rob(): trying = False except: time.sleep(1) - robotiqgrip = Robotiq_Two_Finger_Gripper(rob) # Sets robot arm endpoint offset (x,y,z,rx,ry,rz) self.robot.set_tcp((self.offset_x, self.offset_y, self.offset_z, 0, 0, 0)) @@ -281,7 +281,7 @@ def get_joints_from_xyz_rel(robot, x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_g # Return result return base, shoulder, elbow, wrist1, ry, rz -def get_joints_from_xyz_abs(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset=0): +def get_joints_from_xyz_abs(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset=0, use_closest_path=True): rob = robot.robot joints = get_joints_from_xyz_rel(robot, x, y, z, rx, ry, rz, l3offset=l3offset) @@ -299,8 +299,37 @@ def get_joints_from_xyz_abs(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l #else: #print("Shoulder at", joints[1] * 180/math.pi) - # Return adjusted joint positions - return [o+j*i for j, o, i in zip(joints, offsets, inverse)] + # Get adjusted joint positions + adjusted_joints = [o+j*i for j, o, i in zip(joints, offsets, inverse)] + + curr_joints = rob.getj() + def get_complimentary_angle(joint_angle): + if joint_angle<0: + new_angle = joint_angle + 2*math.pi + else: + new_angle = joint_angle - 2*math.pi + + if abs(new_angle) > math.radians(350): + return joint_angle + else: + return new_angle + + # Use closest path (potentially going beyond 180 degrees) + if use_closest_path: + if abs(get_complimentary_angle(adjusted_joints[0])-curr_joints[0]) < abs(adjusted_joints[0]-curr_joints[0]): + adjusted_joints[0] = get_complimentary_angle(adjusted_joints[0]) + + # final_joint_positions = [] + # for curr_joint, adjusted_joint in zip(curr_joints, adjusted_joints): + # if abs(curr_joint - adjusted_joint) < abs(curr_joint - get_complimentary_angle(adjusted_joint)): + # final_joint_positions.append(adjusted_joint) + # else: + # final_joint_positions.append(get_complimentary_angle(adjusted_joint)) + + # return final_joint_positions + + return adjusted_joints + def move_arc(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2): rob = robot.robot @@ -317,7 +346,7 @@ def move_arc(robot, x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2): rob.movejs(joints, acc=2, vel=2, radius=0.1) -def offset_gripper_angle(robot, x, y, z, gripperangle=35, gripperlength=0.20+0.018, flip=False): +def offset_gripper_angle(robot, x, y, z, gripperangle=30, gripperlength=0.20+0.018, flip=False, use_closest_path=True): # gripper angle: from vertical # gripper length: from joint to start of grip # to flip, you can use flip=True or make gripper angle negative @@ -336,7 +365,7 @@ def offset_gripper_angle(robot, x, y, z, gripperangle=35, 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) + return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle + math.radians(180), l3offset=-gripperx, ry=math.pi/2, rz=rz, use_closest_path=use_closest_path) else: gripperangle = math.radians(gripperangle) @@ -345,11 +374,13 @@ def offset_gripper_angle(robot, x, y, z, gripperangle=35, gripperlength=0.20+0.0 gripperx = math.sin(gripperangle) * gripperlength gripperx += (1-math.cos(gripperangle)) * limb3 - return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle, l3offset=-gripperx, rz=rz) + return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle, l3offset=-gripperx, rz=rz, use_closest_path=use_closest_path) -def goto_holder_index(robot, idx, z=0.05, gripperangle=35, flip=False): - joint = config["position_map"][idx] - print("Going to cable holder index", joint["index"], "at position", joint["pos"]) +def goto_holder_index(robot, idx, z=0.05, gripperangle=30, flip=False, verbose=False): + joint = robot.config["position_map"][idx] + + if verbose: + print("Going to cable holder index", joint["index"], "at position", joint["pos"]) safe_move(robot, joint["pos"][1]/1000, joint["pos"][0]/1000, z) #angles = offset_gripper_angle(joint["pos"][1]/1000, joint["pos"][0]/1000, z, gripperangle=gripperangle, flip=flip) @@ -370,8 +401,8 @@ def flip(robot): rob = robot.robot # A list of safe positions to flip - safe_positions = [(-0.205, -0.108, 0.3), - (0.205, -0.108, 0.3)] + safe_positions = [(-0.18, -0.108, 0.35), + (0.18, -0.108, 0.35)] # Find the closest safe position curr_pos = rob.getl()[:3] @@ -384,12 +415,12 @@ 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()), vel=2, acc=2) # Move to safe position - rob.movej(offset_gripper_angle(robot, *safe_pos, flip=(not is_flipped())), vel=2, acc=2) # 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 def safe_move(robot, x, y, z): rob = robot.robot - flip_radius = 0.17 # Min radius on which to flip + flip_radius = 0.22 # Min radius on which to flip r = math.sqrt(x**2 + y**2) # Get position radius # Flip gripper if needed @@ -398,6 +429,33 @@ def safe_move(robot, x, y, z): rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot)), vel=2, acc=2) +def pick_up_routine(robot, holder_index, verbose=False): + rob = robot.robot + + if verbose: + print('Pickup routine for index', holder_index) + + goto_holder_index(robot, holder_index, 0.2) + curr_pos = rob.getl() + new_pos = curr_pos + new_pos[2] = 0 + rob.movel(new_pos, vel=0.3, acc=1) + # goto_holder_index(robot, holder_index, 0.0) + + # Close Gripper code + time.sleep(0.5) + + new_pos[2] = 0.2 + rob.movel(new_pos, vel=0.3, acc=1) + was_flipped = is_flipped(robot) + + # Tray position 1 + rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=is_flipped(robot), use_closest_path=False), vel=2, acc=2) + rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=2, acc=2) + time.sleep(0.5) + + # Back to safe position + rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=2, acc=2) if __name__ == "__main__": @@ -409,6 +467,7 @@ if __name__ == "__main__": #global config config = yaml.safe_load(fileread) robot = Rob(config) # robot of type Rob is the custom class above + robot.init_arm() rob = robot.robot # rob is robot.robot is the urx robot class, what we've been using previously print("Current tool pose is: ", rob.getl()) @@ -447,58 +506,37 @@ if __name__ == "__main__": 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) # move_arc(0, 0.3, 0.1) # move_arc(0, -0.3, 0.3) + for i in range(20, 25): + pick_up_routine(robot, i, verbose=True) + + - # for i in range(20): - # goto_holder_index(i, 0.1) + # goto_holder_index(robot, 7, 0.0) - flip(robot) - flip(robot) + # 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) + + #rob.movej(goto_holder_index(24, 0.2, 0), acc=2, vel=2). #joints = [] - #for i in np.linspace(0, 340, 340): - # joints.append(goto_holder_index(24, 0.5, i)) - #rob.movejs(joints, acc=1, vel=3) - angle = 30 - rob.movej(goto_holder_index(robot, 26, 0.1, angle), acc=2, vel=2) - time.sleep(1) - rob.movej(goto_holder_index(robot, 25, 0.1, angle), acc=2, vel=2) - time.sleep(1) - rob.movej(goto_holder_index(robot, 24, 0.1, angle, flip=True), acc=2, vel=2) - #rob.movej(goto_holder_index(32, 0.2, angle), acc=2, vel=2) - #rob.movej(goto_holder_index(38, 0.2, angle), acc=2, vel=2) - #rob.movej(goto_holder_index(25, 0.1, angle, flip=True), acc=2, vel=2) + + # 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(goto_holder_index(25, 0.2, angle, flip=True), acc=2, vel=2) - #rob.movej(goto_holder_index(24, 0.1, angle, flip=True), acc=2, vel=2) - #time.sleep(1) - #rob.movej(goto_holder_index(25, 0.1, angle, flip=True), acc=2, vel=2) - #rob.movej(goto_holder_index(49, 0.1, angle), acc=2, vel=2) - #rob.movej(goto_holder_index(49, 0.1, angle, flip=True), acc=2, vel=2) - # rob.movej(goto_holder_index(50, 0.1, angle, flip=True), acc=2, vel=2) - # rob.movej(goto_holder_index(51, 0.1, angle, flip=True), acc=2, vel=2) - # rob.movej(goto_holder_index(52, 0.1, angle, flip=True), acc=2, vel=2) - # rob.movej(goto_holder_index(53, 0.1, angle, flip=True), acc=2, vel=2) - #time.sleep(2) - #rob.movej(goto_holder_index(24, 0.15, 35, flip=True), acc=2, vel=2) - #time.sleep(10) - # time.sleep(4) - # goto_holder_index(26, 0.1, 20) - # time.sleep(4) - # goto_holder_index(26, 0.1, 30) - # time.sleep(4) - # goto_holder_index(26, 0.1, 40) - # for joint in config["position_map"]: - #joint = config["position_map"][26] - #print("Going to cable holder index", joint["index"], "at position", joint["pos"]) - #angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0.05, )# rx=math.pi / 5) - #joints.append(angles) #rob.movej(angles, acc=2, vel=2) #time.sleep(10) @@ -511,25 +549,6 @@ if __name__ == "__main__": # time.sleep(5) - # angles = get_joints_from_xyz_abs(0, -0.6, 0) - # rob.movej(angles, acc=2, vel=2) - - - - # set_pos_abs(*p1) - # move = move_to_polar(p1, p2) - - - # for p in move: - # print(math.degrees(p)) - # print("Safe? :", is_safe_move(p1, p2)) - - - # #set_pos_rel_rot_abs(0, 0, -0.2, math.pi, 0, -math.pi) - # set_pos_abs(0.3, -0.2, 0.5, math.pi, 0, -math.pi) - # set_pos_abs(0, 0.2, 0.6, math.pi, 0, -math.pi) - # set_pos_abs(-0.5, -0.2, 0.4, math.pi, 0, -math.pi) - # #set_pos_rel_rot_abs(0, 0, 0, math.pi, 0, -math.pi) # print("Current tool pose is: ", rob.getl()) # print("getj(): ", rob.getj())