From 32260595a1dd12d31dbc1df034867ef325ddf811 Mon Sep 17 00:00:00 2001
From: Cole Deck <cdeck@hawk.iit.edu>
Date: Tue, 14 May 2024 20:21:09 -0500
Subject: [PATCH] Open gripper before picking up

---
 ur5_control.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/ur5_control.py b/ur5_control.py
index 72a12e2..31f686d 100755
--- a/ur5_control.py
+++ b/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