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-07-05 15:10:51 -07:00
parent a46f133e13
commit 42dda61e9f
10 changed files with 95 additions and 69 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Search-based Planning/.idea/workspace.xml
Generated
+14 -3
View File
@@ -19,7 +19,18 @@
<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$/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/Field_D_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/Field_D_star.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/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/dijkstra.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_2D/dijkstra.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" />
</list>
<option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
<option name="SHOW_DIALOG" value="false" />
<option name="HIGHLIGHT_CONFLICTS" value="true" />
@@ -48,7 +59,7 @@
<property name="ASKED_ADD_EXTERNAL_FILES" value="true" />
<property name="RunOnceActivity.OpenProjectViewOnStart" value="true" />
<property name="RunOnceActivity.ShowReadmeOnStart" value="true" />
<property name="last_opened_file_path" value="$PROJECT_DIR$" />
<property name="last_opened_file_path" value="$PROJECT_DIR$/../../PythonRobotics-master/PathPlanning" />
<property name="restartRequiresConfirmation" value="false" />
<property name="run.code.analysis.last.selected.profile" value="aDefault" />
<property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
@@ -209,8 +220,8 @@
<list>
<item itemvalue="Python.Field_D_star" />
<item itemvalue="Python.D_star_Lite" />
<item itemvalue="Python.D_star" />
<item itemvalue="Python.LPAstar" />
<item itemvalue="Python.D_star" />
<item itemvalue="Python.ARAstar" />
</list>
</recent_temporary>
Search-based Planning/Search_2D/D_star_Lite.py
+15 -9
View File
@@ -31,8 +31,8 @@ class DStar:
self.g, self.rhs, self.U = {}, {}, {}
self.km = 0
for i in range(self.Env.x_range):
for j in range(self.Env.y_range):
for i in range(1, self.Env.x_range - 1):
for j in range(1, self.Env.y_range - 1):
self.rhs[(i, j)] = float("inf")
self.g[(i, j)] = float("inf")
@@ -60,7 +60,7 @@ class DStar:
s_curr = self.s_start
s_last = self.s_start
i = 0
path = []
path = [self.s_start]
while s_curr != self.s_goal:
s_list = {}
@@ -188,19 +188,25 @@ class DStar:
return nei_list
def extract_path(self):
path = []
"""
Extract the path based on the PARENT set.
:return: The planning path
"""
path = [self.s_start]
s = self.s_start
count = 0
while True:
count += 1
for k in range(100):
g_list = {}
for x in self.get_neighbor(s):
if not self.is_collision(s, x):
g_list[x] = self.g[x]
s = min(g_list, key=g_list.get)
if s == self.s_goal or count > 100:
return list(reversed(path))
path.append(s)
if s == self.s_goal:
break
return list(path)
def plot_path(self, path):
px = [x[0] for x in path]
Search-based Planning/Search_2D/Field_D_star.py
+54 -45
View File
@@ -28,21 +28,36 @@ class FieldDStar:
self.x = self.Env.x_range
self.y = self.Env.y_range
self.g, self.rhs, self.U = {}, {}, {}
self.g, self.rhs, self.OPEN = {}, {}, {}
self.parent = {}
self.cknbr = {}
self.ccknbr = {}
self.bptr = {}
self.init_table()
for i in range(self.Env.x_range):
for j in range(self.Env.y_range):
self.rhs[(i, j)] = float("inf")
self.g[(i, j)] = float("inf")
self.parent[(i, j)] = (0, 0)
self.bptr[(i, j)] = (0, 0)
self.rhs[self.s_goal] = 0.0
self.U[self.s_goal] = self.CalculateKey(self.s_goal)
self.OPEN[self.s_goal] = self.CalculateKey(self.s_goal)
self.visited = set()
self.count = 0
self.fig = plt.figure()
def init_table(self):
for i in range(1, self.Env.x_range - 1):
for j in range(1, self.Env.y_range - 1):
s_neighbor = self.get_neighbor_pure((i, j))
s_neighbor.append(s_neighbor[0])
for k in range(8):
self.cknbr[((i, j), s_neighbor[k])] = s_neighbor[k + 1]
s_neighbor = list(reversed(s_neighbor))
for k in range(8):
self.ccknbr[((i, j), s_neighbor[k])] = s_neighbor[k + 1]
def run(self):
self.Plot.plot_grid("Field D*")
self.ComputeShortestPath()
@@ -62,13 +77,19 @@ class FieldDStar:
if (x, y) not in self.obs:
self.obs.add((x, y))
plt.plot(x, y, 'sk')
sn_list = self.get_neighbor((x, y))
else:
self.obs.remove((x, y))
plt.plot(x, y, marker='s', color='white')
self.UpdateVertex((x, y))
sn_list = [(x, y)]
sn_list += self.get_neighbor((x, y))
for s_n in self.get_neighbor((x, y)):
self.UpdateVertex(s_n)
for s in sn_list:
v_list = []
for sn in self.get_neighbor(s):
v_list.append(self.ComputeCost(s, sn, self.ccknbr[(s, sn)]))
self.rhs[s] = min(v_list)
self.UpdateVertex(s)
self.ComputeShortestPath()
self.plot_visited(self.visited)
@@ -82,40 +103,35 @@ class FieldDStar:
self.rhs[self.s_start] == self.g[self.s_start]:
break
k_old = v
self.U.pop(s)
self.visited.add(s)
if k_old < self.CalculateKey(s):
self.U[s] = self.CalculateKey(s)
elif self.g[s] > self.rhs[s]:
if self.g[s] > self.rhs[s]:
self.g[s] = self.rhs[s]
for x in self.get_neighbor(s):
self.UpdateVertex(x)
self.OPEN.pop(s)
for sn in self.get_neighbor(s):
if self.rhs[sn] > self.ComputeCost(sn, s, self.ccknbr[(sn, s)]):
self.rhs[sn] = self.ComputeCost(sn, s, self.ccknbr[(sn, s)])
self.bptr[sn] = s
if self.rhs[sn] > self.ComputeCost(sn, s, self.cknbr[(sn, s)]):
self.rhs[sn] = self.ComputeCost(sn, self.cknbr[(sn, s)], s)
self.bptr[sn] = self.cknbr[(sn, s)]
self.UpdateVertex(sn)
else:
self.g[s] = float("inf")
for sn in self.get_neighbor(s):
if self.bptr[sn] == s or self.bptr[sn] == self.cknbr[(sn, s)]:
v_list = []
ssn_list = self.get_neighbor(sn)
for ssn in ssn_list:
v_list.append(self.ComputeCost(sn, ssn, self.ccknbr[(sn, ssn)]))
self.rhs[sn] = min(v_list)
self.bptr[sn] = ssn_list[v_list.index(min(v_list))]
self.UpdateVertex(sn)
self.UpdateVertex(s)
for x in self.get_neighbor(s):
self.UpdateVertex(x)
def UpdateVertex(self, s):
if s != self.s_goal:
value = []
s_plist = []
sn_list = self.get_neighbor_pure(s)
sn_list.append(sn_list[0])
for k in range(8):
v, sp = self.ComputeCost(s, sn_list[k], sn_list[k + 1])
value.append(v)
s_plist.append(sp)
self.rhs[s] = min(value)
self.parent[s] = s_plist[value.index(min(value))]
if s in self.U:
self.U.pop(s)
if self.g[s] != self.rhs[s]:
self.U[s] = self.CalculateKey(s)
self.OPEN[s] = self.CalculateKey(s)
elif s in self.OPEN:
self.OPEN.pop(s)
def get_neighbor_pure(self, s):
s_list = []
@@ -138,7 +154,6 @@ class FieldDStar:
c = self.cost(s, s2)
b = self.cost(s, s1)
y = 0
if min(c, b) == float("inf"):
vs = float("inf")
@@ -149,6 +164,7 @@ class FieldDStar:
if f <= b:
if c <= f:
vs = math.sqrt(2) * c + self.g[s2]
print("test loop!")
else:
y = min(f / (math.sqrt(c ** 2 - f ** 2)), 1)
vs = c * math.sqrt(1 + y ** 2) + f * (1 - y) + self.g[s2]
@@ -159,17 +175,15 @@ class FieldDStar:
x = 1 - min(b / (math.sqrt(c ** 2 - b ** 2)), 1)
vs = c * math.sqrt(1 + (1 - x) ** 2) + b * x + self.g[s2]
ss = (y * s1[0] + (1 - y) * s2[0], y * s1[1] + (1 - y) * s2[1])
return vs, ss
return vs
def TopKey(self):
"""
:return: return the min key and its value.
"""
s = min(self.U, key=self.U.get)
return s, self.U[s]
s = min(self.OPEN, key=self.OPEN.get)
return s, self.OPEN[s]
def h(self, s_start, s_goal):
heuristic_type = self.heuristic_type # heuristic type
@@ -225,12 +239,7 @@ class FieldDStar:
count = 0
while True:
count += 1
g_list = {}
for x in self.get_neighbor(s):
if not self.is_collision(s, x):
g_list[x] = self.g[x]
ss = self.parent[s]
s = min(g_list, key=g_list.get)
s = self.bptr[s]
path.append(s)
if s == self.s_goal or count > 100:
Search-based Planning/Search_2D/LRTAstar.py
+4 -4
View File
@@ -39,7 +39,7 @@ class LrtAstarN:
s_start = self.s_start # initialize start node
while True:
OPEN, CLOSED = self.Astar(s_start, self.N) # U, CLOSED sets in each iteration
OPEN, CLOSED = self.Astar(s_start, self.N) # OPEN, CLOSED sets in each iteration
if OPEN == "FOUND": # reach the goal node
self.path.append(CLOSED)
@@ -50,7 +50,7 @@ class LrtAstarN:
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 U set
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)
def extract_path_in_CLOSE(self, s_start, h_value):
@@ -68,7 +68,7 @@ class LrtAstarN:
path.append(s_key) # generate path
s = s_key # use end of this iteration as the start of next
if s_key not in h_value: # reach the expected node in U set
if s_key not in h_value: # reach the expected node in OPEN set
return s_key, path
def iteration(self, CLOSED):
@@ -92,7 +92,7 @@ class LrtAstarN:
return h_value
def Astar(self, x_start, N):
OPEN = queue.QueuePrior() # U set
OPEN = queue.QueuePrior() # OPEN set
OPEN.put(x_start, self.h(x_start))
CLOSED = [] # CLOSED set
g_table = {x_start: 0, self.s_goal: float("inf")} # cost to come
Search-based Planning/Search_2D/RTAAstar.py
+2 -2
View File
@@ -87,7 +87,7 @@ class RtaAstar:
return h_value
def Astar(self, x_start, N):
OPEN = queue.QueuePrior() # U set
OPEN = queue.QueuePrior() # OPEN set
OPEN.put(x_start, self.h_table[x_start])
CLOSED = [] # CLOSED set
g_table = {x_start: 0, self.s_goal: float("inf")} # cost to come
@@ -149,7 +149,7 @@ class RtaAstar:
path.append(s_key) # generate path
s = s_key # use end of this iteration as the start of next
if s_key == s_end: # reach the expected node in U set
if s_key == s_end: # reach the expected node in OPEN set
return s_start, list(reversed(path))
def extract_path(self, x_start, parent):
Search-based Planning/Search_2D/bfs.py
+1 -1
View File
@@ -24,7 +24,7 @@ class BFS:
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.OPEN = queue.QueueFIFO() # U set: visited nodes
self.OPEN = queue.QueueFIFO() # OPEN set: visited nodes
self.OPEN.put(self.s_start)
self.CLOSED = [] # CLOSED set: explored nodes
self.PARENT = {self.s_start: self.s_start}
Search-based Planning/Search_2D/bidirectional_a_star.py
+2 -2
View File
@@ -28,10 +28,10 @@ class BidirectionalAstar:
self.g_fore = {self.s_start: 0, self.s_goal: float("inf")} # cost to come: from s_start
self.g_back = {self.s_goal: 0, self.s_start: float("inf")} # cost to come: form s_goal
self.OPEN_fore = queue.QueuePrior() # U set for foreward searching
self.OPEN_fore = queue.QueuePrior() # OPEN set for foreward searching
self.OPEN_fore.put(self.s_start,
self.g_fore[self.s_start] + self.h(self.s_start, self.s_goal))
self.OPEN_back = queue.QueuePrior() # U set for backward searching
self.OPEN_back = queue.QueuePrior() # OPEN set for backward searching
self.OPEN_back.put(self.s_goal,
self.g_back[self.s_goal] + self.h(self.s_goal, self.s_start))
Search-based Planning/Search_2D/dijkstra.py
+1 -1
View File
@@ -26,7 +26,7 @@ class Dijkstra:
self.obs = self.Env.obs # position of obstacles
self.g = {self.s_start: 0, self.s_goal: float("inf")} # cost to come
self.OPEN = queue.QueuePrior() # priority queue / U set
self.OPEN = queue.QueuePrior() # priority queue / OPEN set
self.OPEN.put(self.s_start, 0)
self.CLOSED = [] # closed set & visited
self.PARENT = {self.s_start: self.s_start}
Search-based Planning/Search_3D/Astar3D.py
+1 -1
View File
@@ -75,7 +75,7 @@ class Weighted_A_star(object):
# update priority of xj
self.OPEN.put(strxj, a + 1 * self.h[strxj])
else:
# add xj in to U set
# add xj in to OPEN set
self.OPEN.put(strxj, a + 1 * self.h[strxj])
# For specified expanded nodes, used primarily in LRTA*
if N:
Search-based Planning/Search_3D/LRT_Astar3D.py
+1 -1
View File
@@ -48,7 +48,7 @@ class LRT_A_star2:
st = self.Astar.start
ind = 0
# find the lowest path down hill
while strst in self.Astar.CLOSED: # when minchild in CLOSED then continue, when minchild in U, stop
while strst in self.Astar.CLOSED: # when minchild in CLOSED then continue, when minchild in OPEN, stop
# strChildren = self.children(st)
strChildren = [hash3D(i) for i in self.Astar.children(st)]
minh, minchild = np.inf, None