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-06-21 21:50:11 -07:00
parent e9a57e0b88
commit 022d9f4727
19 changed files with 149 additions and 134 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.idea/.gitignore
Generated
+3
View File
@@ -0,0 +1,3 @@
# Default ignored files
/shelf/
/workspace.xml
.idea/inspectionProfiles/profiles_settings.xml
Generated
+6
View File
@@ -0,0 +1,6 @@
<component name="InspectionProjectProfileManager">
<settings>
<option name="USE_PROJECT_PROFILE" value="false" />
<version value="1.0" />
</settings>
</component>
.idea/misc.xml
Generated
+4
View File
@@ -0,0 +1,4 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectRootManager" version="2" project-jdk-name="Python 3.5" project-jdk-type="Python SDK" />
</project>
.idea/modules.xml
Generated
+8
View File
@@ -0,0 +1,8 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectModuleManager">
<modules>
<module fileurl="file://$PROJECT_DIR$/.idea/path-planning-algorithms.iml" filepath="$PROJECT_DIR$/.idea/path-planning-algorithms.iml" />
</modules>
</component>
</project>
.idea/path-planning-algorithms.iml
Generated
+12
View File
@@ -0,0 +1,12 @@
<?xml version="1.0" encoding="UTF-8"?>
<module type="PYTHON_MODULE" version="4">
<component name="NewModuleRootManager">
<content url="file://$MODULE_DIR$" />
<orderEntry type="inheritedJdk" />
<orderEntry type="sourceFolder" forTests="false" />
</component>
<component name="PyDocumentationSettings">
<option name="format" value="PLAIN" />
<option name="myDocStringFormat" value="Plain" />
</component>
</module>
.idea/vcs.xml
Generated
+6
View File
@@ -0,0 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="VcsDirectoryMappings">
<mapping directory="$PROJECT_DIR$" vcs="Git" />
</component>
</project>
Search-based Planning/.idea/Search-based Planning.iml
Generated
+1 -4
View File
@@ -2,10 +2,7 @@
<module type="PYTHON_MODULE" version="4">
<component name="NewModuleRootManager">
<content url="file://$MODULE_DIR$" />
<orderEntry type="jdk" jdkName="Python 3.7" jdkType="Python SDK" />
<orderEntry type="jdk" jdkName="Python 3.5" jdkType="Python SDK" />
<orderEntry type="sourceFolder" forTests="false" />
</component>
<component name="TestRunnerService">
<option name="PROJECT_TEST_RUNNER" value="Unittests" />
</component>
</module>
Search-based Planning/.idea/misc.xml
Generated
+1 -1
View File
@@ -1,4 +1,4 @@
<?xml version="1.0" encoding="UTF-8"?>
<project version="4">
<component name="ProjectRootManager" version="2" project-jdk-name="Python 3.7" project-jdk-type="Python SDK" />
<component name="ProjectRootManager" version="2" project-jdk-name="Python 3.5" project-jdk-type="Python SDK" />
</project>
Search-based Planning/.idea/workspace.xml
Generated
+45 -20
View File
@@ -1,8 +1,18 @@
<?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="" />
<option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
<list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
<change beforePath="$PROJECT_DIR$/.idea/Search-based Planning.iml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/Search-based Planning.iml" afterDir="false" />
<change beforePath="$PROJECT_DIR$/.idea/misc.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/misc.xml" afterDir="false" />
<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$/plotting.py" beforeDir="false" afterPath="$PROJECT_DIR$/plotting.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/queue.py" beforeDir="false" afterPath="$PROJECT_DIR$/queue.py" afterDir="false" />
</list>
<option name="SHOW_DIALOG" value="false" />
<option name="HIGHLIGHT_CONFLICTS" value="true" />
<option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
@@ -22,25 +32,19 @@
<component name="ProjectLevelVcsManager" settingsEditedManually="true">
<ConfirmationsSetting value="2" id="Add" />
</component>
<component name="ProjectViewState">
<option name="hideEmptyMiddlePackages" value="true" />
<option name="showLibraryContents" value="true" />
</component>
<component name="PropertiesComponent">
<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="restartRequiresConfirmation" value="false" />
<property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
</component>
<component name="RunDashboard">
<option name="ruleStates">
<list>
<RuleState>
<option name="name" value="ConfigurationTypeDashboardGroupingRule" />
</RuleState>
<RuleState>
<option name="name" value="StatusDashboardGroupingRule" />
</RuleState>
</list>
</option>
</component>
<component name="RunManager" selected="Python.dfs">
<component name="RunManager" selected="Python.bfs">
<configuration name="a_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
@@ -148,10 +152,10 @@
</configuration>
<recent_temporary>
<list>
<item itemvalue="Python.dfs" />
<item itemvalue="Python.bfs" />
<item itemvalue="Python.dijkstra" />
<item itemvalue="Python.a_star" />
<item itemvalue="Python.dfs" />
<item itemvalue="Python.dijkstra" />
<item itemvalue="Python.searching" />
</list>
</recent_temporary>
@@ -174,12 +178,33 @@
<map>
<entry key="MAIN">
<value>
<State>
<option name="COLUMN_ORDER" />
</State>
<State />
</value>
</entry>
</map>
</option>
<option name="oldMeFiltersMigrated" value="true" />
</component>
<component name="WindowStateProjectService">
<state width="1832" height="296" key="GridCell.Tab.0.bottom" timestamp="1592800940514">
<screen x="1920" y="0" width="1920" height="1080" />
</state>
<state width="1832" height="296" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592800940514" />
<state width="1832" height="296" key="GridCell.Tab.0.center" timestamp="1592800940514">
<screen x="1920" y="0" width="1920" height="1080" />
</state>
<state width="1832" height="296" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592800940514" />
<state width="1832" height="296" key="GridCell.Tab.0.left" timestamp="1592800940514">
<screen x="1920" y="0" width="1920" height="1080" />
</state>
<state width="1832" height="296" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592800940514" />
<state width="1832" height="296" key="GridCell.Tab.0.right" timestamp="1592800940514">
<screen x="1920" y="0" width="1920" height="1080" />
</state>
<state width="1832" height="296" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592800940514" />
<state x="2406" y="174" key="SettingsEditor" timestamp="1592800563456">
<screen x="1920" y="0" width="1920" height="1080" />
</state>
<state x="2406" y="174" key="SettingsEditor/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592800563456" />
</component>
</project>
Search-based Planning/__pycache__/env.cpython-37.pyc
BIN
View File
Binary file not shown.
Search-based Planning/__pycache__/plotting.cpython-35.pyc
BIN
View File
Binary file not shown.
Search-based Planning/__pycache__/queue.cpython-35.pyc
BIN
View File
Binary file not shown.
Search-based Planning/a_star.py
+14 -23
View File
@@ -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")
astar = Astar(x_Start, x_Goal, "manhattan")
Search-based Planning/bfs.py
+12 -19
View File
@@ -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)
Search-based Planning/dfs.py
+11 -18
View File
@@ -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)
Search-based Planning/dijkstra.py
+14 -22
View File
@@ -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)
Search-based Planning/env.py
+2 -4
View File
@@ -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
Search-based Planning/plotting.py
+1 -10
View File
@@ -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)
Search-based Planning/queue.py
+9 -13
View File
@@ -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