Added open gripper to init routine

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