From a164faa7f20496eba6865d560764663320157d40 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Tue, 23 Oct 2018 21:49:08 +0900 Subject: [PATCH] replace math.radians to np.deg2rad --- .../extended_kalman_filter.py | 6 +++--- .../particle_filter/particle_filter.py | 20 +++++++++++-------- .../unscented_kalman_filter.py | 6 +++--- Mapping/circle_fitting/circle_fitting.py | 8 ++++---- .../raycasting_grid_map.py | 2 +- .../rectangle_fitting/rectangle_fitting.py | 8 ++++---- .../closed_loop_rrt_star_car.py | 10 ++++------ .../ClosedLoopRRTStar/pure_pursuit.py | 4 ++-- .../ClosedLoopRRTStar/unicycle_model.py | 6 +++--- .../DubinsPath/dubins_path_planning.py | 12 +++++------ PathPlanning/HybridAStar/hybrid_a_star.py | 6 +++--- .../lookuptable_generator.py | 2 +- .../model_predictive_trajectory_generator.py | 8 ++++---- .../quintic_polynomials_planner.py | 4 ++-- PathPlanning/RRT/rrt_with_pathsmoothing.py | 4 ++-- .../RRTDubins/dubins_path_planning.py | 9 +++++---- PathPlanning/RRTDubins/rrt_dubins.py | 8 +++----- .../RRTStarDubins/dubins_path_planning.py | 8 ++++---- PathPlanning/RRTStarDubins/rrt_star_dubins.py | 8 ++++---- .../RRTStarReedsShepp/rrt_star_reeds_shepp.py | 8 ++++---- .../reeds_shepp_path_planning.py | 10 +++++----- .../lqr_speed_steer_control.py | 4 ++-- .../lqr_steer_control/lqr_steer_control.py | 4 ++-- ...odel_predictive_speed_and_steer_control.py | 4 ++-- SLAM/EKFSLAM/ekf_slam.py | 8 ++++---- SLAM/FastSLAM1/fast_slam1.py | 10 +++++----- SLAM/FastSLAM2/fast_slam2.py | 10 +++++----- SLAM/GraphBasedSLAM/graph_based_slam.py | 8 ++++---- .../iterative_closest_point.py | 2 +- tests/test_dubins_path_planning.py | 6 +++--- 30 files changed, 107 insertions(+), 106 deletions(-) diff --git a/Localization/extended_kalman_filter/extended_kalman_filter.py b/Localization/extended_kalman_filter/extended_kalman_filter.py index 8c011862..0f7917a2 100644 --- a/Localization/extended_kalman_filter/extended_kalman_filter.py +++ b/Localization/extended_kalman_filter/extended_kalman_filter.py @@ -11,12 +11,12 @@ import math import matplotlib.pyplot as plt # Estimation parameter of EKF -Q = np.diag([0.1, 0.1, math.radians(1.0), 1.0])**2 -R = np.diag([1.0, math.radians(40.0)])**2 +Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2 +R = np.diag([1.0, np.deg2rad(40.0)])**2 # Simulation parameter Qsim = np.diag([0.5, 0.5])**2 -Rsim = np.diag([1.0, math.radians(30.0)])**2 +Rsim = np.diag([1.0, np.deg2rad(30.0)])**2 DT = 0.1 # time tick [s] SIM_TIME = 50.0 # simulation time [s] diff --git a/Localization/particle_filter/particle_filter.py b/Localization/particle_filter/particle_filter.py index 2dd580aa..4b384ae5 100644 --- a/Localization/particle_filter/particle_filter.py +++ b/Localization/particle_filter/particle_filter.py @@ -12,11 +12,11 @@ import matplotlib.pyplot as plt # Estimation parameter of PF Q = np.diag([0.1])**2 # range error -R = np.diag([1.0, math.radians(40.0)])**2 # input error +R = np.diag([1.0, np.deg2rad(40.0)])**2 # input error # Simulation parameter Qsim = np.diag([0.2])**2 -Rsim = np.diag([1.0, math.radians(30.0)])**2 +Rsim = np.diag([1.0, np.deg2rad(30.0)])**2 DT = 0.1 # time tick [s] SIM_TIME = 50.0 # simulation time [s] @@ -170,13 +170,17 @@ def plot_covariance_ellipse(xEst, PEst): t = np.arange(0, 2 * math.pi + 0.1, 0.1) - #eigval[bigind] or eiqval[smallind] were occassionally negative numbers extremely - #close to 0 (~10^-20), catch these cases and set the respective variable to 0 - try: a = math.sqrt(eigval[bigind]) - except ValueError: a = 0 + # eigval[bigind] or eiqval[smallind] were occassionally negative numbers extremely + # close to 0 (~10^-20), catch these cases and set the respective variable to 0 + try: + a = math.sqrt(eigval[bigind]) + except ValueError: + a = 0 - try: b = math.sqrt(eigval[smallind]) - except ValueError: b = 0 + try: + b = math.sqrt(eigval[smallind]) + except ValueError: + b = 0 x = [a * math.cos(it) for it in t] y = [b * math.sin(it) for it in t] diff --git a/Localization/unscented_kalman_filter/unscented_kalman_filter.py b/Localization/unscented_kalman_filter/unscented_kalman_filter.py index 0b6da640..e6b8361a 100644 --- a/Localization/unscented_kalman_filter/unscented_kalman_filter.py +++ b/Localization/unscented_kalman_filter/unscented_kalman_filter.py @@ -12,12 +12,12 @@ import math import matplotlib.pyplot as plt # Estimation parameter of UKF -Q = np.diag([0.1, 0.1, math.radians(1.0), 1.0])**2 -R = np.diag([1.0, math.radians(40.0)])**2 +Q = np.diag([0.1, 0.1, np.deg2rad(1.0), 1.0])**2 +R = np.diag([1.0, np.deg2rad(40.0)])**2 # Simulation parameter Qsim = np.diag([0.5, 0.5])**2 -Rsim = np.diag([1.0, math.radians(30.0)])**2 +Rsim = np.diag([1.0, np.deg2rad(30.0)])**2 DT = 0.1 # time tick [s] SIM_TIME = 50.0 # simulation time [s] diff --git a/Mapping/circle_fitting/circle_fitting.py b/Mapping/circle_fitting/circle_fitting.py index 37799b03..57f4552d 100644 --- a/Mapping/circle_fitting/circle_fitting.py +++ b/Mapping/circle_fitting/circle_fitting.py @@ -93,8 +93,8 @@ def ray_casting_filter(xl, yl, thetal, rangel, angle_reso): def plot_circle(x, y, size, color="-b"): deg = list(range(0, 360, 5)) deg.append(0) - xl = [x + size * math.cos(math.radians(d)) for d in deg] - yl = [y + size * math.sin(math.radians(d)) for d in deg] + xl = [x + size * math.cos(np.deg2rad(d)) for d in deg] + yl = [y + size * math.sin(np.deg2rad(d)) for d in deg] plt.plot(xl, yl, color) @@ -107,8 +107,8 @@ def main(): cx = -2.0 # initial x position of obstacle cy = -8.0 # initial y position of obstacle cr = 1.0 # obstacle radious - theta = math.radians(30.0) # obstacle moving direction - angle_reso = math.radians(3.0) # sensor angle resolution + theta = np.deg2rad(30.0) # obstacle moving direction + angle_reso = np.deg2rad(3.0) # sensor angle resolution time = 0.0 while time <= simtime: diff --git a/Mapping/raycasting_grid_map/raycasting_grid_map.py b/Mapping/raycasting_grid_map/raycasting_grid_map.py index 67ae4fd2..7790685c 100644 --- a/Mapping/raycasting_grid_map/raycasting_grid_map.py +++ b/Mapping/raycasting_grid_map/raycasting_grid_map.py @@ -114,7 +114,7 @@ def main(): print(__file__ + " start!!") xyreso = 0.25 # x-y grid resolution [m] - yawreso = math.radians(10.0) # yaw angle resolution [rad] + yawreso = np.deg2rad(10.0) # yaw angle resolution [rad] for i in range(5): ox = (np.random.rand(4) - 0.5) * 10.0 diff --git a/Mapping/rectangle_fitting/rectangle_fitting.py b/Mapping/rectangle_fitting/rectangle_fitting.py index 07811009..5538d874 100644 --- a/Mapping/rectangle_fitting/rectangle_fitting.py +++ b/Mapping/rectangle_fitting/rectangle_fitting.py @@ -93,8 +93,8 @@ def ray_casting_filter(xl, yl, thetal, rangel, angle_reso): def plot_circle(x, y, size, color="-b"): deg = list(range(0, 360, 5)) deg.append(0) - xl = [x + size * math.cos(math.radians(d)) for d in deg] - yl = [y + size * math.sin(math.radians(d)) for d in deg] + xl = [x + size * math.cos(np.deg2rad(d)) for d in deg] + yl = [y + size * math.sin(np.deg2rad(d)) for d in deg] plt.plot(xl, yl, color) @@ -107,8 +107,8 @@ def main(): cx = -2.0 # initial x position of obstacle cy = -8.0 # initial y position of obstacle cr = 1.0 # obstacle radious - theta = math.radians(30.0) # obstacle moving direction - angle_reso = math.radians(3.0) # sensor angle resolution + theta = np.deg2rad(30.0) # obstacle moving direction + angle_reso = np.deg2rad(3.0) # sensor angle resolution time = 0.0 while time <= simtime: diff --git a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py index f97cf906..ec369325 100644 --- a/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py +++ b/PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py @@ -221,10 +221,8 @@ class RRT(): return newNode - def pi_2_pi(self, angle): - return (angle + math.pi) % (2*math.pi) - math.pi - + return (angle + math.pi) % (2 * math.pi) - math.pi def steer(self, rnd, nind): # print(rnd) @@ -265,7 +263,7 @@ class RRT(): def get_best_last_indexs(self): # print("get_best_last_index") - YAWTH = math.radians(1.0) + YAWTH = np.deg2rad(1.0) XYTH = 0.5 goalinds = [] @@ -420,8 +418,8 @@ def main(): ] # [x,y,size(radius)] # Set Initial parameters - start = [0.0, 0.0, math.radians(0.0)] - goal = [6.0, 7.0, math.radians(90.0)] + start = [0.0, 0.0, np.deg2rad(0.0)] + goal = [6.0, 7.0, np.deg2rad(90.0)] rrt = RRT(start, goal, randArea=[-2.0, 20.0], obstacleList=obstacleList) flag, x, y, yaw, v, t, a, d = rrt.Planning(animation=show_animation) diff --git a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py index e9272bac..7ad06326 100644 --- a/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py +++ b/PathPlanning/ClosedLoopRRTStar/pure_pursuit.py @@ -236,9 +236,9 @@ def main(): # state = unicycle_model.State(x=-1.0, y=-5.0, yaw=0.0, v=-30.0 / 3.6) # state = unicycle_model.State(x=10.0, y=5.0, yaw=0.0, v=-30.0 / 3.6) # state = unicycle_model.State( - # x=3.0, y=5.0, yaw=math.radians(-40.0), v=-10.0 / 3.6) + # x=3.0, y=5.0, yaw=np.deg2rad(-40.0), v=-10.0 / 3.6) # state = unicycle_model.State( - # x=3.0, y=5.0, yaw=math.radians(40.0), v=50.0 / 3.6) + # x=3.0, y=5.0, yaw=np.deg2rad(40.0), v=50.0 / 3.6) lastIndex = len(cx) - 1 time = 0.0 diff --git a/PathPlanning/ClosedLoopRRTStar/unicycle_model.py b/PathPlanning/ClosedLoopRRTStar/unicycle_model.py index 5185eb1c..9ea0f017 100644 --- a/PathPlanning/ClosedLoopRRTStar/unicycle_model.py +++ b/PathPlanning/ClosedLoopRRTStar/unicycle_model.py @@ -10,7 +10,7 @@ import math dt = 0.05 # [s] L = 0.9 # [m] -steer_max = math.radians(40.0) +steer_max = np.deg2rad(40.0) curvature_max = math.tan(steer_max) / L curvature_max = 1.0 / curvature_max + 1.0 @@ -38,7 +38,7 @@ def update(state, a, delta): def pi_2_pi(angle): - return (angle + math.pi) % (2*math.pi) - math.pi + return (angle + math.pi) % (2 * math.pi) - math.pi if __name__ == '__main__': @@ -47,7 +47,7 @@ if __name__ == '__main__': T = 100 a = [1.0] * T - delta = [math.radians(1.0)] * T + delta = [np.deg2rad(1.0)] * T # print(delta) # print(a, delta) diff --git a/PathPlanning/DubinsPath/dubins_path_planning.py b/PathPlanning/DubinsPath/dubins_path_planning.py index 4477cc13..3f04249d 100644 --- a/PathPlanning/DubinsPath/dubins_path_planning.py +++ b/PathPlanning/DubinsPath/dubins_path_planning.py @@ -17,7 +17,7 @@ def mod2pi(theta): def pi_2_pi(angle): - return (angle + math.pi) % (2*math.pi) - math.pi + return (angle + math.pi) % (2 * math.pi) - math.pi def LSL(alpha, beta, d): @@ -221,7 +221,7 @@ def generate_course(length, mode, c): if m is "S": d = 1.0 * c else: # turning couse - d = math.radians(3.0) + d = np.deg2rad(3.0) while pd < abs(l - d): # print(pd, l) @@ -270,11 +270,11 @@ def main(): start_x = 1.0 # [m] start_y = 1.0 # [m] - start_yaw = math.radians(45.0) # [rad] + start_yaw = np.deg2rad(45.0) # [rad] end_x = -3.0 # [m] end_y = -3.0 # [m] - end_yaw = math.radians(-45.0) # [rad] + end_yaw = np.deg2rad(-45.0) # [rad] curvature = 1.0 @@ -304,11 +304,11 @@ def test(): for i in range(NTEST): start_x = (np.random.rand() - 0.5) * 10.0 # [m] start_y = (np.random.rand() - 0.5) * 10.0 # [m] - start_yaw = math.radians((np.random.rand() - 0.5) * 180.0) # [rad] + start_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad] end_x = (np.random.rand() - 0.5) * 10.0 # [m] end_y = (np.random.rand() - 0.5) * 10.0 # [m] - end_yaw = math.radians((np.random.rand() - 0.5) * 180.0) # [rad] + end_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad] curvature = 1.0 / (np.random.rand() * 5.0) diff --git a/PathPlanning/HybridAStar/hybrid_a_star.py b/PathPlanning/HybridAStar/hybrid_a_star.py index 4e1ee11d..caedfa5a 100644 --- a/PathPlanning/HybridAStar/hybrid_a_star.py +++ b/PathPlanning/HybridAStar/hybrid_a_star.py @@ -198,11 +198,11 @@ def main(): oy.append(60.0 - i) # Set Initial parameters - start = [10.0, 10.0, math.radians(90.0)] - goal = [50.0, 50.0, math.radians(-90.0)] + start = [10.0, 10.0, np.deg2rad(90.0)] + goal = [50.0, 50.0, np.deg2rad(-90.0)] xyreso = 2.0 - yawreso = math.radians(15.0) + yawreso = np.deg2rad(15.0) rx, ry, ryaw = hybrid_a_star_planning( start, goal, ox, oy, xyreso, yawreso) diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py index 57597a9d..e575fbbf 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py @@ -12,7 +12,7 @@ import pandas as pd def calc_states_list(): - maxyaw = math.radians(-30.0) + maxyaw = np.deg2rad(-30.0) x = np.arange(1.0, 30.0, 5.0) y = np.arange(0.0, 20.0, 2.0) diff --git a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py b/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py index c8448962..f97c3f5f 100644 --- a/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py +++ b/PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py @@ -133,8 +133,8 @@ def optimize_trajectory(target, k0, p): def test_optimize_trajectory(): - # target = motion_model.State(x=5.0, y=2.0, yaw=math.radians(00.0)) - target = motion_model.State(x=5.0, y=2.0, yaw=math.radians(90.0)) + # target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(00.0)) + target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(90.0)) k0 = 0.0 init_p = np.matrix([6.0, 0.0, 0.0]).T @@ -152,8 +152,8 @@ def test_optimize_trajectory(): def test_trajectory_generate(): s = 5.0 # [m] k0 = 0.0 - km = math.radians(30.0) - kf = math.radians(-30.0) + km = np.deg2rad(30.0) + kf = np.deg2rad(-30.0) # plt.plot(xk, yk, "xr") # plt.plot(t, kp) diff --git a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py index 99d7fc7d..1fa331bc 100644 --- a/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py +++ b/PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.py @@ -182,12 +182,12 @@ def main(): sx = 10.0 # start x position [m] sy = 10.0 # start y position [m] - syaw = math.radians(10.0) # start yaw angle [rad] + syaw = np.deg2rad(10.0) # start yaw angle [rad] sv = 1.0 # start speed [m/s] sa = 0.1 # start accel [m/ss] gx = 30.0 # goal x position [m] gy = -10.0 # goal y position [m] - gyaw = math.radians(20.0) # goal yaw angle [rad] + gyaw = np.deg2rad(20.0) # goal yaw angle [rad] gv = 1.0 # goal speed [m/s] ga = 0.1 # goal accel [m/ss] max_accel = 1.0 # max accel [m/ss] diff --git a/PathPlanning/RRT/rrt_with_pathsmoothing.py b/PathPlanning/RRT/rrt_with_pathsmoothing.py index b0ed483e..242838fd 100644 --- a/PathPlanning/RRT/rrt_with_pathsmoothing.py +++ b/PathPlanning/RRT/rrt_with_pathsmoothing.py @@ -112,8 +112,8 @@ class RRT(): def PlotCircle(self, x, y, size): deg = list(range(0, 360, 5)) deg.append(0) - xl = [x + size * math.cos(math.radians(d)) for d in deg] - yl = [y + size * math.sin(math.radians(d)) for d in deg] + xl = [x + size * math.cos(np.deg2rad(d)) for d in deg] + yl = [y + size * math.sin(np.deg2rad(d)) for d in deg] plt.plot(xl, yl, "-k") def GetNearestListIndex(self, nodeList, rnd): diff --git a/PathPlanning/RRTDubins/dubins_path_planning.py b/PathPlanning/RRTDubins/dubins_path_planning.py index b489ca5f..43186a85 100644 --- a/PathPlanning/RRTDubins/dubins_path_planning.py +++ b/PathPlanning/RRTDubins/dubins_path_planning.py @@ -10,6 +10,7 @@ License MIT """ import math +import numpy as np def mod2pi(theta): @@ -17,7 +18,7 @@ def mod2pi(theta): def pi_2_pi(angle): - return (angle + math.pi) % (2*math.pi) - math.pi + return (angle + math.pi) % (2 * math.pi) - math.pi def LSL(alpha, beta, d): @@ -230,7 +231,7 @@ def generate_course(length, mode, c): if m is "S": d = 1.0 / c else: # turning couse - d = math.radians(3.0) + d = np.deg2rad(3.0) while pd < abs(l - d): # print(pd, l) @@ -281,11 +282,11 @@ if __name__ == '__main__': start_x = 1.0 # [m] start_y = 1.0 # [m] - start_yaw = math.radians(45.0) # [rad] + start_yaw = np.deg2rad(45.0) # [rad] end_x = -3.0 # [m] end_y = -3.0 # [m] - end_yaw = math.radians(-45.0) # [rad] + end_yaw = np.deg2rad(-45.0) # [rad] curvature = 1.0 diff --git a/PathPlanning/RRTDubins/rrt_dubins.py b/PathPlanning/RRTDubins/rrt_dubins.py index 33126335..548b7e3d 100644 --- a/PathPlanning/RRTDubins/rrt_dubins.py +++ b/PathPlanning/RRTDubins/rrt_dubins.py @@ -96,10 +96,8 @@ class RRT(): return newNode - def pi_2_pi(self, angle): - return (angle + math.pi) % (2*math.pi) - math.pi - + return (angle + math.pi) % (2 * math.pi) - math.pi def steer(self, rnd, nind): # print(rnd) @@ -234,8 +232,8 @@ def main(): ] # [x,y,size(radius)] # Set Initial parameters - start = [0.0, 0.0, math.radians(0.0)] - goal = [10.0, 10.0, math.radians(0.0)] + start = [0.0, 0.0, np.deg2rad(0.0)] + goal = [10.0, 10.0, np.deg2rad(0.0)] rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList) path = rrt.Planning(animation=show_animation) diff --git a/PathPlanning/RRTStarDubins/dubins_path_planning.py b/PathPlanning/RRTStarDubins/dubins_path_planning.py index ad9e7e55..71860274 100644 --- a/PathPlanning/RRTStarDubins/dubins_path_planning.py +++ b/PathPlanning/RRTStarDubins/dubins_path_planning.py @@ -13,7 +13,7 @@ def mod2pi(theta): def pi_2_pi(angle): - return (angle + math.pi) % (2*math.pi) - math.pi + return (angle + math.pi) % (2 * math.pi) - math.pi def LSL(alpha, beta, d): @@ -226,7 +226,7 @@ def generate_course(length, mode, c): if m is "S": d = 1.0 / c else: # turning couse - d = math.radians(3.0) + d = np.deg2rad(3.0) while pd < abs(l - d): # print(pd, l) @@ -277,11 +277,11 @@ if __name__ == '__main__': start_x = 1.0 # [m] start_y = 1.0 # [m] - start_yaw = math.radians(45.0) # [rad] + start_yaw = np.deg2rad(45.0) # [rad] end_x = -3.0 # [m] end_y = -3.0 # [m] - end_yaw = math.radians(-45.0) # [rad] + end_yaw = np.deg2rad(-45.0) # [rad] curvature = 1.0 diff --git a/PathPlanning/RRTStarDubins/rrt_star_dubins.py b/PathPlanning/RRTStarDubins/rrt_star_dubins.py index 79aabf70..8888c623 100644 --- a/PathPlanning/RRTStarDubins/rrt_star_dubins.py +++ b/PathPlanning/RRTStarDubins/rrt_star_dubins.py @@ -97,7 +97,7 @@ class RRT(): return newNode def pi_2_pi(self, angle): - return (angle + math.pi) % (2*math.pi) - math.pi + return (angle + math.pi) % (2 * math.pi) - math.pi def steer(self, rnd, nind): # print(rnd) @@ -138,7 +138,7 @@ class RRT(): def get_best_last_index(self): # print("get_best_last_index") - YAWTH = math.radians(1.0) + YAWTH = np.deg2rad(1.0) XYTH = 0.5 goalinds = [] @@ -281,8 +281,8 @@ def main(): ] # [x,y,size(radius)] # Set Initial parameters - start = [0.0, 0.0, math.radians(0.0)] - goal = [10.0, 10.0, math.radians(0.0)] + start = [0.0, 0.0, np.deg2rad(0.0)] + goal = [10.0, 10.0, np.deg2rad(0.0)] rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList) path = rrt.Planning(animation=show_animation) diff --git a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py index 089915bb..8e4dd8e4 100644 --- a/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py +++ b/PathPlanning/RRTStarReedsShepp/rrt_star_reeds_shepp.py @@ -106,7 +106,7 @@ class RRT(): return newNode def pi_2_pi(self, angle): - return (angle + math.pi) % (2*math.pi) - math.pi + return (angle + math.pi) % (2 * math.pi) - math.pi def steer(self, rnd, nind): @@ -148,7 +148,7 @@ class RRT(): def get_best_last_index(self): # print("get_best_last_index") - YAWTH = math.radians(3.0) + YAWTH = np.deg2rad(3.0) XYTH = 0.5 goalinds = [] @@ -308,8 +308,8 @@ def main(): ] # [x,y,size(radius)] # Set Initial parameters - start = [0.0, 0.0, math.radians(0.0)] - goal = [6.0, 7.0, math.radians(90.0)] + start = [0.0, 0.0, np.deg2rad(0.0)] + goal = [6.0, 7.0, np.deg2rad(90.0)] rrt = RRT(start, goal, randArea=[-2.0, 15.0], obstacleList=obstacleList) path = rrt.Planning(animation=show_animation) diff --git a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py index 28e4ef9d..f1ecea68 100644 --- a/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py +++ b/PathPlanning/ReedsSheppPath/reeds_shepp_path_planning.py @@ -331,7 +331,7 @@ def generate_local_course(L, lengths, mode, maxc, step_size): def pi_2_pi(angle): - return (angle + math.pi) % (2*math.pi) - math.pi + return (angle + math.pi) % (2 * math.pi) - math.pi def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size): @@ -387,11 +387,11 @@ def test(): for i in range(NTEST): start_x = (np.random.rand() - 0.5) * 10.0 # [m] start_y = (np.random.rand() - 0.5) * 10.0 # [m] - start_yaw = math.radians((np.random.rand() - 0.5) * 180.0) # [rad] + start_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad] end_x = (np.random.rand() - 0.5) * 10.0 # [m] end_y = (np.random.rand() - 0.5) * 10.0 # [m] - end_yaw = math.radians((np.random.rand() - 0.5) * 180.0) # [rad] + end_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad] curvature = 1.0 / (np.random.rand() * 20.0) step_size = 0.1 @@ -424,11 +424,11 @@ def main(): start_x = -1.0 # [m] start_y = -4.0 # [m] - start_yaw = math.radians(-20.0) # [rad] + start_yaw = np.deg2rad(-20.0) # [rad] end_x = 5.0 # [m] end_y = 5.0 # [m] - end_yaw = math.radians(25.0) # [rad] + end_yaw = np.deg2rad(25.0) # [rad] curvature = 1.0 step_size = 0.1 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 6468737d..ee2e7470 100644 --- a/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py +++ b/PathTracking/lqr_speed_steer_control/lqr_speed_steer_control.py @@ -21,7 +21,7 @@ R = np.eye(2) # parameters dt = 0.1 # time tick[s] L = 0.5 # Wheel base of the vehicle [m] -max_steer = math.radians(45.0) # maximum steering angle[rad] +max_steer = np.deg2rad(45.0) # maximum steering angle[rad] show_animation = True @@ -51,7 +51,7 @@ def update(state, a, delta): def pi_2_pi(angle): - return (angle + math.pi) % (2*math.pi) - math.pi + 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 c6f87aef..bc29e25b 100644 --- a/PathTracking/lqr_steer_control/lqr_steer_control.py +++ b/PathTracking/lqr_steer_control/lqr_steer_control.py @@ -24,7 +24,7 @@ R = np.eye(1) # parameters dt = 0.1 # time tick[s] L = 0.5 # Wheel base of the vehicle [m] -max_steer = math.radians(45.0) # maximum steering angle[rad] +max_steer = np.deg2rad(45.0) # maximum steering angle[rad] show_animation = True # show_animation = False @@ -61,7 +61,7 @@ def PIDControl(target, current): def pi_2_pi(angle): - return (angle + math.pi) % (2*math.pi) - math.pi + return (angle + math.pi) % (2 * math.pi) - math.pi def solve_DARE(A, B, Q, R): diff --git a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py index 4272f4bd..4125aca9 100644 --- a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py +++ b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py @@ -45,8 +45,8 @@ WHEEL_WIDTH = 0.2 # [m] TREAD = 0.7 # [m] WB = 2.5 # [m] -MAX_STEER = math.radians(45.0) # maximum steering angle [rad] -MAX_DSTEER = math.radians(30.0) # maximum steering speed [rad/s] +MAX_STEER = np.deg2rad(45.0) # maximum steering angle [rad] +MAX_DSTEER = np.deg2rad(30.0) # maximum steering speed [rad/s] MAX_SPEED = 55.0 / 3.6 # maximum speed [m/s] MIN_SPEED = -20.0 / 3.6 # minimum speed [m/s] MAX_ACCEL = 1.0 # maximum accel [m/ss] diff --git a/SLAM/EKFSLAM/ekf_slam.py b/SLAM/EKFSLAM/ekf_slam.py index c8b5ae03..a698a567 100644 --- a/SLAM/EKFSLAM/ekf_slam.py +++ b/SLAM/EKFSLAM/ekf_slam.py @@ -12,11 +12,11 @@ import matplotlib.pyplot as plt # EKF state covariance -Cx = np.diag([0.5, 0.5, math.radians(30.0)])**2 +Cx = np.diag([0.5, 0.5, np.deg2rad(30.0)])**2 # Simulation parameter -Qsim = np.diag([0.2, math.radians(1.0)])**2 -Rsim = np.diag([1.0, math.radians(10.0)])**2 +Qsim = np.diag([0.2, np.deg2rad(1.0)])**2 +Rsim = np.diag([1.0, np.deg2rad(10.0)])**2 DT = 0.1 # time tick [s] SIM_TIME = 50.0 # simulation time [s] @@ -203,7 +203,7 @@ def jacobH(q, delta, x, i): def pi_2_pi(angle): - return (angle + math.pi) % (2*math.pi) - math.pi + 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 eb2f03fa..c9d029f9 100644 --- a/SLAM/FastSLAM1/fast_slam1.py +++ b/SLAM/FastSLAM1/fast_slam1.py @@ -12,12 +12,12 @@ import matplotlib.pyplot as plt # Fast SLAM covariance -Q = np.diag([3.0, math.radians(10.0)])**2 -R = np.diag([1.0, math.radians(20.0)])**2 +Q = np.diag([3.0, np.deg2rad(10.0)])**2 +R = np.diag([1.0, np.deg2rad(20.0)])**2 # Simulation parameter -Qsim = np.diag([0.3, math.radians(2.0)])**2 -Rsim = np.diag([0.5, math.radians(10.0)])**2 +Qsim = np.diag([0.3, np.deg2rad(2.0)])**2 +Rsim = np.diag([0.5, np.deg2rad(10.0)])**2 OFFSET_YAWRATE_NOISE = 0.01 DT = 0.1 # time tick [s] @@ -325,7 +325,7 @@ def motion_model(x, u): def pi_2_pi(angle): - return (angle + math.pi) % (2*math.pi) - math.pi + 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 76fb2928..8134f8fd 100644 --- a/SLAM/FastSLAM2/fast_slam2.py +++ b/SLAM/FastSLAM2/fast_slam2.py @@ -12,12 +12,12 @@ import matplotlib.pyplot as plt # Fast SLAM covariance -Q = np.diag([3.0, math.radians(10.0)])**2 -R = np.diag([1.0, math.radians(20.0)])**2 +Q = np.diag([3.0, np.deg2rad(10.0)])**2 +R = np.diag([1.0, np.deg2rad(20.0)])**2 # Simulation parameter -Qsim = np.diag([0.3, math.radians(2.0)])**2 -Rsim = np.diag([0.5, math.radians(10.0)])**2 +Qsim = np.diag([0.3, np.deg2rad(2.0)])**2 +Rsim = np.diag([0.5, np.deg2rad(10.0)])**2 OFFSET_YAWRATE_NOISE = 0.01 DT = 0.1 # time tick [s] @@ -354,7 +354,7 @@ def motion_model(x, u): def pi_2_pi(angle): - return (angle + math.pi) % (2*math.pi) - math.pi + 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 eb64076d..a8776c6e 100644 --- a/SLAM/GraphBasedSLAM/graph_based_slam.py +++ b/SLAM/GraphBasedSLAM/graph_based_slam.py @@ -18,8 +18,8 @@ import matplotlib.pyplot as plt # Simulation parameter -Qsim = np.diag([0.2, math.radians(1.0)])**2 -Rsim = np.diag([0.1, math.radians(10.0)])**2 +Qsim = np.diag([0.2, np.deg2rad(1.0)])**2 +Rsim = np.diag([0.1, np.deg2rad(10.0)])**2 DT = 2.0 # time tick [s] SIM_TIME = 100.0 # simulation time [s] @@ -29,7 +29,7 @@ STATE_SIZE = 3 # State size [x,y,yaw] # Covariance parameter of Graph Based SLAM C_SIGMA1 = 0.1 C_SIGMA2 = 0.1 -C_SIGMA3 = math.radians(1.0) +C_SIGMA3 = np.deg2rad(1.0) MAX_ITR = 20 # Maximum iteration @@ -253,7 +253,7 @@ def motion_model(x, u): def pi_2_pi(angle): - return (angle + math.pi) % (2*math.pi) - math.pi + return (angle + math.pi) % (2 * math.pi) - math.pi def main(): diff --git a/SLAM/iterative_closest_point/iterative_closest_point.py b/SLAM/iterative_closest_point/iterative_closest_point.py index c219d508..6d301aa6 100644 --- a/SLAM/iterative_closest_point/iterative_closest_point.py +++ b/SLAM/iterative_closest_point/iterative_closest_point.py @@ -138,7 +138,7 @@ def main(): # simulation parameters nPoint = 10 fieldLength = 50.0 - motion = [0.5, 2.0, math.radians(-10.0)] # movement [x[m],y[m],yaw[deg]] + motion = [0.5, 2.0, np.deg2rad(-10.0)] # movement [x[m],y[m],yaw[deg]] nsim = 3 # number of simulation diff --git a/tests/test_dubins_path_planning.py b/tests/test_dubins_path_planning.py index 828f8e63..b83655b2 100644 --- a/tests/test_dubins_path_planning.py +++ b/tests/test_dubins_path_planning.py @@ -1,6 +1,6 @@ from unittest import TestCase from PathPlanning.DubinsPath import dubins_path_planning -import math +import numpy as np class Test(TestCase): @@ -8,11 +8,11 @@ class Test(TestCase): def test1(self): start_x = 1.0 # [m] start_y = 1.0 # [m] - start_yaw = math.radians(45.0) # [rad] + start_yaw = np.deg2rad(45.0) # [rad] end_x = -3.0 # [m] end_y = -3.0 # [m] - end_yaw = math.radians(-45.0) # [rad] + end_yaw = np.deg2rad(-45.0) # [rad] curvature = 1.0