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

update

Browse Source
This commit is contained in:
zhm-real
2020-06-18 17:08:10 -07:00
parent a01e6acc1d
commit a0e6fc92a5
14 changed files with 125 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Search-based Planning/.idea/workspace.xml
Generated
+6 -8
View File
@@ -2,18 +2,16 @@
<project version="4">
<component name="ChangeListManager">
<list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
<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 afterPath="$PROJECT_DIR$/../Stochastic Shortest Path/motion_model.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$/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" />
<change beforePath="$PROJECT_DIR$/../Stochastic Shortest Path/env.py" beforeDir="false" afterPath="$PROJECT_DIR$/../Stochastic Shortest Path/env.py" afterDir="false" />
<change beforePath="$PROJECT_DIR$/../Stochastic Shortest Path/motion model.py" beforeDir="false" />
<change beforePath="$PROJECT_DIR$/../Stochastic Shortest Path/value_iteration.py" beforeDir="false" afterPath="$PROJECT_DIR$/../Stochastic Shortest Path/value_iteration.py" afterDir="false" />
</list>
<option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
<option name="SHOW_DIALOG" value="false" />
@@ -52,7 +50,7 @@
</list>
</option>
</component>
<component name="RunManager" selected="Python.dijkstra">
<component name="RunManager" selected="Python.dfs">
<configuration name="a_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
<module name="Search-based Planning" />
<option name="INTERPRETER_OPTIONS" value="" />
@@ -160,10 +158,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>
Search-based Planning/__pycache__/env.cpython-37.pyc
BIN
View File
Binary file not shown.
Search-based Planning/a_star.py
+2 -3
View File
@@ -10,10 +10,9 @@ import env
import motion_model
class Astar:
def __init__(self, x_start, x_goal, x_range, y_range, heuristic_type):
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.x_range, self.y_range = x_range, y_range
self.obs = env.obs_map() # position of obstacles
self.heuristic_type = heuristic_type
@@ -84,6 +83,6 @@ class Astar:
if __name__ == '__main__':
x_Start = (5, 5) # Starting node
x_Goal = (49, 5) # Goal node
astar = Astar(x_Start, x_Goal, env.x_range, env.y_range, "manhattan")
astar = Astar(x_Start, x_Goal, "manhattan")
[path_astar, actions_astar] = astar.searching()
tools.showPath(x_Start, x_Goal, path_astar) # Plot path and visited nodes
Search-based Planning/bfs.py
+2 -3
View File
@@ -14,10 +14,9 @@ class BFS:
BFS -> Breadth-first Searching
"""
def __init__(self, x_start, x_goal, x_range, y_range):
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.x_range, self.y_range = x_range, y_range
self.obs = env.obs_map() # position of obstacles
env.show_map(self.xI, self.xG, self.obs, "breadth-first searching")
@@ -53,6 +52,6 @@ class BFS:
if __name__ == '__main__':
x_Start = (5, 5) # Starting node
x_Goal = (49, 5) # Goal node
bfs = BFS(x_Start, x_Goal, env.x_range, env.y_range)
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
+2 -3
View File
@@ -14,10 +14,9 @@ class DFS:
DFS -> Depth-first Searching
"""
def __init__(self, x_start, x_goal, x_range, y_range):
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.x_range, self.y_range = x_range, y_range
self.obs = env.obs_map() # position of obstacles
env.show_map(self.xI, self.xG, self.obs, "depth-first searching")
@@ -53,6 +52,6 @@ class DFS:
if __name__ == '__main__':
x_Start = (5, 5) # Starting node
x_Goal = (49, 5) # Goal node
dfs = DFS(x_Start, x_Goal, env.x_range, env.y_range)
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
+2 -3
View File
@@ -10,10 +10,9 @@ import tools
import motion_model
class Dijkstra:
def __init__(self, x_start, x_goal, x_range, y_range):
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.x_range, self.y_range = x_range, y_range
self.obs = env.obs_map() # position of obstacles
env.show_map(self.xI, self.xG, self.obs, "dijkstra searching")
@@ -66,6 +65,6 @@ class Dijkstra:
if __name__ == '__main__':
x_Start = (5, 5) # Starting node
x_Goal = (49, 5) # Goal node
dijkstra = Dijkstra(x_Start, x_Goal, env.x_range, env.y_range)
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
+11 -10
View File
@@ -15,28 +15,28 @@ def obs_map():
:return: map of obstacles
"""
obs_map = []
obs = []
for i in range(x_range):
obs_map.append((i, 0))
obs.append((i, 0))
for i in range(x_range):
obs_map.append((i, y_range-1))
obs.append((i, y_range - 1))
for i in range(y_range):
obs_map.append((0, i))
obs.append((0, i))
for i in range(y_range):
obs_map.append((x_range-1, i))
obs.append((x_range - 1, i))
for i in range(10, 21):
obs_map.append((i, 15))
obs.append((i, 15))
for i in range(15):
obs_map.append((20, i))
obs.append((20, i))
for i in range(15, 30):
obs_map.append((30, i))
obs.append((30, i))
for i in range(16):
obs_map.append((40, i))
obs.append((40, i))
return obs_map
return obs
def show_map(xI, xG, obs_map, name):
@@ -49,3 +49,4 @@ def show_map(xI, xG, obs_map, name):
plt.title(name, fontdict=None)
plt.grid(True)
plt.axis("equal")
Stochastic Shortest Path/__pycache__/env.cpython-37.pyc
BIN
View File
Binary file not shown.
Stochastic Shortest Path/__pycache__/motion_model.cpython-37.pyc
BIN
View File
Binary file not shown.
Stochastic Shortest Path/__pycache__/tools.cpython-37.pyc
BIN
View File
Binary file not shown.
Stochastic Shortest Path/env.py
+24 -11
View File
@@ -15,37 +15,50 @@ def obs_map():
:return: map of obstacles
"""
obs_map = []
obs = []
for i in range(x_range):
obs_map.append((i, 0))
obs.append((i, 0))
for i in range(x_range):
obs_map.append((i, y_range-1))
obs.append((i, y_range - 1))
for i in range(y_range):
obs_map.append((0, i))
obs.append((0, i))
for i in range(y_range):
obs_map.append((x_range-1, i))
obs.append((x_range - 1, i))
for i in range(10, 21):
obs_map.append((i, 15))
obs.append((i, 15))
for i in range(15):
obs_map.append((20, i))
obs.append((20, i))
for i in range(15, 30):
obs_map.append((30, i))
obs.append((30, i))
for i in range(16):
obs_map.append((40, i))
obs.append((40, i))
return obs_map
return obs
def show_map(xI, xG, obs_map, name):
def lose_map():
lose = []
for i in range(27, 34):
lose.append((i, 13))
return lose
def show_map(xI, xG, obs_map, lose_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))]
lose_x = [lose_map[i][0] for i in range(len(lose_map))]
lose_y = [lose_map[i][1] for i in range(len(lose_map))]
plt.plot(xI[0], xI[1], "bs")
plt.plot(xG[0], xG[1], "gs")
plt.plot(obs_x, obs_y, "sk")
plt.plot(lose_x, lose_y, marker = 's', color = '#A52A2A')
plt.title(name, fontdict=None)
plt.grid(True)
plt.axis("equal")
plt.show()
Stochastic Shortest Path/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
Stochastic Shortest Path/motion_model.py
+37
View File
@@ -0,0 +1,37 @@
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
@author: huiming zhou
"""
import numpy as np
motions = [(1, 0), (-1, 0), (0, 1), (0, -1)] # feasible motion sets
def move_prob(x, u, obs, eta = 0.2):
"""
Motion model of robots,
:param x: current state (node)
:param u: input
:param obs: obstacle map
:param eta: noise in motion model
:return: next states and corresponding probability
"""
p_next = [1 - eta, eta / 2, eta / 2]
x_next = []
if u == (0, 1):
u_real = [(0, 1), (-1, 0), (1, 0)]
elif u == (0, -1):
u_real = [(0, -1), (-1, 0), (1, 0)]
elif u == (-1, 0):
u_real = [(-1, 0), (0, 1), (0, -1)]
else:
u_real = [(1, 0), (0, 1), (0, -1)]
for act in u_real:
if (x[0] + act[0], x[1] + act[1]) in obs:
x_next.append(x)
else:
x_next.append((x[0] + act[0], x[1] + act[1]))
return x_next, p_next
Stochastic Shortest Path/value_iteration.py
+37
View File
@@ -4,4 +4,41 @@
@author: huiming zhou
"""
import env
import tools
import motion_model
import numpy as np
import copy
class Value_iteration:
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.T = 500
self.gamma = 0.9
self.obs = env.obs_map() # position of obstacles
self.lose = env.lose_map()
self.name = "value_iteration, T=" + str(self.T) + ", gamma=" + str(self.gamma)
env.show_map(self.xI, self.xG, self.obs, self.lose, self.name)
def iteration(self):
value_table = {}
policy = {}
for i in range(env.x_range):
for j in range(env.y_range):
if (i, j) not in self.obs:
value_table[(i, j)] = 0
for k in range(self.T):
value_table_update = copy.deepcopy(value_table)
for key in value_table:
if __name__ == '__main__':
x_Start = (5, 5) # Starting node
x_Goal = (49, 5) # Goal node
VI = Value_iteration(x_Start, x_Goal)