Using util.angle_mod in all codes. AtsushiSakai#684 (AtsushiSakai#946)

* switched to using utils.angle_mod()

* switched to using utils.angle_mod()

* renamed mod2pi to pi_2_pi

* Removed linting errors

* switched to using utils.angle_mod()

* switched to using utils.angle_mod()

* renamed mod2pi to pi_2_pi

* Removed linting errors

* annotation changes and round precision

* Reverted to mod2pi

---------

Co-authored-by: Videh Patel <[email protected]>

Author: videh25

ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py

@@ -11,6 +11,7 @@
 
 import numpy as np
 from ArmNavigation.n_joint_arm_to_point_control.NLinkArm import NLinkArm
+from utils.angle import angle_mod
 
 # Simulation parameters
 Kp = 2
@@ -159,9 +160,9 @@ def ang_diff(theta1, theta2):
     """
     Returns the difference between two angles in the range -pi to +pi
     """
-    return (theta1 - theta2 + np.pi) % (2 * np.pi) - np.pi
+    return angle_mod(theta1 - theta2)
 
 
 if __name__ == '__main__':
     # main()
-    animation()
+    animation()

ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py

@@ -15,6 +15,7 @@
 import matplotlib.pyplot as plt
 import numpy as np
 import math
+from utils.angle import angle_mod
 
 
 # Simulation parameters
@@ -110,7 +111,7 @@ def plot_arm(theta1, theta2, target_x, target_y):  # pragma: no cover
 
 def ang_diff(theta1, theta2):
     # Returns the difference between two angles in the range -pi to +pi
-    return (theta1 - theta2 + np.pi) % (2 * np.pi) - np.pi
+    return angle_mod(theta1 - theta2)
 
 
 def click(event):  # pragma: no cover

Control/move_to_pose/move_to_pose.py

@@ -13,7 +13,7 @@
 import matplotlib.pyplot as plt
 import numpy as np
 from random import random
