diff --git a/PathPlanning/DynamicWindowApproach/animation.gif b/PathPlanning/DynamicWindowApproach/animation.gif index 47b6b2d1..2beeaf10 100644 Binary files a/PathPlanning/DynamicWindowApproach/animation.gif and b/PathPlanning/DynamicWindowApproach/animation.gif differ diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index 0969d97f..43f7a86a 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -55,14 +55,11 @@ def calc_dynamic_window(x, config): x[3] + config.max_accel * config.dt, x[4] - config.max_dyawrate * config.dt, x[4] + config.max_dyawrate * config.dt] - # print(Vs, Vd) # [vmin,vmax, yawrate min, yawrate max] dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]), max(Vs[2], Vd[2]), min(Vs[3], Vd[3])] - #print(dw) - return dw @@ -76,7 +73,6 @@ def calc_trajectory(xinit, v, y, config): traj = np.vstack((traj, x)) time += config.dt - # print(len(traj)) return traj @@ -98,7 +94,7 @@ def calc_final_input(x, u, dw, config, goal, ob): speed_cost = config.speed_cost_gain * \ (config.max_speed - traj[-1, 3]) ob_cost = calc_obstacle_cost(traj, ob, config) - #print(ob_cost) + # print(ob_cost) final_cost = to_goal_cost + speed_cost + ob_cost @@ -110,9 +106,6 @@ def calc_final_input(x, u, dw, config, goal, ob): min_u = [v, y] best_traj = traj - # print(min_u) - # input() - return min_u, best_traj @@ -144,8 +137,8 @@ def calc_to_goal_cost(traj, goal, config): goal_magnitude = math.sqrt(goal[0]**2 + goal[1]**2) traj_magnitude = math.sqrt(traj[-1, 0]**2 + traj[-1, 1]**2) - dot_product = (goal[0]*traj[-1, 0]) + (goal[1]*traj[-1, 1]) - error = dot_product / (goal_magnitude*traj_magnitude) + dot_product = (goal[0] * traj[-1, 0]) + (goal[1] * traj[-1, 1]) + error = dot_product / (goal_magnitude * traj_magnitude) error_angle = math.acos(error) cost = config.to_goal_cost_gain * error_angle @@ -197,7 +190,7 @@ def main(): x = motion(x, u, config.dt) traj = np.vstack((traj, x)) # store state history - #print(traj) + # print(traj) if show_animation: plt.cla() @@ -218,7 +211,9 @@ def main(): print("Done") if show_animation: plt.plot(traj[:, 0], traj[:, 1], "-r") - plt.show() + plt.pause(0.0001) + + plt.show() if __name__ == '__main__':