1 files changed, 38 insertions, 0 deletions
diff --git a/controllers/final/utilities.py b/controllers/final/utilities.py
new file mode 100644
index 0000000..9b3abdd
--- /dev/null
+++ b/controllers/final/utilities.py
@@ -0,0 +1,38 @@
+import numpy as np
+from constants import ARM_OFFSET
+
+# Convert radian measurements to degrees.
+def deg(radians):
+    return radians * 180 / np.pi
+
+# Make sure a variable is between -bound and +bound.
+def bound_by(variable, bound):
+    return max(-bound, min(bound, variable))
+
+# Calculate the euclidean distance between two points.
+# Only uses the x and y coordinates.
+def euclidean(u, v):
+    return np.sqrt( (u[0]-v[0])**2 + (u[1]-v[1])**2 )
+
+# Calculate the position to which the robot should move to grab the jar, given
+# the position of the jar provided by the camera and the robot's current
+# position.
+def grasp_point(jar_pos, robot_pos, jar_offset=0.):
+    d_dist = jar_pos - robot_pos[:2] # dx,dy between the jar and the robot.
+    distance = np.linalg.norm(d_dist) # Euclidean distance.
+    d_dist *= (distance - ARM_OFFSET) / distance # Account for ARM_OFFSET.
+    d_dist[0] += jar_offset # Add the jar_offset to the x-axis.
+    return robot_pos[:2] + d_dist
+
+# Make sure angles are between -pi and +pi.
+def fix_arctan(radians):
+    radians += np.pi
+    radians %= 2 * np.pi
+    return radians - np.pi
+
+# Convert from robot coordinates to world coordinates.
+def robot_to_world_coords(robot_coor, gps_pos, theta):
+    c, s = np.cos(theta), np.sin(theta)
+    matrix = np.array([[c, -s], [s, c]]) # Rotation matrix for theta.
+    # Adjust for angle and add the robot's position to convert to world coords.
+    return matrix @ robot_coor[:2] + gps_pos[:2]