From 275cbd027e9cc77a68d8f54e1305ae477f48cb5d Mon Sep 17 00:00:00 2001
From: BlueOceanWave <97416032+BlueOceanWave@users.noreply.github.com>
Date: Wed, 24 Apr 2024 14:42:07 -0500
Subject: [PATCH] Added open gripper to init routine

---
 ur5_control.py | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/ur5_control.py b/ur5_control.py
index fd83af0..b8b56f3 100755
--- a/ur5_control.py
+++ b/ur5_control.py
@@ -96,7 +96,7 @@ def init_arm(robot):
 
     # setup - in case of fail. open gripper, move up, then go home.
     rob = robot.robot
-    # open_gripper()
+    open_gripper()
     curr_pos = rob.getl()
     new_pos = curr_pos
     new_pos[2] += 0.025
@@ -718,9 +718,9 @@ if __name__ == "__main__":
     #     print('Drop off', i+1)
     #     drop_off_holder(robot, i+1)
 
-    holder_to_tray(robot, 0, 1)
-    tray_to_camera(robot, 1)
-   
+    holder_to_camera(robot, 0)
+    camera_to_holder(robot, 0)
+
     print("Current tool pose is: ",  rob.getl())
     curr_pos = rob.getl()
     config = None