From 2f2dee0bdb13a91c3204fae19fcef4d79df46049 Mon Sep 17 00:00:00 2001 From: zhm-real Date: Mon, 22 Jun 2020 11:41:03 -0700 Subject: [PATCH] update --- Sampling-based Planning/.idea/.gitignore | 3 + .../.idea/Sampling-based Planning.iml | 11 ++++ .../inspectionProfiles/profiles_settings.xml | 6 ++ Sampling-based Planning/.idea/misc.xml | 4 ++ Sampling-based Planning/.idea/modules.xml | 8 +++ Sampling-based Planning/.idea/vcs.xml | 6 ++ Sampling-based Planning/RRT.py | 49 ++++++++++++++++ .../__pycache__/env.cpython-37.pyc | Bin 0 -> 681 bytes .../__pycache__/plotting.cpython-37.pyc | Bin 0 -> 2653 bytes Sampling-based Planning/env.py | 30 ++++++++++ Sampling-based Planning/plotting.py | 53 ++++++++++++++++++ Search-based Planning/.idea/workspace.xml | 44 ++++++++++++--- 12 files changed, 206 insertions(+), 8 deletions(-) create mode 100644 Sampling-based Planning/.idea/.gitignore create mode 100644 Sampling-based Planning/.idea/Sampling-based Planning.iml create mode 100644 Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml create mode 100644 Sampling-based Planning/.idea/misc.xml create mode 100644 Sampling-based Planning/.idea/modules.xml create mode 100644 Sampling-based Planning/.idea/vcs.xml create mode 100644 Sampling-based Planning/RRT.py create mode 100644 Sampling-based Planning/__pycache__/env.cpython-37.pyc create mode 100644 Sampling-based Planning/__pycache__/plotting.cpython-37.pyc create mode 100644 Sampling-based Planning/env.py create mode 100644 Sampling-based Planning/plotting.py 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 0000000000000000000000000000000000000000..de7dec16b595eda00b215a1a93d0cf45026beb0f GIT binary patch literal 681 zcmZuvv2N5r5S_Jscg~SClo3MikdYEglo3J^2q7p=kwWEy)#i3{XUSfhU0*~jy5vKk z~s6!ngMtX$0aUZ_4S97!*j3XDnfcry$ z79UBX1rY30@R`6uP26&p25Y3y`UxNn92c%1GLQ1f2kDeun0#`*A^oJj@Ul`(AA+Q! zkGv@_zmSVj@as*stSggs%X*caxq9JLl{Kne9yO)1wy=vMRW52*w9Cq8=c;N--n&rV z=;Umhk{Nq-++6pul!Yx?DR~}029Oxz>-M4a`0nr@$t-)29D%efPIPal$<|)uwnCLf zXA+Fws(ESrqok=lwrlIByTlV`+Lf~>31lYq<)7?bMk*W+gfKxEv~OUKmm>i0d!C&3 zx_dnE_St6Xh?QAY87cdzlvS-)CG&kL->+1;IdS|84Hx}&IX|%*I)mYHBuxWpODP}~R?s9BNN|Wk#fK^mgh)LgSt^#}aW_u)o;M!9dGmX}*-s`XI|SO# zPyhVpwnNAt_%Lo9j5{F3O;Caenvj%wj8-=H_+28H;D2li z*rR+3QoIAIq$OjdBt8mNY{ZYyVkOyRBg@#fAg~%;?Kzt5egSe<^C;WV{BuyPe7*3~ zV4yh|J+7@HN*+l3gtS0CAre#d?5o9%JdGBI8~OI)n#}t$NEZiz+PF1Hf-HjwZv{y| zm$BMNi^Y3EI!JJAJt!h^W38UDIH-5CFxb`^p3RHw%pdC7#9`AW!JR`(F zuwmb9{!Mr&$%vHvh*Gp55w*oiiq?4BT(|MdeqY8yca6Cd7qN;&?ZA!nEZK$?wS^D5 zHJ)H)K^mD9RB8g}w1X^80~O~PKGwl^F!-4!_-0CoK){C4+aS1u;wMnwkq6WO!c|1k z$8cZ=^niXw4(L8JJ`P}c(Z>}ZSuooc);ux2Jx_#l#3`vgPVI37dRk*o$x1HV`l(gr z2;Oi-yUA8(HKY(3HVhj>z64TS0o8{j-=EpXKmm2IFqtPmvWFI8OX6G5x!5ty7M3)P z;TcnUlDKD`dtn5To4Y48;I4TlVGHd$-VX<<3Z!yqYEz!<;s|3W)H6PldGh%SSA zAv!q+qcb3NSY}W16&Uc?df~A?NU`PF(ZlT^>9w^jgRCEEo1eKfwH3d>!07d9EdW~J7SE>>@L0OZp24#pa zJOx7#zkpFvs1dSRRRkbw_lFWNcK`7{FPUH;l6|JwCI@7|k%DG$us}{oOyAppC_^Hf z_Q*ks6755B1XNfu1^79jqH~pO5xFTQ_O0xCGuoXcwdYVwg2xgU_ANl4H3xLM#pE2U z>WYhqx`%ULS9LnIdXjHV-ygQ#`e+G%n&!YtZJ?9rQ7oV|?J!Srshu>CTaompx1v2? zMo9!z<3~GDrW%;w!6U-%ffTP|Dge1AfEfimd_XfMNFVu|uWBbM!e9_pKxUf5vf7Gv zW7VT@>9o;hV?TQQZR7|Le)q z-@aU5`xp@B$SBQsq6$q|g7PXTkN3j|o)ZSyPEdFgK(&6DXIT^~IS+1a4}$F?>e*HC zVVz;lwlk&au66*eeYNr2UL=AHKVS--&@Q!Dhq{2eIX*=lI!hg9u`U^2K6iT!_>eDJ zAco?0z<7)VU4yT-4N0G(KCXbw&LzIH9= zAqWhOO(Bh)OJY?evNJq^by#K0lqT}T639#F3onq<2ma6W^)DeI{rty}Zl8oCUjtiH zDG^pZE?>u0ZlFS}_P?F|_3rP(Lx7lk7YFgU%xy0*8iOL)z)ijfW1dxPni#OQkP!uj5{lINr0pX}HBPfFxdr|0HO^ cc1&OGA73?d&s$u(%TIH=jT+-v9sr literal 0 HcmV?d00001 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 @@ + + + + + + + + + + + + + + + + +