-
+from utils.angle import angle_mod
 
 class PathFinderController:
     """
@@ -71,9 +71,8 @@ def calc_control_command(self, x_diff, y_diff, theta, theta_goal):
         # from 0 rad to 2*pi rad with slight turn
 
         rho = np.hypot(x_diff, y_diff)
-        alpha = (np.arctan2(y_diff, x_diff)
-                 - theta + np.pi) % (2 * np.pi) - np.pi
-        beta = (theta_goal - theta - alpha + np.pi) % (2 * np.pi) - np.pi
+        alpha = angle_mod(np.arctan2(y_diff, x_diff) - theta)
+        beta = angle_mod(theta_goal - theta - alpha)
         v = self.Kp_rho * rho
         w = self.Kp_alpha * alpha - controller.Kp_beta * beta
 
@@ -173,16 +172,14 @@ def transformation_matrix(x, y, theta):
 def main():
 
     for i in range(5):
-        x_start = 20 * random()
-        y_start = 20 * random()
-        theta_start = 2 * np.pi * random() - np.pi
+        x_start = 20.0 * random()
+        y_start = 20.0 * random()
+        theta_start: float = 2 * np.pi * random() - np.pi
         x_goal = 20 * random()
         y_goal = 20 * random()
         theta_goal = 2 * np.pi * random() - np.pi
-        print("Initial x: %.2f m\nInitial y: %.2f m\nInitial theta: %.2f rad\n" %
-              (x_start, y_start, theta_start))
-        print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" %
-              (x_goal, y_goal, theta_goal))
+        print(f"Initial x: {round(x_start, 2)} m\nInitial y: {round(y_start, 2)} m\nInitial theta: {round(theta_start, 2)} rad\n")
+        print(f"Goal x: {round(x_goal, 2)} m\nGoal y: {round(y_goal, 2)} m\nGoal theta: {round(theta_goal, 2)} rad\n")
         move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal)
 
 

Localization/ensemble_kalman_filter/ensemble_kalman_filter.py

@@ -16,6 +16,7 @@
 import math
 import matplotlib.pyplot as plt
 import numpy as np
+from utils.angle import angle_mod
 
 from utils.angle import rot_mat_2d
 
@@ -179,7 +180,7 @@ def plot_covariance_ellipse(xEst, PEst):  # pragma: no cover
 
 
 def pi_2_pi(angle):
-    return (angle + math.pi) % (2 * math.pi) - math.pi
+    return angle_mod(angle)
 
 
 def main():

PathPlanning/ClosedLoopRRTStar/unicycle_model.py

@@ -8,6 +8,7 @@
 
 import math
 import numpy as np
+from utils.angle import angle_mod
 
 dt = 0.05  # [s]
 L = 0.9  # [m]
@@ -39,7 +40,7 @@ def update(state, a, delta):
 
 
 def pi_2_pi(angle):
-    return (angle + math.pi) % (2 * math.pi) - math.pi
+    return angle_mod(angle)
 
 
 if __name__ == '__main__':  # pragma: no cover

PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py

@@ -1,6 +1,7 @@
 import math
 import numpy as np
 from scipy.interpolate import interp1d
+from utils.angle import angle_mod
 
 # motion parameter
 L = 1.0  # wheel base
@@ -18,7 +19,7 @@ def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
 
 
 def pi_2_pi(angle):
-    return (angle + math.pi) % (2 * math.pi) - math.pi
+    return angle_mod(angle)
 
 
 def update(state, v, delta, dt, L):

PathPlanning/ModelPredictiveTrajectoryGenerator/trajectory_generator.py

@@ -18,7 +18,7 @@
 
 # optimization parameter
 max_iter = 100
-h = np.array([0.5, 0.02, 0.02]).T  # parameter sampling distance
+h: np.ndarray = np.array([0.5, 0.02, 0.02]).T  # parameter sampling distance
 cost_th = 0.1
 
 show_animation = True

PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py

@@ -9,6 +9,7 @@
 
 import matplotlib.pyplot as plt
 import numpy as np
+from utils.angle import angle_mod
 
 show_animation = True
 
@@ -40,6 +41,9 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
         plt.plot(x, y)
 
 
+def pi_2_pi(x):
+    return angle_mod(x)
+
 def mod2pi(x):
     # Be consistent with fmod in cplusplus here.
     v = np.mod(x, np.copysign(2.0 * math.pi, x))
@@ -50,7 +54,6 @@ def mod2pi(x):
             v -= 2.0 * math.pi
     return v
 
-
 def straight_left_straight(x, y, phi):
     phi = mod2pi(phi)
     # only take phi in (0.01*math.pi, 0.99*math.pi) for the sake of speed.
@@ -296,10 +299,6 @@ def interpolate(dist, length, mode, max_curvature, origin_x, origin_y,
     return x, y, yaw, 1 if length > 0.0 else -1
 
 
-def pi_2_pi(angle):
-    return (angle + math.pi) % (2 * math.pi) - math.pi
-
-
 def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size):
     q0 = [sx, sy, syaw]
     q1 = [gx, gy, gyaw]

PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py

@@ -11,6 +11,7 @@
 import numpy as np
 import scipy.linalg as la
 import pathlib
+from utils.angle import angle_mod
 sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
 
 from PathPlanning.CubicSpline import cubic_spline_planner
@@ -52,8 +53,7 @@ def update(state, a, delta):
 
 
 def pi_2_pi(angle):
-    return (angle + math.pi) % (2 * math.pi) - math.pi
-
+    return angle_mod(angle)
 
 def solve_dare(A, B, Q, R):
     """

PathTracking/lqr_steer_control/lqr_steer_control.py

@@ -11,6 +11,7 @@
 import numpy as np
 import sys
 import pathlib
+from utils.angle import angle_mod
 sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
 
 from PathPlanning.CubicSpline import cubic_spline_planner
@@ -61,7 +62,7 @@ def PIDControl(target, current):
 
 
 def pi_2_pi(angle):
-    return (angle + math.pi) % (2 * math.pi) - math.pi
+    return angle_mod(angle)
 
 
 def solve_DARE(A, B, Q, R):

PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py

@@ -11,6 +11,7 @@
 import numpy as np
 import sys
 import pathlib
+from utils.angle import angle_mod
 sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
 
 from PathPlanning.CubicSpline import cubic_spline_planner
@@ -69,13 +70,7 @@ def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
 
 
 def pi_2_pi(angle):
-    while(angle > math.pi):
-        angle = angle - 2.0 * math.pi
-
-    while(angle < -math.pi):
-        angle = angle + 2.0 * math.pi
-
-    return angle
+    return angle_mod(angle)
 
 
 def get_linear_model_matrix(v, phi, delta):

PathTracking/rear_wheel_feedback/rear_wheel_feedback.py

@@ -8,6 +8,7 @@
 import matplotlib.pyplot as plt
 import math
 import numpy as np
+from utils.angle import angle_mod
 
 from scipy import interpolate
 from scipy import optimize
@@ -51,21 +52,21 @@ def __init__(self, x, y):
         self.ddY = self.Y.derivative(2)
 
         self.length = s[-1]
