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

Merge pull request #246 from goktug97/fix_dwa_rot

Browse Source 
DWA Fix wrong rotation direction
This commit is contained in:
Atsushi Sakai
2019-10-27 20:50:09 +09:00
committed by GitHub
parent 5b2f86de57 97570de50c
commit a737062f17
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