Open gripper before picking up

ur5_control.py

@@ -547,7 +547,7 @@ def drop_off_holder(robot, pos_updates, holder_index, verbose=False):
 def tray_routine(robot, slot=0, pick_up=True):
     robot = connect(robot)
     rob = robot.robot
-
+    
     # Default to 0 if invalid value
     if slot not in [0, 1, 2, 3]:
         slot = 0
@@ -588,6 +588,8 @@ def tray_routine(robot, slot=0, pick_up=True):
         [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],
     ]
+    if pick_up:
+        open_gripper()
     rob.movel(slot_position[slot],vel=0.2, acc=1)
 
     # Place/Grab the tube