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

Merge pull request #199 from KoenHan/dwa_only

Browse Source 
DWA cost function toward goal
This commit is contained in:
Atsushi Sakai
2019-06-02 09:24:20 +09:00
committed by GitHub
parent 323ca861bc a4f33de46e
commit 6fbb3cb20e
1 changed files with 6 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

14
PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
View File

@@ -31,7 +31,7 @@ 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.robot_radius = 1.0 # [m]
@@ -137,14 +137,12 @@ def calc_obstacle_cost(traj, ob, config):
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