Update the pi_2_pi function to a use of a modulo instead of two while.

AlexisTM · AlexisTM · commit b243750a13ee · 2018-07-10T09:22:18.000+02:00
diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py
@@ -221,14 +221,10 @@ def choose_parent(self, newNode, nearinds):
 
         return newNode
 
-    def pi_2_pi(self, angle):
-        while(angle > math.pi):
-            angle = angle - 2.0 * math.pi
 
-        while(angle < -math.pi):
-            angle = angle + 2.0 * math.pi
+    def pi_2_pi(self, angle):
+        return (angle + math.pi) % (2*math.pi) - math.pi
 
-        return angle
 
     def steer(self, rnd, nind):
         #  print(rnd)
diff --git a/PathPlanning/ClosedLoopRRTStar/unicycle_model.py b/PathPlanning/ClosedLoopRRTStar/unicycle_model.py
@@ -38,13 +38,7 @@ def update(state, a, delta):
 
 
 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 + math.pi) % (2*math.pi) - math.pi
 
 
 if __name__ == '__main__':
diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py
@@ -17,13 +17,7 @@ def mod2pi(theta):
 
 
 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 + math.pi) % (2*math.pi) - math.pi
 
 
 def LSL(alpha, beta, d):
diff --git a/PathPlanning/LQRRRTStar/lqr_rrt_star.py b/PathPlanning/LQRRRTStar/lqr_rrt_star.py
@@ -109,13 +109,7 @@ def choose_parent(self, newNode, nearinds):
         return newNode
 
     def pi_2_pi(self, 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 + math.pi) % (2*math.pi) - math.pi
 
     def sample_path(self, wx, wy, step):
 
diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py
@@ -18,13 +18,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 + math.pi) % (2*math.pi) - math.pi
 
 
 def update(state, v, delta, dt, L):
diff --git a/PathPlanning/RRTDubins/dubins_path_planning.py b/PathPlanning/RRTDubins/dubins_path_planning.py
@@ -17,13 +17,7 @@ def mod2pi(theta):
 
 
 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 + math.pi) % (2*math.pi) - math.pi
 
 
 def LSL(alpha, beta, d):
diff --git a/PathPlanning/RRTDubins/rrt_dubins.py b/PathPlanning/RRTDubins/rrt_dubins.py
@@ -96,14 +96,10 @@ def choose_parent(self, newNode, nearinds):
 
         return newNode
 
-    def pi_2_pi(self, angle):
-        while(angle >= math.pi):
-            angle = angle - 2.0 * math.pi
 
-        while(angle <= -math.pi):
-            angle = angle + 2.0 * math.pi
+    def pi_2_pi(self, angle):
+        return (angle + math.pi) % (2*math.pi) - math.pi
 
-        return angle
 
     def steer(self, rnd, nind):
         #  print(rnd)
diff --git a/PathPlanning/RRTStarDubins/dubins_path_planning.py b/PathPlanning/RRTStarDubins/dubins_path_planning.py
@@ -13,13 +13,7 @@ def mod2pi(theta):
 
 
 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 + math.pi) % (2*math.pi) - math.pi
 
 
 def LSL(alpha, beta, d):
diff --git a/PathPlanning/RRTStarDubins/rrt_star_dubins.py b/PathPlanning/RRTStarDubins/rrt_star_dubins.py
@@ -97,13 +97,7 @@ def choose_parent(self, newNode, nearinds):
         return newNode
 
     def pi_2_pi(self, 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 + math.pi) % (2*math.pi) - math.pi
 
     def steer(self, rnd, nind):
         #  print(rnd)
diff --git a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py
@@ -106,13 +106,7 @@ def choose_parent(self, newNode, nearinds):
         return newNode
 
     def pi_2_pi(self, 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 + math.pi) % (2*math.pi) - math.pi
 
     def steer(self, rnd, nind):
 
diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py
@@ -331,13 +331,7 @@ def generate_local_course(L, lengths, mode, maxc, step_size):
 
 
 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 + math.pi) % (2*math.pi) - math.pi
 
 
 def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size):
diff --git a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py
@@ -51,13 +51,7 @@ def update(state, a, delta):
 
 
 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 + math.pi) % (2*math.pi) - math.pi
 
 
 def solve_DARE(A, B, Q, R):
diff --git a/PathTracking/lqr_steer_control/lqr_steer_control.py b/PathTracking/lqr_steer_control/lqr_steer_control.py
@@ -61,13 +61,7 @@ def PIDControl(target, current):
 
 
 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 + math.pi) % (2*math.pi) - math.pi
 
 
 def solve_DARE(A, B, Q, R):
diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py
@@ -203,13 +203,7 @@ def jacobH(q, delta, x, i):
 
 
 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 + math.pi) % (2*math.pi) - math.pi
 
 
 def main():
diff --git a/SLAM/FastSLAM1/fast_slam1.py b/SLAM/FastSLAM1/fast_slam1.py
@@ -325,13 +325,7 @@ def motion_model(x, u):
 
 
 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 + math.pi) % (2*math.pi) - math.pi
 
 
 def main():
diff --git a/SLAM/FastSLAM2/fast_slam2.py b/SLAM/FastSLAM2/fast_slam2.py
@@ -354,13 +354,7 @@ def motion_model(x, u):
 
 
 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 + math.pi) % (2*math.pi) - math.pi
 
 
 def main():
diff --git a/SLAM/GraphBasedSLAM/graph_based_slam.py b/SLAM/GraphBasedSLAM/graph_based_slam.py
@@ -253,13 +253,7 @@ def motion_model(x, u):
 
 
 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 + math.pi) % (2*math.pi) - math.pi
 
 
 def main():
