diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index 978aad7..77f181c 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -2,18 +2,16 @@ - - - - + - - + + + - + - + diff --git a/Search-based Planning/__pycache__/env.cpython-37.pyc b/Search-based Planning/__pycache__/env.cpython-37.pyc index e8ca2a5..7f7c2da 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/a_star.py b/Search-based Planning/a_star.py index 8da19b1..2cc6c8c 100644 --- a/Search-based Planning/a_star.py +++ b/Search-based Planning/a_star.py @@ -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 \ No newline at end of file diff --git a/Search-based Planning/bfs.py b/Search-based Planning/bfs.py index f756286..2626b2c 100644 --- a/Search-based Planning/bfs.py +++ b/Search-based Planning/bfs.py @@ -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) diff --git a/Search-based Planning/dfs.py b/Search-based Planning/dfs.py index 3bd4a2d..14dc0bd 100644 --- a/Search-based Planning/dfs.py +++ b/Search-based Planning/dfs.py @@ -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) \ No newline at end of file diff --git a/Search-based Planning/dijkstra.py b/Search-based Planning/dijkstra.py index 95e0759..9a6d5ad 100644 --- a/Search-based Planning/dijkstra.py +++ b/Search-based Planning/dijkstra.py @@ -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) \ No newline at end of file diff --git a/Search-based Planning/env.py b/Search-based Planning/env.py index f777d06..56136c1 100644 --- a/Search-based Planning/env.py +++ b/Search-based Planning/env.py @@ -6,7 +6,7 @@ import matplotlib.pyplot as plt -x_range, y_range = 51, 31 # size of background +x_range, y_range = 51, 31 # size of background def obs_map(): """ @@ -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") + diff --git a/Stochastic Shortest Path/__pycache__/env.cpython-37.pyc b/Stochastic Shortest Path/__pycache__/env.cpython-37.pyc new file mode 100644 index 0000000..9a289c4 Binary files /dev/null and b/Stochastic Shortest Path/__pycache__/env.cpython-37.pyc differ diff --git a/Stochastic Shortest Path/__pycache__/motion_model.cpython-37.pyc b/Stochastic Shortest Path/__pycache__/motion_model.cpython-37.pyc new file mode 100644 index 0000000..79388a0 Binary files /dev/null and b/Stochastic Shortest Path/__pycache__/motion_model.cpython-37.pyc differ diff --git a/Stochastic Shortest Path/__pycache__/tools.cpython-37.pyc b/Stochastic Shortest Path/__pycache__/tools.cpython-37.pyc new file mode 100644 index 0000000..816b7b4 Binary files /dev/null and b/Stochastic Shortest Path/__pycache__/tools.cpython-37.pyc differ diff --git a/Stochastic Shortest Path/env.py b/Stochastic Shortest Path/env.py index f777d06..ba435c3 100644 --- a/Stochastic Shortest Path/env.py +++ b/Stochastic Shortest Path/env.py @@ -6,7 +6,7 @@ import matplotlib.pyplot as plt -x_range, y_range = 51, 31 # size of background +x_range, y_range = 51, 31 # size of background def obs_map(): """ @@ -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() + diff --git a/Stochastic Shortest Path/motion model.py b/Stochastic Shortest Path/motion model.py deleted file mode 100644 index 144bbcc..0000000 --- a/Stochastic Shortest Path/motion model.py +++ /dev/null @@ -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 \ No newline at end of file diff --git a/Stochastic Shortest Path/motion_model.py b/Stochastic Shortest Path/motion_model.py new file mode 100644 index 0000000..592d6d9 --- /dev/null +++ b/Stochastic Shortest Path/motion_model.py @@ -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 \ No newline at end of file diff --git a/Stochastic Shortest Path/value_iteration.py b/Stochastic Shortest Path/value_iteration.py index d9a2968..7dd5a7d 100644 --- a/Stochastic Shortest Path/value_iteration.py +++ b/Stochastic Shortest Path/value_iteration.py @@ -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) \ No newline at end of file