-    
+
     def calc_yaw(self, s):
         dx, dy = self.dX(s), self.dY(s)
         return np.arctan2(dy, dx)
-    
+
     def calc_curvature(self, s):
         dx, dy   = self.dX(s), self.dY(s)
         ddx, ddy   = self.ddX(s), self.ddY(s)
         return (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2))
-    
+
     def __find_nearest_point(self, s0, x, y):
         def calc_distance(_s, *args):
             _x, _y= self.X(_s), self.Y(_s)
             return (_x - args[0])**2 + (_y - args[1])**2
-        
+
         def calc_distance_jacobian(_s, *args):
             _x, _y = self.X(_s), self.Y(_s)
             _dx, _dy = self.dX(_s), self.dY(_s)
@@ -76,7 +77,7 @@ def calc_distance_jacobian(_s, *args):
 
     def calc_track_error(self, x, y, s0):
         ret = self.__find_nearest_point(s0, x, y)
-        
+
         s = ret[0][0]
         e = ret[1]
 
@@ -96,13 +97,7 @@ def pid_control(target, current):
     return a
 
 def pi_2_pi(angle):
-    while(angle > math.pi):
-        angle = angle - 2.0 * math.pi
-
-    while(angle < -math.pi):
-        angle = angle + 2.0 * math.pi
-
-    return angle
+    return angle_mod(angle)
 
 def rear_wheel_feedback_control(state, e, k, yaw_ref):
     v = state.v
@@ -170,7 +165,7 @@ def simulate(path_ref, goal):
             plt.plot(path_ref.X(s0), path_ref.Y(s0), "xg", label="target")
             plt.axis("equal")
             plt.grid(True)
-            plt.title("speed[km/h]:{:.2f}, target s-param:{:.2f}".format(round(state.v * 3.6, 2), s0))
+            plt.title(f"speed[km/h]:{round(state.v * 3.6, 2):.2f}, target s-param:{s0:.2f}")
             plt.pause(0.0001)
 
     return t, x, y, yaw, v, goal_flag
@@ -184,7 +179,7 @@ def calc_target_speed(state, yaw_ref):
     if switch:
         state.direction *= -1
         return 0.0
-    
+
     if state.direction != 1:
         return -target_speed
 

PathTracking/stanley_controller/stanley_controller.py

@@ -13,6 +13,7 @@
 import matplotlib.pyplot as plt
 import sys
 import pathlib
+from utils.angle import angle_mod
 sys.path.append(str(pathlib.Path(__file__).parent.parent.parent))
 
 from PathPlanning.CubicSpline import cubic_spline_planner
@@ -26,7 +27,7 @@
 show_animation = True
 
 
-class State(object):
+class State:
     """
     Class representing the state of a vehicle.
 
@@ -38,7 +39,7 @@ class State(object):
 
     def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
         """Instantiate the object."""
-        super(State, self).__init__()
+        super().__init__()
         self.x = x
         self.y = y
         self.yaw = yaw
@@ -106,13 +107,7 @@ def normalize_angle(angle):
     :param angle: (float)
     :return: (float) Angle in radian in [-pi, pi]
     """
-    while angle > np.pi:
-        angle -= 2.0 * np.pi
-
-    while angle < -np.pi:
-        angle += 2.0 * np.pi
-
-    return angle
+    return angle_mod(angle)
 
 
 def calc_target_index(state, cx, cy):

SLAM/EKFSLAM/ekf_slam.py

@@ -8,6 +8,7 @@
 
 import matplotlib.pyplot as plt
 import numpy as np
+from utils.angle import angle_mod
 
 # EKF state covariance
 Cx = np.diag([0.5, 0.5, np.deg2rad(30.0)]) ** 2
@@ -192,7 +193,7 @@ def jacob_h(q, delta, x, i):
 
 
 def pi_2_pi(angle):
-    return (angle + math.pi) % (2 * math.pi) - math.pi
+    return angle_mod(angle)
 
 
 def main():

SLAM/FastSLAM1/fast_slam1.py

@@ -10,6 +10,7 @@
 
 import matplotlib.pyplot as plt
 import numpy as np
+from utils.angle import angle_mod
 
 # Fast SLAM covariance
 Q = np.diag([3.0, np.deg2rad(10.0)]) ** 2
@@ -321,7 +322,7 @@ def motion_model(x, u):
 
 
 def pi_2_pi(angle):
-    return (angle + math.pi) % (2 * math.pi) - math.pi
+    return angle_mod(angle)
 
 
 def main():