From 97570de50c222c3362dfda765fd951f37cfb900f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?G=C3=B6ktu=C4=9F=20Karaka=C5=9Fl=C4=B1?= <20567087+goktug97@users.noreply.github.com> Date: Wed, 23 Oct 2019 10:14:14 +0300 Subject: [PATCH] Fix wrong rotation direction - The obstacle rotation for the rectangle obstacle calculation is wrong. It should be positive instead of negative. --- PathPlanning/DynamicWindowApproach/dynamic_window_approach.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py index c2a8674a..c8a44349 100644 --- a/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py +++ b/PathPlanning/DynamicWindowApproach/dynamic_window_approach.py @@ -2,7 +2,7 @@ Mobile robot motion planning sample with Dynamic Window Approach -author: Atsushi Sakai (@Atsushi_twi), Goktug Karakasli +author: Atsushi Sakai (@Atsushi_twi), Göktuğ Karakaşlı """ @@ -172,7 +172,7 @@ def calc_obstacle_cost(trajectory, ob, config): rot = np.transpose(rot, [2, 0, 1]) local_ob = ob[:, None] - trajectory[:, 0:2] local_ob = local_ob.reshape(-1, local_ob.shape[-1]) - local_ob = np.array([local_ob @ -x for x in rot]) + local_ob = np.array([local_ob @ x for x in rot]) local_ob = local_ob.reshape(-1, local_ob.shape[-1]) upper_check = local_ob[:, 0] <= config.robot_length / 2 right_check = local_ob[:, 1] <= config.robot_width / 2