Modified wrap_to_pi() to take input of radians or degrees.

micah-huth · micah-huth · commit bcde356d1a3c · 2020-06-23T10:38:37.000+10:00
diff --git a/graphics/common_functions.py b/graphics/common_functions.py
@@ -5,9 +5,28 @@
 z_axis_vector = vector(0, 0, 1)
 
 
-# TODO handle radians (needed more than degrees)
-def wrap_to_pi(angle):
-    angle = angle % radians(360)
-    if angle > radians(180):
-        angle -= radians(360)
+def wrap_to_pi(angle_type, angle):
+    """
+    Wrap the given angle (deg or rad) to [-pi pi]
+
+    :param angle_type: String of whether the angle is deg or rad
+    :type angle_type: `str`
+    :param angle: The angle to wrap
+    :type angle: `float`
+    :return: The wrapped angle
+    :rtype: `float`
+    """
+    if angle_type == "deg":
+        angle = angle % 360
+        if angle > 180:
+            angle -= 360
+
+    elif angle_type == "rad":
+        angle = angle % radians(360)
+        if angle > radians(180):
+            angle -= radians(360)
+
+    else:
+        raise ValueError('angle_type must be "deg" or "rad"')
+
     return angle
diff --git a/graphics/graphics_robot.py b/graphics/graphics_robot.py
@@ -84,13 +84,13 @@ def rotate_around_joint_axis(self, angle_of_rotation, axis_of_rotation):
         # Then add the rotation amount to the axis counter
         if axis_of_rotation.equals(x_axis_vector):
             rotation_axis = self.__x_vector
-            self.__x_rotation = wrap_to_pi(self.__x_rotation + angle_of_rotation)
+            self.__x_rotation = wrap_to_pi("rad", self.__x_rotation + angle_of_rotation)
         elif axis_of_rotation.equals(y_axis_vector):
             rotation_axis = self.__y_vector
-            self.__y_rotation = wrap_to_pi(self.__y_rotation + angle_of_rotation)
+            self.__y_rotation = wrap_to_pi("rad", self.__y_rotation + angle_of_rotation)
         elif axis_of_rotation.equals(z_axis_vector):
             rotation_axis = self.__z_vector
-            self.__z_rotation = wrap_to_pi(self.__z_rotation + angle_of_rotation)
+            self.__z_rotation = wrap_to_pi("rad", self.__z_rotation + angle_of_rotation)
         else:
             error_str = "Bad input vector given ({0}). Must be either x_axis_vector ({1}), y_axis_vector ({2})," \
                         "or z_axis_vector ({3}). Use rotate_around_vector for rotation about an arbitrary vector."
@@ -140,11 +140,11 @@ def rotate_around_vector(self, angle_of_rotation, axis_of_rotation):
         # axis of rotation will have the smallest (it's less affected by the rotation)
         min_angle_diff = min(angle_diff_x, angle_diff_y, angle_diff_z)
         if min_angle_diff == angle_diff_x:
-            self.__x_rotation = wrap_to_pi(self.__x_rotation + angle_of_rotation)
+            self.__x_rotation = wrap_to_pi("rad", self.__x_rotation + angle_of_rotation)
         elif min_angle_diff == angle_diff_y:
-            self.__y_rotation = wrap_to_pi(self.__y_rotation + angle_of_rotation)
+            self.__y_rotation = wrap_to_pi("rad", self.__y_rotation + angle_of_rotation)
         else:
-            self.__z_rotation = wrap_to_pi(self.__z_rotation + angle_of_rotation)
+            self.__z_rotation = wrap_to_pi("rad", self.__z_rotation + angle_of_rotation)
 
         # Calculate the updated toolpoint location
         self.__connect_dir.rotate(angle=angle_of_rotation, axis=axis_of_rotation)
@@ -353,10 +353,10 @@ def rotate_joint(self, new_angle):
         :type new_angle: float (radians)
         """
         # Wrap given angle to -pi to pi
-        new_angle = wrap_to_pi(new_angle)
+        new_angle = wrap_to_pi("rad", new_angle)
         current_angle = self.rotation_angle
         # Calculate amount to rotate the link
-        angle_diff = wrap_to_pi(new_angle - current_angle)
+        angle_diff = wrap_to_pi("rad", new_angle - current_angle)
         # Update the link
         self.rotate_around_joint_axis(angle_diff, self.rotation_axis)
         self.rotation_angle = new_angle
