diff --git a/.idea/PathPlanning.iml b/.idea/PathPlanning.iml
new file mode 100644
index 0000000..7c9d48f
--- /dev/null
+++ b/.idea/PathPlanning.iml
@@ -0,0 +1,12 @@
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml
new file mode 100644
index 0000000..105ce2d
--- /dev/null
+++ b/.idea/inspectionProfiles/profiles_settings.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/misc.xml b/.idea/misc.xml
new file mode 100644
index 0000000..a2e120d
--- /dev/null
+++ b/.idea/misc.xml
@@ -0,0 +1,4 @@
+
+
+
+
\ No newline at end of file
diff --git a/.idea/modules.xml b/.idea/modules.xml
new file mode 100644
index 0000000..d8bd288
--- /dev/null
+++ b/.idea/modules.xml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/.idea/vcs.xml b/.idea/vcs.xml
new file mode 100644
index 0000000..94a25f7
--- /dev/null
+++ b/.idea/vcs.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/Sampling_based_Planning/gif/FMT.gif b/Sampling_based_Planning/gif/FMT.gif
new file mode 100644
index 0000000..4648087
Binary files /dev/null and b/Sampling_based_Planning/gif/FMT.gif differ
diff --git a/Sampling_based_Planning/rrt_2D/fast_marching_trees.py b/Sampling_based_Planning/rrt_2D/fast_marching_trees.py
index e69de29..efda0e5 100644
--- a/Sampling_based_Planning/rrt_2D/fast_marching_trees.py
+++ b/Sampling_based_Planning/rrt_2D/fast_marching_trees.py
@@ -0,0 +1,220 @@
+"""
+Fast Marching Trees (FMT*)
+@author: huiming zhou
+"""
+
+import os
+import sys
+import math
+import random
+import numpy as np
+import matplotlib.pyplot as plt
+import matplotlib.patches as patches
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
+ "/../../Sampling_based_Planning/")
+
+from Sampling_based_Planning.rrt_2D import env, plotting, utils
+
+
+class Node:
+ def __init__(self, n):
+ self.x = n[0]
+ self.y = n[1]
+ self.parent = None
+ self.cost = np.inf
+
+
+class FMT:
+ def __init__(self, x_start, x_goal, search_radius):
+ self.x_init = Node(x_start)
+ self.x_goal = Node(x_goal)
+ self.search_radius = search_radius
+
+ self.env = env.Env()
+ self.plotting = plotting.Plotting(x_start, x_goal)
+ self.utils = utils.Utils()
+
+ self.fig, self.ax = plt.subplots()
+ self.delta = self.utils.delta
+ self.x_range = self.env.x_range
+ self.y_range = self.env.y_range
+ self.obs_circle = self.env.obs_circle
+ self.obs_rectangle = self.env.obs_rectangle
+ self.obs_boundary = self.env.obs_boundary
+
+ self.V = set()
+ self.V_unvisited = set()
+ self.V_open = set()
+ self.V_closed = set()
+ self.sample_numbers = 1000
+
+ def Init(self):
+ samples = self.SampleFree()
+
+ self.x_init.cost = 0.0
+ self.V.add(self.x_init)
+ self.V.update(samples)
+ self.V_unvisited.update(samples)
+ self.V_unvisited.add(self.x_goal)
+ self.V_open.add(self.x_init)
+
+ def Planning(self):
+ self.Init()
+ z = self.x_init
+ n = self.sample_numbers
+ rn = self.search_radius * math.sqrt((math.log(n) / n))
+ Visited = []
+
+ while z is not self.x_goal:
+ V_open_new = set()
+ X_near = self.Near(self.V_unvisited, z, rn)
+ Visited.append(z)
+
+ for x in X_near:
+ Y_near = self.Near(self.V_open, x, rn)
+ cost_list = {y: y.cost + self.Cost(y, x) for y in Y_near}
+ y_min = min(cost_list, key=cost_list.get)
+
+ if not self.utils.is_collision(y_min, x):
+ x.parent = y_min
+ V_open_new.add(x)
+ self.V_unvisited.remove(x)
+ x.cost = y_min.cost + self.Cost(y_min, x)
+
+ self.V_open.update(V_open_new)
+ self.V_open.remove(z)
+ self.V_closed.add(z)
+
+ if not self.V_open:
+ print("open set empty!")
+ break
+
+ cost_open = {y: y.cost for y in self.V_open}
+ z = min(cost_open, key=cost_open.get)
+
+ # node_end = self.ChooseGoalPoint()
+ path_x, path_y = self.ExtractPath()
+ self.animation(path_x, path_y, Visited[1: len(Visited)])
+
+ def ChooseGoalPoint(self):
+ Near = self.Near(self.V, self.x_goal, 2.0)
+ cost = {y: y.cost + self.Cost(y, self.x_goal) for y in Near}
+
+ return min(cost, key=cost.get)
+
+ def ExtractPath(self):
+ path_x, path_y = [], []
+ node = self.x_goal
+
+ while node.parent:
+ path_x.append(node.x)
+ path_y.append(node.y)
+ node = node.parent
+
+ path_x.append(self.x_init.x)
+ path_y.append(self.x_init.y)
+
+ return path_x, path_y
+
+ def Cost(self, x_start, x_end):
+ if self.utils.is_collision(x_start, x_end):
+ return np.inf
+ else:
+ return self.calc_dist(x_start, x_end)
+
+ @staticmethod
+ def calc_dist(x_start, x_end):
+ return math.hypot(x_start.x - x_end.x, x_start.y - x_end.y)
+
+ @staticmethod
+ def Near(nodelist, z, rn):
+ return {nd for nd in nodelist
+ if 0 < (nd.x - z.x) ** 2 + (nd.y - z.y) ** 2 <= rn ** 2}
+
+ def SampleFree(self):
+ n = self.sample_numbers
+ delta = self.utils.delta
+ Sample = set()
+
+ ind = 0
+ while ind < n:
+ node = Node((random.uniform(self.x_range[0] + delta, self.x_range[1] - delta),
+ random.uniform(self.y_range[0] + delta, self.y_range[1] - delta)))
+ if self.utils.is_inside_obs(node):
+ continue
+ else:
+ Sample.add(node)
+ ind += 1
+
+ return Sample
+
+ def animation(self, path_x, path_y, visited):
+ self.plot_grid("Fast Marching Trees (FMT*)")
+
+ for node in self.V:
+ plt.plot(node.x, node.y, marker='.', color='lightgrey', markersize=3)
+
+ count = 0
+ for node in visited:
+ count += 1
+ plt.plot([node.x, node.parent.x], [node.y, node.parent.y], '-g')
+ plt.gcf().canvas.mpl_connect(
+ 'key_release_event',
+ lambda event: [exit(0) if event.key == 'escape' else None])
+ if count % 10 == 0:
+ plt.pause(0.001)
+
+ plt.plot(path_x, path_y, linewidth=2, color='red')
+ plt.pause(0.01)
+ plt.show()
+
+ def plot_grid(self, name):
+
+ for (ox, oy, w, h) in self.obs_boundary:
+ self.ax.add_patch(
+ patches.Rectangle(
+ (ox, oy), w, h,
+ edgecolor='black',
+ facecolor='black',
+ fill=True
+ )
+ )
+
+ for (ox, oy, w, h) in self.obs_rectangle:
+ self.ax.add_patch(
+ patches.Rectangle(
+ (ox, oy), w, h,
+ edgecolor='black',
+ facecolor='gray',
+ fill=True
+ )
+ )
+
+ for (ox, oy, r) in self.obs_circle:
+ self.ax.add_patch(
+ patches.Circle(
+ (ox, oy), r,
+ edgecolor='black',
+ facecolor='gray',
+ fill=True
+ )
+ )
+
+ plt.plot(self.x_init.x, self.x_init.y, "bs", linewidth=3)
+ plt.plot(self.x_goal.x, self.x_goal.y, "rs", linewidth=3)
+
+ plt.title(name)
+ plt.axis("equal")
+
+
+def main():
+ x_start = (18, 8) # Starting node
+ x_goal = (37, 18) # Goal node
+
+ fmt = FMT(x_start, x_goal, 40)
+ fmt.Planning()
+
+
+if __name__ == '__main__':
+ main()
diff --git a/Search_based_Planning/Search_2D/Bidirectional_a_star.py b/Search_based_Planning/Search_2D/Bidirectional_a_star.py
index 6da37b4..692dc5e 100644
--- a/Search_based_Planning/Search_2D/Bidirectional_a_star.py
+++ b/Search_based_Planning/Search_2D/Bidirectional_a_star.py
@@ -25,7 +25,7 @@ class BidirectionalAstar:
self.u_set = self.Env.motions # feasible input set
self.obs = self.Env.obs # position of obstacles
- self.g_fore = {self.s_start: 0, self.s_goal: float("inf")} # Cost to come: from x_start
+ self.g_fore = {self.s_start: 0, self.s_goal: float("inf")} # Cost to come: from x_init
self.g_back = {self.s_goal: 0, self.s_start: float("inf")} # Cost to come: form x_goal
self.OPEN_fore = queue.QueuePrior() # OPEN set for foreward searching
diff --git a/Search_based_Planning/Search_2D/Dijkstra.py b/Search_based_Planning/Search_2D/Dijkstra.py
index 5cd7325..0b06239 100644
--- a/Search_based_Planning/Search_2D/Dijkstra.py
+++ b/Search_based_Planning/Search_2D/Dijkstra.py
@@ -10,9 +10,7 @@ import math
sys.path.append(os.path.dirname(os.path.abspath(__file__)) +
"/../../Search_based_Planning/")
-from Search_2D import queue
-from Search_2D import plotting
-from Search_2D import env
+from Search_based_Planning.Search_2D import queue, plotting, env
class Dijkstra:
diff --git a/Search_based_Planning/Search_2D/LRTAstar.py b/Search_based_Planning/Search_2D/LRTAstar.py
index cd91bd3..3d7bc23 100644
--- a/Search_based_Planning/Search_2D/LRTAstar.py
+++ b/Search_based_Planning/Search_2D/LRTAstar.py
@@ -50,7 +50,7 @@ class LrtAstarN:
for x in h_value:
self.h_table[x] = h_value[x]
- s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value) # x_start -> expected node in OPEN set
+ s_start, path_k = self.extract_path_in_CLOSE(s_start, h_value) # x_init -> expected node in OPEN set
self.path.append(path_k)
def extract_path_in_CLOSE(self, s_start, h_value):
diff --git a/Search_based_Planning/Search_3D/Anytime_Dstar3D.py b/Search_based_Planning/Search_3D/Anytime_Dstar3D.py
index 10b8eb8..30e6200 100644
--- a/Search_based_Planning/Search_3D/Anytime_Dstar3D.py
+++ b/Search_based_Planning/Search_3D/Anytime_Dstar3D.py
@@ -207,9 +207,9 @@ class Anytime_Dstar(object):
def path(self, s_start=None):
'''After ComputeShortestPath()
- returns, one can then follow a shortest path from x_start to
+ returns, one can then follow a shortest path from x_init to
x_goal by always moving from the current vertex s, starting
- at x_start. , to any successor s' that minimizes c(s,s') + g(s')
+ at x_init. , to any successor s' that minimizes c(s,s') + g(s')
until x_goal is reached (ties can be broken arbitrarily).'''
path = []
s_goal = self.xt
diff --git a/Search_based_Planning/Search_3D/DstarLite3D.py b/Search_based_Planning/Search_3D/DstarLite3D.py
index 3621eda..8706fa3 100644
--- a/Search_based_Planning/Search_3D/DstarLite3D.py
+++ b/Search_based_Planning/Search_3D/DstarLite3D.py
@@ -186,9 +186,9 @@ class D_star_Lite(object):
def path(self, s_start=None):
'''After ComputeShortestPath()
- returns, one can then follow a shortest path from x_start to
+ returns, one can then follow a shortest path from x_init to
x_goal by always moving from the current vertex s, starting
- at x_start. , to any successor s' that minimizes c(s,s') + g(s')
+ at x_init. , to any successor s' that minimizes c(s,s') + g(s')
until x_goal is reached (ties can be broken arbitrarily).'''
path = []
s_goal = self.xt