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

update

Browse Source
This commit is contained in:
zhm-real
2020-06-20 12:46:49 -07:00
parent 68edcb939e
commit 5ccc649dc1
13 changed files with 279 additions and 183 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Search-based Planning/.idea/workspace.xml
Generated
+12 -3
View File
@@ -1,7 +1,16 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<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$/a_star.py" beforeDir="false" afterPath="$PROJECT_DIR$/a_star.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/bfs.py" beforeDir="false" afterPath="$PROJECT_DIR$/bfs.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/dfs.py" beforeDir="false" afterPath="$PROJECT_DIR$/dfs.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/dijkstra.py" beforeDir="false" afterPath="$PROJECT_DIR$/dijkstra.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/env.py" beforeDir="false" afterPath="$PROJECT_DIR$/env.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/motion_model.py" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/tools.py" beforeDir="false" afterPath="$PROJECT_DIR$/plotting.py" afterDir="false" />
</list>
<option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
<option name="SHOW_DIALOG" value="false" />
<option name="HIGHLIGHT_CONFLICTS" value="true" />
@@ -23,7 +32,7 @@
<ConfirmationsSetting value="2" id="Add" />
</component>
<component name="PropertiesComponent">
<property name="last_opened_file_path" value="$PROJECT_DIR$" />
<property name="last_opened_file_path" value="$PROJECT_DIR$/../../test" />
<property name="restartRequiresConfirmation" value="false" />
<property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
</component>
@@ -148,9 +157,9 @@
<recent_temporary>
<list>
<item itemvalue="Python.a_star" />
<item itemvalue="Python.dijkstra" />
<item itemvalue="Python.dfs" />
<item itemvalue="Python.bfs" />
<item itemvalue="Python.dijkstra" />
<item itemvalue="Python.searching" />
</list>
</recent_temporary>
Search-based Planning/__pycache__/env.cpython-37.pyc
BIN
View File
Binary file not shown.
Search-based Planning/__pycache__/motion_model.cpython-37.pyc
BIN
View File
Binary file not shown.
Search-based Planning/__pycache__/plotting.cpython-37.pyc
BIN
View File
Binary file not shown.
Search-based Planning/__pycache__/tools.cpython-37.pyc
BIN
View File
Binary file not shown.
Search-based Planning/a_star.py
+47 -22
View File
@@ -5,22 +5,26 @@
"""
import queue
import tools
import plotting
import env
import motion_model
class Astar:
def __init__(self, x_start, x_goal, heuristic_type):
self.u_set = motion_model.motions # feasible input set
self.xI, self.xG = x_start, x_goal
self.obs = env.obs_map() # position of obstacles
self.heuristic_type = heuristic_type
tools.show_map(self.xI, self.xG, self.obs, "a_star searching")
self.Env = env.Env()
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"
plotting.animation(self.xI, self.xG, self.obs,
self.path, self.visited, self.fig_name) # animation generate
def searching(self):
def searching(self, xI, xG, heuristic_type):
"""
Searching using A_star.
@@ -28,30 +32,53 @@ class Astar:
"""
q_astar = queue.QueuePrior() # priority queue
q_astar.put(self.xI, 0)
parent = {self.xI: self.xI} # record parents of nodes
action = {self.xI: (0, 0)} # record actions of nodes
cost = {self.xI: 0}
q_astar.put(xI, 0)
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 == self.xG: # stop condition
if x_current == xG: # stop condition
break
if x_current != self.xI:
tools.plot_dots(x_current, len(parent))
visited.append(x_current)
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
cost[x_next] = new_cost
priority = new_cost + self.Heuristic(x_next, self.xG, self.heuristic_type)
priority = new_cost + self.Heuristic(x_next, xG, heuristic_type)
q_astar.put(x_next, priority) # put node into queue using priority "f+h"
parent[x_next] = x_current
action[x_next] = u_next
[path_astar, actions_astar] = tools.extract_path(self.xI, self.xG, parent, action)
parent[x_next], action[x_next] = x_current, u_next
return path_astar, actions_astar
[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.
:param xI: Starting node
:param xG: Goal node
:param parent: Relationship between nodes
:param policy: Action needed for transfer between two nodes
:return: The planning path
"""
path_back = [xG]
acts_back = [policy[xG]]
x_current = xG
while True:
x_current = parent[x_current]
path_back.append(x_current)
acts_back.append(policy[x_current])
if x_current == xI: break
return list(path_back), list(acts_back)
def get_cost(self, x, u):
@@ -88,6 +115,4 @@ class Astar:
if __name__ == '__main__':
x_Start = (5, 5) # Starting node
x_Goal = (49, 5) # Goal node
astar = Astar(x_Start, x_Goal, "manhattan")
[path_astar, actions_astar] = astar.searching()
tools.showPath(x_Start, x_Goal, path_astar)
astar = Astar(x_Start, x_Goal, "manhattan")
Search-based Planning/bfs.py
+43 -18
View File
@@ -5,21 +5,25 @@
"""
import queue
import tools
import plotting
import env
import motion_model
class BFS:
def __init__(self, x_start, x_goal):
self.u_set = motion_model.motions # feasible input set
self.xI, self.xG = x_start, x_goal
self.obs = env.obs_map() # position of obstacles
tools.show_map(self.xI, self.xG, self.obs, "breadth-first searching")
self.Env = env.Env()
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"
plotting.animation(self.xI, self.xG, self.obs,
self.path, self.visited, self.fig_name) # animation generate
def searching(self):
def searching(self, xI, xG):
"""
Searching using BFS.
@@ -27,30 +31,51 @@ class BFS:
"""
q_bfs = queue.QueueFIFO() # first-in-first-out queue
q_bfs.put(self.xI)
parent = {self.xI: self.xI} # record parents of nodes
action = {self.xI: (0, 0)} # record actions of nodes
q_bfs.put(xI)
parent = {xI: xI} # record parents of nodes
action = {xI: (0, 0)} # record actions of nodes
visited = []
while not q_bfs.empty():
x_current = q_bfs.get()
if x_current == self.xG:
if x_current == xG:
break
if x_current != self.xI:
tools.plot_dots(x_current, len(parent))
visited.append(x_current)
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
q_bfs.put(x_next)
parent[x_next] = x_current
action[x_next] = u_next
[path_bfs, action_bfs] = tools.extract_path(self.xI, self.xG, parent, action) # extract path
parent[x_next], action[x_next] = x_current, u_next
return path_bfs, action_bfs
[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.
:param xI: Starting node
:param xG: Goal node
:param parent: Relationship between nodes
:param policy: Action needed for transfer between two nodes
:return: The planning path
"""
path_back = [xG]
acts_back = [policy[xG]]
x_current = xG
while True:
x_current = parent[x_current]
path_back.append(x_current)
acts_back.append(policy[x_current])
if x_current == xI: break
return list(path_back), list(acts_back)
if __name__ == '__main__':
x_Start = (5, 5) # Starting node
x_Goal = (49, 5) # Goal node
bfs = BFS(x_Start, x_Goal)
[path_bf, actions_bf] = bfs.searching()
tools.showPath(x_Start, x_Goal, path_bf)
Search-based Planning/dfs.py
+43 -18
View File
@@ -5,21 +5,25 @@
"""
import queue
import tools
import plotting
import env
import motion_model
class DFS:
def __init__(self, x_start, x_goal):
self.u_set = motion_model.motions # feasible input set
self.xI, self.xG = x_start, x_goal
self.obs = env.obs_map() # position of obstacles
tools.show_map(self.xI, self.xG, self.obs, "depth-first searching")
self.Env = env.Env()
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"
plotting.animation(self.xI, self.xG, self.obs,
self.path, self.visited, self.fig_name) # animation generate
def searching(self):
def searching(self, xI, xG):
"""
Searching using DFS.
@@ -27,30 +31,51 @@ class DFS:
"""
q_dfs = queue.QueueLIFO() # last-in-first-out queue
q_dfs.put(self.xI)
parent = {self.xI: self.xI} # record parents of nodes
action = {self.xI: (0, 0)} # record actions of nodes
q_dfs.put(xI)
parent = {xI: xI} # record parents of nodes
action = {xI: (0, 0)} # record actions of nodes
visited = []
while not q_dfs.empty():
x_current = q_dfs.get()
if x_current == self.xG:
if x_current == xG:
break
if x_current != self.xI:
tools.plot_dots(x_current, len(parent))
visited.append(x_current)
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
q_dfs.put(x_next)
parent[x_next] = x_current
action[x_next] = u_next
[path_dfs, action_dfs] = tools.extract_path(self.xI, self.xG, parent, action)
parent[x_next], action[x_next] = x_current, u_next
return path_dfs, action_dfs
[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.
:param xI: Starting node
:param xG: Goal node
:param parent: Relationship between nodes
:param policy: Action needed for transfer between two nodes
:return: The planning path
"""
path_back = [xG]
acts_back = [policy[xG]]
x_current = xG
while True:
x_current = parent[x_current]
path_back.append(x_current)
acts_back.append(policy[x_current])
if x_current == xI: break
return list(path_back), list(acts_back)
if __name__ == '__main__':
x_Start = (5, 5) # Starting node
x_Goal = (49, 5) # Goal node
dfs = DFS(x_Start, x_Goal)
[path_dfs, action_dfs] = dfs.searching()
tools.showPath(x_Start, x_Goal, path_dfs)
Search-based Planning/dijkstra.py
+48 -23
View File
@@ -6,51 +6,55 @@
import queue
import env
import tools
import motion_model
import plotting
class Dijkstra:
def __init__(self, x_start, x_goal):
self.u_set = motion_model.motions # feasible input set
self.xI, self.xG = x_start, x_goal
self.obs = env.obs_map() # position of obstacles
tools.show_map(self.xI, self.xG, self.obs, "dijkstra searching")
self.Env = env.Env()
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"
plotting.animation(self.xI, self.xG, self.obs,
self.path, self.visited, self.fig_name) # animation generate
def searching(self):
def searching(self, xI, xG):
"""
Searching using Dijkstra.
:return: planning path, action in each node, visited nodes in the planning process
"""
q_dijk = queue.QueuePrior() # priority queue
q_dijk.put(self.xI, 0)
parent = {self.xI: self.xI} # record parents of nodes
action = {self.xI: (0, 0)} # record actions of nodes
cost = {self.xI: 0}
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
cost = {xI: 0}
while not q_dijk.empty():
x_current = q_dijk.get()
if x_current == self.xG: # stop condition
if x_current == xG: # stop condition
break
if x_current != self.xI:
tools.plot_dots(x_current, len(parent))
for u_next in self.u_set: # explore neighborhoods of current node
visited.append(x_current)
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
parent[x_next] = x_current
action[x_next] = u_next
[path_dijk, action_dijk] = tools.extract_path(self.xI, self.xG, parent, action)
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
return path_dijk, action_dijk
[path, policy] = self.extract_path(xI, xG, parent, action)
return path, policy, visited
def get_cost(self, x, u):
@@ -66,9 +70,30 @@ class Dijkstra:
return 1
def extract_path(self, xI, xG, parent, policy):
"""
Extract the path based on the relationship of nodes.
:param xI: Starting node
:param xG: Goal node
:param parent: Relationship between nodes
:param policy: Action needed for transfer between two nodes
:return: The planning path
"""
path_back = [xG]
acts_back = [policy[xG]]
x_current = xG
while True:
x_current = parent[x_current]
path_back.append(x_current)
acts_back.append(policy[x_current])
if x_current == xI: break
return list(path_back), list(acts_back)
if __name__ == '__main__':
x_Start = (5, 5) # Starting node
x_Goal = (49, 5) # Goal node
dijkstra = Dijkstra(x_Start, x_Goal)
[path_dijk, actions_dijk] = dijkstra.searching()
tools.showPath(x_Start, x_Goal, path_dijk)
Search-based Planning/env.py
+34 -24
View File
@@ -4,34 +4,44 @@
@author: huiming zhou
"""
x_range, y_range = 51, 31 # size of background
class Env():
def __init__(self):
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():
"""
Initialize obstacles' positions
:return: map of obstacles
"""
def obs_map(self):
"""
Initialize obstacles' positions
obs = []
for i in range(x_range):
obs.append((i, 0))
for i in range(x_range):
obs.append((i, y_range - 1))
:return: map of obstacles
"""
for i in range(y_range):
obs.append((0, i))
for i in range(y_range):
obs.append((x_range - 1, i))
x = self.x_range
y = self.y_range
obs = []
for i in range(10, 21):
obs.append((i, 15))
for i in range(15):
obs.append((20, i))
for i in range(x):
obs.append((i, 0))
for i in range(x):
obs.append((i, y - 1))
for i in range(15, 30):
obs.append((30, i))
for i in range(16):
obs.append((40, i))
for i in range(y):
obs.append((0, i))
for i in range(y):
obs.append((x - 1, i))
for i in range(10, 21):
obs.append((i, 15))
for i in range(15):
obs.append((20, i))
for i in range(15, 30):
obs.append((30, i))
for i in range(16):
obs.append((40, i))
return obs
return obs
Search-based Planning/motion_model.py
-7
View File
@@ -1,7 +0,0 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@author: huiming zhou
"""
motions = [(1, 0), (-1, 0), (0, 1), (0, -1)] # feasible motion sets
Search-based Planning/plotting.py
+52
View File
@@ -0,0 +1,52 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@author: huiming zhou
"""
import matplotlib.pyplot as plt
def animation(xI, xG, obs, path, visited, name):
"""
generate animation for exploring process of algorithm
:param xI: starting state
:param xG: goal state
:param obs: obstacle map
:param path: optimal path
:param visited: visited nodes
:param name: name of this figure
:return: animation
"""
visited.remove(xI)
path.remove(xI)
path.remove(xG)
# plot gridworld
obs_x = [obs[i][0] for i in range(len(obs))]
obs_y = [obs[i][1] for i in range(len(obs))]
plt.plot(xI[0], xI[1], "bs")
plt.plot(xG[0], xG[1], "gs")
plt.plot(obs_x, obs_y, "sk")
plt.title(name)
plt.axis("equal")
# animation for the exploring order of visited nodes
count = 0
for x in visited:
count += 1
plt.plot(x[0], x[1], linewidth='3', color='#808080', marker='o')
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if count % 20 == 0: plt.pause(0.01)
# plot optimal path
path_x = [path[i][0] for i in range(len(path))]
path_y = [path[i][1] for i in range(len(path))]
plt.plot(path_x, path_y, linewidth='3', color='r', marker='o')
# show animation
plt.pause(0.01)
plt.show()
Search-based Planning/tools.py
-68
View File
@@ -1,68 +0,0 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@author: huiming zhou
"""
import matplotlib.pyplot as plt
def extract_path(xI, xG, parent, actions):
"""
Extract the path based on the relationship of nodes.
:param xI: Starting node
:param xG: Goal node
:param parent: Relationship between nodes
:param actions: Action needed for transfer between two nodes
:return: The planning path
"""
path_back = [xG]
acts_back = [actions[xG]]
x_current = xG
while True:
x_current = parent[x_current]
path_back.append(x_current)
acts_back.append(actions[x_current])
if x_current == xI: break
return list(reversed(path_back)), list(reversed(acts_back))
def show_map(xI, xG, obs_map, name):
obs_x = [obs_map[i][0] for i in range(len(obs_map))]
obs_y = [obs_map[i][1] for i in range(len(obs_map))]
plt.plot(xI[0], xI[1], "bs")
plt.plot(xG[0], xG[1], "gs")
plt.plot(obs_x, obs_y, "sk")
plt.title(name, fontdict=None)
plt.axis("equal")
def showPath(xI, xG, path):
"""
Plot the path.
:param xI: Starting node
:param xG: Goal node
:param path: Planning path
:return: A plot
"""
path.remove(xI)
path.remove(xG)
path_x = [path[i][0] for i in range(len(path))]
path_y = [path[i][1] for i in range(len(path))]
plt.plot(path_x, path_y, linewidth='5', color='r', linestyle='-')
plt.pause(0.001)
plt.show()
def plot_dots(x, length):
plt.plot(x[0], x[1], linewidth='3', color='#808080', marker='o')
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
if length % 15 == 0: plt.pause(0.001)