From b243750a13ee332717cf5e80b5dc60640a04c721 Mon Sep 17 00:00:00 2001 From: Alexis Paques Date: Tue, 10 Jul 2018 09:22:18 +0200 Subject: [PATCH] Update the pi_2_pi function to a use of a modulo instead of two while. --- .../ClosedLoopRRTStar/closed_loop_rrt_star_car.py | 8 ++------ PathPlanning/ClosedLoopRRTStar/unicycle_model.py | 8 +------- PathPlanning/DubinsPath/dubins_path_planning.py | 8 +------- PathPlanning/LQRRRTStar/lqr_rrt_star.py | 8 +------- .../ModelPredictiveTrajectoryGenerator/motion_model.py | 8 +------- PathPlanning/RRTDubins/dubins_path_planning.py | 8 +------- PathPlanning/RRTDubins/rrt_dubins.py | 8 ++------ PathPlanning/RRTStarDubins/dubins_path_planning.py | 8 +------- PathPlanning/RRTStarDubins/rrt_star_dubins.py | 8 +------- PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py | 8 +------- PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py | 8 +------- .../lqr_speed_steer_control/lqr_speed_steer_control.py | 8 +------- PathTracking/lqr_steer_control/lqr_steer_control.py | 8 +------- SLAM/EKFSLAM/ekf_slam.py | 8 +------- SLAM/FastSLAM1/fast_slam1.py | 8 +------- SLAM/FastSLAM2/fast_slam2.py | 8 +------- SLAM/GraphBasedSLAM/graph_based_slam.py | 8 +------- 17 files changed, 19 insertions(+), 117 deletions(-) diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py index 1500abbf..f97cf906 100644 --- a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py +++ b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py @@ -221,14 +221,10 @@ class RRT(): return newNode + def pi_2_pi(self, angle): - while(angle > math.pi): - angle = angle - 2.0 * math.pi + return (angle + math.pi) % (2*math.pi) - math.pi - while(angle < -math.pi): - angle = angle + 2.0 * 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 index fe6e05ae..5185eb1c 100644 --- 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 index 52eb1c1a..4477cc13 100644 --- 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 index 1eb7bb6e..326c787f 100644 --- a/PathPlanning/LQRRRTStar/lqr_rrt_star.py +++ b/PathPlanning/LQRRRTStar/lqr_rrt_star.py @@ -109,13 +109,7 @@ class RRT(): 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 index 6319c5a0..39675198 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py @@ -18,13 +18,7 @@ class State: 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 index a5bc8bbd..b489ca5f 100644 --- 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 index 71c84d1b..33126335 100644 --- a/PathPlanning/RRTDubins/rrt_dubins.py +++ b/PathPlanning/RRTDubins/rrt_dubins.py @@ -96,14 +96,10 @@ class RRT(): return newNode + def pi_2_pi(self, angle): - while(angle >= math.pi): - angle = angle - 2.0 * math.pi + return (angle + math.pi) % (2*math.pi) - math.pi - while(angle <= -math.pi): - angle = angle + 2.0 * 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 index 92565c6e..ad9e7e55 100644 --- 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 index fdc659e6..79aabf70 100644 --- a/PathPlanning/RRTStarDubins/rrt_star_dubins.py +++ b/PathPlanning/RRTStarDubins/rrt_star_dubins.py @@ -97,13 +97,7 @@ class RRT(): 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 index e4cab301..089915bb 100644 --- a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py +++ b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py @@ -106,13 +106,7 @@ class RRT(): 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 index 322accbb..28e4ef9d 100644 --- 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 index 50331b3f..2422208b 100644 --- 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 index 341bb278..20e45e12 100644 --- 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 index 776fb017..c8b5ae03 100644 --- 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 index bd9da70c..eb2f03fa 100644 --- 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 index 6985343d..76fb2928 100644 --- 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 index a3f7604c..eb64076d 100644 --- 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():