diff --git a/ur5_control.py b/ur5_control.py
index 4878725..7a8e630 100755
--- a/ur5_control.py
+++ b/ur5_control.py
@@ -190,6 +190,11 @@ def move_to_polar(start_pos, end_pos):
 
     return rx_intermediate
 
+def degtorad(angle):
+        return angle/180.0 * math.pi
+def radtodeg(angle):
+        return angle*180.0 / math.pi
+
 def move_to_home():
     global rob
 
@@ -216,37 +221,35 @@ def normalize_degree(theta):
     # Return angle
     return normalized_theta
 
+# Calculate base angle and r relative to shoulder joint
+def calculate_theta(x, y, a):
+    # Calculate if we need the + or - in our equations
+    if (x>-a and y>=0) or (x>a and y<0):
+        flip = 1
+    elif (x<-a and y>=0) or (x<a and y<0):
+        flip = -1
+    else: 
+        # Critical section (x=a, or x=-a). Infinite slope
+        # Return 0 or 180 depending on sign
+        return math.atan2(y, 0)
+    
+    # Calculate tangent line y = mx + b
+    m = (x*y - math.sqrt(x*x*y*y-(x*x-a*a)*(y*y-a*a)))/(x*x-a*a)
+    b = flip * a * math.sqrt(1+m*m)
+    # Calculate equivalent tangent point on circle
+    cx = (-flip*m*b)/(1+m*m)
+    cy = m*cx + flip*b
+    # Calculate base angle, make angle negative if flip=1
+    theta = math.atan2(cy, cx) + (-math.pi if flip==1 else 0)
+    return 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)):
+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, .09, .0997)    # Limb lengths
+    l_bs, l1, l2, l3, l_wt = (0.105, .425, .39225, .1, .0997)    # Limb lengths
     #l3=0.15
     
-    # Calculate base angle and r relative to shoulder joint
-    def calculate_theta(x, y, a):
-        # Calculate if we need the + or - in our equations
-        if (x>-a and y>=0) or (x>a and y<0):
-            flip = 1
-        elif (x<-a and y>=0) or (x<a and y<0):
-            flip = -1
-        else: 
-            # Critical section (x=a, or x=-a). Infinite slope
-            # Return 0 or 180 depending on sign
-            return math.atan2(y, 0)
-        
-        # Calculate tangent line y = mx + b
-        m = (x*y - math.sqrt(x*x*y*y-(x*x-a*a)*(y*y-a*a)))/(x*x-a*a)
-        b = flip * a * math.sqrt(1+m*m)
-
-        # Calculate equivalent tangent point on circle
-        cx = (-flip*m*b)/(1+m*m)
-        cy = m*cx + flip*b
-
-        # Calculate base angle, make angle negative if flip=1
-        theta = math.atan2(cy, cx) + (-math.pi if flip==1 else 0)
-
-        return theta 
+    l3 += l3offset
     
     base_theta = calculate_theta(x, y, l_bs)
     cx, cy = l_bs*math.cos(base_theta), l_bs*math.sin(base_theta)
@@ -264,28 +267,58 @@ def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess =
 
     # Normalize angles
     base, shoulder, elbow, wrist1 = [normalize_degree(deg) for deg in [base_theta, *fsolve(inv_kin_r_z, initial_guess)]]
-    rz = base
+    wrist1 += rx
     # Return result
     return base, shoulder, elbow, wrist1, ry, rz
 
-def get_joints_from_xyz_abs(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2):
-    joints = get_joints_from_xyz_rel(x, y, z, rx, ry, rz)
+def get_joints_from_xyz_abs(x, y, z, rx=0, ry=-math.pi/2, rz=math.pi/2, l3offset=0):
+    joints = get_joints_from_xyz_rel(x, y, z, rx, ry, rz, l3offset=l3offset)
 
     # Joint offsets
     # Base, Shoulder, Elbow, Wrist
     inverse = [1, -1, 1, 1, 1, 1]
     offsets = [-math.pi/2, 0, 0, -math.pi/2, 0, 0]
-
+    if radtodeg(joints[1]) > 137:
+        print("CRASH! Shoulder at", joints[1] * 180/math.pi)
+    #else:
+        #print("Shoulder at", joints[1] * 180/math.pi)
     # Return adjusted joint positions
     return [o+j*i for j, o, i in zip(joints, offsets, inverse)]
 
