github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-01-13 14:48:02 -05:00
Code Issues Packages Projects Releases Wiki Activity

Fix wrong rotation direction

Browse Source 
- The obstacle rotation for the rectangle obstacle calculation
is wrong. It should be positive instead of negative.
This commit is contained in:
Göktuğ Karakaşlı
2019-10-23 10:14:14 +03:00
parent 66fe2f5ef6
commit 97570de50c
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
View File

@@ -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