From f16242f5bed799332ecde1c253bae26c8a5aa58f Mon Sep 17 00:00:00 2001
From: Cole Deck <cdeck@hawk.iit.edu>
Date: Sun, 17 Mar 2024 19:53:49 -0500
Subject: [PATCH] Re-merge calculate_theta into get_joints_from_xyz_rel

---
 ur5_control.py | 46 ++++++++++++++++++++++++----------------------
 1 file changed, 24 insertions(+), 22 deletions(-)

diff --git a/ur5_control.py b/ur5_control.py
index 7a8e630..3b33b99 100755
--- a/ur5_control.py
+++ b/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)