replace math.radians to np.deg2rad

Author: AtsushiSakai

Localization/extended_kalman_filter/extended_kalman_filter.py

@@ -11,12 +11,12 @@
 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]

Localization/particle_filter/particle_filter.py

@@ -12,11 +12,11 @@
 
 # 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
-
-    try: b = math.sqrt(eigval[smallind])
-    except ValueError: b = 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
 
     x = [a * math.cos(it) for it in t]
     y = [b * math.sin(it) for it in t]

Localization/unscented_kalman_filter/unscented_kalman_filter.py

@@ -12,12 +12,12 @@
 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]

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:

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

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:

PathPlanning/ClosedLoopRRTStar/closed_loop_rrt_star_car.py

@@ -221,10 +221,8 @@ def choose_parent(self, newNode, nearinds):
 
         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 @@ def get_random_point(self):
     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)

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

PathPlanning/ClosedLoopRRTStar/unicycle_model.py

@@ -10,7 +10,7 @@
 
 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 @@ def pi_2_pi(angle):
 
     T = 100
     a = [1.0] * T
-    delta = [math.radians(1.0)] * T
+    delta = [np.deg2rad(1.0)] * T
     #  print(delta)
     #  print(a, delta)
 

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)
 

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)

PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py

@@ -12,7 +12,7 @@
 
 
 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)

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)

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]

PathPlanning/RRT/rrt_with_pathsmoothing.py

@@ -112,8 +112,8 @@ def DrawGraph(self, rnd=None):
     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):

PathPlanning/RRTDubins/dubins_path_planning.py

@@ -10,14 +10,15 @@
 
 """
 import math
+import numpy as np
 
 
 def mod2pi(theta):
     return theta - 2.0 * math.pi * math.floor(theta / 2.0 / math.pi)
 
 
 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 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"):
 
     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
 

PathPlanning/RRTDubins/rrt_dubins.py

@@ -96,10 +96,8 @@ def choose_parent(self, newNode, nearinds):
 
         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)