From f7a2228cd71fd8488246fb8a86eaa62972c55b21 Mon Sep 17 00:00:00 2001 From: zhm-real Date: Mon, 22 Jun 2020 22:53:28 -0700 Subject: [PATCH] update RRT --- Sampling-based Planning/RRT.py | 171 +++++------------- .../__pycache__/env.cpython-37.pyc | Bin 1530 -> 1107 bytes .../__pycache__/node.cpython-37.pyc | Bin 489 -> 0 bytes .../__pycache__/plotting.cpython-37.pyc | Bin 3155 -> 2439 bytes Sampling-based Planning/env.py | 56 +++--- Sampling-based Planning/node.py | 7 - Sampling-based Planning/plotting.py | 74 +++----- Search-based Planning/.idea/workspace.xml | 2 +- 8 files changed, 90 insertions(+), 220 deletions(-) delete mode 100644 Sampling-based Planning/__pycache__/node.cpython-37.pyc delete mode 100644 Sampling-based Planning/node.py diff --git a/Sampling-based Planning/RRT.py b/Sampling-based Planning/RRT.py index 172610a..15db973 100644 --- a/Sampling-based Planning/RRT.py +++ b/Sampling-based Planning/RRT.py @@ -1,18 +1,14 @@ import env import plotting -import node import numpy as np import math -import matplotlib.pyplot as plt -import matplotlib.patches as patches - class Node: - def __init__(self, x, y): - self.x = x - self.y = y + def __init__(self, n): + self.x = n[0] + self.y = n[1] self.path_x = [] self.path_y = [] self.parent = None @@ -20,96 +16,39 @@ class Node: class RRT: def __init__(self, xI, xG): - self.xI = Node(xI[0], xI[1]) - self.xG = node.Node(xG[0], xG[1]) + self.xI = Node(xI) + self.xG = Node(xG) + self.expand_len = 0.8 + self.goal_sample_rate = 0.05 + self.iterations = 5000 + self.node_list = [self.xI] self.env = env.Env() - # self.plotting = plotting.Plotting(xI, xG) + self.plotting = plotting.Plotting(xI, xG) self.x_range = self.env.x_range self.y_range = self.env.y_range + self.obs_circle = self.env.obs + self.obs_rectangle = self.env.obs_boundary - # self.obs_boundary = self.env.obs_boundary - # self.obs_circle = self.env.obs_circle - - self.obstacleList = [ - (5, 5, 1), - (3, 6, 2), - (3, 8, 2), - (3, 10, 2), - (7, 5, 2), - (9, 5, 2), - (8, 10, 1) - ] # [x, y, radius] - - self.expand_range = 0.8 - self.goal_sample_rate = 0.05 - self.iterations = 500 - self.node_list = [] - - path = self.planning() - - if path is None: - print("No path!") - else: - print("get it!") - - self.draw_graph() - - plt.plot([x[0] for x in path], [x[1] for x in path], '-r') - plt.grid(True) - plt.pause(0.01) # Need for Mac - plt.show() + self.path = self.planning() + self.plotting.animation(self.node_list, self.path) def planning(self): - self.node_list = [self.xI] for i in range(self.iterations): node_rand = self.generate_random_node() node_near = self.get_nearest_node(self.node_list, node_rand) + node_new = self.new_node(node_near, node_rand, self.expand_len) - node_new = self.new_node(node_near, node_rand, self.expand_range) - - if not self.check_collision(node_new, self.obstacleList): + if not self.check_collision(node_new, self.obs_circle, self.obs_rectangle): self.node_list.append(node_new) - self.draw_graph(node_rand) - - if self.cal_dis_to_goal(self.node_list[-1]) <= self.expand_range: - node_end = self.new_node(self.node_list[-1], node.Node(self.xG.x, self.xG.y), self.expand_range) - if not self.check_collision(node_end, self.obstacleList): - return self.extract_path(self.node_list) + if self.cal_dis_to_goal(self.node_list[-1]) <= self.expand_len: + self.new_node(self.node_list[-1], self.xG, self.expand_len) + return self.extract_path(self.node_list) return None - def draw_graph(self, rnd=None): - plt.clf() - # for stopping simulation with the esc key. - plt.gcf().canvas.mpl_connect('key_release_event', - lambda event: [exit(0) if event.key == 'escape' else None]) - if rnd is not None: - plt.plot(rnd.x, rnd.y, "^k") - for node_x in self.node_list: - if node_x.parent: - plt.plot(node_x.path_x, node_x.path_y, "-g") - - for (ox, oy, size) in self.obstacleList: - self.plot_circle(ox, oy, size) - - plt.plot(self.xI.x, self.xI.y, "xr") - plt.plot(self.xG.x, self.xG.y, "xr") - plt.axis("equal") - plt.axis([-2, 15, -2, 15]) - plt.grid(True) - plt.pause(0.01) - - @staticmethod - def plot_circle(x, y, size, color="-b"): # pragma: no cover - deg = list(range(0, 360, 5)) - deg.append(0) - xl = [x + size * math.cos(np.deg2rad(d)) for d in deg] - yl = [y + size * math.sin(np.deg2rad(d)) for d in deg] - plt.plot(xl, yl, color) - def extract_path(self, nodelist): path = [(self.xG.x, self.xG.y)] node_now = nodelist[-1] @@ -121,20 +60,20 @@ class RRT: return path def cal_dis_to_goal(self, node_cal): - return math.hypot((node_cal.x - self.xG.x), (node_cal.y - self.xG.y)) + return math.hypot(node_cal.x - self.xG.x, node_cal.y - self.xG.y) - def new_node(self, node_start, node_goal, expand_range): - new_node = node.Node(node_start.x, node_start.y) + def new_node(self, node_start, node_goal, expand_len): + new_node = Node((node_start.x, node_start.y)) d, theta = self.calc_distance_and_angle(new_node, node_goal) new_node.path_x = [new_node.x] new_node.path_y = [new_node.y] - if d < expand_range: - expand_range = d + if d < expand_len: + expand_len = d - new_node.x += expand_range * math.cos(theta) - new_node.y += expand_range * math.sin(theta) + new_node.x += expand_len * math.cos(theta) + new_node.y += expand_len * math.sin(theta) new_node.path_x.append(new_node.x) new_node.path_y.append(new_node.y) @@ -144,58 +83,38 @@ class RRT: def generate_random_node(self): if np.random.random() > self.goal_sample_rate: - return node.Node(np.random.uniform(self.x_range[0], self.x_range[1]), - np.random.uniform(self.y_range[0], self.y_range[1])) - else: - return node.Node(self.xG.x, self.xG.y) + return Node((np.random.uniform(self.x_range[0], self.x_range[1]), + np.random.uniform(self.y_range[0], self.y_range[1]))) + return self.xG - def get_nearest_node(self, node_list, node_random): - dlist = [(nod.x - node_random.x) ** 2 + (nod.y - node_random.y) ** 2 - for nod in node_list] - minind = dlist.index(min(dlist)) - - return self.node_list[minind] + def get_nearest_node(self, node_list, n): + return self.node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y) + for nd in node_list]))] @staticmethod def calc_distance_and_angle(from_node, to_node): dx = to_node.x - from_node.x dy = to_node.y - from_node.y - d = math.hypot(dx, dy) - theta = math.atan2(dy, dx) - return d, theta + return math.hypot(dx, dy), math.atan2(dy, dx) - def check_collision(self, node_check, obstacleList): - - if node_check is None: + @staticmethod + def check_collision(node_end, obs_circle, obs_rectangle): + if node_end is None: return True - for (ox, oy, size) in obstacleList: - dx_list = [ox - x for x in node_check.path_x] - dy_list = [oy - y for y in node_check.path_y] - d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)] + for (ox, oy, r) in obs_circle: + if math.hypot(node_end.x - ox, node_end.y - oy) <= r: + return True - if min(d_list) <= size ** 2: - return True # collision + for (ox, oy, w, h) in obs_rectangle: + if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: + return True - return False # safe - # - # def check_collision(self, node_check): - # for obs in self.obs_boundary: - # dx = node_check.x - obs[0] - # dy = node_check.y - obs[1] - # if 0 <= dx <= obs[2] and 0 <= dy <= obs[2]: - # return True - # - # for obs in self.obs_circle: - # d = (node_check.x - obs[0]) ** 2 + (node_check.y - obs[1]) ** 2 - # if d <= obs[2] ** 2: - # return True - # - # return False + return False if __name__ == '__main__': - x_Start = (0, 0) # Starting node - x_Goal = (6, 10) # Goal node + x_Start = (15, 5) # Starting node + x_Goal = (45, 25) # 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 index 07e65751c585e01c4bef47f5fbd4213ffe333ecc..e743b7320985ce2eccebe676e73f15122d4afa73 100644 GIT binary patch literal 1107 zcmZuvO^eh(5baJfNp{TauHx58L=+4-8}KZ$xQO6kSlC+*Hks*|8OX=j$t=#Gr=9=c zbv^o*a`lv>=*fdvuQM5f*iBWxuBxu;dfl%^qama5=gW_GzYNB{W77?jz$r<2i_{w? zndDoNDG$fbCmW5_)NmE)|cE!U1;eoU@wxvUP+}eDd zU(Wb06;`?mJ0mF{lal-s6I7Bkp=;wSBSgDvQzJ_abQtNt%-E5&|XYad9qyBt`LiSQ3-(y zmM$sAZJI;A@rZ_#&b=oWd>~Boai0K41)ZQ#I?_$t&%_9Ea4_*L-FMqO4l0XOX9ZSx zCw16bv9t&J);-o?y9~|k?wqzZM><1;-=mJ9ZgzgErVDASt^nj)&C@QT&8z*O*XPQC z3%FGt5NzmMe?Mj#ygzMoa}2in&p4xCJ z?-EW(Xn3a-b%#K6kCZ|-GCYFe07~;~Hc#N}M~qRAfL>1 literal 1530 zcmbtUJ8u&~5Z=f6?&3Hhf+%sU@DLJ+6G@zc5}}BvL}5jWM35yooxK;^=w7+AgKgPW z@&hRN0ZGXpp`@bPmg-76L_wHY=T3154Qw=@C$sx~JF`1`kLTu!1V(@3``a%CLcZZY z+2G)8fh83HK?L86y`~-iI|O?y9N;#c!J<+!H!6syPDua zJ0gd)UI4!!bmi;905wzcXTd>}o+;rk1A79NEC2{PBK8D96bP(lvGmvJ>1lcerY$Co zz8yP3%az7p;#gE_iRL*K0q+hMP|9fz&h@vD*3ZLde369k>0weEPW zFz$5Qep20W{K&&t!%19mYx`_T)eR0dqJcDRyAyP}whgC85)FV*oeoP-!$vmM!~$^W zU;KTL zj5A%#CD;8x_t!vq6O{#alQwl)OWA1&J<9|bjhsP+Av8)l%&H7$L>xN%57Yix|TeOXm^h5UwNKK)45Bamj+HZbHK* zk<)ag?nIFr2uqi=FX=$i_p%f=61x%h0^!6E=t%lbJ&OsvTI!-|e*ijguOs|1O?^<- zEDlc`TRFCNgGRQQSoOZkjjr0zbH3HZtD(_`c6@>f(EUC|SzYRAlhw;;X6n?)GNKvd zrjS*YSLs+5X>Ia0D{VBFWm8b?MOv+{X1ye;@PbLRc7YIimA69B8r%IuHt3(ti0+&m z6fs_o@=BIk2sad>tktrh_*{s$r7ZSu3=QeN^oY)=#*he>b_J}<<8befv~;AQ1{d7! LSKLw84+8!G)SGOY diff --git a/Sampling-based Planning/__pycache__/plotting.cpython-37.pyc b/Sampling-based Planning/__pycache__/plotting.cpython-37.pyc index ab088896db25747af4a198d6f1bf10abe2146691..59dd57e00357cf0ee8bde11666fa5d1896f602a1 100644 GIT binary patch literal 2439 zcmb7F&u`pB6rLG-z240xA|VA*3JRAZ3zAd_0YV`~w5TA#5;YZ-Mik5Iadw?}?d{k} zH_PUfNSu1$#xXhe!XLxYH^iyOid)5j?~ON`G|B~AGxKJ~@AzFC)NJvNVE#}$J!N=Mm z&1cV4<~mLyCb?o#D6YgYla{iSjn!5)qFh2i;IjL%V7Q(p#mwnPf!~ zr`?dG(nBkaI~e&bOk+M4f{mF^gBRk&IweYs*^bz;LDdZ9fT}A7sl4sEr5JvTaj&$Z zbicH|#?;C>x!=k5QWfZfvN7G27Ra(m`!LpF5_wi>=TWk)Neyjddd74M%gZ0udRaeO z8}+iiwN0IMbxbS`8#&Q=E<8!=;cQk+Inboszu6EHU;28_Wc3r(#0N^7|fNsG>CxGM>J5XZG zCzLfdXU0)hdo(j7sTjU zdIlPlZknkmiSweg>Dc(Inr7*`5u|ZHDB>)oZBE`~+{J4PW9`D!+UbX(ppWp>Fz6p9 zB1#YiOgm*ii9>N{4L1n=yX?`&hxXVyu?l;qHntUCVwlAe+ZI?Yv$K~Nq7reb>Q5bV z$K%C^Z1@@Y9q_MC`RiBlgU>abHN*MfA+*?W!P_wZq7{EbeA}w|6gf4yZBA~R=62@% z_GG4l1h%wPi)0!|X4{&>!slip2Rx@Bl(kM0gu4$sOEx0ajlwL+v}|k#;e@oe<0O$b z0`fqLPVVutfzXSd#H#2?(an*EkN1Man=P$DQb;%7>kyu~rjV13Kq()ng}t(FmME8v z`%zc~X*Y>9RTt^pF}VSB!FQe+Wvz&dB$9S8jB~G6#e$s%{YYBdaaW39Sn}beXwdVJ z_y$IP6BFYt(c(7V>s;WSv#Z_WWcMzAm-68}LQEu5P=@3sddNo(hKZ0LK`4I6hu@7E zaJ_qo;8)^Pc8EH%!*sLYNC&ZlMA$)OoLaz&pVUqf1t)djW9rr^DkM%_RX1oiA6DqN z6*~G&{9+_lx^jLuI`DOrL`ZQz+KJA8q+2-#~_>ivo%r{Ge{ zL2B`;-4MV>94!Z}XF?kQs{}j|6?{lBw2VoDx$UW7LWiomL5OVVLdc^Q;shOSiTx%X z+tf`AD8?cnBUW$eWqf+c!k5i;s(oSBA8g#lbxyJ!HoEz$Y2@0Ac$~5PZQ?bZO1WU<^?kxMO? z)a**3Sn^~6^(8=i?}54$$fbWv(c|2To_Gv$OAENaH@hNb$EWOK-n^MN-|siSH@{q2 zsWE&v-u~*dzd`#C70y2k%6mxJk02zIJYjv_61>o@zTL7*-Dx>@nH17GWzu>qTCU^| zSks=PAKME_o_~ume}B0N22I@pt7bRUIHH=l5L`_TES(NN6T7j}5zGSM7?3<5w zx`TeSGwu$KcJ8S`M+Ns;r*ZE46%3zw5R9+#8vgP6`Jrr|x7k}953R{XGRN~zC^?()GsYK3fdMD$#winPxx&PF#T`~&vswW&3+dY;-fEv#M@rJ8i8#o_KF z8kLT>F5N3QbmWyQOnPK3EsfIBFqUd*S&_9fZrZHKI>1nuD;>M6S(U4$c1d0;wKcg$ znsMLyo?btQPJ9(55suG~j-xaWIkrk4MV*>LTNqZ`(J0QFTsvl5|t&`5i?lc3+0!R|UrUn8;cjfFRYH0}p^JV>{1 zCxb9ZvRx=e^Rf8sU@UHS%v5ulZ!FqxjYa1zOmuh@B=i41efo6%E)4VQ5Zbzb|KY>V zpEmz-_uu2wJI!@%4U@cOg~`6QI^n){!yr8lGF=Cf{BV$_QJ8C+0Hpay^Aqh31BHX3 zJn#CWGCx@&SKGlT&a{)qc@k;6qhhI@VQ`d1O((0w|mY6mr=E`K`Osk9L*J^Ii*n4iz&crKrKM&p=UA^#c!}c167_T=4VvqF1`_HrwmGK* z&`vuE!h?sPnrb9FQ8-8jN>}%TuvG2+I7!G~!O1y>)D}86SJlhZ-^-5Lgo8{GY3OPo zr4I~*U0pF*lKv%!hZ3_+*}Km{k} zl?i{$W$k5lz*JAxr*``4M3l9SO;%b?t}GW#c!V zJsIkB(evkBzSFvh;>-$~Lj9KX-$7u#AD!1L3bASr;*Ut>lGM+kYp&ET@}{N8!|FW} zzkpaq$g27^Rg%xl!%RB}SDi>1U@wtJC{wo}${<(Few}J>kTB;~uE|9Fg39P1?!%tZ zjV!2)2y2s`W6u#5HKjMEM^8ASO5fKt-|r9PQ9^pd_bK2P7De$=y-ngai4RHqp2YHw z8qFL{;Uuhr!n4Ocw|3ck#d}FnJ>InIczw`iNa8kLkfb5%o4kk&8yD>q5u!}}l;)y< YYX*7Ita$SjYu}}3OGb}NVsg*=Keipq+yDRo diff --git a/Sampling-based Planning/env.py b/Sampling-based Planning/env.py index 8a8ecb7..85a0aba 100644 --- a/Sampling-based Planning/env.py +++ b/Sampling-based Planning/env.py @@ -1,48 +1,34 @@ -import numpy as np - - class Env: def __init__(self): - self.x_range = (-2, 15) # size of background - self.y_range = (-2, 15) - # self.obs_boundary = self.obs_boundary(self.x_range, self.y_range) - # self.obs_circle = self.obs_circle() + self.x_range = (0, 50) + self.y_range = (0, 30) + self.obs_boundary = self.obs_boundary() + self.obs = self.obs_circle() @staticmethod - def obs_boundary(x, y): - w = 1 - 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] + 1, y[1] - w, (y[1] - y[0] - 2 * w) // w + 1): - obs_boundary.append((x[0], j, w)) - for j in np.linspace(y[0] + 1, y[1] - w, (y[1] - y[0] - 2 * w) // w + 1): - obs_boundary.append((x[1], j, w)) - - for i in np.linspace(10, 20, 10 // w + 1): - obs_boundary.append((i, 15, w)) - for j in np.linspace(1, 14, 13 // w + 1): - obs_boundary.append((20, j, w)) - - for j in np.linspace(15, 29, 14 // w + 1): - obs_boundary.append((30, j, w)) - for j in np.linspace(1, 14, 13 // w + 1): - obs_boundary.append((40, j, w)) + def obs_boundary(): + obs_boundary = [ + (0, 0, 1, 30), + (0, 30, 50, 1), + (1, 0, 50, 1), + (50, 1, 1, 30), + (20, 1, 1, 15), + (10, 15, 10, 1), + (30, 15, 1, 15), + (40, 1, 1, 15) + ] return obs_boundary @staticmethod def obs_circle(): obs_cir = [ - (8, 8, 3), - (10, 23, 3), - (20, 25, 2.5), - (30, 7, 3), - (40, 25, 2), - (43, 20, 2.5) + (5, 10, 3), + (10, 22, 3.5), + (21, 23, 3), + (34, 9, 4), + (37, 23, 3), + (45, 20, 2) ] return obs_cir diff --git a/Sampling-based Planning/node.py b/Sampling-based Planning/node.py deleted file mode 100644 index 76e419a..0000000 --- a/Sampling-based Planning/node.py +++ /dev/null @@ -1,7 +0,0 @@ -class Node: - def __init__(self, x, y): - self.x = x - self.y = y - self.path_x = [] - self.path_y = [] - self.parent = None diff --git a/Sampling-based Planning/plotting.py b/Sampling-based Planning/plotting.py index d19181a..de838bc 100644 --- a/Sampling-based Planning/plotting.py +++ b/Sampling-based Planning/plotting.py @@ -8,35 +8,23 @@ class Plotting: self.xI, self.xG = xI, xG self.env = env.Env() self.obs_bound = self.env.obs_boundary - self.obs_circle = self.env.obs_circle + self.obs_circle = self.env.obs - def animation(self, nodelist, node_rand=None): - plt.clf() - # for stopping simulation with the esc key. - plt.gcf().canvas.mpl_connect('key_release_event', - lambda event: [exit(0) if event.key == 'escape' else None]) - if node_rand is not None: - plt.plot(node_rand.x, node_rand.y, "^k") + def animation(self, nodelist, path): + if path is None: + print("No path found!") + return - for node in nodelist: - if node.parent: - plt.plot(node.path_x, node.path_y, "-g") - - plt.plot(self.xI[0], self.xI[1], "b*") - plt.plot(self.xG[0], self.xG[1], "g*") - - plt.axis("equal") - plt.axis([-5, 55, -5, 35]) - plt.title("RRT") - plt.grid(True) - plt.pause(0.01) + self.plot_visited(nodelist) + self.plot_path(path) def plot_grid(self, name): fig, ax = plt.subplots() + for x in self.obs_bound: ax.add_patch( patches.Rectangle( - (x[0], x[1]), x[2], x[2], + (x[0], x[1]), x[2], x[3], edgecolor='black', facecolor='black', fill=True @@ -47,44 +35,28 @@ class Plotting: ax.add_patch( patches.Circle( (x[0], x[1]), x[2], - edgecolor='gray', + edgecolor='black', facecolor='gray', fill=True ) ) - plt.plot(self.xI[0], self.xI[1], "b*") - plt.plot(self.xG[0], self.xG[1], "g*") - + plt.plot(self.xI[0], self.xI[1], "bs", linewidth=3) + plt.plot(self.xG[0], self.xG[1], "gs", linewidth=3) plt.title(name) plt.axis("equal") - plt.show() - def plot_visited(self, visited): - visited.remove(self.xI) - count = 0 + @staticmethod + def plot_visited(nodelist): + for node in nodelist: + if node.parent: + plt.plot(node.path_x, node.path_y, "-g") + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) + plt.pause(0.001) - 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') + @staticmethod + def plot_path(path): + plt.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2) plt.pause(0.01) plt.show() diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index 54ec76f..ac72159 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -20,9 +20,9 @@ - +