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

Fixed motion calculation.

Browse Source 
To goal cost calculation is now based on heading error rather than the distance to the goal as suggested by the paper.
This commit is contained in:
Debby Nirwan
2018-11-04 23:04:00 +08:00
parent ad867d7386
commit 72d1728d35
1 changed files with 15 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
PathPlanning/DynamicWindowApproach/dynamic_window_approach.py
View File

@@ -35,9 +35,9 @@ class Config():
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]
@@ -60,7 +60,8 @@ def calc_dynamic_window(x, config):
# [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)
#print(dw)
return dw
@@ -97,10 +98,12 @@ 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
#print (final_cost)
# search minimum trajectory
if min_cost >= final_cost:
min_cost = final_cost
@@ -128,7 +131,7 @@ def calc_obstacle_cost(traj, ob, config):
r = math.sqrt(dx**2 + dy**2)
if r <= config.robot_radius:
return float("Inf") # collisiton
return float("Inf") # collision
if minr >= r:
minr = r
@@ -139,10 +142,12 @@ def calc_obstacle_cost(traj, ob, config):
def calc_to_goal_cost(traj, goal, config):
# calc to goal cost. It is 2D norm.
dy = goal[0] - traj[-1, 0]
dx = goal[1] - traj[-1, 1]
goal_dis = math.sqrt(dx**2 + dy**2)
cost = config.to_goal_cost_gain * goal_dis
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
return cost
@@ -192,6 +197,8 @@ def main():
x = motion(x, u, config.dt)
traj = np.vstack((traj, x)) # store state history
#print(traj)
if show_animation:
plt.cla()
plt.plot(ltraj[:, 0], ltraj[:, 1], "-g")