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-18 15:52:17 -07:00
parent 97db5259e4
commit a01e6acc1d
14 changed files with 129 additions and 113 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Search-based Planning/.idea/workspace.xml
Generated
+12 -6
View File
@@ -2,14 +2,18 @@
<project version="4">
<component name="ChangeListManager">
<list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
<change afterPath="$PROJECT_DIR$/env.py" afterDir="false" />
<change afterPath="$PROJECT_DIR$/motion_model.py" afterDir="false" />
<change afterPath="$PROJECT_DIR$/../Stochastic Shortest Path/env.py" afterDir="false" />
<change afterPath="$PROJECT_DIR$/../Stochastic Shortest Path/motion model.py" afterDir="false" />
<change afterPath="$PROJECT_DIR$/../Stochastic Shortest Path/value_iteration.py" 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$/environment.py" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/tools.py" beforeDir="false" afterPath="$PROJECT_DIR$/tools.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/env.py" beforeDir="false" afterPath="$PROJECT_DIR$/env.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/../Stochastic Shortest Path/environment.py" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/../Stochastic Shortest Path/tools.py" beforeDir="false" afterPath="$PROJECT_DIR$/../Stochastic Shortest Path/tools.py" afterDir="false" />
</list>
<option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
<option name="SHOW_DIALOG" value="false" />
@@ -48,7 +52,7 @@
</list>
</option>
</component>
<component name="RunManager" selected="Python.dfs">
<component name="RunManager" selected="Python.dijkstra">
<configuration name="a_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
@@ -156,10 +160,10 @@
</configuration>
<recent_temporary>
<list>
<item itemvalue="Python.dijkstra" />
<item itemvalue="Python.dfs" />
<item itemvalue="Python.bfs" />
<item itemvalue="Python.a_star" />
<item itemvalue="Python.dijkstra" />
<item itemvalue="Python.searching" />
</list>
</recent_temporary>
@@ -182,7 +186,9 @@
<map>
<entry key="MAIN">
<value>
<State />
<State>
<option name="COLUMN_ORDER" />
</State>
</value>
</entry>
</map>
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/a_star.py
+7 -5
View File
@@ -5,18 +5,20 @@
"""
import queue
import env
import tools
import env
import motion_model
class Astar:
def __init__(self, x_start, x_goal, x_range, y_range, heuristic_type):
self.u_set = env.motions # feasible input set
self.u_set = motion_model.motions # feasible input set
self.xI, self.xG = x_start, x_goal
self.x_range, self.y_range = x_range, y_range
self.obs = env.obs_map(self.xI, self.xG, "a_star searching") # position of obstacles
self.obs = env.obs_map() # position of obstacles
self.heuristic_type = heuristic_type
env.show_map(self.xI, self.xG, self.obs, "a_star searching")
def searching(self):
"""
Searching using A_star.
@@ -27,7 +29,7 @@ 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
action = {self.xI: (0, 0)} # record actions of nodes
cost = {self.xI: 0}
while not q_astar.empty():
@@ -43,7 +45,7 @@ class Astar:
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)
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] = x_current
action[x_next] = u_next
[path_astar, actions_astar] = tools.extract_path(self.xI, self.xG, parent, action)
Search-based Planning/bfs.py
+5 -2
View File
@@ -7,6 +7,7 @@
import queue
import tools
import env
import motion_model
class BFS:
"""
@@ -14,10 +15,12 @@ class BFS:
"""
def __init__(self, x_start, x_goal, x_range, y_range):
self.u_set = env.motions # feasible input set
self.u_set = motion_model.motions # feasible input set
self.xI, self.xG = x_start, x_goal
self.x_range, self.y_range = x_range, y_range
self.obs = env.obs_map(self.xI, self.xG, "breadth-first searching") # position of obstacles
self.obs = env.obs_map() # position of obstacles
env.show_map(self.xI, self.xG, self.obs, "breadth-first searching")
def searching(self):
"""
Search-based Planning/dfs.py
+5 -2
View File
@@ -7,6 +7,7 @@
import queue
import tools
import env
import motion_model
class DFS:
"""
@@ -14,10 +15,12 @@ class DFS:
"""
def __init__(self, x_start, x_goal, x_range, y_range):
self.u_set = env.motions # feasible input set
self.u_set = motion_model.motions # feasible input set
self.xI, self.xG = x_start, x_goal
self.x_range, self.y_range = x_range, y_range
self.obs = env.obs_map(self.xI, self.xG, "depth-first searching") # position of obstacles
self.obs = env.obs_map() # position of obstacles
env.show_map(self.xI, self.xG, self.obs, "depth-first searching")
def searching(self):
"""
Search-based Planning/dijkstra.py
+7 -4
View File
@@ -7,13 +7,16 @@
import queue
import env
import tools
import motion_model
class Dijkstra:
def __init__(self, x_start, x_goal, x_range, y_range):
self.u_set = env.motions # feasible input set
self.u_set = motion_model.motions # feasible input set
self.xI, self.xG = x_start, x_goal
self.x_range, self.y_range = x_range, y_range
self.obs = env.obs_map(self.xI, self.xG, "dijkstra searching") # position of obstacles
self.obs = env.obs_map() # position of obstacles
env.show_map(self.xI, self.xG, self.obs, "dijkstra searching")
def searching(self):
"""
@@ -25,7 +28,7 @@ class Dijkstra:
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
action = {self.xI: (0, 0)} # record actions of nodes
cost = {self.xI: 0}
while not q_dijk.empty():
@@ -36,7 +39,7 @@ class Dijkstra:
tools.plot_dots(x_current, len(parent))
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
Search-based Planning/env.py
+6 -9
View File
@@ -7,15 +7,11 @@
import matplotlib.pyplot as plt
x_range, y_range = 51, 31 # size of background
motions = [(1, 0), (-1, 0), (0, 1), (0, -1)] # feasible motion sets
def obs_map(xI, xG, name):
def obs_map():
"""
Initialize obstacles' positions
:param xI: starting node
:param xG: goal node
:param name: title of figure
:return: map of obstacles
"""
@@ -40,15 +36,16 @@ def obs_map(xI, xG, name):
for i in range(16):
obs_map.append((40, i))
return obs_map
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.title(name, fontdict=None)
plt.grid(True)
plt.axis("equal")
return obs_map
Search-based Planning/motion_model.py
+7
View File
@@ -0,0 +1,7 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@author: huiming zhou
"""
motions = [(1, 0), (-1, 0), (0, 1), (0, -1)] # feasible motion sets
Stochastic Shortest Path/env.py
+51
View File
@@ -0,0 +1,51 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@author: huiming zhou
"""
import matplotlib.pyplot as plt
x_range, y_range = 51, 31 # size of background
def obs_map():
"""
Initialize obstacles' positions
:return: map of obstacles
"""
obs_map = []
for i in range(x_range):
obs_map.append((i, 0))
for i in range(x_range):
obs_map.append((i, y_range-1))
for i in range(y_range):
obs_map.append((0, i))
for i in range(y_range):
obs_map.append((x_range-1, i))
for i in range(10, 21):
obs_map.append((i, 15))
for i in range(15):
obs_map.append((20, i))
for i in range(15, 30):
obs_map.append((30, i))
for i in range(16):
obs_map.append((40, i))
return obs_map
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.grid(True)
plt.axis("equal")
Stochastic Shortest Path/environment.py
-49
View File
@@ -1,49 +0,0 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@author: huiming zhou
"""
import numpy as np
col, row = 50, 30 # size of background
motions = [(1, 0), (-1, 0), (0, 1), (0, -1)] # feasible motion sets
def obstacles():
"""
Design the obstacles' positions.
:return: the map of obstacles.
"""
background = [[[1., 1., 1.]
for x in range(col)] for y in range(row)]
for j in range(col):
background[0][j] = [0., 0., 0.]
background[row - 1][j] = [0., 0., 0.]
for i in range(row):
background[i][0] = [0., 0., 0.]
background[i][col - 1] = [0., 0., 0.]
for i in range(10, 20):
background[15][i] = [0., 0., 0.]
for i in range(15):
background[row - 1 - i][30] = [0., 0., 0.]
background[i + 1][20] = [0., 0., 0.]
background[i + 1][40] = [0., 0., 0.]
return background
def map_obs():
"""
Using a matrix to represent the position of obstacles,
which is used for obstacle detection.
:return: a matrix, in which '1' represents obstacle.
"""
obs_map = np.zeros((col, row))
pos_map = obstacles()
for i in range(col):
for j in range(row):
if pos_map[j][i] == [0., 0., 0.]:
obs_map[i][j] = 1
return obs_map
Stochastic Shortest Path/motion model.py
+7
View File
@@ -0,0 +1,7 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@author: huiming zhou
"""
motions = [(1, 0), (-1, 0), (0, 1), (0, -1)] # feasible motion sets
Stochastic Shortest Path/tools.py
+15 -36
View File
@@ -5,30 +5,10 @@
"""
import matplotlib.pyplot as plt
import environment
def obs_detect(x, u, obs_map):
"""
Detect if the next state is in obstacles using this input.
:param x: current state
:param u: input
:param obs_map: map of obstacles
:return: in obstacles: True / not in obstacles: False
"""
x_next = [x[0] + u[0], x[1] + u[1]] # next state using input 'u'
if u not in environment.motions or \
obs_map[x_next[0]][x_next[1]] == 1: # if 'u' is feasible and next state is not in obstacles
return True
return False
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
@@ -47,28 +27,27 @@ def extract_path(xI, xG, parent, actions):
return list(reversed(path_back)), list(reversed(acts_back))
def showPath(xI, xG, path, visited, name):
def showPath(xI, xG, path):
"""
Plot the path.
:param xI: Starting node
:param xG: Goal node
:param path: Planning path
:param visited: Visited nodes
:param name: Name of this figure
:return: A plot
"""
background = environment.obstacles()
fig, ax = plt.subplots()
for k in range(len(visited)):
background[visited[k][1]][visited[k][0]] = [.5, .5, .5] # visited nodes: gray color
for k in range(len(path)):
background[path[k][1]][path[k][0]] = [1., 0., 0.] # path: red color
background[xI[1]][xI[0]] = [0., 0., 1.] # starting node: blue color
background[xG[1]][xG[0]] = [0., 1., .5] # goal node: green color
ax.imshow(background)
ax.invert_yaxis() # put origin of coordinate to left-bottom
plt.title(name, fontdict=None)
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)
Stochastic Shortest Path/value_iteration.py
+7
View File
@@ -0,0 +1,7 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@author: huiming zhou
"""