DWA code clean up

Author: AtsushiSakai

PathPlanning/DynamicWindowApproach/dynamic_window_approach.py

@@ -10,15 +10,26 @@
 import numpy as np
 import matplotlib.pyplot as plt
 
-import sys
-sys.path.append("../../")
-
 
 show_animation = True
 
 
+def dwa_control(x, u, config, goal, ob):
+    """
+        Dynamic Window Approach control
+    """
+
+    dw = calc_dynamic_window(x, config)
+
+    u, traj = calc_final_input(x, u, dw, config, goal, ob)
+
+    return u, traj
+
+
 class Config():
-    # simulation parameters
+    """
+    simulation parameter class
+    """
 
     def __init__(self):
         # robot parameter
@@ -29,15 +40,19 @@ def __init__(self):
         self.max_dyawrate = 40.0 * math.pi / 180.0  # [rad/ss]
         self.v_reso = 0.01  # [m/s]
         self.yawrate_reso = 0.1 * math.pi / 180.0  # [rad/s]
-        self.dt = 0.1  # [s]
+        self.dt = 0.1  # [s] Time tick for motion prediction
         self.predict_time = 3.0  # [s]
         self.to_goal_cost_gain = 0.15
         self.speed_cost_gain = 1.0
-        self.robot_radius = 1.0  # [m]
+        self.obstacle_cost_gain = 1.0
+        self.robot_radius = 1.0  # [m] for collision check
+
 
 
 def motion(x, u, dt):
-    # motion model
+    """
+    motion model
+    """
 
     x[2] += u[1] * dt
     x[0] += u[0] * math.cos(x[2]) * dt
@@ -49,6 +64,9 @@ def motion(x, u, dt):
 
 
 def calc_dynamic_window(x, config):
+    """
+    calculation dynamic window based on current state x
+    """
 
     # Dynamic window from robot specification
     Vs = [config.min_speed, config.max_speed,
@@ -67,9 +85,12 @@ def calc_dynamic_window(x, config):
     return dw
 
 
-def calc_trajectory(xinit, v, y, config):
+def predict_trajectory(x_init, v, y, config):
+    """
+    predict trajectory with an input
+    """
 
-    x = np.array(xinit)
+    x = np.array(x_init)
     traj = np.array(x)
     time = 0
     while time <= config.predict_time:
@@ -81,42 +102,44 @@ def calc_trajectory(xinit, v, y, config):
 
 
 def calc_final_input(x, u, dw, config, goal, ob):
+    """
+    calculation final input with dinamic window
+    """
 
-    xinit = x[:]
-    min_cost = 10000.0
-    min_u = u
-    min_u[0] = 0.0
+    x_init = x[:]
+    min_cost = float("inf")
+    best_u = [0.0, 0.0]
     best_traj = np.array([x])
 
     # evalucate all trajectory with sampled input in dynamic window
     for v in np.arange(dw[0], dw[1], config.v_reso):
         for y in np.arange(dw[2], dw[3], config.yawrate_reso):
-            traj = calc_trajectory(xinit, v, y, config)
+
+            traj = predict_trajectory(x_init, v, y, config)
 
             # calc cost
-            to_goal_cost = calc_to_goal_cost(traj, goal, config)
+            to_goal_cost = config.to_goal_cost_gain * calc_to_goal_cost(traj, goal, config)
             speed_cost = config.speed_cost_gain * \
                 (config.max_speed - traj[-1, 3])
-            ob_cost = calc_obstacle_cost(traj, ob, config)
-            # print(ob_cost)
+            ob_cost = config.obstacle_cost_gain*calc_obstacle_cost(traj, ob, config)
 
             final_cost = to_goal_cost + speed_cost + ob_cost
 
-            #print (final_cost)
-
             # search minimum trajectory
             if min_cost >= final_cost:
                 min_cost = final_cost
-                min_u = [v, y]
+                best_u = [v, y]
                 best_traj = traj
 
-    return min_u, best_traj
+    return best_u, best_traj
 
 
 def calc_obstacle_cost(traj, ob, config):
-    # calc obstacle cost inf: collistion, 0:free
+    """
+        calc obstacle cost inf: collision
+    """
 
-    skip_n = 2
+    skip_n = 2 # for speed up
     minr = float("inf")
 
     for ii in range(0, len(traj[:, 1]), skip_n):
@@ -137,26 +160,18 @@ def calc_obstacle_cost(traj, ob, config):
 
 
 def calc_to_goal_cost(traj, goal, config):
-    # calc to goal cost.
+    """
+        calc to goal cost with angle difference
+    """
 
     dx = goal[0] - traj[-1, 0]
     dy = goal[1] - traj[-1, 1]
     error_angle = math.atan2(dy, dx)
-    cost = config.to_goal_cost_gain * abs(error_angle - traj[-1, 2])
+    cost = abs(error_angle - traj[-1, 2])
 
     return cost
 
 
-def dwa_control(x, u, config, goal, ob):
-    # Dynamic Window control
-
-    dw = calc_dynamic_window(x, config)
-
-    u, traj = calc_final_input(x, u, dw, config, goal, ob)
-
-    return u, traj
-
-
 def plot_arrow(x, y, yaw, length=0.5, width=0.1):  # pragma: no cover
     plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
               head_length=width, head_width=width)
@@ -182,21 +197,20 @@ def main(gx=10, gy=10):
                    [12.0, 12.0]
                    ])
 
+    # input [forward speed, yawrate]
     u = np.array([0.0, 0.0])
     config = Config()
     traj = np.array(x)
 
-    for i in range(1000):
-        u, ltraj = dwa_control(x, u, config, goal, ob)
+    while True:
+        u, ptraj = dwa_control(x, u, config, goal, ob)
 
-        x = motion(x, u, config.dt)
+        x = motion(x, u, config.dt) # simulate robot
         traj = np.vstack((traj, x))  # store state history
 
-        # print(traj)
-
         if show_animation:
             plt.cla()
-            plt.plot(ltraj[:, 0], ltraj[:, 1], "-g")
+            plt.plot(ptraj[:, 0], ptraj[:, 1], "-g")
             plt.plot(x[0], x[1], "xr")
             plt.plot(goal[0], goal[1], "xb")
             plt.plot(ob[:, 0], ob[:, 1], "ok")
@@ -205,8 +219,9 @@ def main(gx=10, gy=10):
             plt.grid(True)
             plt.pause(0.0001)
 
-        # check goal
-        if math.sqrt((x[0] - goal[0])**2 + (x[1] - goal[1])**2) <= config.robot_radius:
+        # check reaching goal
+        dist_to_goal = math.sqrt((x[0] - goal[0])**2 + (x[1] - goal[1])**2)  
+        if dist_to_goal <= config.robot_radius:
             print("Goal!!")
             break