+# gripper angle: from vertical
+# gripper length: from joint to start of grip
+# to flip, you can use flip=True or make gripper angle negative
+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
+        gripperx = math.sin(gripperangle) * gripperlength
+        gripperx += (1-math.cos(gripperangle)) * 0.1
+    else:
+        gripperangle = degtorad(gripperangle)
+        grippery = gripperlength - math.cos(gripperangle) * gripperlength
+        grippery -= math.sin(gripperangle) * 0.1
+        gripperx = math.sin(gripperangle) * gripperlength
+        gripperx += (1-math.cos(gripperangle)) * 0.1
+    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):
+    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)
+    #rob.movej(angles, acc=2, vel=2)
+    return angles
+    #angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0.05, )
+
 if __name__ == "__main__":
     
     #rob.movej((0, 0, 0, 0, 0, 0), 0.1, 0.2)
     #rob.movel((x, y, z, rx, ry, rz), a, v)
     init("192.168.1.145")
     print("Current tool pose is: ",  rob.getl())
-    move_to_home()
+    #move_to_home()
 
     home_pose = [-0.4999999077032916, 
                -0.2000072960336574, 
@@ -338,21 +371,49 @@ if __name__ == "__main__":
     # angles = get_joints_from_xyz_abs(0, -0.13, 0)
     # rob.movej(angles, acc=2, vel=2)
     config = None
-    # joints = []
-    # for i in np.linspace(-0.2, -0.7, 50):
-    #     joints.append(get_joints_from_xyz_abs(i, 0, 0))
-    # rob.movejs(joints, acc=2, vel=2)
+    joints = []
+    for i in np.linspace(-0.2, -0.7, 50):
+         joints.append(get_joints_from_xyz_abs(i, 0, 0))
+    #rob.movejs(joints, acc=2, vel=2)
     import yaml
     with open('config.yml', 'r') as fileread:
         #global config
         config = yaml.safe_load(fileread)
 
+    rob.movej(goto_holder_index(26, 0.2, 0), acc=2, vel=2)
     joints = []
-    for joint in config["position_map"]:
-        print("Going to cable holder index", joint["index"], "at position", joint["pos"])   
-        angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0)
-        joints.append(angles)
-    rob.movejs(joints, acc=2, vel=2)
+    
+    for i in np.linspace(1, 85, 50):
+        joints.append(goto_holder_index(26, 0.2, i))
+    rob.movejs(joints, acc=0.4, vel=2)
+
+    rob.movej(goto_holder_index(24, 0.2, -10), acc=2, vel=2)
+    rob.movej(goto_holder_index(24, 0.2, -15), acc=2, vel=2)
+    rob.movej(goto_holder_index(24, 0.2, -20), acc=2, vel=2)
+    rob.movej(goto_holder_index(24, 0.2, -28), acc=2, vel=2)
+    rob.movej(goto_holder_index(24, 0.2, -35), acc=2, vel=2)
+    rob.movej(goto_holder_index(24, 0.2, -40), acc=2, vel=2)
+    rob.movej(goto_holder_index(24, 0.2, -45), acc=2, vel=2)
+    rob.movej(goto_holder_index(24, 0.2, -50), acc=2, vel=2)
+    #rob.movej(goto_holder_index(24, 0.2, 60, flip=True), acc=2, vel=2)
+    #time.sleep(2)
+    #rob.movej(goto_holder_index(24, 0.15, 35, flip=True), acc=2, vel=2)
+    #time.sleep(10)
+    # time.sleep(4)
+    # goto_holder_index(26, 0.1, 20)
+    # time.sleep(4)
+    # goto_holder_index(26, 0.1, 30)
+    # time.sleep(4)
+    # goto_holder_index(26, 0.1, 40)
+    # for joint in config["position_map"]:
+    #joint = config["position_map"][26]
+    #print("Going to cable holder index", joint["index"], "at position", joint["pos"])   
+    #angles = get_joints_from_xyz_abs(joint["pos"][1]/1000, joint["pos"][0]/1000, 0.05, )# rx=math.pi / 5)
+    #joints.append(angles)
+
+    #rob.movej(angles, acc=2, vel=2)
+    #time.sleep(10)
+    #rob.movejs(joints, acc=2, vel=2)
     # joints = []
     # for i in np.linspace(-0.3, -0.7, 50):
     #     joints.append(get_joints_from_xyz_abs(i, 0, 0))