saberbud/PathPlanning
1
0
Fork 0
mirror of https://github.com/zhm-real/PathPlanning.git synced 2026-08-29 16:40:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

update ARA*

Browse Source
This commit is contained in:
zhm-real
2020-06-27 00:21:29 -07:00
parent 47e0e823a7
commit 3a87e5d577
4 changed files with 82 additions and 49 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Search-based Planning/.idea/workspace.xml
Generated
+15 -11
View File
@@ -19,7 +19,11 @@
<select />
</component>
<component name="ChangeListManager">
<list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="" />
<list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
<change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
<change beforePath="$PROJECT_DIR$/ara_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/ara_star.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/plotting.py" beforeDir="false" afterPath="$PROJECT_DIR$/plotting.py" afterDir="false" />
</list>
<option name="SHOW_DIALOG" value="false" />
<option name="HIGHLIGHT_CONFLICTS" value="true" />
<option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
@@ -51,7 +55,7 @@
<property name="restartRequiresConfirmation" value="false" />
<property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
</component>
<component name="RunManager" selected="Python.a_star">
<component name="RunManager" selected="Python.ara_star">
<configuration name="a_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
@@ -159,8 +163,8 @@
</configuration>
<recent_temporary>
<list>
<item itemvalue="Python.a_star" />
<item itemvalue="Python.ara_star" />
<item itemvalue="Python.a_star" />
<item itemvalue="Python.bfs" />
<item itemvalue="Python.dfs" />
<item itemvalue="Python.dijkstra" />
@@ -198,22 +202,22 @@
</state>
<state x="2700" y="297" width="424" height="482" key="FileChooserDialogImpl/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592802293738" />
<state x="819" y="314" key="FileChooserDialogImpl/65.24.1855.1056/1920.0.1920.1080@65.24.1855.1056" timestamp="1592933974409" />
<state width="1832" height="206" key="GridCell.Tab.0.bottom" timestamp="1593133319558">
<state width="1832" height="206" key="GridCell.Tab.0.bottom" timestamp="1593242428508">
<screen x="1920" y="0" width="1920" height="1080" />
</state>
<state width="1832" height="206" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593133319558" />
<state width="1832" height="206" key="GridCell.Tab.0.center" timestamp="1593133319558">
<state width="1832" height="206" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593242428508" />
<state width="1832" height="206" key="GridCell.Tab.0.center" timestamp="1593242428508">
<screen x="1920" y="0" width="1920" height="1080" />
</state>
<state width="1832" height="206" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593133319558" />
<state width="1832" height="206" key="GridCell.Tab.0.left" timestamp="1593133319558">
<state width="1832" height="206" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593242428508" />
<state width="1832" height="206" key="GridCell.Tab.0.left" timestamp="1593242428508">
<screen x="1920" y="0" width="1920" height="1080" />
</state>
<state width="1832" height="206" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593133319558" />
<state width="1832" height="206" key="GridCell.Tab.0.right" timestamp="1593133319558">
<state width="1832" height="206" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593242428508" />
<state width="1832" height="206" key="GridCell.Tab.0.right" timestamp="1593242428508">
<screen x="1920" y="0" width="1920" height="1080" />
</state>
<state width="1832" height="206" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593133319558" />
<state width="1832" height="206" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593242428508" />
<state x="2406" y="174" key="SettingsEditor" timestamp="1592801555194">
<screen x="1920" y="0" width="1920" height="1080" />
</state>
Search-based Planning/__pycache__/plotting.cpython-37.pyc
BIN
View File
Binary file not shown.
Search-based Planning/ara_star.py
+25 -28
View File
@@ -15,7 +15,7 @@ class AraStar:
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.e = 3
self.e = 2.5
self.g = {self.xI: 0, self.xG: float("inf")}
self.fig_name = "ARA_Star Algorithm"
@@ -24,47 +24,57 @@ class AraStar:
self.INCONS = []
self.parent = {self.xI: self.xI}
def searching(self):
path = []
self.path = []
self.visited = []
def searching(self):
self.OPEN.put(self.xI, self.fvalue(self.xI))
self.ImprovePath()
path.append(self.extract_path())
self.path.append(self.extract_path())
while self.update_e() > 1:
self.e -= 0.5
print(self.e)
OPEN_mid = [x for (p, x) in self.OPEN.enumerate()] + self.INCONS
self.OPEN = queue.QueuePrior()
self.OPEN.put(self.xI, self.fvalue(self.xI))
for x in OPEN_mid:
self.OPEN.put(x, self.fvalue(x))
self.INCONS = []
self.CLOSED = []
self.ImprovePath()
self.path.append(self.extract_path())
path.append(self.extract_path())
return path
return self.path, self.visited
def ImprovePath(self):
while (not self.OPEN.empty() and self.fvalue(self.xG) >
visited_each = []
while (self.fvalue(self.xG) >
min([self.fvalue(x) for (p, x) in self.OPEN.enumerate()])):
s = self.OPEN.get()
self.CLOSED.append(s)
if s not in self.CLOSED:
self.CLOSED.append(s)
for u_next in self.u_set:
s_next = tuple([s[i] + u_next[i] for i in range(len(s))])
if s_next not in self.obs:
new_cost = self.g[s] + self.get_cost(s, u_next)
if s_next not in self.g or new_cost < self.g[s_next]:
self.g[s_next] = new_cost
self.parent[s_next] = s
visited_each.append(s_next)
if s_next not in self.CLOSED:
self.OPEN.put(s_next, self.fvalue(s_next))
else:
self.INCONS.append(s_next)
self.visited.append(visited_each)
def update_e(self):
c_OPEN, c_INCONS = float("inf"), float("inf")
@@ -76,8 +86,8 @@ class AraStar:
if min(c_OPEN, c_INCONS) == float("inf"):
return 1
else:
return min(self.e, self.g[self.xG] / min(c_OPEN, c_INCONS))
return min(self.e, self.g[self.xG] / min(c_OPEN, c_INCONS))
def fvalue(self, x):
h = self.e * self.Heuristic(x)
@@ -143,23 +153,10 @@ def main():
arastar = AraStar(x_start, x_goal, "manhattan")
plot = plotting.Plotting(x_start, x_goal)
path = arastar.searching()
fig_name = "ARA* algorithm"
path, visited = arastar.searching()
plot.plot_grid("ARA*")
print(arastar.e)
for path_i in path:
path_i.remove(x_start)
path_i.remove(x_goal)
path_x = [path_i[i][0] for i in range(len(path_i))]
path_y = [path_i[i][1] for i in range(len(path_i))]
plt.plot(path_x, path_y, linewidth='3', marker='o')
plt.pause(1)
plt.show()
plot.animation_ara_star(path, visited, fig_name)
if __name__ == '__main__':
Search-based Planning/plotting.py
+42 -10
View File
@@ -12,6 +12,7 @@ class Plotting:
self.plot_grid(name)
self.plot_visited(visited)
self.plot_path(path)
plt.show()
def plot_grid(self, name):
obs_x = [self.obs[i][0] for i in range(len(self.obs))]
@@ -23,31 +24,62 @@ class Plotting:
plt.title(name)
plt.axis("equal")
def plot_visited(self, visited):
visited.remove(self.xI)
def plot_visited(self, visited, cl='gray'):
if self.xI in visited:
visited.remove(self.xI)
if self.xG in visited:
visited.remove(self.xG)
count = 0
for x in visited:
count += 1
plt.plot(x[0], x[1], linewidth='3', color='#808080', marker='o')
plt.plot(x[0], x[1], linewidth='3', color=cl, marker='o')
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if count < len(visited) / 3:
length = 15
elif count < len(visited) * 2 / 3:
length = 30
length = 25
else:
length = 45
length = 35
if count % length == 0: plt.pause(0.001)
if count % length == 0:
plt.pause(0.001)
plt.pause(0.01)
def plot_path(self, path, cl='r', flag=False):
if self.xI in path:
path.remove(self.xI)
if self.xG in path:
path.remove(self.xG)
def plot_path(self, path):
path.remove(self.xI)
path.remove(self.xG)
path_x = [path[i][0] for i in range(len(path))]
path_y = [path[i][1] for i in range(len(path))]
plt.plot(path_x, path_y, linewidth='3', color='r', marker='o')
if not flag:
plt.plot(path_x, path_y, linewidth='3', color='r', marker='o')
else:
plt.plot(path_x, path_y, linewidth='3', color=cl, marker='o')
plt.pause(0.01)
def animation_ara_star(self, path, visited, name):
self.plot_grid(name)
cl_v, cl_p = self.color_list()
for k in range(len(path)):
self.plot_visited(visited[k], cl_v[k])
self.plot_path(path[k], cl_p[k], True)
plt.pause(0.5)
plt.show()
@staticmethod
def color_list():
cl_v = ['silver', 'wheat', 'lightskyblue', 'plum', 'slategray']
cl_p = ['gray', 'orange', 'deepskyblue', 'red', 'm']
return cl_v, cl_p