github/PythonRobotics
1
1
Fork 0
mirror of https://github.com/AtsushiSakai/PythonRobotics.git synced 2026-04-22 03:00:22 -04:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://github.com/AtsushiSakai/PythonRobotics

Browse Source
This commit is contained in:
Göktuğ Karakaşlı
2019-10-12 14:40:53 +03:00
parent 8fb4b8a564 0cca27df3a
commit 07f07814a2
3 changed files with 60 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
View File

@@ -155,7 +155,8 @@ def calc_to_goal_cost(traj, goal, config):
dx = goal[0] - traj[-1, 0]
dy = goal[1] - traj[-1, 1]
error_angle = math.atan2(dy, dx)
cost = abs(error_angle - traj[-1, 2])
cost_angle = error_angle - traj[-1, 2]
cost = abs(math.atan2(math.sin(cost_angle), math.cos(cost_angle)))
return cost