Re-merge calculate_theta into get_joints_from_xyz_rel

ur5_control.py

@@ -221,27 +221,7 @@ 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), l3offset=0):
     # Get limbs and offsets
@@ -249,8 +229,30 @@ def get_joints_from_xyz_rel(x, y, z, rx=0, ry=-math.pi/2, rz=0, initial_guess =
     l_bs, l1, l2, l3, l_wt = (0.105, .425, .39225, .1, .0997)    # Limb lengths
     #l3=0.15
     
-    l3 += l3offset
+    l3 += l3offset # add wrist offset, used for gripper angle calculations
     
+    # 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(base_theta)
     r = math.sqrt((x+cx)**2 + (y+cy)**2)