saberbud/PathPlanning
1
0
Fork 0
mirror of https://github.com/zhm-real/PathPlanning.git synced 2026-08-31 01:20:51 +08:00
Code Issues Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
zhm-real
2020-08-10 12:22:06 -07:00
parent b1ccf848b4
commit 7bcd3f24a6
11 changed files with 131 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files
README.md
+2 -2
View File
@@ -50,8 +50,8 @@ Directory Structure
<div align=right>
<table>
<tr>
<td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/Astar.gif" alt="astar" width="400"/></a></td>
<td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/Bi-Astar.gif" alt="biastar" width="400"/></a></td>
<td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/AStar.gif" alt="astar" width="400"/></a></td>
<td><img src="https://github.com/zhm-real/path-planning-algorithms/blob/master/Search_based_Planning/gif/Bi-AStar.gif" alt="biastar" width="400"/></a></td>
</tr>
</table>
<table>
Search_based_Planning/Search_2D/Astar.py
+13 -13
View File
@@ -20,15 +20,15 @@ class AStar:
self.s_goal = s_goal
self.heuristic_type = heuristic_type
self.Env = env.Env() # class Env
self.Env = env.Env() # class Env
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.OPEN = [] # priority queue / OPEN set
self.CLOSED = [] # CLOSED set / VISITED order
self.PARENT = dict() # recorded parent
self.g = dict() # cost to come
self.OPEN = [] # priority queue / OPEN set
self.CLOSED = [] # CLOSED set / VISITED order
self.PARENT = dict() # recorded parent
self.g = dict() # cost to come
def searching(self):
"""
@@ -46,7 +46,7 @@ class AStar:
_, s = heapq.heappop(self.OPEN)
self.CLOSED.append(s)
if s == self.s_goal: # stop condition
if s == self.s_goal: # stop condition
break
for s_n in self.get_neighbor(s):
@@ -55,7 +55,7 @@ class AStar:
if s_n not in self.g:
self.g[s_n] = math.inf
if new_cost < self.g[s_n]: # conditions for updating Cost
if new_cost < self.g[s_n]: # conditions for updating Cost
self.g[s_n] = new_cost
self.PARENT[s_n] = s
heapq.heappush(self.OPEN, (self.f_value(s_n), s_n))
@@ -108,7 +108,7 @@ class AStar:
if s_n not in g:
g[s_n] = math.inf
if new_cost < g[s_n]: # conditions for updating Cost
if new_cost < g[s_n]: # conditions for updating Cost
g[s_n] = new_cost
PARENT[s_n] = s
heapq.heappush(OPEN, (g[s_n] + e * self.heuristic(s_n), s_n))
@@ -196,8 +196,8 @@ class AStar:
:return: heuristic function value
"""
heuristic_type = self.heuristic_type # heuristic type
goal = self.s_goal # goal node
heuristic_type = self.heuristic_type # heuristic type
goal = self.s_goal # goal node
if heuristic_type == "manhattan":
return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
@@ -213,7 +213,7 @@ def main():
plot = plotting.Plotting(s_start, s_goal)
path, visited = astar.searching()
plot.animation(path, visited, "A*") # animation
plot.animation(path, visited, "A*") # animation
# path, visited = astar.searching_repeated_astar(2.5) # initial weight e = 2.5
# plot.animation_ara_star(path, visited, "Repeated A*")
Search_based_Planning/Search_2D/Best_First.py
+6 -6
View File
@@ -22,12 +22,12 @@ class BestFirst:
self.Env = env.Env()
self.plotting = plotting.Plotting(self.s_start, self.s_goal)
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.OPEN = [] # OPEN set: visited nodes
self.CLOSED = [] # CLOSED set / visited order
self.PARENT = dict() # recorded parent
self.OPEN = [] # OPEN set: visited nodes
self.CLOSED = [] # CLOSED set / visited order
self.PARENT = dict() # recorded parent
def searching(self):
"""
@@ -50,7 +50,7 @@ class BestFirst:
if self.is_collision(s, s_n):
continue
if s_n not in self.PARENT: # node not explored
if s_n not in self.PARENT: # node not explored
heapq.heappush(self.OPEN, (self.heuristic(s_n), s_n))
self.PARENT[s_n] = s
Search_based_Planning/Search_2D/Bidirectional_a_star.py
+12 -12
View File
@@ -20,19 +20,19 @@ class BidirectionalAStar:
self.s_goal = s_goal
self.heuristic_type = heuristic_type
self.Env = env.Env() # class Env
self.Env = env.Env() # class Env
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.OPEN_fore = [] # OPEN set for forward searching
self.OPEN_back = [] # OPEN set for backward searching
self.CLOSED_fore = [] # CLOSED set for forward
self.CLOSED_back = [] # CLOSED set for backward
self.PARENT_fore = dict() # recorded parent for forward
self.PARENT_back = dict() # recorded parent for backward
self.g_fore = dict() # cost to come for forward
self.g_back = dict() # cost to come for backward
self.OPEN_fore = [] # OPEN set for forward searching
self.OPEN_back = [] # OPEN set for backward searching
self.CLOSED_fore = [] # CLOSED set for forward
self.CLOSED_back = [] # CLOSED set for backward
self.PARENT_fore = dict() # recorded parent for forward
self.PARENT_back = dict() # recorded parent for backward
self.g_fore = dict() # cost to come for forward
self.g_back = dict() # cost to come for backward
def init(self):
"""
@@ -220,7 +220,7 @@ def main():
bastar = BidirectionalAStar(x_start, x_goal, "euclidean")
plot = plotting.Plotting(x_start, x_goal)
path, visited_fore, visited_back = bastar.searching()
plot.animation_bi_astar(path, visited_fore, visited_back, "Bidirectional-A*") # animation
Search_based_Planning/Search_2D/D_star.py
+1 -2
View File
@@ -11,8 +11,7 @@ import matplotlib.pyplot as plt
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../Search_based_Planning/")
from Search_2D import plotting
from Search_2D import env
from Search_based_Planning.Search_2D import plotting, env
class Dstar:
Search_based_Planning/Search_2D/Dijkstra.py
+7 -7
View File
@@ -22,13 +22,13 @@ class Dijkstra:
self.Env = env.Env()
self.plotting = plotting.Plotting(self.s_start, self.s_goal)
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.OPEN = [] # priority queue / OPEN set
self.CLOSED = [] # closed set & visited
self.PARENT = dict() # record parent
self.g = dict() # Cost to come
self.OPEN = [] # priority queue / OPEN set
self.CLOSED = [] # closed set & visited
self.PARENT = dict() # record parent
self.g = dict() # Cost to come
def searching(self):
"""
@@ -132,7 +132,7 @@ def main():
plot = plotting.Plotting(s_start, s_goal)
path, visited = dijkstra.searching()
plot.animation(path, visited, "Dijkstra's") # animation generate
plot.animation(path, visited, "Dijkstra's") # animation generate
if __name__ == '__main__':
Search_based_Planning/Search_2D/LRTAstar.py
+41 -34
View File
@@ -11,46 +11,51 @@ import math
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
from Search_based_Planning.Search_2D import queue, plotting, env
class LrtAstarN:
class LrtAStarN:
def __init__(self, s_start, s_goal, N, heuristic_type):
self.s_start, self.s_goal = s_start, s_goal
self.heuristic_type = heuristic_type
self.Env = env.Env()
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
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 = {}
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 = {} # h_value table
def init(self):
"""
initialize the h_value of all nodes in the environment.
it is a global 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
self.h_table[(i, j)] = self.h((i, j))
def searching(self):
s_start = self.s_start # initialize start node
self.init()
s_start = self.s_start # initialize start node
while True:
OPEN, CLOSED = self.Astar(s_start, self.N) # OPEN, 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
if OPEN == "FOUND": # reach the goal node
self.path.append(CLOSED)
break
h_value = self.iteration(CLOSED) # h_value table of CLOSED nodes
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) # x_init -> expected node in OPEN set
s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value) # x_init -> expected node in OPEN set
self.path.append(path_k)
def extract_path_in_CLOSE(self, s_start, h_value):
@@ -59,23 +64,25 @@ class LrtAstarN:
while True:
h_list = {}
for s_n in self.get_neighbor(s):
if s_n in h_value:
h_list[s_n] = h_value[s_n]
else:
h_list[s_n] = self.h_table[s_n]
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
if s_key not in h_value: # reach the expected node in OPEN set
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
if s_key not in h_value: # reach the expected node in OPEN set
return s_key, path
def iteration(self, CLOSED):
h_value = {}
for s in CLOSED:
h_value[s] = float("inf") # initialize h_value of CLOSED nodes
h_value[s] = float("inf") # initialize h_value of CLOSED nodes
while True:
h_value_rec = copy.deepcopy(h_value)
@@ -86,25 +93,25 @@ class LrtAstarN:
h_list.append(self.cost(s, s_n) + self.h_table[s_n])
else:
h_list.append(self.cost(s, s_n) + h_value[s_n])
h_value[s] = min(h_list) # update h_value of current node
h_value[s] = min(h_list) # update h_value of current node
if h_value == h_value_rec: # h_value table converged
if h_value == h_value_rec: # h_value table converged
return h_value
def Astar(self, x_start, N):
OPEN = queue.QueuePrior() # OPEN set
def AStar(self, x_start, N):
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
PARENT = {x_start: x_start} # relations
count = 0 # counter
CLOSED = [] # CLOSED set
g_table = {x_start: 0, self.s_goal: float("inf")} # Cost to come
PARENT = {x_start: x_start} # relations
count = 0 # counter
while not OPEN.empty():
count += 1
s = OPEN.get()
CLOSED.append(s)
if s == self.s_goal: # reach the goal node
if s == self.s_goal: # reach the goal node
self.visited.append(CLOSED)
return "FOUND", self.extract_path(x_start, PARENT)
@@ -113,15 +120,15 @@ class LrtAstarN:
new_cost = g_table[s] + self.cost(s, s_n)
if s_n not in g_table:
g_table[s_n] = float("inf")
if new_cost < g_table[s_n]: # conditions for updating Cost
if new_cost < g_table[s_n]: # conditions for updating Cost
g_table[s_n] = new_cost
PARENT[s_n] = s
OPEN.put(s_n, g_table[s_n] + self.h_table[s_n])
if count == N: # expand needed CLOSED nodes
if count == N: # expand needed CLOSED nodes
break
self.visited.append(CLOSED) # visited nodes in each iteration
self.visited.append(CLOSED) # visited nodes in each iteration
return OPEN, CLOSED
@@ -211,7 +218,7 @@ def main():
s_start = (10, 5)
s_goal = (45, 25)
lrta = LrtAstarN(s_start, s_goal, 250, "euclidean")
lrta = LrtAStarN(s_start, s_goal, 250, "euclidean")
plot = plotting.Plotting(s_start, s_goal)
lrta.searching()
Search_based_Planning/Search_2D/RTAAstar.py → Search_based_Planning/Search_2D/RTAAStar.py
+37 -32
View File
@@ -11,38 +11,43 @@ import math
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
from Search_based_Planning.Search_2D import queue, plotting, env
class RtaAstar:
class RTAAStar:
def __init__(self, s_start, s_goal, N, heuristic_type):
self.s_start, self.s_goal = s_start, s_goal
self.heuristic_type = heuristic_type
self.Env = env.Env()
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
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 = {}
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 = {} # h_value table
def init(self):
"""
initialize the h_value of all nodes in the environment.
it is a global 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
self.h_table[(i, j)] = self.h((i, j))
def searching(self):
s_start = self.s_start # initialize start node
self.init()
s_start = self.s_start # initialize start node
while True:
OPEN, CLOSED, g_table, PARENT = \
self.Astar(s_start, self.N)
if OPEN == "FOUND": # reach the goal node
if OPEN == "FOUND": # reach the goal node
self.path.append(CLOSED)
break
@@ -70,7 +75,7 @@ class RtaAstar:
h_value = {}
for s in CLOSED:
h_value[s] = float("inf") # initialize h_value of CLOSED nodes
h_value[s] = float("inf") # initialize h_value of CLOSED nodes
while True:
h_value_rec = copy.deepcopy(h_value)
@@ -81,25 +86,25 @@ class RtaAstar:
h_list.append(self.cost(s, s_n) + self.h_table[s_n])
else:
h_list.append(self.cost(s, s_n) + h_value[s_n])
h_value[s] = min(h_list) # update h_value of current node
h_value[s] = min(h_list) # update h_value of current node
if h_value == h_value_rec: # h_value table converged
if h_value == h_value_rec: # h_value table converged
return h_value
def Astar(self, x_start, N):
OPEN = queue.QueuePrior() # OPEN 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
PARENT = {x_start: x_start} # relations
count = 0 # counter
CLOSED = [] # CLOSED set
g_table = {x_start: 0, self.s_goal: float("inf")} # Cost to come
PARENT = {x_start: x_start} # relations
count = 0 # counter
while not OPEN.empty():
count += 1
s = OPEN.get()
CLOSED.append(s)
if s == self.s_goal: # reach the goal node
if s == self.s_goal: # reach the goal node
self.visited.append(CLOSED)
return "FOUND", self.extract_path(x_start, PARENT), [], []
@@ -108,15 +113,15 @@ class RtaAstar:
new_cost = g_table[s] + self.cost(s, s_n)
if s_n not in g_table:
g_table[s_n] = float("inf")
if new_cost < g_table[s_n]: # conditions for updating Cost
if new_cost < g_table[s_n]: # conditions for updating Cost
g_table[s_n] = new_cost
PARENT[s_n] = s
OPEN.put(s_n, g_table[s_n] + self.h_table[s_n])
if count == N: # expand needed CLOSED nodes
if count == N: # expand needed CLOSED nodes
break
self.visited.append(CLOSED) # visited nodes in each iteration
self.visited.append(CLOSED) # visited nodes in each iteration
return OPEN, CLOSED, g_table, PARENT
@@ -145,11 +150,11 @@ class RtaAstar:
for s_n in self.get_neighbor(s):
if s_n in h_value:
h_list[s_n] = h_value[s_n]
s_key = max(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
s_key = max(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
if s_key == s_end: # reach the expected node in OPEN 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):
@@ -176,8 +181,8 @@ class RtaAstar:
:return: heuristic function value
"""
heuristic_type = self.heuristic_type # heuristic type
goal = self.s_goal # goal node
heuristic_type = self.heuristic_type # heuristic type
goal = self.s_goal # goal node
if heuristic_type == "manhattan":
return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
@@ -220,7 +225,7 @@ def main():
s_start = (10, 5)
s_goal = (45, 25)
rtaa = RtaAstar(s_start, s_goal, 240, "euclidean")
rtaa = RTAAStar(s_start, s_goal, 240, "euclidean")
plot = plotting.Plotting(s_start, s_goal)
rtaa.searching()
Search_based_Planning/Search_2D/__pycache__/queue.cpython-37.pyc
BIN
View File
Binary file not shown.
Search_based_Planning/Search_2D/bfs.py
+6 -6
View File
@@ -21,12 +21,12 @@ class BFS:
self.Env = env.Env()
self.plotting = plotting.Plotting(self.s_start, self.s_goal)
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.OPEN = deque() # OPEN set: visited nodes
self.PARENT = dict() # recorded parent
self.CLOSED = [] # CLOSED set: explored nodes
self.OPEN = deque() # OPEN set: visited nodes
self.PARENT = dict() # recorded parent
self.CLOSED = [] # CLOSED set: explored nodes
def searching(self):
"""
@@ -47,7 +47,7 @@ class BFS:
for s_n in self.get_neighbor(s):
if self.is_collision(s, s_n):
continue
if s_n not in self.PARENT: # node not explored
if s_n not in self.PARENT: # node not explored
self.OPEN.append(s_n)
self.PARENT[s_n] = s
Search_based_Planning/Search_2D/dfs.py
+6 -6
View File
@@ -21,12 +21,12 @@ class DFS:
self.Env = env.Env()
self.plotting = plotting.Plotting(self.s_start, self.s_goal)
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.OPEN = deque() # OPEN set: visited nodes
self.PARENT = dict() # recorded parent
self.CLOSED = [] # CLOSED set / visited order
self.OPEN = deque() # OPEN set: visited nodes
self.PARENT = dict() # recorded parent
self.CLOSED = [] # CLOSED set / visited order
def searching(self):
"""
@@ -47,7 +47,7 @@ class DFS:
for s_n in self.get_neighbor(s):
if self.is_collision(s, s_n):
continue
if s_n not in self.PARENT: # node not explored
if s_n not in self.PARENT: # node not explored
self.OPEN.append(s_n)
self.PARENT[s_n] = s