mirror of
https://github.com/zhm-real/PathPlanning.git
synced 2026-08-29 16:40:46 +08:00
update D* Lite
This commit is contained in:
zhm-real
+13
-20
@@ -20,21 +20,9 @@
|
||||
</component>
|
||||
<component name="ChangeListManager">
|
||||
<list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
|
||||
<change afterPath="$PROJECT_DIR$/Search_2D/D_star_Lite.py" afterDir="false" />
|
||||
<change afterPath="$PROJECT_DIR$/Search_2D/LPAstar_backup.py" afterDir="false" />
|
||||
<change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
|
||||
<change beforePath="$PROJECT_DIR$/Search_2D/ARAstar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/ARAstar.py" 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/astar.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/astar.py" afterDir="false" />
|
||||
<change beforePath="$PROJECT_DIR$/Search_2D/bfs.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/bfs.py" afterDir="false" />
|
||||
<change beforePath="$PROJECT_DIR$/Search_2D/bidirectional_a_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/bidirectional_a_star.py" afterDir="false" />
|
||||
<change beforePath="$PROJECT_DIR$/Search_2D/dfs.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/dfs.py" afterDir="false" />
|
||||
<change beforePath="$PROJECT_DIR$/Search_2D/dijkstra.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/dijkstra.py" afterDir="false" />
|
||||
<change beforePath="$PROJECT_DIR$/Search_2D/queue.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/queue.py" afterDir="false" />
|
||||
<change beforePath="$PROJECT_DIR$/Search_3D/Astar3D.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_3D/Astar3D.py" afterDir="false" />
|
||||
<change beforePath="$PROJECT_DIR$/Search_3D/LRT_Astar3D.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_3D/LRT_Astar3D.py" afterDir="false" />
|
||||
<change beforePath="$PROJECT_DIR$/Search_2D/D_star_Lite.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/D_star_Lite.py" afterDir="false" />
|
||||
<change beforePath="$PROJECT_DIR$/Search_2D/LPAstar_backup.py" beforeDir="false" />
|
||||
</list>
|
||||
<option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
|
||||
<option name="SHOW_DIALOG" value="false" />
|
||||
@@ -68,6 +56,11 @@
|
||||
<property name="restartRequiresConfirmation" value="false" />
|
||||
<property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
|
||||
</component>
|
||||
<component name="RecentsManager">
|
||||
<key name="MoveFile.RECENT_KEYS">
|
||||
<recent name="C:\Users\Huiming Zhou\Desktop\path planning algorithms\Search-based Planning\Search_2D" />
|
||||
</key>
|
||||
</component>
|
||||
<component name="RunDashboard">
|
||||
<option name="ruleStates">
|
||||
<list>
|
||||
@@ -80,8 +73,8 @@
|
||||
</list>
|
||||
</option>
|
||||
</component>
|
||||
<component name="RunManager" selected="Python.LPAstar">
|
||||
<configuration name="Astar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
|
||||
<component name="RunManager" selected="Python.D_star_Lite">
|
||||
<configuration name="D_star_Lite" type="PythonConfigurationType" factoryName="Python" temporary="true">
|
||||
<module name="Search-based Planning" />
|
||||
<option name="INTERPRETER_OPTIONS" value="" />
|
||||
<option name="PARENT_ENVS" value="true" />
|
||||
@@ -89,11 +82,11 @@
|
||||
<env name="PYTHONUNBUFFERED" value="1" />
|
||||
</envs>
|
||||
<option name="SDK_HOME" value="" />
|
||||
<option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_3D" />
|
||||
<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_3D/Astar3D.py" />
|
||||
<option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/D_star_Lite.py" />
|
||||
<option name="PARAMETERS" value="" />
|
||||
<option name="SHOW_COMMAND_LINE" value="false" />
|
||||
<option name="EMULATE_TERMINAL" value="false" />
|
||||
@@ -213,15 +206,15 @@
|
||||
<item itemvalue="Python.LRTAstar" />
|
||||
<item itemvalue="Python.RTAAstar" />
|
||||
<item itemvalue="Python.LRT_Astar3D" />
|
||||
<item itemvalue="Python.Astar3D" />
|
||||
<item itemvalue="Python.D_star_Lite" />
|
||||
</list>
|
||||
<recent_temporary>
|
||||
<list>
|
||||
<item itemvalue="Python.D_star_Lite" />
|
||||
<item itemvalue="Python.LPAstar" />
|
||||
<item itemvalue="Python.RTAAstar" />
|
||||
<item itemvalue="Python.LRTAstar" />
|
||||
<item itemvalue="Python.LRT_Astar3D" />
|
||||
<item itemvalue="Python.Astar3D" />
|
||||
</list>
|
||||
</recent_temporary>
|
||||
</component>
|
||||
|
||||
@@ -16,15 +16,18 @@ from Search_2D import plotting
|
||||
from Search_2D import env
|
||||
|
||||
|
||||
class DStarLite:
|
||||
class LpaStar:
|
||||
def __init__(self, x_start, x_goal, heuristic_type):
|
||||
self.xI, self.xG = x_start, x_goal
|
||||
self.heuristic_type = heuristic_type
|
||||
|
||||
self.Env = env.Env() # class Env
|
||||
self.Plot = plotting.Plotting(x_start, x_goal)
|
||||
|
||||
self.u_set = self.Env.motions # feasible input set
|
||||
self.obs = self.Env.obs # position of obstacles
|
||||
self.x = self.Env.x_range
|
||||
self.y = self.Env.y_range
|
||||
|
||||
self.U = queue.QueuePrior() # priority queue / U set
|
||||
self.g, self.rhs = {}, {}
|
||||
@@ -36,32 +39,177 @@ class DStarLite:
|
||||
self.g[(i, j)] = float("inf")
|
||||
|
||||
self.rhs[self.xG] = 0
|
||||
self.U.put(self.xG, self.CalculateKey(self.xG))
|
||||
self.U.put(self.xG, self.Key(self.xG))
|
||||
self.fig = plt.figure()
|
||||
|
||||
def CalculateKey(self, s):
|
||||
return [min(self.g[s], self.rhs[s]) + self.h(self.xI, s) + self.km,
|
||||
def searching(self):
|
||||
self.Plot.plot_grid("Lifelong Planning A*")
|
||||
|
||||
self.ComputePath()
|
||||
self.plot_path(self.extract_path_test())
|
||||
|
||||
# self.fig.canvas.mpl_connect('button_press_event', self.on_press)
|
||||
|
||||
plt.show()
|
||||
|
||||
def on_press(self, event):
|
||||
x, y = event.xdata, event.ydata
|
||||
if x < 0 or x > self.x - 1 or y < 0 or y > self.y - 1:
|
||||
print("Please choose right area!")
|
||||
else:
|
||||
x, y = int(x), int(y)
|
||||
print("Change position: x =", x, ",", "y =", y)
|
||||
if (x, y) not in self.obs:
|
||||
self.obs.add((x, y))
|
||||
plt.plot(x, y, 'sk')
|
||||
self.rhs[(x, y)] = float("inf")
|
||||
self.g[(x, y)] = float("inf")
|
||||
for node in self.getSucc((x, y)):
|
||||
self.UpdateVertex(node)
|
||||
else:
|
||||
self.obs.remove((x, y))
|
||||
plt.plot(x, y, marker='s', color='white')
|
||||
self.UpdateVertex((x, y))
|
||||
self.ComputePath()
|
||||
self.plot_path(self.extract_path_test())
|
||||
self.fig.canvas.draw_idle()
|
||||
|
||||
@staticmethod
|
||||
def plot_path(path):
|
||||
px = [x[0] for x in path]
|
||||
py = [x[1] for x in path]
|
||||
plt.plot(px, py, marker='o')
|
||||
|
||||
def ComputePath(self):
|
||||
count = 0
|
||||
while self.U.top_key() < self.Key(self.xI) or \
|
||||
self.rhs[self.xI] != self.g[self.xI]:
|
||||
count += 1
|
||||
print(count)
|
||||
k_old = self.U.top_key()
|
||||
s = self.U.get()
|
||||
if k_old < self.Key(s):
|
||||
self.U.put(s, self.Key(s))
|
||||
elif self.g[s] > self.rhs[s]:
|
||||
self.g[s] = self.rhs[s]
|
||||
for x in self.getPred(s):
|
||||
self.UpdateVertex(x)
|
||||
else:
|
||||
self.g[s] = float("inf")
|
||||
self.UpdateVertex(s)
|
||||
for x in self.getPred(s):
|
||||
self.UpdateVertex(x)
|
||||
|
||||
def getSucc(self, s):
|
||||
nei_list = set()
|
||||
for u in self.u_set:
|
||||
s_next = tuple([s[i] + u[i] for i in range(2)])
|
||||
if s_next not in self.obs and self.g[s_next] >= self.g[s]:
|
||||
nei_list.add(s_next)
|
||||
return nei_list
|
||||
|
||||
def getPred(self, s):
|
||||
nei_list = set()
|
||||
for u in self.u_set:
|
||||
s_next = tuple([s[i] + u[i] for i in range(2)])
|
||||
if s_next not in self.obs and self.g[s_next] <= self.g[s]:
|
||||
nei_list.add(s_next)
|
||||
return nei_list
|
||||
|
||||
def UpdateVertex(self, s):
|
||||
if s != self.xG:
|
||||
self.rhs[s] = float("inf")
|
||||
for x in self.getSucc(s):
|
||||
self.rhs[s] = min(self.rhs[s], self.g[x] + self.get_cost(s, x))
|
||||
self.U.remove(s)
|
||||
if self.g[s] != self.rhs[s]:
|
||||
self.U.put(s, self.Key(s))
|
||||
|
||||
def extract_path_test(self):
|
||||
path = []
|
||||
s = self.xG
|
||||
|
||||
for k in range(100):
|
||||
g_list = {}
|
||||
for x in self.get_neighbor(s):
|
||||
g_list[x] = self.g[x]
|
||||
s = min(g_list, key=g_list.get)
|
||||
if s == self.xI:
|
||||
return list(reversed(path))
|
||||
path.append(s)
|
||||
return list(reversed(path))
|
||||
|
||||
def Key(self, s):
|
||||
return [min(self.g[s], self.rhs[s]) + self.h(s) + self.km,
|
||||
min(self.g[s], self.rhs[s])]
|
||||
|
||||
def h(self, s_start, s):
|
||||
heuristic_type = self.heuristic_type # heuristic type
|
||||
def h(self, s):
|
||||
heuristic_type = self.heuristic_type # heuristic type
|
||||
s_start = self.xI # goal node
|
||||
|
||||
if heuristic_type == "manhattan":
|
||||
return abs(s[0] - s_start[0]) + abs(s[1] - s_start[1])
|
||||
else:
|
||||
return math.hypot(s[0] - s_start[0], s[1] - s_start[1])
|
||||
|
||||
def UpdateVertex(self, s):
|
||||
if s != self.xG:
|
||||
@staticmethod
|
||||
def get_cost(s_start, s_end):
|
||||
"""
|
||||
Calculate cost for this motion
|
||||
|
||||
:param s_start:
|
||||
:param s_end:
|
||||
:return: cost for this motion
|
||||
:note: cost function could be more complicate!
|
||||
"""
|
||||
|
||||
def getNeighbor(self, s):
|
||||
v_list = set()
|
||||
return 1
|
||||
|
||||
def get_neighbor(self, s):
|
||||
nei_list = set()
|
||||
for u in self.u_set:
|
||||
s_next = tuple([s[i] + u[i] for i in range(2)])
|
||||
if s_next not in self.obs:
|
||||
v_list.add(s_next)
|
||||
nei_list.add(s_next)
|
||||
|
||||
return v_list
|
||||
return nei_list
|
||||
|
||||
def getCost(self, s_start, s_end):
|
||||
def extract_path(self):
|
||||
path = []
|
||||
s = self.xG
|
||||
|
||||
while True:
|
||||
g_list = {}
|
||||
for x in self.get_neighbor(s):
|
||||
g_list[x] = self.g[x]
|
||||
s = min(g_list, key=g_list.get)
|
||||
if s == self.xI:
|
||||
return list(reversed(path))
|
||||
path.append(s)
|
||||
|
||||
def print_g(self):
|
||||
print("he")
|
||||
for k in range(self.Env.y_range):
|
||||
j = self.Env.y_range - k - 1
|
||||
string = ""
|
||||
for i in range(self.Env.x_range):
|
||||
if self.g[(i, j)] == float("inf"):
|
||||
string += ("00" + ', ')
|
||||
else:
|
||||
if self.g[(i, j)] // 10 == 0:
|
||||
string += ("0" + str(self.g[(i, j)]) + ', ')
|
||||
else:
|
||||
string += (str(self.g[(i, j)]) + ', ')
|
||||
print(string)
|
||||
|
||||
|
||||
def main():
|
||||
x_start = (5, 5)
|
||||
x_goal = (45, 25)
|
||||
|
||||
lpastar = LpaStar(x_start, x_goal, "euclidean")
|
||||
lpastar.searching()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
|
||||
@@ -1,209 +0,0 @@
|
||||
"""
|
||||
LPA_star 2D
|
||||
@author: huiming zhou
|
||||
"""
|
||||
|
||||
import os
|
||||
import sys
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
|
||||
"/../../Search-based Planning/")
|
||||
|
||||
from Search_2D import queue
|
||||
from Search_2D import plotting
|
||||
from Search_2D import env
|
||||
|
||||
|
||||
class LpaStar:
|
||||
def __init__(self, x_start, x_goal, heuristic_type):
|
||||
self.xI, self.xG = x_start, x_goal
|
||||
self.heuristic_type = heuristic_type
|
||||
|
||||
self.Env = env.Env() # class Env
|
||||
|
||||
self.u_set = self.Env.motions # feasible input set
|
||||
self.obs = self.Env.obs # position of obstacles
|
||||
|
||||
self.OPEN = queue.QueuePrior() # priority queue / U set
|
||||
self.g, self.v = {}, {}
|
||||
|
||||
for i in range(self.Env.x_range):
|
||||
for j in range(self.Env.y_range):
|
||||
self.v[(i, j)] = float("inf")
|
||||
self.g[(i, j)] = float("inf")
|
||||
|
||||
self.v[self.xI] = 0
|
||||
self.OPEN.put(self.xI, self.Key(self.xI))
|
||||
self.CLOSED = set()
|
||||
|
||||
def searching(self):
|
||||
self.ComputePath()
|
||||
path = [self.extract_path()]
|
||||
# self.print_g()
|
||||
|
||||
obs_change = set()
|
||||
for i in range(25, 30):
|
||||
self.obs.add((i, 15))
|
||||
obs_change.add((i, 15))
|
||||
|
||||
self.obs.add((30, 14))
|
||||
obs_change.add((30, 14))
|
||||
|
||||
for s in obs_change:
|
||||
self.v[s] = float("inf")
|
||||
self.g[s] = float("inf")
|
||||
for x in self.get_neighbor(s):
|
||||
self.UpdateMembership(x)
|
||||
|
||||
# for x in obs_change:
|
||||
# self.obs.remove(x)
|
||||
# for x in obs_change:
|
||||
# self.UpdateVertex(x)
|
||||
|
||||
self.ComputePath()
|
||||
path.append(self.extract_path_test())
|
||||
self.print_g()
|
||||
|
||||
return path, obs_change
|
||||
|
||||
def ComputePath(self):
|
||||
while self.Key(self.xG) > self.OPEN.top_key() \
|
||||
or self.v[self.xG] < self.g[self.xG]:
|
||||
s = self.OPEN.get()
|
||||
if self.v[s] > self.g[s]:
|
||||
self.v[s] = self.g[s]
|
||||
self.CLOSED.add(s)
|
||||
|
||||
|
||||
|
||||
while self.OPEN.top_key() < self.Key(self.xG) \
|
||||
or self.v[self.xG] != self.g[self.xG]:
|
||||
s = self.OPEN.get()
|
||||
if self.g[s] > self.v[s]:
|
||||
self.g[s] = self.v[s]
|
||||
else:
|
||||
self.g[s] = float("inf")
|
||||
self.UpdateMembership(s)
|
||||
for x in self.get_neighbor(s):
|
||||
self.UpdateMembership(x)
|
||||
# return self.extract_path()
|
||||
|
||||
def UpdateMembership(self, s):
|
||||
if self.v[s] != self.g[s]:
|
||||
if s not in self.CLOSED:
|
||||
self.OPEN.put(s, self.Key(s))
|
||||
else:
|
||||
if s in self.OPEN:
|
||||
self.OPEN.remove(s)
|
||||
|
||||
def print_g(self):
|
||||
print("he")
|
||||
for k in range(self.Env.y_range):
|
||||
j = self.Env.y_range - k - 1
|
||||
string = ""
|
||||
for i in range(self.Env.x_range):
|
||||
if self.g[(i, j)] == float("inf"):
|
||||
string += ("00" + ', ')
|
||||
else:
|
||||
if self.g[(i, j)] // 10 == 0:
|
||||
string += ("0" + str(self.g[(i, j)]) + ', ')
|
||||
else:
|
||||
string += (str(self.g[(i, j)]) + ', ')
|
||||
print(string)
|
||||
|
||||
def extract_path(self):
|
||||
path = []
|
||||
s = self.xG
|
||||
|
||||
while True:
|
||||
g_list = {}
|
||||
for x in self.get_neighbor(s):
|
||||
g_list[x] = self.g[x]
|
||||
s = min(g_list, key=g_list.get)
|
||||
if s == self.xI:
|
||||
return list(reversed(path))
|
||||
path.append(s)
|
||||
|
||||
def extract_path_test(self):
|
||||
path = []
|
||||
s = self.xG
|
||||
|
||||
for k in range(70):
|
||||
g_list = {}
|
||||
for x in self.get_neighbor(s):
|
||||
g_list[x] = self.g[x]
|
||||
s = min(g_list, key=g_list.get)
|
||||
if s == self.xI:
|
||||
return list(reversed(path))
|
||||
path.append(s)
|
||||
return list(reversed(path))
|
||||
|
||||
def get_neighbor(self, s):
|
||||
nei_list = set()
|
||||
for u in self.u_set:
|
||||
s_next = tuple([s[i] + u[i] for i in range(2)])
|
||||
if s_next not in self.obs:
|
||||
nei_list.add(s_next)
|
||||
|
||||
return nei_list
|
||||
|
||||
def Key(self, s):
|
||||
return [min(self.g[s], self.v[s]) + self.h(s),
|
||||
min(self.g[s], self.v[s])]
|
||||
|
||||
def h(self, s):
|
||||
heuristic_type = self.heuristic_type # heuristic type
|
||||
goal = self.xG # goal node
|
||||
|
||||
if heuristic_type == "manhattan":
|
||||
return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
|
||||
elif heuristic_type == "euclidean":
|
||||
return ((goal[0] - s[0]) ** 2 + (goal[1] - s[1]) ** 2) ** (1 / 2)
|
||||
else:
|
||||
print("Please choose right heuristic type!")
|
||||
|
||||
def get_cost(self, s_start, s_end):
|
||||
"""
|
||||
Calculate cost for this motion
|
||||
|
||||
:param s_start:
|
||||
:param s_end:
|
||||
:return: cost for this motion
|
||||
:note: cost function could be more complicate!
|
||||
"""
|
||||
|
||||
# if s_start not in self.obs:
|
||||
# if s_end not in self.obs:
|
||||
# return 1
|
||||
# else:
|
||||
# return float("inf")
|
||||
# return float("inf")
|
||||
return 1
|
||||
|
||||
def main():
|
||||
x_start = (5, 5)
|
||||
x_goal = (45, 25)
|
||||
|
||||
lpastar = LpaStar(x_start, x_goal, "manhattan")
|
||||
plot = plotting.Plotting(x_start, x_goal)
|
||||
|
||||
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()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
Reference in New Issue
Block a user