code clean up

PathPlanning/DynamicWindowApproach/dynamic_window_approach.py

@@ -4,20 +4,6 @@ Mobile robot motion planning sample with Dynamic Window Approach
 
 author: Atsushi Sakai (@Atsushi_twi)
 
-
-bstacleR=0.5;%衝突判定用の障害物の半径
-
-function dist=CalcDistEval(x,ob,R)
-%障害物との距離評価値を計算する関数
-
-dist=2;
-for io=1:length(ob(:,1))
-    disttmp=norm(ob(io,:)-x(1:2)')-R;%パスの位置と障害物とのノルム誤差を計算
-    if dist>disttmp%最小値を見つける
-        dist=disttmp;
-    end
-end
-
 """
 
 import math
@@ -26,31 +12,27 @@ import matplotlib.pyplot as plt
 
 
 class Config():
+    # simulation parameters
 
     def __init__(self):
         # robot parameter
-        self.max_speed = 1.0
-        self.min_speed = -0.5
-        self.max_yawrate = 40.0 * math.pi / 180.0
-        self.max_accel = 0.5
-        self.max_dyawrate = 40.0 * math.pi / 180.0
-        self.v_reso = 0.01
-        self.yawrate_reso = 0.1 * math.pi / 180.0
+        self.max_speed = 1.0  # [m/s]
+        self.min_speed = -0.5  # [m/s]
+        self.max_yawrate = 40.0 * math.pi / 180.0  # [rad/s]
+        self.max_accel = 0.3  # [m/ss]
+        self.max_dyawrate = 20.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.predict_time = 2.0  # [s]
-        self.to_goal_cost_gain = 1.0  # [s]
-        self.speed_cost_gain = 1.0  # [s]
-        self.robot_radius = 1.0  # [s]
-
-
-def plot_arrow(x, y, yaw, length=0.5, width=0.1):
-    plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
-              head_length=width, head_width=width)
-    plt.plot(x, y)
+        self.to_goal_cost_gain = 1.0
+        self.speed_cost_gain = 1.0
+        self.robot_radius = 1.0  # [m]
 
 
 def motion(x, u, dt):
+    # motion model
 
     x[0] += u[0] * math.cos(x[2]) * dt
     x[1] += u[0] * math.sin(x[2]) * dt
@@ -96,18 +78,20 @@ def calc_trajectory(xinit, v, y, config):
     return traj
 
 
-def evaluate_path(x, dw, config, goal, ob):
+def calc_final_input(x, u, dw, config, goal, ob):
 
     xinit = x[:]
     min_cost = 10000.0
-    min_u = [0.0, 0.0]
-    best_traj = None
+    min_u = u
+    min_u[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)
-            #  print(v, ",", y)
 
+            # calc cost
             to_goal_cost = calc_to_goal_cost(traj, goal, config)
             speed_cost = config.speed_cost_gain * \
                 (config.max_speed - traj[-1, 3])
@@ -116,22 +100,22 @@ def evaluate_path(x, dw, config, goal, ob):
 
             final_cost = to_goal_cost + speed_cost + ob_cost
 
+            # search minimum trajectory
             if min_cost >= final_cost:
                 min_cost = final_cost
                 min_u = [v, y]
                 best_traj = traj
 
-    if best_traj is not None:
-        plt.plot(best_traj[:, 0], best_traj[:, 1])
     #  print(min_u)
     #  input()
 
-    return min_u
+    return min_u, best_traj
 
 
 def calc_obstacle_cost(traj, ob, config):
+    # calc obstacle cost inf: collistion, 0:free
 
-    for ii in range(len(traj[:, 1])):
+    for ii in range(0, len(traj[:, 1]), 1):
         for i in range(len(ob[:, 0])):
             ox = ob[i, 0]
             oy = ob[i, 1]
@@ -140,42 +124,40 @@ def calc_obstacle_cost(traj, ob, config):
 
             r = math.sqrt(dx**2 + dy**2)
             if r <= config.robot_radius:
-                return float("Inf")
+                return float("Inf")  # collisiton
 
-    return 0.0
+    return 0.0  # OK
 
 
 def calc_to_goal_cost(traj, goal, config):
-    cost = 0
+    # calc to goal cost. It is 2D norm.
 
-    #  traj_end_yaw = traj[-1, 2]
     dy = goal[0] - traj[-1, 0]
     dx = goal[1] - traj[-1, 1]
-
-    #  goal_yaw = math.atan2(dy, dx)
-
     goal_dis = math.sqrt(dx**2 + dy**2)
-    #  print(goal_dis)
-
-    #  cost = config.angle_cost_gain * abs(goal_yaw - traj_end_yaw) + goal_dis
     cost = config.to_goal_cost_gain * goal_dis
-    #  print(cost)
 
     return cost
 
 
-def dwa_control(x, u, config, dt, goal, ob):
+def dwa_control(x, u, config, goal, ob):
+    # Dynamic Window control
 
     dw = calc_dynamic_window(x, config)
 
-    u = evaluate_path(x, dw, config, goal, ob)
+    u, traj = calc_final_input(x, u, dw, config, goal, ob)
 
-    return u
+    return u, traj
+
+
+def plot_arrow(x, y, yaw, length=0.5, width=0.1):
+    plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw),
+              head_length=width, head_width=width)
+    plt.plot(x, y)
 
 
 def main():
     print(__file__ + " start!!")
-
     # initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
     x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])
     # goal position [x(m), y(m)]
@@ -193,16 +175,18 @@ def main():
                     [12.0, 12.0]
                     ])
 
-    dt = 0.1
     u = np.array([0.0, 0.0])
     config = Config()
+    traj = np.array(x)
 
     for i in range(1000):
         plt.cla()
-        u = dwa_control(x, u, config, dt, goal, ob)
+        u, ltraj = dwa_control(x, u, config, goal, ob)
 
-        x = motion(x, u, dt)
+        x = motion(x, u, config.dt)
+        traj = np.vstack((traj, x))  # store state history
 
+        plt.plot(ltraj[:, 0], ltraj[:, 1], "-g")
         plt.plot(x[0], x[1], "xr")
         plt.plot(goal[0], goal[1], "xb")
         plt.plot(ob[:, 0], ob[:, 1], "ok")
@@ -211,7 +195,13 @@ def main():
         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:
+            print("Goal!!")
+            break
+
     print("Done")
+    plt.plot(traj[:, 0], traj[:, 1], "-r")
     plt.show()