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

Browse Source
This commit is contained in:
zhm-real
2020-06-30 14:12:07 -07:00
parent 14a0c658d7
commit 89ecf8c810
10 changed files with 141 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Search-based Planning/.idea/workspace.xml
Generated
+35 -29
View File
@@ -21,6 +21,12 @@
<component name="ChangeListManager">
<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$/Search_2D/LPAstar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/LPAstar.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/Search_2D/LRTAstar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/LRTAstar.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/Search_2D/RTAAstar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/RTAAstar.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/Search_2D/env.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/env.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/Search_2D/plotting.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/plotting.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/Search_2D/test.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/test.py" afterDir="false" />
</list>
<option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
<option name="SHOW_DIALOG" value="false" />
@@ -66,7 +72,7 @@
</list>
</option>
</component>
<component name="RunManager" selected="Python.bfs">
<component name="RunManager" selected="Python.RTAAstar">
<configuration name="LPAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
@@ -88,28 +94,7 @@
<option name="INPUT_FILE" value="" />
<method v="2" />
</configuration>
<configuration name="LRT_Astar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
<envs>
<env name="PYTHONUNBUFFERED" value="1" />
</envs>
<option name="SDK_HOME" value="" />
<option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_3D" />
<option name="IS_MODULE_SDK" value="true" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_3D/LRT_Astar3D.py" />
<option name="PARAMETERS" value="" />
<option name="SHOW_COMMAND_LINE" value="false" />
<option name="EMULATE_TERMINAL" value="false" />
<option name="MODULE_MODE" value="false" />
<option name="REDIRECT_INPUT" value="false" />
<option name="INPUT_FILE" value="" />
<method v="2" />
</configuration>
<configuration name="astar" type="PythonConfigurationType" factoryName="Python" temporary="true">
<configuration name="LRTAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
@@ -121,7 +106,28 @@
<option name="IS_MODULE_SDK" value="true" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/astar.py" />
<option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/LRTAstar.py" />
<option name="PARAMETERS" value="" />
<option name="SHOW_COMMAND_LINE" value="false" />
<option name="EMULATE_TERMINAL" value="false" />
<option name="MODULE_MODE" value="false" />
<option name="REDIRECT_INPUT" value="false" />
<option name="INPUT_FILE" value="" />
<method v="2" />
</configuration>
<configuration name="RTAAstar" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
<option name="PARENT_ENVS" value="true" />
<envs>
<env name="PYTHONUNBUFFERED" value="1" />
</envs>
<option name="SDK_HOME" value="" />
<option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
<option name="IS_MODULE_SDK" value="true" />
<option name="ADD_CONTENT_ROOTS" value="true" />
<option name="ADD_SOURCE_ROOTS" value="true" />
<option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/RTAAstar.py" />
<option name="PARAMETERS" value="" />
<option name="SHOW_COMMAND_LINE" value="false" />
<option name="EMULATE_TERMINAL" value="false" />
@@ -195,19 +201,19 @@
</configuration>
<list>
<item itemvalue="Python.dijkstra" />
<item itemvalue="Python.astar" />
<item itemvalue="Python.LRT_Astar3D" />
<item itemvalue="Python.test" />
<item itemvalue="Python.LPAstar" />
<item itemvalue="Python.bfs" />
<item itemvalue="Python.LRTAstar" />
<item itemvalue="Python.RTAAstar" />
</list>
<recent_temporary>
<list>
<item itemvalue="Python.bfs" />
<item itemvalue="Python.RTAAstar" />
<item itemvalue="Python.LRTAstar" />
<item itemvalue="Python.LPAstar" />
<item itemvalue="Python.bfs" />
<item itemvalue="Python.test" />
<item itemvalue="Python.astar" />
<item itemvalue="Python.LRT_Astar3D" />
</list>
</recent_temporary>
</component>
Search-based Planning/Search_2D/LPAstar.py
+69 -19
View File
@@ -34,12 +34,38 @@ class LpaStar:
self.g[(i, j)] = float("inf")
self.rhs[self.xI] = 0
self.U.put(self.xI, [self.h(self.xI), 0])
self.U.put(self.xI, self.CalculateKey(self.xI))
def searching(self):
self.computePath()
path = self.extract_path()
return path
path = [self.extract_path()]
obs_change = set()
for j in range(14, 15):
self.obs.add((30, j))
obs_change.add((30, j))
for s in obs_change:
self.rhs[s] = float("inf")
self.g[s] = float("inf")
for x in self.get_neighbor(s):
self.UpdateVertex(x)
# for x in obs_change:
# self.obs.remove(x)
# for x in obs_change:
# self.UpdateVertex(x)
print(self.g[(29, 15)])
print(self.g[(29, 14)])
print(self.g[(29, 13)])
print(self.g[(30, 13)])
print(self.g[(31, 13)])
print(self.g[(32, 13)])
print(self.g[(33, 13)])
print(self.g[(34, 13)])
self.computePath()
path.append(self.extract_path_test())
return path, obs_change
def computePath(self):
while self.U.top_key() < self.CalculateKey(self.xG) \
@@ -47,13 +73,12 @@ class LpaStar:
s = self.U.get()
if self.g[s] > self.rhs[s]:
self.g[s] = self.rhs[s]
for x in self.get_neighbor(s):
self.UpdateVertex(x)
else:
self.g[s] = float("inf")
self.UpdateVertex(s)
for x in self.get_neighbor(s):
self.UpdateVertex(x)
for x in self.get_neighbor(s):
self.UpdateVertex(x)
# return self.extract_path()
def extract_path(self):
path = []
@@ -68,6 +93,18 @@ class LpaStar:
return list(reversed(path))
path.append(s)
def extract_path_test(self):
path = []
s = self.xG
for k in range(30):
g_list = {}
for x in self.get_neighbor(s):
g_list[x] = self.g[x]
s = min(g_list, key=g_list.get)
path.append(s)
return list(reversed(path))
def get_neighbor(self, s):
nei_list = set()
for u in self.u_set:
@@ -85,7 +122,7 @@ class LpaStar:
if u != self.xI:
u_min = float("inf")
for x in self.get_neighbor(u):
u_min = min(u_min, self.g[x] + 1)
u_min = min(u_min, self.g[x] + self.get_cost(u, x))
self.rhs[u] = u_min
self.U.check_remove(u)
if self.g[u] != self.rhs[u]:
@@ -102,32 +139,45 @@ class LpaStar:
else:
print("Please choose right heuristic type!")
@staticmethod
def get_cost(x, u):
def get_cost(self, s_start, s_end):
"""
Calculate cost for this motion
:param x: current node
:param u: current input
:param s_start:
:param s_end:
:return: cost for this motion
:note: cost function could be more complicate!
"""
return 1
if s_start not in self.obs:
if s_end not in self.obs:
return 1
else:
return float("inf")
return float("inf")
def main():
x_start = (5, 5)
x_goal = (45, 25)
lpastar = LpaStar(x_start, x_goal, "manhattan")
lpastar = LpaStar(x_start, x_goal, "euclidean")
plot = plotting.Plotting(x_start, x_goal)
path = lpastar.searching()
plot.plot_grid("test")
px = [x[0] for x in path]
py = [x[1] for x in path]
plt.plot(px, py, color='red', marker='o')
path, obs = lpastar.searching()
plot.plot_grid("Lifelong Planning A*")
p = path[0]
px = [x[0] for x in p]
py = [x[1] for x in p]
plt.plot(px, py, marker='o')
plt.pause(0.5)
p = path[1]
px = [x[0] for x in p]
py = [x[1] for x in p]
plt.plot(px, py, marker='o')
plt.pause(0.01)
plt.show()
Search-based Planning/Search_2D/LRTAstar.py
+13 -4
View File
@@ -29,6 +29,11 @@ class LrtAstarN:
self.N = N # number of expand nodes each iteration
self.visited = [] # order of visited nodes in planning
self.path = [] # path of each iteration
self.h_table = {}
for i in range(self.Env.x_range):
for j in range(self.Env.y_range):
self.h_table[(i, j)] = self.h((i, j)) # initialize h_value
def searching(self):
s_start = self.xI # initialize start node
@@ -41,6 +46,10 @@ class LrtAstarN:
break
h_value = self.iteration(CLOSED) # h_value table of CLOSED nodes
for x in h_value:
self.h_table[x] = h_value[x]
s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value) # s_start -> expected node in OPEN set
self.path.append(path_k)
@@ -56,7 +65,7 @@ class LrtAstarN:
if s_next in h_value:
h_list[s_next] = h_value[s_next]
else:
h_list[s_next] = self.h(s_next)
h_list[s_next] = self.h_table[s_next]
s_key = min(h_list, key=h_list.get) # move to the smallest node with min h_value
path.append(s_key) # generate path
s = s_key # use end of this iteration as the start of next
@@ -78,7 +87,7 @@ class LrtAstarN:
s_next = tuple([s[i] + u[i] for i in range(2)])
if s_next not in self.obs:
if s_next not in CLOSED:
h_list.append(self.get_cost(s, s_next) + self.h(s_next))
h_list.append(self.get_cost(s, s_next) + self.h_table[s_next])
else:
h_list.append(self.get_cost(s, s_next) + h_value[s_next])
h_value[s] = min(h_list) # update h_value of current node
@@ -114,7 +123,7 @@ class LrtAstarN:
if new_cost < g_table[s_next]: # conditions for updating cost
g_table[s_next] = new_cost
PARENT[s_next] = s
OPEN.put(s_next, g_table[s_next] + self.h(s_next))
OPEN.put(s_next, g_table[s_next] + self.h_table[s_next])
if count == N: # expand needed CLOSED nodes
break
@@ -171,7 +180,7 @@ def main():
x_start = (10, 5)
x_goal = (45, 25)
lrta = LrtAstarN(x_start, x_goal, 220, "euclidean")
lrta = LrtAstarN(x_start, x_goal, 100, "euclidean")
plot = plotting.Plotting(x_start, x_goal)
fig_name = "Learning Real-time A* (LRTA*)"
Search-based Planning/Search_2D/RTAAstar.py
+12 -3
View File
@@ -29,6 +29,11 @@ class RtaAstar:
self.N = N # number of expand nodes each iteration
self.visited = [] # order of visited nodes in planning
self.path = [] # path of each iteration
self.h_table = {}
for i in range(self.Env.x_range):
for j in range(self.Env.y_range):
self.h_table[(i, j)] = self.h((i, j)) # initialize h_value
def searching(self):
s_start = self.xI # initialize start node
@@ -42,6 +47,10 @@ class RtaAstar:
break
s_next, h_value = self.cal_h_value(OPEN, CLOSED, g_table, PARENT)
for x in h_value:
self.h_table[x] = h_value[x]
s_start, path_k = self.extract_path_in_CLOSE(s_start, s_next, h_value)
self.path.append(path_k)
@@ -49,7 +58,7 @@ class RtaAstar:
v_open = {}
h_value = {}
for (_, x) in OPEN.enumerate():
v_open[x] = g_table[PARENT[x]] + 1 + self.h(x)
v_open[x] = g_table[PARENT[x]] + 1 + self.h_table[x]
s_open = min(v_open, key=v_open.get)
f_min = min(v_open.values())
for x in CLOSED:
@@ -88,7 +97,7 @@ class RtaAstar:
s_next = tuple([s[i] + u[i] for i in range(2)])
if s_next not in self.obs:
if s_next not in CLOSED:
h_list.append(self.get_cost(s, s_next) + self.h(s_next))
h_list.append(self.get_cost(s, s_next) + self.h_table[s_next])
else:
h_list.append(self.get_cost(s, s_next) + h_value[s_next])
h_value[s] = min(h_list) # update h_value of current node
@@ -124,7 +133,7 @@ class RtaAstar:
if new_cost < g_table[s_next]: # conditions for updating cost
g_table[s_next] = new_cost
PARENT[s_next] = s
OPEN.put(s_next, g_table[s_next] + self.h(s_next))
OPEN.put(s_next, g_table[s_next] + self.h_table[s_next])
if count == N: # expand needed CLOSED nodes
break
Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc
BIN
View File
Binary file not shown.
Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc
BIN
View File
Binary file not shown.
Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc
BIN
View File
Binary file not shown.
Search-based Planning/Search_2D/env.py
+9 -9
View File
@@ -20,26 +20,26 @@ class Env:
x = self.x_range
y = self.y_range
obs = []
obs = set()
for i in range(x):
obs.append((i, 0))
obs.add((i, 0))
for i in range(x):
obs.append((i, y - 1))
obs.add((i, y - 1))
for i in range(y):
obs.append((0, i))
obs.add((0, i))
for i in range(y):
obs.append((x - 1, i))
obs.add((x - 1, i))
for i in range(10, 21):
obs.append((i, 15))
obs.add((i, 15))
for i in range(15):
obs.append((20, i))
obs.add((20, i))
for i in range(15, 30):
obs.append((30, i))
obs.add((30, i))
for i in range(16):
obs.append((40, i))
obs.add((40, i))
return obs
Search-based Planning/Search_2D/plotting.py
+2 -2
View File
@@ -59,8 +59,8 @@ class Plotting:
plt.show()
def plot_grid(self, name):
obs_x = [self.obs[i][0] for i in range(len(self.obs))]
obs_y = [self.obs[i][1] for i in range(len(self.obs))]
obs_x = [x[0] for x in self.obs]
obs_y = [x[1] for x in self.obs]
plt.plot(self.xI[0], self.xI[1], "bs")
plt.plot(self.xG[0], self.xG[1], "gs")
Search-based Planning/Search_2D/test.py
+1 -1
View File
@@ -17,4 +17,4 @@ from Search_2D import env
U = queue.QueuePrior()
U.put((1, 2), [2, 3])
U.put((2, 3), [1, 5])
print(U.get())
print(U.get())