diff --git a/Sampling-based Planning/.idea/Sampling-based Planning.iml b/Sampling-based Planning/.idea/Sampling-based Planning.iml
index 5965bde..c444878 100644
--- a/Sampling-based Planning/.idea/Sampling-based Planning.iml
+++ b/Sampling-based Planning/.idea/Sampling-based Planning.iml
@@ -2,7 +2,7 @@
-
+
\ No newline at end of file
diff --git a/Sampling-based Planning/.idea/misc.xml b/Sampling-based Planning/.idea/misc.xml
index 0e7ac62..a2e120d 100644
--- a/Sampling-based Planning/.idea/misc.xml
+++ b/Sampling-based Planning/.idea/misc.xml
@@ -1,4 +1,4 @@
-
+
\ No newline at end of file
diff --git a/Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc
index 12d1bb9..47fc5c8 100644
Binary files a/Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc and b/Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc differ
diff --git a/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc
index 8eee08a..bbadd8b 100644
Binary files a/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc and b/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc differ
diff --git a/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc
index 3083612..68d12de 100644
Binary files a/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc and b/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc differ
diff --git a/Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc
index c7c08f6..d3f1a64 100644
Binary files a/Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc and b/Sampling-based Planning/rrt_2D/__pycache__/utils.cpython-37.pyc differ
diff --git a/Sampling-based Planning/rrt_2D/rrt_star.py b/Sampling-based Planning/rrt_2D/rrt_star.py
index 70db37e..af36877 100644
--- a/Sampling-based Planning/rrt_2D/rrt_star.py
+++ b/Sampling-based Planning/rrt_2D/rrt_star.py
@@ -161,7 +161,7 @@ def main():
x_start = (2, 2) # Starting node
x_goal = (49, 24) # Goal node
- rrt_star = RrtStar(x_start, x_goal, 8, 0.10, 20, 20000)
+ rrt_star = RrtStar(x_start, x_goal, 8, 0.10, 20, 10000)
path = rrt_star.planning()
if path:
diff --git a/Search-based Planning/.idea/Search-based Planning.iml b/Search-based Planning/.idea/Search-based Planning.iml
index 5965bde..c444878 100644
--- a/Search-based Planning/.idea/Search-based Planning.iml
+++ b/Search-based Planning/.idea/Search-based Planning.iml
@@ -2,7 +2,7 @@
-
+
\ No newline at end of file
diff --git a/Search-based Planning/.idea/misc.xml b/Search-based Planning/.idea/misc.xml
index 0e7ac62..a2e120d 100644
--- a/Search-based Planning/.idea/misc.xml
+++ b/Search-based Planning/.idea/misc.xml
@@ -1,4 +1,4 @@
-
+
\ No newline at end of file
diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml
index 3e86ea3..48f135c 100644
--- a/Search-based Planning/.idea/workspace.xml
+++ b/Search-based Planning/.idea/workspace.xml
@@ -19,9 +19,21 @@
-
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -50,7 +62,7 @@
-
+
@@ -59,8 +71,20 @@
-
-
+
+
+
+
+
@@ -72,28 +96,7 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
@@ -123,27 +126,6 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
@@ -165,6 +147,48 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -188,19 +212,19 @@
-
@@ -230,12 +254,13 @@
-
diff --git a/Search-based Planning/Search_2D/ARAstar.py b/Search-based Planning/Search_2D/ARAstar.py
index aa7fbdf..654afef 100644
--- a/Search-based Planning/Search_2D/ARAstar.py
+++ b/Search-based Planning/Search_2D/ARAstar.py
@@ -5,6 +5,7 @@ ARA_star 2D (Anytime Repairing A*)
import os
import sys
+import math
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../Search-based Planning/")
@@ -15,8 +16,8 @@ from Search_2D import env
class AraStar:
- def __init__(self, x_start, x_goal, e, heuristic_type):
- self.xI, self.xG = x_start, x_goal
+ def __init__(self, s_start, s_goal, e, heuristic_type):
+ self.s_start, self.s_goal = s_start, s_goal
self.heuristic_type = heuristic_type
self.Env = env.Env() # class Env
@@ -24,17 +25,17 @@ class AraStar:
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.e = e # initial weight
- self.g = {self.xI: 0, self.xG: float("inf")} # cost to come
+ self.g = {self.s_start: 0, self.s_goal: float("inf")} # cost to come
self.OPEN = queue.QueuePrior() # priority queue / U
self.CLOSED = set() # closed set
self.INCONS = [] # incons set
- self.PARENT = {self.xI: self.xI} # relations
+ self.PARENT = {self.s_start: self.s_start} # relations
self.path = [] # planning path
self.visited = [] # order of visited nodes
def searching(self):
- self.OPEN.put(self.xI, self.fvalue(self.xI))
+ self.OPEN.put(self.s_start, self.fvalue(self.s_start))
self.ImprovePath()
self.path.append(self.extract_path())
@@ -42,7 +43,7 @@ class AraStar:
self.e -= 0.5 # increase weight
OPEN_mid = [x for (p, x) in self.OPEN.enumerate()] + self.INCONS # combine two sets
self.OPEN = queue.QueuePrior()
- self.OPEN.put(self.xI, self.fvalue(self.xI))
+ self.OPEN.put(self.s_start, self.fvalue(self.s_start))
for x in OPEN_mid:
self.OPEN.put(x, self.fvalue(x)) # update priority
@@ -61,94 +62,105 @@ class AraStar:
visited_each = []
- while (self.fvalue(self.xG) >
+ while (self.fvalue(self.s_goal) >
min([self.fvalue(x) for (p, x) in self.OPEN.enumerate()])):
s = self.OPEN.get()
if s not in self.CLOSED:
self.CLOSED.add(s)
- for u_next in self.u_set:
- s_next = tuple([s[i] + u_next[i] for i in range(len(s))])
- if s_next not in self.obs:
- new_cost = self.g[s] + self.get_cost(s, u_next)
- if s_next not in self.g or new_cost < self.g[s_next]:
- self.g[s_next] = new_cost
- self.PARENT[s_next] = s
- visited_each.append(s_next)
+ for s_n in self.get_neighbor(s):
+ new_cost = self.g[s] + self.cost(s, s_n)
+ if s_n not in self.g or new_cost < self.g[s_n]:
+ self.g[s_n] = new_cost
+ self.PARENT[s_n] = s
+ visited_each.append(s_n)
- if s_next not in self.CLOSED:
- self.OPEN.put(s_next, self.fvalue(s_next))
- else:
- self.INCONS.append(s_next)
+ if s_n not in self.CLOSED:
+ self.OPEN.put(s_n, self.fvalue(s_n))
+ else:
+ self.INCONS.append(s_n)
self.visited.append(visited_each)
+ def get_neighbor(self, s):
+ """
+ find neighbors of state s that not in obstacles.
+ :param s: state
+ :return: neighbors
+ """
+
+ s_list = set()
+
+ for u in self.u_set:
+ s_next = tuple([s[i] + u[i] for i in range(2)])
+ if s_next not in self.obs:
+ s_list.add(s_next)
+
+ return s_list
+
def update_e(self):
c_OPEN, c_INCONS = float("inf"), float("inf")
if self.OPEN:
- c_OPEN = min(self.g[x] + self.Heuristic(x) for (p, x) in self.OPEN.enumerate())
-
+ c_OPEN = min(self.g[x] +
+ self.Heuristic(x) for (p, x) in self.OPEN.enumerate())
if self.INCONS:
- c_INCONS = min(self.g[x] + self.Heuristic(x) for x in self.INCONS)
-
+ c_INCONS = min(self.g[x] +
+ self.Heuristic(x) for x in self.INCONS)
if min(c_OPEN, c_INCONS) == float("inf"):
return 1
- return min(self.e, self.g[self.xG] / min(c_OPEN, c_INCONS))
+ return min(self.e, self.g[self.s_goal] / min(c_OPEN, c_INCONS))
def fvalue(self, x):
return self.g[x] + self.e * self.Heuristic(x)
def extract_path(self):
"""
- Extract the path based on the relationship of nodes.
-
+ Extract the path based on the PARENT set.
:return: The planning path
"""
- path_back = [self.xG]
- x_current = self.xG
+ path = [self.s_goal]
+ s = self.s_goal
while True:
- x_current = self.PARENT[x_current]
- path_back.append(x_current)
+ s = self.PARENT[s]
+ path.append(s)
- if x_current == self.xI:
+ if s == self.s_start:
break
- return list(path_back)
+ return list(path)
+
+ def Heuristic(self, s):
+ """
+ Calculate heuristic.
+ :param s: current node (state)
+ :return: heuristic function value
+ """
+
+ heuristic_type = self.heuristic_type # heuristic type
+ goal = self.s_goal # goal node
+
+ if heuristic_type == "manhattan":
+ return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
+ else:
+ return math.hypot(goal[0] - s[0], goal[1] - s[1])
@staticmethod
- def get_cost(x, u):
+ def cost(s_start, s_goal):
"""
Calculate cost for this motion
- :param x: current node
- :param u: input
+ :param s_start: starting node
+ :param s_goal: end node
:return: cost for this motion
:note: cost function could be more complicate!
"""
return 1
- def Heuristic(self, state):
- """
- Calculate heuristic.
- :param state: current node (state)
- :return: heuristic
- """
-
- heuristic_type = self.heuristic_type
- goal = self.xG
-
- if heuristic_type == "manhattan":
- return abs(goal[0] - state[0]) + abs(goal[1] - state[1])
- elif heuristic_type == "euclidean":
- return ((goal[0] - state[0]) ** 2 + (goal[1] - state[1]) ** 2) ** (1 / 2)
- else:
- print("Please choose right heuristic type!")
-
def main():
x_start = (5, 5) # Starting node
diff --git a/Search-based Planning/Search_2D/LPAstar.py b/Search-based Planning/Search_2D/LPAstar.py
index fd6310b..8bd2af3 100644
--- a/Search-based Planning/Search_2D/LPAstar.py
+++ b/Search-based Planning/Search_2D/LPAstar.py
@@ -18,7 +18,7 @@ from Search_2D import env
class LpaStar:
def __init__(self, x_start, x_goal, heuristic_type):
- self.xI, self.xG = x_start, x_goal
+ self.s_start, self.s_goal = x_start, x_goal
self.heuristic_type = heuristic_type
self.Env = env.Env()
@@ -37,18 +37,16 @@ class LpaStar:
self.rhs[(i, j)] = float("inf")
self.g[(i, j)] = float("inf")
- self.rhs[self.xI] = 0
- self.U.put(self.xI, self.Key(self.xI))
+ self.rhs[self.s_start] = 0
+ self.U.put(self.s_start, self.Key(self.s_start))
self.fig = plt.figure()
def run(self):
self.Plot.plot_grid("Lifelong Planning A*")
self.ComputePath()
- self.plot_path(self.extract_path_test())
-
+ self.plot_path(self.extract_path())
self.fig.canvas.mpl_connect('button_press_event', self.on_press)
- print("hahha")
plt.show()
@@ -62,55 +60,54 @@ class LpaStar:
if (x, y) not in self.obs:
self.obs.add((x, y))
plt.plot(x, y, 'sk')
- plt.pause(0.001)
- self.rhs[(x, y)] = float("inf")
- self.g[(x, y)] = float("inf")
- for node in self.get_neighbor((x, y)):
- self.UpdateVertex(node)
else:
self.obs.remove((x, y))
plt.plot(x, y, marker='s', color='white')
self.UpdateVertex((x, y))
+
+ for s_n in self.get_neighbor((x, y)):
+ self.UpdateVertex(s_n)
+
self.ComputePath()
- self.plot_path(self.extract_path_test())
+ self.plot_path(self.extract_path())
self.fig.canvas.draw_idle()
- @staticmethod
- def plot_path(path):
- px = [x[0] for x in path]
- py = [x[1] for x in path]
- plt.plot(px, py, marker='o')
-
def ComputePath(self):
- while self.U.top_key() < self.Key(self.xG) or \
- self.rhs[self.xG] != self.g[self.xG]:
+ while self.U.top_key() < self.Key(self.s_goal) or \
+ self.rhs[self.s_goal] != self.g[self.s_goal]:
s = self.U.get()
- if self.g[s] > self.rhs[s]:
+
+ if self.g[s] > self.rhs[s]: # over-consistent: deleted obstacles
self.g[s] = self.rhs[s]
- else:
+ else: # under-consistent: added obstacles
self.g[s] = float("inf")
self.UpdateVertex(s)
- for x in self.get_neighbor(s):
- self.UpdateVertex(x)
+ for s_n in self.get_neighbor(s):
+ self.UpdateVertex(s_n)
def UpdateVertex(self, s):
- if s != self.xI:
- u_min = float("inf")
- for x in self.get_neighbor(s):
- u_min = min(u_min, self.g[x] + self.cost(x, s))
- self.rhs[s] = u_min
+ if s != self.s_start:
+ self.rhs[s] = min([self.g[s_n] + self.cost(s_n, s)
+ for s_n in self.get_neighbor(s)])
self.U.remove(s)
if self.g[s] != self.rhs[s]:
self.U.put(s, self.Key(s))
def get_neighbor(self, s):
- nei_list = set()
+ """
+ find neighbors of state s that not in obstacles.
+ :param s: state
+ :return: neighbors
+ """
+
+ s_list = set()
+
for u in self.u_set:
s_next = tuple([s[i] + u[i] for i in range(2)])
if s_next not in self.obs:
- nei_list.add(s_next)
+ s_list.add(s_next)
- return nei_list
+ return s_list
def Key(self, s):
return [min(self.g[s], self.rhs[s]) + self.h(s),
@@ -118,7 +115,7 @@ class LpaStar:
def h(self, s):
heuristic_type = self.heuristic_type # heuristic type
- goal = self.xG # goal node
+ goal = self.s_goal # goal node
if heuristic_type == "manhattan":
return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
@@ -132,31 +129,24 @@ class LpaStar:
def extract_path(self):
path = []
- s = self.xG
-
- while True:
- g_list = {}
- for x in self.get_neighbor(s):
- g_list[x] = self.g[x]
- s = min(g_list, key=g_list.get)
- if s == self.xI:
- return list(reversed(path))
- path.append(s)
-
- def extract_path_test(self):
- path = []
- s = self.xG
+ s = self.s_goal
for k in range(100):
g_list = {}
for x in self.get_neighbor(s):
g_list[x] = self.g[x]
s = min(g_list, key=g_list.get)
- if s == self.xI:
- return list(reversed(path))
+ if s == self.s_start:
+ break
path.append(s)
return list(reversed(path))
+ @staticmethod
+ def plot_path(path):
+ px = [x[0] for x in path]
+ py = [x[1] for x in path]
+ plt.plot(px, py, marker='o')
+
def print_g(self):
print("he")
for k in range(self.Env.y_range):
diff --git a/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc
index ff2b686..0561250 100644
Binary files a/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc and b/Search-based Planning/Search_2D/__pycache__/env.cpython-37.pyc differ
diff --git a/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc
index ac34951..33df9b2 100644
Binary files a/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc and b/Search-based Planning/Search_2D/__pycache__/plotting.cpython-37.pyc differ
diff --git a/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc b/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc
index 9185c91..ad642f8 100644
Binary files a/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc and b/Search-based Planning/Search_2D/__pycache__/queue.cpython-37.pyc differ
diff --git a/Search-based Planning/Search_2D/astar.py b/Search-based Planning/Search_2D/astar.py
index a773fd7..5bda0f8 100644
--- a/Search-based Planning/Search_2D/astar.py
+++ b/Search-based Planning/Search_2D/astar.py
@@ -37,7 +37,7 @@ class Astar:
:return: path, order of visited nodes
"""
- while not self.OPEN.empty():
+ while self.OPEN:
s = self.OPEN.get()
self.CLOSED.append(s)
@@ -45,14 +45,13 @@ class Astar:
break
for s_n in self.get_neighbor(s):
- if s_n not in self.CLOSED:
- new_cost = self.g[s] + self.cost(s, s_n)
- if s_n not in self.g:
- self.g[s_n] = float("inf")
- if new_cost < self.g[s_n]: # conditions for updating cost
- self.g[s_n] = new_cost
- self.PARENT[s_n] = s
- self.OPEN.put(s_n, self.fvalue(s_n))
+ new_cost = self.g[s] + self.cost(s, s_n)
+ if s_n not in self.g:
+ self.g[s_n] = float("inf")
+ if new_cost < self.g[s_n]: # conditions for updating cost
+ self.g[s_n] = new_cost
+ self.PARENT[s_n] = s
+ self.OPEN.put(s_n, self.fvalue(s_n))
return self.extract_path(self.PARENT), self.CLOSED
@@ -150,18 +149,6 @@ class Astar:
return list(path)
- @staticmethod
- def cost(s_start, s_goal):
- """
- Calculate cost for this motion
- :param s_start: starting node
- :param s_goal: end node
- :return: cost for this motion
- :note: cost function could be more complicate!
- """
-
- return 1
-
def Heuristic(self, s):
"""
Calculate heuristic.
@@ -177,6 +164,18 @@ class Astar:
else:
return math.hypot(goal[0] - s[0], goal[1] - s[1])
+ @staticmethod
+ def cost(s_start, s_goal):
+ """
+ Calculate cost for this motion
+ :param s_start: starting node
+ :param s_goal: end node
+ :return: cost for this motion
+ :note: cost function could be more complicate!
+ """
+
+ return 1
+
def main():
s_start = (5, 5)
diff --git a/Search-based Planning/Search_2D/bfs.py b/Search-based Planning/Search_2D/bfs.py
index b8412f5..a8031a2 100644
--- a/Search-based Planning/Search_2D/bfs.py
+++ b/Search-based Planning/Search_2D/bfs.py
@@ -15,67 +15,83 @@ from Search_2D import env
class BFS:
- def __init__(self, x_start, x_goal):
- self.xI, self.xG = x_start, x_goal
+ def __init__(self, s_start, s_goal):
+ self.s_start, self.s_goal = s_start, s_goal
self.Env = env.Env()
- self.plotting = plotting.Plotting(self.xI, self.xG)
+ self.plotting = plotting.Plotting(self.s_start, self.s_goal)
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
self.OPEN = queue.QueueFIFO() # U set: visited nodes
- self.OPEN.put(self.xI)
+ self.OPEN.put(self.s_start)
self.CLOSED = [] # CLOSED set: explored nodes
- self.PARENT = {self.xI: self.xI} # relations
+ self.PARENT = {self.s_start: self.s_start}
def searching(self):
"""
- :return: path, order of visited nodes in the planning
+ Breadth-first Searching.
+ :return: path, visited order
"""
- while not self.OPEN.empty():
+ while self.OPEN:
s = self.OPEN.get()
- if s == self.xG:
+
+ if s == self.s_goal:
break
self.CLOSED.append(s)
- for u_next in self.u_set: # explore neighborhoods
- s_next = tuple([s[i] + u_next[i] for i in range(2)])
- if s_next not in self.PARENT and s_next not in self.obs: # node not visited and not in obstacles
- self.OPEN.put(s_next)
- self.PARENT[s_next] = s
+ for s_n in self.get_neighbor(s):
+ if s_n not in self.PARENT: # node not explored
+ self.OPEN.put(s_n)
+ self.PARENT[s_n] = s
return self.extract_path(), self.CLOSED
+ def get_neighbor(self, s):
+ """
+ find neighbors of state s that not in obstacles.
+ :param s: state
+ :return: neighbors
+ """
+
+ s_list = set()
+
+ for u in self.u_set:
+ s_next = tuple([s[i] + u[i] for i in range(2)])
+ if s_next not in self.obs:
+ s_list.add(s_next)
+
+ return s_list
+
def extract_path(self):
"""
- Extract the path based on the relationship of nodes.
+ Extract the path based on the PARENT set.
:return: The planning path
"""
- path = [self.xG]
- s = self.xG
+ path = [self.s_goal]
+ s = self.s_goal
while True:
s = self.PARENT[s]
path.append(s)
- if s == self.xI:
+ if s == self.s_start:
break
return list(path)
def main():
- x_start = (5, 5) # Starting node
- x_goal = (45, 25) # Goal node
+ s_start = (5, 5)
+ s_goal = (45, 25)
- bfs = BFS(x_start, x_goal)
- plot = plotting.Plotting(x_start, x_goal)
- fig_name = "Breadth-first Searching (BFS)"
+ bfs = BFS(s_start, s_goal)
+ plot = plotting.Plotting(s_start, s_goal)
path, visited = bfs.searching()
- plot.animation(path, visited, fig_name) # animation
+ plot.animation(path, visited, "Breadth-first Searching (BFS)")
if __name__ == '__main__':
diff --git a/Search-based Planning/Search_2D/bidirectional_a_star.py b/Search-based Planning/Search_2D/bidirectional_a_star.py
index 3e49caf..4f8035f 100644
--- a/Search-based Planning/Search_2D/bidirectional_a_star.py
+++ b/Search-based Planning/Search_2D/bidirectional_a_star.py
@@ -5,6 +5,7 @@ Bidirectional_a_star 2D
import os
import sys
+import math
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../Search-based Planning/")
@@ -15,50 +16,51 @@ from Search_2D import env
class BidirectionalAstar:
- def __init__(self, x_start, x_goal, heuristic_type):
- self.xI, self.xG = x_start, x_goal
+ def __init__(self, s_start, s_goal, heuristic_type):
+ self.s_start, self.s_goal = s_start, s_goal
self.heuristic_type = heuristic_type
- self.Env = env.Env() # class Env
+ self.Env = env.Env() # class Env
- 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.g_fore = {self.xI: 0, self.xG: float("inf")} # cost to come: from x_start
- self.g_back = {self.xG: 0, self.xI: float("inf")} # cost to come: form x_goal
+ self.g_fore = {self.s_start: 0, self.s_goal: float("inf")} # cost to come: from s_start
+ self.g_back = {self.s_goal: 0, self.s_start: float("inf")} # cost to come: form s_goal
- self.OPEN_fore = queue.QueuePrior() # U set for foreward searching
- self.OPEN_fore.put(self.xI, self.g_fore[self.xI] + self.h(self.xI, self.xG))
- self.OPEN_back = queue.QueuePrior() # U set for backward searching
- self.OPEN_back.put(self.xG, self.g_back[self.xG] + self.h(self.xG, self.xI))
+ self.OPEN_fore = queue.QueuePrior() # U set for foreward searching
+ self.OPEN_fore.put(self.s_start,
+ self.g_fore[self.s_start] + self.h(self.s_start, self.s_goal))
+ self.OPEN_back = queue.QueuePrior() # U set for backward searching
+ self.OPEN_back.put(self.s_goal,
+ self.g_back[self.s_goal] + self.h(self.s_goal, self.s_start))
- self.CLOSED_fore = [] # CLOSED set for foreward
- self.CLOSED_back = [] # CLOSED set for backward
+ self.CLOSED_fore = [] # CLOSED set for foreward
+ self.CLOSED_back = [] # CLOSED set for backward
- self.PARENT_fore = {self.xI: self.xI}
- self.PARENT_back = {self.xG: self.xG}
+ self.PARENT_fore = {self.s_start: self.s_start}
+ self.PARENT_back = {self.s_goal: self.s_goal}
def searching(self):
- s_meet = self.xI
+ s_meet = self.s_start
- while not self.OPEN_fore.empty() and not self.OPEN_back.empty():
+ while self.OPEN_fore and self.OPEN_back:
# solve foreward-search
s_fore = self.OPEN_fore.get()
+
if s_fore in self.PARENT_back:
s_meet = s_fore
break
self.CLOSED_fore.append(s_fore)
- for u in self.u_set:
- s_next = tuple([s_fore[i] + u[i] for i in range(2)])
- if s_next not in self.obs:
- new_cost = self.g_fore[s_fore] + self.get_cost(s_fore, u)
- if s_next not in self.g_fore:
- self.g_fore[s_next] = float("inf")
- if new_cost < self.g_fore[s_next]:
- self.g_fore[s_next] = new_cost
- self.PARENT_fore[s_next] = s_fore
- self.OPEN_fore.put(s_next, new_cost + self.h(s_next, self.xG))
+ for s_n in self.get_neighbor(s_fore):
+ new_cost = self.g_fore[s_fore] + self.cost(s_fore, s_n)
+ if s_n not in self.g_fore:
+ self.g_fore[s_n] = float("inf")
+ if new_cost < self.g_fore[s_n]:
+ self.g_fore[s_n] = new_cost
+ self.PARENT_fore[s_n] = s_fore
+ self.OPEN_fore.put(s_n, new_cost + self.h(s_n, self.s_goal))
# solve backward-search
s_back = self.OPEN_back.get()
@@ -67,20 +69,40 @@ class BidirectionalAstar:
break
self.CLOSED_back.append(s_back)
- for u in self.u_set:
- s_next = tuple([s_back[i] + u[i] for i in range(len(s_back))])
- if s_next not in self.obs:
- new_cost = self.g_back[s_back] + self.get_cost(s_back, u)
- if s_next not in self.g_back:
- self.g_back[s_next] = float("inf")
- if new_cost < self.g_back[s_next]:
- self.g_back[s_next] = new_cost
- self.PARENT_back[s_next] = s_back
- self.OPEN_back.put(s_next, new_cost + self.h(s_next, self.xI))
+ for s_n in self.get_neighbor(s_back):
+ new_cost = self.g_back[s_back] + self.cost(s_back, s_n)
+ if s_n not in self.g_back:
+ self.g_back[s_n] = float("inf")
+ if new_cost < self.g_back[s_n]:
+ self.g_back[s_n] = new_cost
+ self.PARENT_back[s_n] = s_back
+ self.OPEN_back.put(s_n, new_cost + self.h(s_n, self.s_start))
return self.extract_path(s_meet), self.CLOSED_fore, self.CLOSED_back
+ def get_neighbor(self, s):
+ """
+ find neighbors of state s that not in obstacles.
+ :param s: state
+ :return: neighbors
+ """
+
+ s_list = set()
+
+ for u in self.u_set:
+ s_next = tuple([s[i] + u[i] for i in range(2)])
+ if s_next not in self.obs:
+ s_list.add(s_next)
+
+ return s_list
+
def extract_path(self, s_meet):
+ """
+ extract path from start and goal
+ :param s_meet: meet point of bi-direction a*
+ :return: path
+ """
+
# extract path for foreward part
path_fore = [s_meet]
s = s_meet
@@ -88,7 +110,7 @@ class BidirectionalAstar:
while True:
s = self.PARENT_fore[s]
path_fore.append(s)
- if s == self.xI:
+ if s == self.s_start:
break
# extract path for backward part
@@ -98,7 +120,7 @@ class BidirectionalAstar:
while True:
s = self.PARENT_back[s]
path_back.append(s)
- if s == self.xG:
+ if s == self.s_goal:
break
return list(reversed(path_fore)) + list(path_back)
@@ -115,17 +137,15 @@ class BidirectionalAstar:
if heuristic_type == "manhattan":
return abs(goal[0] - s[0]) + abs(goal[1] - s[1])
- elif heuristic_type == "euclidean":
- return ((goal[0] - s[0]) ** 2 + (goal[1] - s[1]) ** 2) ** (1 / 2)
else:
- print("Please choose right heuristic type!")
+ return math.hypot(goal[0] - s[0], goal[1] - s[1])
@staticmethod
- def get_cost(x, u):
+ def cost(s_start, s_goal):
"""
Calculate cost for this motion
- :param x: current node
- :param u: input
+ :param s_start: starting node
+ :param s_goal: end node
:return: cost for this motion
:note: cost function could be more complicate!
"""
@@ -139,10 +159,9 @@ def main():
bastar = BidirectionalAstar(x_start, x_goal, "euclidean")
plot = plotting.Plotting(x_start, x_goal)
- fig_name = "Bidirectional-A*"
path, visited_fore, visited_back = bastar.searching()
- plot.animation_bi_astar(path, visited_fore, visited_back, fig_name) # animation
+ plot.animation_bi_astar(path, visited_fore, visited_back, "Bidirectional-A*") # animation
if __name__ == '__main__':
diff --git a/Search-based Planning/Search_2D/dfs.py b/Search-based Planning/Search_2D/dfs.py
index edd02cc..c168be1 100644
--- a/Search-based Planning/Search_2D/dfs.py
+++ b/Search-based Planning/Search_2D/dfs.py
@@ -1,5 +1,5 @@
"""
-DFS 2D
+Depth-first Searching_2D (DFS)
@author: huiming zhou
"""
@@ -15,69 +15,83 @@ from Search_2D import env
class DFS:
- def __init__(self, x_start, x_goal):
- self.xI, self.xG = x_start, x_goal
+ def __init__(self, s_start, s_goal):
+ self.s_start, self.s_goal = s_start, s_goal
self.Env = env.Env()
- self.plotting = plotting.Plotting(self.xI, self.xG)
+ self.plotting = plotting.Plotting(self.s_start, self.s_goal)
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
- self.OPEN = queue.QueueLIFO() # U set: visited nodes
- self.OPEN.put(self.xI)
- self.CLOSED = [] # CLOSED set: explored nodes
- self.PARENT = {self.xI: self.xI} # relations
+ self.OPEN = queue.QueueLIFO() # OPEN set
+ self.OPEN.put(self.s_start)
+ self.CLOSED = [] # CLOSED set / visited order
+ self.PARENT = {self.s_start: self.s_start}
def searching(self):
"""
- Searching using DFS.
-
- :return: planning path, action in each node, visited nodes in the planning process
+ Depth-first Searching
+ :return: planning path, visited order
"""
- while not self.OPEN.empty():
+ while self.OPEN:
s = self.OPEN.get()
- if s == self.xG:
+
+ if s == self.s_goal:
break
self.CLOSED.append(s)
- for u in self.u_set: # explore neighborhoods
- s_next = tuple([s[i] + u[i] for i in range(2)])
- if s_next not in self.PARENT and s_next not in self.obs: # node not visited and not in obstacles
- self.OPEN.put(s_next)
- self.PARENT[s_next] = s
+ for s_n in self.get_neighbor(s):
+ if s_n not in self.PARENT: # node not explored
+ self.OPEN.put(s_n)
+ self.PARENT[s_n] = s
return self.extract_path(), self.CLOSED
+ def get_neighbor(self, s):
+ """
+ find neighbors of state s that not in obstacles.
+ :param s: state
+ :return: neighbors
+ """
+
+ s_list = []
+
+ for u in self.u_set:
+ s_next = tuple([s[i] + u[i] for i in range(2)])
+ if s_next not in self.obs:
+ s_list.append(s_next)
+
+ return s_list
+
def extract_path(self):
"""
Extract the path based on the relationship of nodes.
:return: The planning path
"""
- path = [self.xG]
- s = self.xG
+ path = [self.s_goal]
+ s = self.s_goal
while True:
s = self.PARENT[s]
path.append(s)
- if s == self.xI:
+ if s == self.s_start:
break
return list(path)
def main():
- x_start = (5, 5)
- x_goal = (45, 25)
+ s_start = (5, 5)
+ s_goal = (45, 25)
- dfs = DFS(x_start, x_goal)
- plot = plotting.Plotting(x_start, x_goal)
- fig_name = "Depth-first Searching (DFS)"
+ dfs = DFS(s_start, s_goal)
+ plot = plotting.Plotting(s_start, s_goal)
path, visited = dfs.searching()
- plot.animation(path, visited, fig_name) # animation
+ plot.animation(path, visited, "Depth-first Searching (DFS)") # animation
if __name__ == '__main__':
diff --git a/Search-based Planning/Search_2D/dijkstra.py b/Search-based Planning/Search_2D/dijkstra.py
index 896968e..e8667de 100644
--- a/Search-based Planning/Search_2D/dijkstra.py
+++ b/Search-based Planning/Search_2D/dijkstra.py
@@ -15,72 +15,85 @@ from Search_2D import env
class Dijkstra:
- def __init__(self, x_start, x_goal):
- self.xI, self.xG = x_start, x_goal
+ def __init__(self, s_start, s_goal):
+ self.s_start, self.s_goal = s_start, s_goal
self.Env = env.Env()
- self.plotting = plotting.Plotting(self.xI, self.xG)
+ self.plotting = plotting.Plotting(self.s_start, self.s_goal)
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
- self.g = {self.xI: 0, self.xG: float("inf")} # cost to come
+ self.g = {self.s_start: 0, self.s_goal: float("inf")} # cost to come
self.OPEN = queue.QueuePrior() # priority queue / U set
- self.OPEN.put(self.xI, 0)
+ self.OPEN.put(self.s_start, 0)
self.CLOSED = [] # closed set & visited
- self.PARENT = {self.xI: self.xI} # relations
+ self.PARENT = {self.s_start: self.s_start}
def searching(self):
"""
- Searching using Dijkstra.
+ Dijkstra Searching.
:return: path, order of visited nodes in the planning
"""
- while not self.OPEN.empty():
+ while self.OPEN:
s = self.OPEN.get()
- if s == self.xG: # stop condition
+
+ if s == self.s_goal: # stop condition
break
self.CLOSED.append(s)
- for u in self.u_set: # explore neighborhoods
- s_next = tuple([s[i] + u[i] for i in range(2)])
- if s_next not in self.obs: # node not visited and not in obstacles
- new_cost = self.g[s] + self.get_cost(s, u)
- if s_next not in self.g:
- self.g[s_next] = float("inf")
- if new_cost < self.g[s_next]:
- self.g[s_next] = new_cost
- self.OPEN.put(s_next, new_cost)
- self.PARENT[s_next] = s
+ for s_n in self.get_neighbor(s):
+ new_cost = self.g[s] + self.cost(s, s_n)
+ if s_n not in self.g:
+ self.g[s_n] = float("inf")
+ if new_cost < self.g[s_n]:
+ self.g[s_n] = new_cost
+ self.OPEN.put(s_n, new_cost)
+ self.PARENT[s_n] = s
return self.extract_path(), self.CLOSED
+ def get_neighbor(self, s):
+ """
+ find neighbors of state s that not in obstacles.
+ :param s: state
+ :return: neighbors
+ """
+
+ s_list = set()
+
+ for u in self.u_set:
+ s_next = tuple([s[i] + u[i] for i in range(2)])
+ if s_next not in self.obs:
+ s_list.add(s_next)
+
+ return s_list
+
def extract_path(self):
"""
- Extract the path based on the relationship of nodes.
-
+ Extract the path based on PARENT set.
:return: The planning path
"""
- path_back = [self.xG]
- x_current = self.xG
+ path = [self.s_goal]
+ s = self.s_goal
while True:
- x_current = self.PARENT[x_current]
- path_back.append(x_current)
+ s = self.PARENT[s]
+ path.append(s)
- if x_current == self.xI:
+ if s == self.s_start:
break
- return list(path_back)
+ return list(path)
@staticmethod
- def get_cost(x, u):
+ def cost(s_start, s_goal):
"""
Calculate cost for this motion
-
- :param x: current node
- :param u: input
+ :param s_start: starting node
+ :param s_goal: end node
:return: cost for this motion
:note: cost function could be more complicate!
"""
@@ -89,15 +102,14 @@ class Dijkstra:
def main():
- x_start = (5, 5)
- x_goal = (45, 25)
+ s_start = (5, 5)
+ s_goal = (45, 25)
- dijkstra = Dijkstra(x_start, x_goal)
- plot = plotting.Plotting(x_start, x_goal) # class Plotting
+ dijkstra = Dijkstra(s_start, s_goal)
+ plot = plotting.Plotting(s_start, s_goal)
- fig_name = "Dijkstra's"
path, visited = dijkstra.searching()
- plot.animation(path, visited, fig_name) # animation generate
+ plot.animation(path, visited, "Dijkstra's") # animation generate
if __name__ == '__main__':
diff --git a/Search-based Planning/Search_2D/env.py b/Search-based Planning/Search_2D/env.py
index ca17d1f..e08da2f 100644
--- a/Search-based Planning/Search_2D/env.py
+++ b/Search-based Planning/Search_2D/env.py
@@ -14,7 +14,6 @@ class Env:
def obs_map(self):
"""
Initialize obstacles' positions
-
:return: map of obstacles
"""