From 1896ee7df2028d142ddc4d03548653bc3180f71e Mon Sep 17 00:00:00 2001 From: KoenHan Date: Sun, 2 Jun 2019 04:21:52 +0900 Subject: [PATCH 1/2] Changed the computation method of to_goal_cost Signed-off-by: KoenHan --- .../dynamic_window_approach.py | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index 3144d9a2..ff588f03 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -31,8 +31,9 @@ class Config(): self.yawrate_reso = 0.1 * math.pi / 180.0 # [rad/s] self.dt = 0.1 # [s] self.predict_time = 3.0 # [s] - self.to_goal_cost_gain = 1.0 + self.to_goal_cost_gain = 0.15 self.speed_cost_gain = 1.0 + self.ob_cost_gain = 1.0 self.robot_radius = 1.0 # [m] @@ -133,18 +134,16 @@ def calc_obstacle_cost(traj, ob, config): if minr >= r: minr = r - return 1.0 / minr # OK + return config.ob_cost_gain / minr # OK def calc_to_goal_cost(traj, goal, config): - # calc to goal cost. It is 2D norm. + # calc to goal cost. - 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 + 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]) return cost From a4f33de46ef2930eca67e86a007bde5f61657d06 Mon Sep 17 00:00:00 2001 From: KoenHan Date: Sun, 2 Jun 2019 04:35:51 +0900 Subject: [PATCH 2/2] Changed the computation method of to_goal_cost Signed-off-by: KoenHan --- PathPlanning/DynamicWindowApproach/dynamic_window_approach.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index ff588f03..f8eefb74 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -33,7 +33,6 @@ class Config(): self.predict_time = 3.0 # [s] self.to_goal_cost_gain = 0.15 self.speed_cost_gain = 1.0 - self.ob_cost_gain = 1.0 self.robot_radius = 1.0 # [m] @@ -134,7 +133,7 @@ def calc_obstacle_cost(traj, ob, config): if minr >= r: minr = r - return config.ob_cost_gain / minr # OK + return 1.0 / minr # OK def calc_to_goal_cost(traj, goal, config):