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

cleanup in dynamic window approach

Browse Source
This commit is contained in:
Yilun Chen
2020-01-17 19:17:28 -05:00
parent 763ca4fd15
commit 0de06534d2
1 changed files with 4 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
View File

@@ -22,7 +22,7 @@ def dwa_control(x, config, goal, ob):
dw = calc_dynamic_window(x, config)
u, trajectory = calc_final_input(x, dw, config, goal, ob)
u, trajectory = calc_control_and_trajectory(x, dw, config, goal, ob)
return u, trajectory
@@ -77,9 +77,9 @@ def motion(x, u, dt):
motion model
"""
x[2] += u[1] * dt
x[0] += u[0] * math.cos(x[2]) * dt
x[1] += u[0] * math.sin(x[2]) * dt
x[2] += u[1] * dt
x[3] = u[0]
x[4] = u[1]
@@ -101,7 +101,7 @@ def calc_dynamic_window(x, config):
x[4] - config.max_dyawrate * config.dt,
x[4] + config.max_dyawrate * config.dt]
# [vmin,vmax, yaw_rate min, yaw_rate max]
# [vmin, vmax, yaw_rate min, yaw_rate max]
dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]),
max(Vs[2], Vd[2]), min(Vs[3], Vd[3])]
@@ -124,7 +124,7 @@ def predict_trajectory(x_init, v, y, config):
return traj
def calc_final_input(x, dw, config, goal, ob):
def calc_control_and_trajectory(x, dw, config, goal, ob):
"""
calculation final input with dynamic window
"""