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

code clean up

Browse Source
This commit is contained in:
Atsushi Sakai
2018-11-18 08:44:10 +09:00
parent 7560cbdca8
commit 4e7fbc3388
2 changed files with 7 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

BIN
PathPlanning/DynamicWindowApproach/animation.gif
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 2.6 MiB

After

Width:  |  Height:  |  Size: 2.3 MiB

19
PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
View File

@@ -55,14 +55,11 @@ def calc_dynamic_window(x, config):
x[3] + config.max_accel * config.dt,
x[4] - config.max_dyawrate * config.dt,
x[4] + config.max_dyawrate * config.dt]
# print(Vs, Vd)
# [vmin,vmax, yawrate min, yawrate max]
dw = [max(Vs[0], Vd[0]), min(Vs[1], Vd[1]),
max(Vs[2], Vd[2]), min(Vs[3], Vd[3])]
#print(dw)
return dw
@@ -76,7 +73,6 @@ def calc_trajectory(xinit, v, y, config):
traj = np.vstack((traj, x))
time += config.dt
# print(len(traj))
return traj
@@ -98,7 +94,7 @@ def calc_final_input(x, u, dw, config, goal, ob):
speed_cost = config.speed_cost_gain * \
(config.max_speed - traj[-1, 3])
ob_cost = calc_obstacle_cost(traj, ob, config)
#print(ob_cost)
# print(ob_cost)
final_cost = to_goal_cost + speed_cost + ob_cost
@@ -110,9 +106,6 @@ def calc_final_input(x, u, dw, config, goal, ob):
min_u = [v, y]
best_traj = traj
# print(min_u)
# input()
return min_u, best_traj
@@ -144,8 +137,8 @@ def calc_to_goal_cost(traj, goal, config):
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)
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
@@ -197,7 +190,7 @@ def main():
x = motion(x, u, config.dt)
traj = np.vstack((traj, x)) # store state history
#print(traj)
# print(traj)
if show_animation:
plt.cla()
@@ -218,7 +211,9 @@ def main():
print("Done")
if show_animation:
plt.plot(traj[:, 0], traj[:, 1], "-r")
plt.show()
plt.pause(0.0001)
plt.show()
if __name__ == '__main__':