diff --git a/Sampling-based Planning/.idea/.gitignore b/Sampling-based Planning/.idea/.gitignore new file mode 100644 index 0000000..26d3352 --- /dev/null +++ b/Sampling-based Planning/.idea/.gitignore @@ -0,0 +1,3 @@ +# Default ignored files +/shelf/ +/workspace.xml diff --git a/Sampling-based Planning/.idea/Sampling-based Planning.iml b/Sampling-based Planning/.idea/Sampling-based Planning.iml new file mode 100644 index 0000000..8dc09e5 --- /dev/null +++ b/Sampling-based Planning/.idea/Sampling-based Planning.iml @@ -0,0 +1,11 @@ + + + + + + + + + + \ No newline at end of file diff --git a/Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml b/Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/Sampling-based Planning/.idea/misc.xml b/Sampling-based Planning/.idea/misc.xml new file mode 100644 index 0000000..0e7ac62 --- /dev/null +++ b/Sampling-based Planning/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/Sampling-based Planning/.idea/modules.xml b/Sampling-based Planning/.idea/modules.xml new file mode 100644 index 0000000..caf2a07 --- /dev/null +++ b/Sampling-based Planning/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Sampling-based Planning/.idea/vcs.xml b/Sampling-based Planning/.idea/vcs.xml new file mode 100644 index 0000000..6c0b863 --- /dev/null +++ b/Sampling-based Planning/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/Sampling-based Planning/RRT.py b/Sampling-based Planning/RRT.py new file mode 100644 index 0000000..2b7e264 --- /dev/null +++ b/Sampling-based Planning/RRT.py @@ -0,0 +1,49 @@ +import env +import plotting +import matplotlib.pyplot as plt +import matplotlib.patches as patches + + +class RRT: + def __init__(self, xI, xG): + # Plotting = plotting.Plotting(xI, xG) + # Plotting.animation([xI, xG], [xI, xG], "zhou") + fig, ax = plt.subplots() + + plt.axis([-5, 5, -5, 5]) + + ax.plot() + + ax.add_patch( + patches.Rectangle( + (1, 1), + 0.5, + 0.5, + edgecolor='black', + facecolor='black', + fill=True + )) + + ax.add_patch( + patches.Circle( + (3, 3), + 0.5, + edgecolor='black', + facecolor='black', + fill=True + ) + ) + + plt.axis("equal") + plt.show() + + + def planning(self): + return + + +if __name__ == '__main__': + x_Start = (5, 5) # Starting node + x_Goal = (49, 5) # Goal node + + rrt = RRT(x_Start, x_Goal) diff --git a/Sampling-based Planning/__pycache__/env.cpython-37.pyc b/Sampling-based Planning/__pycache__/env.cpython-37.pyc new file mode 100644 index 0000000..de7dec1 Binary files /dev/null and b/Sampling-based Planning/__pycache__/env.cpython-37.pyc differ diff --git a/Sampling-based Planning/__pycache__/plotting.cpython-37.pyc b/Sampling-based Planning/__pycache__/plotting.cpython-37.pyc new file mode 100644 index 0000000..667c4d5 Binary files /dev/null and b/Sampling-based Planning/__pycache__/plotting.cpython-37.pyc differ diff --git a/Sampling-based Planning/env.py b/Sampling-based Planning/env.py new file mode 100644 index 0000000..9ee379f --- /dev/null +++ b/Sampling-based Planning/env.py @@ -0,0 +1,30 @@ +import numpy as np + +class Env: + def __init__(self): + self.x_range = (0, 50) # size of background + self.y_range = (0, 30) + self.obs = self.obs_map() + + def obs_map(self): + """ + Initialize obstacles' positions + + :return: map of obstacles + """ + + x = self.x_range + y = self.y_range + w = 2 + + obs_boundary = [] + + for i in np.linspace(x[0], x[1], (x[1]-x[0])//w+1): + obs_boundary.append((i, y[0], w)) + for i in np.linspace(x[0], x[1], (x[1]-x[0])//w+1): + obs_boundary.append((i, y[1], w)) + for j in np.linspace(y[0], y[1], (y[1]-y[0])//w+1): + obs_boundary.append((j, x[0], w)) + for j in np.linspace(y[0], y[1], (y[1]-y[0])//w+1): + obs_boundary.append((j, x[1], w)) + diff --git a/Sampling-based Planning/plotting.py b/Sampling-based Planning/plotting.py new file mode 100644 index 0000000..d552103 --- /dev/null +++ b/Sampling-based Planning/plotting.py @@ -0,0 +1,53 @@ +import matplotlib.pyplot as plt +import env + + +class Plotting: + def __init__(self, xI, xG): + self.xI, self.xG = xI, xG + self.env = env.Env() + self.obs = self.env.obs_map() + + def animation(self, path, visited, name): + self.plot_grid(name) + self.plot_visited(visited) + self.plot_path(path) + + def plot_grid(self, name): + obs_x = [self.obs[i][0] for i in range(len(self.obs))] + obs_y = [self.obs[i][1] for i in range(len(self.obs))] + + plt.plot(self.xI[0], self.xI[1], "bs") + plt.plot(self.xG[0], self.xG[1], "gs") + plt.plot(obs_x, obs_y, "sk") + plt.title(name) + plt.axis("equal") + + def plot_visited(self, visited): + visited.remove(self.xI) + count = 0 + + for x in visited: + count += 1 + plt.plot(x[0], x[1], linewidth='3', color='#808080', marker='o') + plt.gcf().canvas.mpl_connect('key_release_event', lambda event: + [exit(0) if event.key == 'escape' else None]) + + if count < len(visited) / 3: + length = 15 + elif count < len(visited) * 2 / 3: + length = 30 + else: + length = 45 + + if count % length == 0: plt.pause(0.001) + + def plot_path(self, path): + path.remove(self.xI) + path.remove(self.xG) + path_x = [path[i][0] for i in range(len(path))] + path_y = [path[i][1] for i in range(len(path))] + + plt.plot(path_x, path_y, linewidth='3', color='r', marker='o') + plt.pause(0.01) + plt.show() diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index fe08e6f..92bf920 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -1,7 +1,31 @@ + + + + + + + + + + + + + + + + +