diff --git a/config.yml b/config.yml
index fad6678..9096e8e 100644
--- a/config.yml
+++ b/config.yml
@@ -11,7 +11,7 @@ arm:
     offset_y: 0
     offset_z: 0.14
   limbs:
-    limb_base: 0.105
+    limb_base: 0.11
     limb1: 0.425
     limb2: 0.39225
     limb3: 0.1
diff --git a/ur5_control.py b/ur5_control.py
index 1424e6d..084a6ed 100755
--- a/ur5_control.py
+++ b/ur5_control.py
@@ -376,13 +376,13 @@ def offset_gripper_angle(robot, x, y, z, gripperangle=30, gripperlength=0.20+0.0
 
         return get_joints_from_xyz_abs(robot, x, y, z-grippery, rx=gripperangle, l3offset=-gripperx, rz=rz, use_closest_path=use_closest_path)
     
-def goto_holder_index(robot, idx, z=0.05, gripperangle=30, flip=False, verbose=False):
+def goto_holder_index(robot, idx, z=0.05, gripperangle=30, flip=False, use_closest_path=True, verbose=False):
     joint = robot.config["position_map"][idx]
     
     if verbose:
         print("Going to cable holder index", joint["index"], "at position", joint["pos"])   
     
-    safe_move(robot, joint["pos"][1]/1000, joint["pos"][0]/1000, z)
+    safe_move(robot, joint["pos"][0]/1000, joint["pos"][1]/1000, z, use_closest_path=use_closest_path)
     #angles = offset_gripper_angle(joint["pos"][1]/1000, joint["pos"][0]/1000, z, gripperangle=gripperangle, flip=flip)
     #rob.movej(angles, acc=2, vel=2)
     #return angles
@@ -418,7 +418,7 @@ def flip(robot):
     rob.movej(offset_gripper_angle(robot, *safe_pos, flip=is_flipped(robot)), vel=2, acc=2) # Move to safe position
     rob.movej(offset_gripper_angle(robot, *safe_pos, flip=(not is_flipped(robot))), vel=2, acc=2) # Flip gripper
 
-def safe_move(robot, x, y, z):
+def safe_move(robot, x, y, z, use_closest_path=True):
     rob = robot.robot
     flip_radius = 0.22 # Min radius on which to flip
     r = math.sqrt(x**2 + y**2) # Get position radius
@@ -427,7 +427,7 @@ def safe_move(robot, x, y, z):
     if (r <= flip_radius and is_flipped(robot)) or (r > flip_radius and not is_flipped(robot)):
         flip(robot)
 
-    rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot)), vel=2, acc=2)
+    rob.movej(offset_gripper_angle(robot, x, y, z, flip=is_flipped(robot), use_closest_path=use_closest_path), vel=2, acc=2)
 
 def pick_up_routine(robot, holder_index, verbose=False):
     rob = robot.robot
@@ -435,27 +435,27 @@ def pick_up_routine(robot, holder_index, verbose=False):
     if verbose:
         print('Pickup routine for index', holder_index)
 
-    goto_holder_index(robot, holder_index, 0.2)
+    goto_holder_index(robot, holder_index, 0.2, use_closest_path=False)
     curr_pos = rob.getl()
     new_pos = curr_pos
-    new_pos[2] = 0
-    rob.movel(new_pos, vel=0.3, acc=1)
+    new_pos[2] = 0.005
+    rob.movel(new_pos, vel=0.1, acc=1)
     # goto_holder_index(robot, holder_index, 0.0)
 
     # Close Gripper code
     time.sleep(0.5)
 
     new_pos[2] = 0.2
-    rob.movel(new_pos, vel=0.3, acc=1)
+    rob.movel(new_pos, vel=0.1, acc=1)
     was_flipped = is_flipped(robot)
 
-    # Tray position 1
-    rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=is_flipped(robot), use_closest_path=False), vel=2, acc=2)
-    rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=2, acc=2)
-    time.sleep(0.5)
+    # # Tray position 1
+    # rob.movej(offset_gripper_angle(robot, -0.15, -0.15, 0.3, flip=is_flipped(robot), use_closest_path=False), vel=2, acc=2)
+    # rob.movej(get_joints_from_xyz_abs(robot, -0.35, -0.15, 0.0, math.pi/2, 0.1), vel=2, acc=2)
+    # time.sleep(0.5)
 
     # Back to safe position
-    rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=2, acc=2)
+    # rob.movej(offset_gripper_angle(robot, -0.205, -0.108, 0.3, flip=True), vel=2, acc=2)
 
 
 if __name__ == "__main__":
@@ -511,7 +511,10 @@ if __name__ == "__main__":
     # move_arc(0, 0.3, 0.1)
     # move_arc(0, -0.3, 0.3)
 
-    for i in range(20, 25):
+    # 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 [1,50]:
         pick_up_routine(robot, i, verbose=True)