Merge branch 'main' of https://git.myitr.org/Jukebox/jukebox-software

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
+    # Get adjusted joint positions
-    return [o+j*i for j, o, i in zip(joints, offsets, inverse)]
+    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):
+def goto_holder_index(robot, idx, z=0.05, gripperangle=30, flip=False, verbose=False):
-    joint = config["position_map"][idx]
+    joint = robot.config["position_map"][idx]
-    print("Going to cable holder index", joint["index"], "at position", joint["pos"])   
+    
+    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),
+    safe_positions = [(-0.18, -0.108, 0.35),
-                      (0.205, -0.108, 0.3)]
+                      (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=is_flipped(robot)), 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=(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)
     
-    flip(robot)
-    flip(robot)
 
-    #rob.movej(goto_holder_index(24, 0.2, 0), acc=2, vel=2)
+    # 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 = []
     
-    #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)
         
-    #rob.movej(goto_holder_index(25, 0.2, angle, flip=True), acc=2, vel=2)
+    # angle = 30
-    #rob.movej(goto_holder_index(24, 0.1, angle, flip=True), acc=2, vel=2)
+    # goto_holder_index(robot, 26, 0.1, angle)
-    #time.sleep(1)
+    # time.sleep(1)
-    #rob.movej(goto_holder_index(25, 0.1, angle, flip=True), acc=2, vel=2)
+    # goto_holder_index(robot, 25, 0.1, angle)
-    #rob.movej(goto_holder_index(49, 0.1, angle), acc=2, vel=2)
+    # time.sleep(1)
-    #rob.movej(goto_holder_index(49, 0.1, angle, flip=True), acc=2, vel=2)
+    # goto_holder_index(robot, 24, 0.1, angle)
-    # 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())