diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..26d3352 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,3 @@ +# Default ignored files +/shelf/ +/workspace.xml diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..6c993b7 --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000..e889cbd --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/path-planning-algorithms.iml b/.idea/path-planning-algorithms.iml new file mode 100644 index 0000000..8b8c395 --- /dev/null +++ b/.idea/path-planning-algorithms.iml @@ -0,0 +1,12 @@ + + + + + + + + + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 0000000..94a25f7 --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/Search-based Planning/.idea/Search-based Planning.iml b/Search-based Planning/.idea/Search-based Planning.iml index f3d7bc9..fee2751 100644 --- a/Search-based Planning/.idea/Search-based Planning.iml +++ b/Search-based Planning/.idea/Search-based Planning.iml @@ -2,10 +2,7 @@ - + - - \ No newline at end of file diff --git a/Search-based Planning/.idea/misc.xml b/Search-based Planning/.idea/misc.xml index a2e120d..6c993b7 100644 --- a/Search-based Planning/.idea/misc.xml +++ b/Search-based Planning/.idea/misc.xml @@ -1,4 +1,4 @@ - + \ No newline at end of file diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index 54e7b9d..efd2fab 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -1,8 +1,18 @@ - - + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Search-based Planning/__pycache__/env.cpython-37.pyc b/Search-based Planning/__pycache__/env.cpython-37.pyc index 7228c9d..d07c692 100644 Binary files a/Search-based Planning/__pycache__/env.cpython-37.pyc and b/Search-based Planning/__pycache__/env.cpython-37.pyc differ diff --git a/Search-based Planning/__pycache__/plotting.cpython-35.pyc b/Search-based Planning/__pycache__/plotting.cpython-35.pyc new file mode 100644 index 0000000..f7c217b Binary files /dev/null and b/Search-based Planning/__pycache__/plotting.cpython-35.pyc differ diff --git a/Search-based Planning/__pycache__/queue.cpython-35.pyc b/Search-based Planning/__pycache__/queue.cpython-35.pyc new file mode 100644 index 0000000..0d948ad Binary files /dev/null and b/Search-based Planning/__pycache__/queue.cpython-35.pyc differ diff --git a/Search-based Planning/a_star.py b/Search-based Planning/a_star.py index 892e773..0d0e088 100644 --- a/Search-based Planning/a_star.py +++ b/Search-based Planning/a_star.py @@ -1,28 +1,22 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -@author: huiming zhou -""" - import queue import plotting import env + class Astar: def __init__(self, x_start, x_goal, heuristic_type): self.xI, self.xG = x_start, x_goal - self.Env = env.Env() # class Env - self.plotting = plotting.Plotting(self.xI, self.xG) # class Plotting + self.Env = env.Env() # class Env + self.plotting = plotting.Plotting(self.xI, self.xG) # class Plotting - 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.path, self.policy, self.visited] = self.searching(self.xI, self.xG, heuristic_type) self.fig_name = "A* Algorithm" - self.plotting.animation(self.path, self.visited, self.fig_name) # animation generate - + self.plotting.animation(self.path, self.visited, self.fig_name) # animation generate def searching(self, xI, xG, heuristic_type): """ @@ -31,33 +25,32 @@ class Astar: :return: planning path, action in each node, visited nodes in the planning process """ - q_astar = queue.QueuePrior() # priority queue + q_astar = queue.QueuePrior() # priority queue q_astar.put(xI, 0) - parent = {xI: xI} # record parents of nodes - action = {xI: (0, 0)} # record actions of nodes + parent = {xI: xI} # record parents of nodes + action = {xI: (0, 0)} # record actions of nodes visited = [] cost = {xI: 0} while not q_astar.empty(): x_current = q_astar.get() - if x_current == xG: # stop condition + if x_current == xG: # stop condition break visited.append(x_current) - for u_next in self.u_set: # explore neighborhoods of current node + for u_next in self.u_set: # explore neighborhoods of current node x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) if x_next not in self.obs: new_cost = cost[x_current] + self.get_cost(x_current, u_next) - if x_next not in cost or new_cost < cost[x_next]: # conditions for updating cost + if x_next not in cost or new_cost < cost[x_next]: # conditions for updating cost cost[x_next] = new_cost priority = new_cost + self.Heuristic(x_next, xG, heuristic_type) - q_astar.put(x_next, priority) # put node into queue using priority "f+h" + q_astar.put(x_next, priority) # put node into queue using priority "f+h" parent[x_next], action[x_next] = x_current, u_next [path, policy] = self.extract_path(xI, xG, parent, action) return path, policy, visited - def extract_path(self, xI, xG, parent, policy): """ Extract the path based on the relationship of nodes. @@ -80,7 +73,6 @@ class Astar: return list(path_back), list(acts_back) - def get_cost(self, x, u): """ Calculate cost for this motion @@ -93,7 +85,6 @@ class Astar: return 1 - def Heuristic(self, state, goal, heuristic_type): """ Calculate heuristic. @@ -116,4 +107,4 @@ if __name__ == '__main__': x_Start = (5, 5) # Starting node x_Goal = (49, 5) # Goal node - astar = Astar(x_Start, x_Goal, "manhattan") \ No newline at end of file + astar = Astar(x_Start, x_Goal, "manhattan") diff --git a/Search-based Planning/bfs.py b/Search-based Planning/bfs.py index 991912d..a7aa94c 100644 --- a/Search-based Planning/bfs.py +++ b/Search-based Planning/bfs.py @@ -1,13 +1,8 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -@author: huiming zhou -""" - import queue import plotting import env + class BFS: def __init__(self, x_start, x_goal): self.xI, self.xG = x_start, x_goal @@ -15,14 +10,13 @@ class BFS: self.Env = env.Env() self.plotting = plotting.Plotting(self.xI, self.xG) - 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.path, self.policy, self.visited] = self.searching(self.xI, self.xG) self.fig_name = "Breadth-first Searching" - self.plotting.animation(self.path, self.visited, self.fig_name) # animation generate - + self.plotting.animation(self.path, self.visited, self.fig_name) # animation generate def searching(self, xI, xG): """ @@ -31,10 +25,10 @@ class BFS: :return: planning path, action in each node, visited nodes in the planning process """ - q_bfs = queue.QueueFIFO() # first-in-first-out queue + q_bfs = queue.QueueFIFO() # first-in-first-out queue q_bfs.put(xI) - parent = {xI: xI} # record parents of nodes - action = {xI: (0, 0)} # record actions of nodes + parent = {xI: xI} # record parents of nodes + action = {xI: (0, 0)} # record actions of nodes visited = [] while not q_bfs.empty(): @@ -42,17 +36,16 @@ class BFS: if x_current == xG: break visited.append(x_current) - for u_next in self.u_set: # explore neighborhoods of current node + for u_next in self.u_set: # explore neighborhoods of current node x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) - if x_next not in parent and x_next not in self.obs: # node not visited and not in obstacles + if x_next not in parent and x_next not in self.obs: # node not visited and not in obstacles q_bfs.put(x_next) parent[x_next], action[x_next] = x_current, u_next - [path, policy] = self.extract_path(xI, xG, parent, action) # extract path + [path, policy] = self.extract_path(xI, xG, parent, action) # extract path return path, policy, visited - def extract_path(self, xI, xG, parent, policy): """ Extract the path based on the relationship of nodes. @@ -77,6 +70,6 @@ class BFS: if __name__ == '__main__': - x_Start = (5, 5) # Starting node - x_Goal = (49, 5) # Goal node + x_Start = (5, 5) # Starting node + x_Goal = (49, 5) # Goal node bfs = BFS(x_Start, x_Goal) diff --git a/Search-based Planning/dfs.py b/Search-based Planning/dfs.py index 435512d..d3ccae6 100644 --- a/Search-based Planning/dfs.py +++ b/Search-based Planning/dfs.py @@ -1,13 +1,8 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -@author: huiming zhou -""" - import queue import plotting import env + class DFS: def __init__(self, x_start, x_goal): self.xI, self.xG = x_start, x_goal @@ -15,14 +10,13 @@ class DFS: self.Env = env.Env() self.plotting = plotting.Plotting(self.xI, self.xG) - 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.path, self.policy, self.visited] = self.searching(self.xI, self.xG) self.fig_name = "Depth-first Searching" - self.plotting.animation(self.path, self.visited, self.fig_name) # animation generate - + self.plotting.animation(self.path, self.visited, self.fig_name) # animation generate def searching(self, xI, xG): """ @@ -31,10 +25,10 @@ class DFS: :return: planning path, action in each node, visited nodes in the planning process """ - q_dfs = queue.QueueLIFO() # last-in-first-out queue + q_dfs = queue.QueueLIFO() # last-in-first-out queue q_dfs.put(xI) - parent = {xI: xI} # record parents of nodes - action = {xI: (0, 0)} # record actions of nodes + parent = {xI: xI} # record parents of nodes + action = {xI: (0, 0)} # record actions of nodes visited = [] while not q_dfs.empty(): @@ -42,9 +36,9 @@ class DFS: if x_current == xG: break visited.append(x_current) - for u_next in self.u_set: # explore neighborhoods of current node + for u_next in self.u_set: # explore neighborhoods of current node x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) - if x_next not in parent and x_next not in self.obs: # node not visited and not in obstacles + if x_next not in parent and x_next not in self.obs: # node not visited and not in obstacles q_dfs.put(x_next) parent[x_next], action[x_next] = x_current, u_next @@ -52,7 +46,6 @@ class DFS: return path, policy, visited - def extract_path(self, xI, xG, parent, policy): """ Extract the path based on the relationship of nodes. @@ -77,6 +70,6 @@ class DFS: if __name__ == '__main__': - x_Start = (5, 5) # Starting node - x_Goal = (49, 5) # Goal node + x_Start = (5, 5) # Starting node + x_Goal = (49, 5) # Goal node dfs = DFS(x_Start, x_Goal) diff --git a/Search-based Planning/dijkstra.py b/Search-based Planning/dijkstra.py index 290d063..7110f3b 100644 --- a/Search-based Planning/dijkstra.py +++ b/Search-based Planning/dijkstra.py @@ -1,13 +1,8 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -@author: huiming zhou -""" - import queue import env import plotting + class Dijkstra: def __init__(self, x_start, x_goal): self.xI, self.xG = x_start, x_goal @@ -15,14 +10,13 @@ class Dijkstra: self.Env = env.Env() self.plotting = plotting.Plotting(self.xI, self.xG) - 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.path, self.policy, self.visited] = self.searching(self.xI, self.xG) self.fig_name = "Dijkstra's Algorithm" - self.plotting.animation(self.path, self.visited, self.fig_name) # animation generate - + self.plotting.animation(self.path, self.visited, self.fig_name) # animation generate def searching(self, xI, xG): """ @@ -31,33 +25,32 @@ class Dijkstra: :return: planning path, action in each node, visited nodes in the planning process """ - q_dijk = queue.QueuePrior() # priority queue + q_dijk = queue.QueuePrior() # priority queue q_dijk.put(xI, 0) - parent = {xI: xI} # record parents of nodes - action = {xI: (0, 0)} # record actions of nodes - visited = [] # record visited nodes + parent = {xI: xI} # record parents of nodes + action = {xI: (0, 0)} # record actions of nodes + visited = [] # record visited nodes cost = {xI: 0} while not q_dijk.empty(): x_current = q_dijk.get() - if x_current == xG: # stop condition + if x_current == xG: # stop condition break visited.append(x_current) - for u_next in self.u_set: # explore neighborhoods of current node + for u_next in self.u_set: # explore neighborhoods of current node x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) - if x_next not in self.obs: # node not visited and not in obstacles + if x_next not in self.obs: # node not visited and not in obstacles new_cost = cost[x_current] + self.get_cost(x_current, u_next) if x_next not in cost or new_cost < cost[x_next]: cost[x_next] = new_cost priority = new_cost - q_dijk.put(x_next, priority) # put node into queue using cost to come as priority + q_dijk.put(x_next, priority) # put node into queue using cost to come as priority parent[x_next], action[x_next] = x_current, u_next [path, policy] = self.extract_path(xI, xG, parent, action) return path, policy, visited - def get_cost(self, x, u): """ Calculate cost for this motion @@ -70,7 +63,6 @@ class Dijkstra: return 1 - def extract_path(self, xI, xG, parent, policy): """ Extract the path based on the relationship of nodes. @@ -95,6 +87,6 @@ class Dijkstra: if __name__ == '__main__': - x_Start = (5, 5) # Starting node - x_Goal = (49, 5) # Goal node + x_Start = (5, 5) # Starting node + x_Goal = (49, 5) # Goal node dijkstra = Dijkstra(x_Start, x_Goal) diff --git a/Search-based Planning/env.py b/Search-based Planning/env.py index 9ea6036..3168c74 100644 --- a/Search-based Planning/env.py +++ b/Search-based Planning/env.py @@ -1,11 +1,10 @@ -class Env(): +class Env: def __init__(self): - self.x_range = 51 # size of background + self.x_range = 51 # size of background self.y_range = 31 self.motions = [(1, 0), (-1, 0), (0, 1), (0, -1)] self.obs = self.obs_map() - def obs_map(self): """ Initialize obstacles' positions @@ -38,4 +37,3 @@ class Env(): obs.append((40, i)) return obs - diff --git a/Search-based Planning/plotting.py b/Search-based Planning/plotting.py index 75697c5..247d637 100644 --- a/Search-based Planning/plotting.py +++ b/Search-based Planning/plotting.py @@ -1,25 +1,18 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -@author: huiming zhou -""" - import matplotlib.pyplot as plt import env + class Plotting(): def __init__(self, xI, xG): self.xI, self.xG = xI, xG self.env = env.Env() self.obs = self.env.obs_map() - def animation(self, path, visited, name): self.plot_grid(name) self.plot_visited(visited) self.plot_path(path) - 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))] @@ -30,7 +23,6 @@ class Plotting(): plt.title(name) plt.axis("equal") - def plot_visited(self, visited): visited.remove(self.xI) count = 0 @@ -50,7 +42,6 @@ class Plotting(): if count % length == 0: plt.pause(0.001) - def plot_path(self, path): path.remove(self.xI) path.remove(self.xG) diff --git a/Search-based Planning/queue.py b/Search-based Planning/queue.py index 8b41446..6250cd3 100644 --- a/Search-based Planning/queue.py +++ b/Search-based Planning/queue.py @@ -1,13 +1,7 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- - -""" -@author: Huiming Zhou -""" - import collections import heapq + class QueueFIFO: """ Class: QueueFIFO @@ -21,10 +15,11 @@ class QueueFIFO: return len(self.queue) == 0 def put(self, node): - self.queue.append(node) # enter from back + self.queue.append(node) # enter from back def get(self): - return self.queue.popleft() # leave from front + return self.queue.popleft() # leave from front + class QueueLIFO: """ @@ -39,10 +34,11 @@ class QueueLIFO: return len(self.queue) == 0 def put(self, node): - self.queue.append(node) # enter from back + self.queue.append(node) # enter from back def get(self): - return self.queue.pop() # leave from back + return self.queue.pop() # leave from back + class QueuePrior: """ @@ -57,7 +53,7 @@ class QueuePrior: return len(self.queue) == 0 def put(self, item, priority): - heapq.heappush(self.queue, (priority, item)) # reorder x using priority + heapq.heappush(self.queue, (priority, item)) # reorder x using priority def get(self): - return heapq.heappop(self.queue)[1] # pop out the smallest item + return heapq.heappop(self.queue)[1] # pop out the smallest item