Made pickup and dropoff routines for tray and holder. Adjusted home position

tempCodeRunnerFile.py

@@ -0,0 +1,2 @@
+
+    drop_off_tray(robot, 0)

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
+    # Move robot to home position
-    home_pos = [0.10421807948612624, 
+    rob.movej(offset_gripper_angle(robot, *(-0.18, -0.108, 0.35), flip=is_flipped(robot)), vel=2, acc=2) # Move to safe position
-                -2.206111555015423, 
-                1.710679229503537, 
-                -1.075834511928354, 
-                -1.569301366430687, 
-                1.675098295930943]
 
-    # 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]
+    wrist1 = rob.getj()[3]
-
+    
-    if wrist2<-math.pi:
+    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)
+    # Go to the correct holder
-    open_gripper()
+    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 tray_routine(robot, slot=0, pick_up=True):
-def place_tube(robot, slot=0):
     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
+    # Initial position depending on slot and robot orientation
-    print('goto holder')
+    if slot in [0, 1]:
-    goto_holder_index(robot, 25, z=0.3)
+        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 = [(-58, -114.45, 100.52, -45.24, -96.95, 120),
-        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) 
+    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)
+    move_to_home(robot, speed=1, keep_flip=True)  
-   
+def pick_up_tray(robot, slot=0):
-    # return True
+    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