def simulation(self, xI, xG, policy):
"""
simulate a path using converged policy.
:param xI: starting state
:param xG: goal state
:param policy: converged policy
:return: simulation path
"""
plt.figure(1) # path animation
tools.show_map(xI, xG, self.obs, self.lose,
self.name1) # show background
x, path = xI, []
while True:
u = self.u_set[policy[x]]
x_next = (x[0] + u[0], x[1] + u[1])
if x_next in self.obs:
print("Collision!") # collision: simulation failed
else:
x = x_next
if x_next in xG:
break
else:
tools.plot_dots(x) # each state in optimal path
path.append(x)
plt.show()
return path
def __init__(self, x_start, x_goal, heuristic_type):
self.u_set = motion_model.motions # feasible input set
self.xI, self.xG = x_start, x_goal
self.obs = env.obs_map() # position of obstacles
self.heuristic_type = heuristic_type
tools.show_map(self.xI, self.xG, self.obs, "a_star searching")
def __init__(self, x_start, x_goal):
self.u_set = motion_model.motions # feasible input set
self.xI, self.xG = x_start, x_goal
self.obs = env.obs_map() # position of obstacles
tools.show_map(self.xI, self.xG, self.obs, "breadth-first searching")