diff --git a/ur5_control.py b/ur5_control.py
index 3b33b99..dbd29aa 100755
--- a/ur5_control.py
+++ b/ur5_control.py
@@ -14,7 +14,8 @@ from util import fprint
 
 
 rob = None
-
+offset_x, offset_y, offset_z = (0, 0, 0.14)     # Tool offset
+limb_base, limb1, limb2, limb3, limb_wrist = (0.105, .425, .39225, .1, .0997)    # Limb lengths
 
 def init(ip):
     global rob
@@ -225,10 +226,9 @@ def normalize_degree(theta):
 
 def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess = (math.pi/2, math.pi/2, 0), l3offset=0):
     # Get limbs and offsets
-    offset_x, offset_y, offset_z = (0, 0, 0.14)     # Tool offset
-    l_bs, l1, l2, l3, l_wt = (0.105, .425, .39225, .1, .0997)    # Limb lengths
-    #l3=0.15
     
+    #l3=0.15
+    l_bs, l1, l2, l3, l_wt = (limb_base, limb1, limb2, limb3, limb_wrist)    # Limb lengths
     l3 += l3offset # add wrist offset, used for gripper angle calculations
     
     # Calculate base angle and r relative to shoulder joint
@@ -255,7 +255,7 @@ def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess =
 
     base_theta = calculate_theta(x, y, l_bs)
     cx, cy = l_bs*math.cos(base_theta), l_bs*math.sin(base_theta)
-    r = math.sqrt((x+cx)**2 + (y+cy)**2) 
+    r = math.sqrt((x+offset_x+cx)**2 + (y+offset_y+cy)**2) 
 
 
     # Formulas to find out joint positions for (r, z)
@@ -294,19 +294,19 @@ def offset_gripper_angle(x, y, z, gripperangle=35, gripperlength=0.2, flip=False
     if flip:
         gripperangle = -degtorad(gripperangle)
         grippery = gripperlength - math.cos(gripperangle) * gripperlength
-        grippery -= math.sin(gripperangle) * 0.1
+        grippery -= math.sin(gripperangle) * limb3
         gripperx = math.sin(gripperangle) * gripperlength
-        gripperx += (1-math.cos(gripperangle)) * 0.1
+        gripperx += (1-math.cos(gripperangle)) * limb3
     else:
         gripperangle = degtorad(gripperangle)
         grippery = gripperlength - math.cos(gripperangle) * gripperlength
-        grippery -= math.sin(gripperangle) * 0.1
+        grippery -= math.sin(gripperangle) * limb3
         gripperx = math.sin(gripperangle) * gripperlength
-        gripperx += (1-math.cos(gripperangle)) * 0.1
+        gripperx += (1-math.cos(gripperangle)) * limb3
     return get_joints_from_xyz_abs(x, y, z-grippery, rx=gripperangle, l3offset=-gripperx)
     
 
-def goto_holder_index(idx, z=0.05, gripperangle=40, flip=False):
+def goto_holder_index(idx, z=0.05, gripperangle=35, flip=False):
     joint = config["position_map"][idx]
     print("Going to cable holder index", joint["index"], "at position", joint["pos"])   
     angles = offset_gripper_angle(joint["pos"][1]/1000, joint["pos"][0]/1000, z, gripperangle=gripperangle, flip=flip)