Inverse kinematic update to account for base rotation

ur5_control.py

@@ -216,35 +216,65 @@ def normalize_degree(theta):
     # Return angle
     return normalized_theta
 
-def get_joints_from_xyz_rel(x, y, z, initial_guess = (math.pi/2, math.pi/2, 0), limbs=(.422864, .359041, .092124)):
-    # Get polar coordinates of x,y pair
-    r, theta = cartesian_to_polar(x, y)
 
-    # Get length of each limb
-    l1, l2, l3 = limbs
+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)):
+    # Get limbs and offsets
+    offset_x, offset_y, offset_z = (0, 0, 0.14)     # Tool offset
+    l_bs, l1, l2, l3, l_wt = (0.1333, .425, .39225, .1267, .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 
+    
+    base_theta = calculate_theta(x, y, l_bs)
+    cx, cy = l_bs*math.cos(base_theta), l_bs*math.sin(l_bs)
+    r = math.sqrt((x-cx)**2 + (y-cy)**2) 
+
 
     # Formulas to find out joint positions for (r, z)
     def inv_kin_r_z(p):
         a, b, c = p            
 
         return (l1*math.cos(a) + l2*math.cos(a-b) + l3*math.cos(a-b-c) - r, # r
-                l1*math.sin(a) + l2*math.sin(a-b) - l3*math.sin(a-b-c) - z,  # z
+                l1*math.sin(a) + l2*math.sin(a-b) - l3*math.sin(a-b-c) - (l3*math.sin(a-b-c)) - (z + offset_z),  # z
                 a-b-c) # wrist angle
 
 
     # Normalize angles
-    base, shoulder, elbow, wrist = [normalize_degree(deg) for deg in [theta, *fsolve(inv_kin_r_z, initial_guess)]]
+    base, shoulder, elbow, wrist1 = [normalize_degree(deg) for deg in [base_theta, *fsolve(inv_kin_r_z, initial_guess)]]
 
     # Return result
-    return base, shoulder, elbow, wrist
+    return base, shoulder, elbow, wrist1, ry, rz
 
-def get_joints_from_xyz_abs(x, y, z):
-    joints = get_joints_from_xyz_rel(x, y, z)
+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)
 
     # Joint offsets
     # Base, Shoulder, Elbow, Wrist
-    inverse = [1, -1, 1, 1]
-    offsets = [0, 0, 0, -math.pi/2]
+    inverse = [1, -1, 1, 1, 1, 1]
+    offsets = [-math.pi/2, 0, 0, -math.pi/2, 0, 0]
 
     # Return adjusted joint positions
     return [o+j*i for j, o, i in zip(joints, offsets, inverse)]
@@ -262,7 +292,7 @@ if __name__ == "__main__":
                0.40002172976662786, 
                0, 
                -3.14152741295329, 
-               0]
+               math.radians(62)]
 
     # time.sleep(.5)
 
@@ -290,14 +320,21 @@ if __name__ == "__main__":
     # set_pos_abs(*home_pose)
 
 
-    angles = get_joints_from_xyz_abs(0.3, 0.3, 0.3)
-    rob.movej([*angles, *rob.getj()[4:]], acc=1, vel=1)
     
-    angles = get_joints_from_xyz_abs(-0.3, -0.3, 0.7)
-    rob.movej([*angles, *rob.getj()[4:]], acc=1, vel=1)
+    angles = get_joints_from_xyz_abs(-0.7, 0, 0)
+    rob.movej(angles, 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))
+    # rob.movejs(joints, acc=2, vel=2)
+
+    # time.sleep(5)
+
+
+    # angles = get_joints_from_xyz_abs(0, -0.6, 0)
+    # rob.movej(angles, acc=2, vel=2)
     
-    angles = get_joints_from_xyz_abs(-0.3, 0.4, 0.2)
-    rob.movej([*angles, *rob.getj()[4:]], acc=1, vel=1)
 
 
     # set_pos_abs(*p1)