From 72d1728d3553c78ee887453d3dfbf57a2caa9316 Mon Sep 17 00:00:00 2001
From: Debby Nirwan <debby.nirwan@arctec.co>
Date: Sun, 4 Nov 2018 23:04:00 +0800
Subject: [PATCH] Fixed motion calculation. To goal cost calculation is now
 based on heading error rather than the distance to the goal as suggested by
 the paper.

---
 .../dynamic_window_approach.py                | 23 ++++++++++++-------
 1 file changed, 15 insertions(+), 8 deletions(-)

diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
index c1892aa2..0969d97f 100644
--- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
+++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
@@ -35,9 +35,9 @@ class Config():
 def motion(x, u, dt):
     # motion model
 
+    x[2] += u[1] * dt
     x[0] += u[0] * math.cos(x[2]) * dt
     x[1] += u[0] * math.sin(x[2]) * dt
-    x[2] += u[1] * dt
     x[3] = u[0]
     x[4] = u[1]
 
@@ -60,7 +60,8 @@ def calc_dynamic_window(x, config):
     #  [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)
+
+    #print(dw)
 
     return dw
 
@@ -97,10 +98,12 @@ 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
 
+            #print (final_cost)
+
             # search minimum trajectory
             if min_cost >= final_cost:
                 min_cost = final_cost
@@ -128,7 +131,7 @@ def calc_obstacle_cost(traj, ob, config):
 
             r = math.sqrt(dx**2 + dy**2)
             if r <= config.robot_radius:
-                return float("Inf")  # collisiton
+                return float("Inf")  # collision
 
             if minr >= r:
                 minr = r
@@ -139,10 +142,12 @@ def calc_obstacle_cost(traj, ob, config):
 def calc_to_goal_cost(traj, goal, config):
     # calc to goal cost. It is 2D norm.
 
-    dy = goal[0] - traj[-1, 0]
-    dx = goal[1] - traj[-1, 1]
-    goal_dis = math.sqrt(dx**2 + dy**2)
-    cost = config.to_goal_cost_gain * goal_dis
+    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)
+    error_angle = math.acos(error)
+    cost = config.to_goal_cost_gain * error_angle
 
     return cost
 
@@ -192,6 +197,8 @@ def main():
         x = motion(x, u, config.dt)
         traj = np.vstack((traj, x))  # store state history
 
+        #print(traj)
+
         if show_animation:
             plt.cla()
             plt.plot(ltraj[:, 0], ltraj[:, 1], "-g")