diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml
index 6847f38..3567508 100644
--- a/Search-based Planning/.idea/workspace.xml
+++ b/Search-based Planning/.idea/workspace.xml
@@ -24,6 +24,7 @@
+
@@ -78,6 +79,27 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -141,34 +163,13 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
@@ -203,22 +204,22 @@
-
+
-
-
+
+
-
-
+
+
-
-
+
+
-
+
diff --git a/Search-based Planning/__pycache__/plotting.cpython-37.pyc b/Search-based Planning/__pycache__/plotting.cpython-37.pyc
index a7403ea..b091284 100644
Binary files a/Search-based Planning/__pycache__/plotting.cpython-37.pyc and b/Search-based Planning/__pycache__/plotting.cpython-37.pyc differ
diff --git a/Search-based Planning/__pycache__/queue.cpython-37.pyc b/Search-based Planning/__pycache__/queue.cpython-37.pyc
index e1eea17..82b863f 100644
Binary files a/Search-based Planning/__pycache__/queue.cpython-37.pyc and b/Search-based Planning/__pycache__/queue.cpython-37.pyc differ
diff --git a/Search-based Planning/a_star.py b/Search-based Planning/a_star.py
index e7fabf1..5d4cfb9 100644
--- a/Search-based Planning/a_star.py
+++ b/Search-based Planning/a_star.py
@@ -6,14 +6,21 @@ import env
class Astar:
def __init__(self, x_start, x_goal, e, heuristic_type):
self.xI, self.xG = x_start, x_goal
- self.e = e
self.heuristic_type = heuristic_type
self.Env = env.Env() # class Env
+ self.e = e
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")}
+ self.fig_name = "A* Algorithm"
+
+ self.OPEN = queue.QueuePrior() # priority queue / OPEN
+ self.OPEN.put(self.xI, self.fvalue(self.xI))
+ self.parent = {self.xI: self.xI}
+
def searching(self):
"""
Searching using A_star.
@@ -21,61 +28,46 @@ class Astar:
:return: planning path, action in each node, visited nodes in the planning process
"""
- xI = self.xI
- xG = self.xG
- heuristic_type = self.heuristic_type
- e = self.e
-
- q_astar = queue.QueuePrior() # priority queue / OPEN
- q_astar.put(xI, e * self.Heuristic(xI, xG, heuristic_type))
- 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
+ while not self.OPEN.empty():
+ s = self.OPEN.get()
+ if s == self.xG: # stop condition
break
- visited.append(x_current)
+ visited.append(s)
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
- cost[x_next] = new_cost
- priority = new_cost + e * self.Heuristic(x_next, xG, heuristic_type)
- q_astar.put(x_next, priority) # put node into queue using priority "f+h"
- parent[x_next], action[x_next] = x_current, u_next
+ 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]: # conditions for updating cost
+ self.g[s_next] = new_cost
+ self.parent[s_next] = s
+ self.OPEN.put(s_next, self.fvalue(s_next))
- [path, policy] = self.extract_path(xI, xG, parent, action)
+ return self.extract_path(), visited
- return path, policy, visited
+ def fvalue(self, x):
+ h = self.e * self.Heuristic(x)
+ return self.g[x] + h
- @staticmethod
- def extract_path(xI, xG, parent, policy):
+ def extract_path(self):
"""
Extract the path based on the relationship of nodes.
- :param xI: Starting node
- :param xG: Goal node
- :param parent: Relationship between nodes
- :param policy: Action needed for transfer between two nodes
:return: The planning path
"""
- path_back = [xG]
- acts_back = [policy[xG]]
- x_current = xG
- while True:
- x_current = parent[x_current]
- path_back.append(x_current)
- acts_back.append(policy[x_current])
+ path_back = [self.xG]
+ x_current = self.xG
- if x_current == xI:
+ while True:
+ x_current = self.parent[x_current]
+ path_back.append(x_current)
+
+ if x_current == self.xI:
break
- return list(path_back), list(acts_back)
+ return list(path_back)
@staticmethod
def get_cost(x, u):
@@ -90,8 +82,7 @@ class Astar:
return 1
- @staticmethod
- def Heuristic(state, goal, heuristic_type):
+ def Heuristic(self, state):
"""
Calculate heuristic.
@@ -101,6 +92,9 @@ class Astar:
: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":
@@ -117,7 +111,7 @@ def main():
plot = plotting.Plotting(x_start, x_goal) # class Plotting
fig_name = "A* Algorithm"
- path, policy, visited = astar.searching()
+ path, visited = astar.searching()
plot.animation(path, visited, fig_name) # animation generate
diff --git a/Search-based Planning/ara_star.py b/Search-based Planning/ara_star.py
index 992b5fe..c5f9683 100644
--- a/Search-based Planning/ara_star.py
+++ b/Search-based Planning/ara_star.py
@@ -2,20 +2,165 @@ import queue
import plotting
import env
+import matplotlib.pyplot as plt
+
class AraStar:
- def __init__(self, x_start, x_goal):
+ def __init__(self, x_start, x_goal, heuristic_type):
self.xI, self.xG = x_start, x_goal
+ self.heuristic_type = heuristic_type
self.Env = env.Env() # class Env
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
+ self.e = 3
+ self.g = {self.xI: 0, self.xG: float("inf")}
+ self.fig_name = "ARA_Star Algorithm"
+
+ self.OPEN = queue.QueuePrior() # priority queue / OPEN
+ self.CLOSED = []
+ self.INCONS = []
+ self.parent = {self.xI: self.xI}
+
+ def searching(self):
+ path = []
+
+ self.OPEN.put(self.xI, self.fvalue(self.xI))
+ self.ImprovePath()
+
+ path.append(self.extract_path())
+
+ while self.update_e() > 1:
+ self.e -= 0.5
+ OPEN_mid = [x for (p, x) in self.OPEN.enumerate()] + self.INCONS
+ self.OPEN = queue.QueuePrior()
+
+ for x in OPEN_mid:
+ self.OPEN.put(x, self.fvalue(x))
+ self.INCONS = []
+ self.CLOSED = []
+ self.ImprovePath()
+
+ path.append(self.extract_path())
+
+ return path
+
+ def ImprovePath(self):
+ while (not self.OPEN.empty() and self.fvalue(self.xG) >
+ min([self.fvalue(x) for (p, x) in self.OPEN.enumerate()])):
+ s = self.OPEN.get()
+ self.CLOSED.append(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
+ if s_next not in self.CLOSED:
+ self.OPEN.put(s_next, self.fvalue(s_next))
+ else:
+ self.INCONS.append(s_next)
+
+ def update_e(self):
+ c_OPEN, c_INCONS = float("inf"), float("inf")
+
+ if not self.OPEN.empty():
+ c_OPEN = min(self.g[x] + self.Heuristic(x) for (p, x) in self.OPEN.enumerate())
+
+ if len(self.INCONS) != 0:
+ c_INCONS = min(self.g[x] + self.Heuristic(x) for x in self.INCONS)
+
+ if min(c_OPEN, c_INCONS) == float("inf"):
+ return 1
+ else:
+ return min(self.e, self.g[self.xG] / min(c_OPEN, c_INCONS))
+
+ def fvalue(self, x):
+ h = self.e * self.Heuristic(x)
+ return self.g[x] + h
+
+ def extract_path(self):
+ """
+ Extract the path based on the relationship of nodes.
+
+ :param policy: Action needed for transfer between two nodes
+ :return: The planning path
+ """
+
+ path_back = [self.xG]
+ x_current = self.xG
+
+ while True:
+ x_current = self.parent[x_current]
+ path_back.append(x_current)
+
+ if x_current == self.xI:
+ break
+
+ return list(path_back)
+
+ @staticmethod
+ def get_cost(x, u):
+ """
+ Calculate cost for this motion
+
+ :param x: current node
+ :param u: input
+ :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)
+ :param goal: goal node (state)
+ :param heuristic_type: choosing different heuristic functions
+ :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
x_goal = (49, 5) # Goal node
- arastar = AraStar(x_start, x_goal)
+ arastar = AraStar(x_start, x_goal, "manhattan")
+ plot = plotting.Plotting(x_start, x_goal)
+
+ path = arastar.searching()
+
+ plot.plot_grid("ARA*")
+
+ print(arastar.e)
+
+ for path_i in path:
+ path_i.remove(x_start)
+ path_i.remove(x_goal)
+
+ path_x = [path_i[i][0] for i in range(len(path_i))]
+ path_y = [path_i[i][1] for i in range(len(path_i))]
+
+ plt.plot(path_x, path_y, linewidth='3', marker='o')
+ plt.pause(1)
+
+ plt.show()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/Search-based Planning/queue.py b/Search-based Planning/queue.py
index 6250cd3..8f481ae 100644
--- a/Search-based Planning/queue.py
+++ b/Search-based Planning/queue.py
@@ -57,3 +57,6 @@ class QueuePrior:
def get(self):
return heapq.heappop(self.queue)[1] # pop out the smallest item
+
+ def enumerate(self):
+ return self.queue