From dfee5555c55e08586a7344e1d615092cb864163b Mon Sep 17 00:00:00 2001 From: zhm-real Date: Tue, 23 Jun 2020 18:46:44 -0700 Subject: [PATCH] update RRT* --- Sampling-based Planning/RRT*.py | 200 ++++++++++++++++++ Sampling-based Planning/RRT.py | 100 ++++----- .../__pycache__/env.cpython-37.pyc | Bin 1107 -> 1273 bytes .../__pycache__/env3D.cpython-37.pyc | Bin 0 -> 1528 bytes .../__pycache__/plotting.cpython-37.pyc | Bin 2439 -> 2590 bytes .../__pycache__/utils3D.cpython-37.pyc | Bin 0 -> 5351 bytes Sampling-based Planning/env.py | 25 ++- Sampling-based Planning/plotting.py | 47 ++-- Search-based Planning/.idea/workspace.xml | 11 +- 9 files changed, 301 insertions(+), 82 deletions(-) create mode 100644 Sampling-based Planning/RRT*.py create mode 100644 Sampling-based Planning/__pycache__/env3D.cpython-37.pyc create mode 100644 Sampling-based Planning/__pycache__/utils3D.cpython-37.pyc diff --git a/Sampling-based Planning/RRT*.py b/Sampling-based Planning/RRT*.py new file mode 100644 index 0000000..c4d1167 --- /dev/null +++ b/Sampling-based Planning/RRT*.py @@ -0,0 +1,200 @@ +import env +import plotting + +import numpy as np +import math + + +class Node: + def __init__(self, n): + self.x = n[0] + self.y = n[1] + self.cost = 0.0 + self.parent = None + + +class RRT: + def __init__(self, xI, xG): + self.xI = Node(xI) + self.xG = Node(xG) + self.expand_len = 1 + self.goal_sample_rate = 0.05 + self.connect_dist = 10 + self.iterations = 5000 + self.node_list = [self.xI] + + self.env = env.Env() + 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_circle + self.obs_rectangle = self.env.obs_rectangle + self.obs_boundary = self.env.obs_boundary + + self.path = self.planning() + self.plotting.animation(self.node_list, self.path, False) + + def planning(self): + for k in range(self.iterations): + node_rand = self.random_state() + node_near = self.nearest_neighbor(self.node_list, node_rand) + node_new = self.new_state(node_near, node_rand) + + if not self.check_collision(node_new): + neighbor_index = self.find_near_neighbor(node_new) + node_new = self.choose_parent(node_new, neighbor_index) + if node_new: + self.node_list.append(node_new) + self.rewire(node_new, neighbor_index) + + # if self.dis_to_goal(self.node_list[-1]) <= self.expand_len: + # self.new_state(self.node_list[-1], self.xG) + # return self.extract_path() + + index = self.search_best_goal_node() + self.xG.parent = self.node_list[index] + return self.extract_path() + + def random_state(self): + if np.random.random() > self.goal_sample_rate: + 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 nearest_neighbor(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]))] + + def new_state(self, node_start, node_goal): + node_new = Node((node_start.x, node_start.y)) + dist, theta = self.get_distance_and_angle(node_new, node_goal) + dist = min(self.expand_len, dist) + + node_new.x += dist * math.cos(theta) + node_new.y += dist * math.sin(theta) + node_new.parent = node_start + + return node_new + + def find_near_neighbor(self, node_new): + n = len(self.node_list) + 1 + r = min(self.connect_dist * math.sqrt((math.log(n) / n)), self.expand_len) + + dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.node_list] + node_index = [dist_table.index(d) for d in dist_table if d <= r] + + return node_index + + def choose_parent(self, node_new, neighbor_index): + if not neighbor_index: + return None + + cost = [] + + for i in neighbor_index: + node_near = self.node_list[i] + node_mid = self.new_state(node_near, node_new) + + if node_mid and not self.check_collision(node_mid): + cost.append(self.update_cost(node_near, node_mid)) + else: + cost.append(float("inf")) + + if min(cost) != float('inf'): + index = int(np.argmin(cost)) + neighbor_min = neighbor_index[index] + node_new = self.new_state(self.node_list[neighbor_min], node_new) + node_new.cost = min(cost) + return node_new + + return None + + def search_best_goal_node(self): + dist_to_goal_list = [self.dis_to_goal(n) for n in self.node_list] + goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.expand_len] + + return goal_inds[0] + # safe_goal_inds = [] + # for goal_ind in goal_inds: + # t_node = self.new_state(self.node_list[goal_ind], self.xG) + # if self.check_collision(t_node): + # safe_goal_inds.append(goal_ind) + # + # if not safe_goal_inds: + # print('hahhah') + # return None + # + # min_cost = min([self.node_list[i].cost for i in safe_goal_inds]) + # for i in safe_goal_inds: + # if self.node_list[i].cost == min_cost: + # self.xG.parent = self.node_list[i] + + def rewire(self, node_new, neighbor_index): + for i in neighbor_index: + node_near = self.node_list[i] + node_edge = self.new_state(node_new, node_near) + if not node_edge: + continue + + node_edge.cost = self.update_cost(node_new, node_near) + collision = self.check_collision(node_edge) + improved_cost = node_near.cost > node_edge.cost + + if not collision and improved_cost: + self.node_list[i] = node_edge + self.propagate_cost_to_leaves(node_new) + + def update_cost(self, node_start, node_end): + dist, theta = self.get_distance_and_angle(node_start, node_end) + return node_start.cost + dist + + def propagate_cost_to_leaves(self, parent_node): + for node in self.node_list: + if node.parent == parent_node: + node.cost = self.update_cost(parent_node, node) + self.propagate_cost_to_leaves(node) + + def extract_path(self): + path = [[self.xG.x, self.xG.y]] + node = self.xG + while node.parent is not None: + path.append([node.x, node.y]) + node = node.parent + path.append([node.x, node.y]) + + return path + + def dis_to_goal(self, node_cal): + return math.hypot(node_cal.x - self.xG.x, node_cal.y - self.xG.y) + + def check_collision(self, node_end): + if node_end is None: + return True + + for (ox, oy, r) in self.obs_circle: + if math.hypot(node_end.x - ox, node_end.y - oy) <= r: + return True + + for (ox, oy, w, h) in self.obs_rectangle: + if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: + return True + + for (ox, oy, w, h) in self.obs_boundary: + if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: + return True + + return False + + @staticmethod + def get_distance_and_angle(node_start, node_end): + dx = node_end.x - node_start.x + dy = node_end.y - node_start.y + return math.hypot(dx, dy), math.atan2(dy, dx) + + +if __name__ == '__main__': + x_Start = (2, 2) # Starting node + x_Goal = (49, 28) # Goal node + + rrt = RRT(x_Start, x_Goal) diff --git a/Sampling-based Planning/RRT.py b/Sampling-based Planning/RRT.py index 15db973..6052cbb 100644 --- a/Sampling-based Planning/RRT.py +++ b/Sampling-based Planning/RRT.py @@ -9,8 +9,6 @@ class Node: def __init__(self, n): self.x = n[0] self.y = n[1] - self.path_x = [] - self.path_y = [] self.parent = None @@ -18,7 +16,7 @@ class RRT: def __init__(self, xI, xG): self.xI = Node(xI) self.xG = Node(xG) - self.expand_len = 0.8 + self.expand_len = 0.4 self.goal_sample_rate = 0.05 self.iterations = 5000 self.node_list = [self.xI] @@ -28,27 +26,49 @@ class RRT: 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_circle = self.env.obs_circle + self.obs_rectangle = self.env.obs_rectangle + self.obs_boundary = self.env.obs_boundary self.path = self.planning() self.plotting.animation(self.node_list, self.path) def planning(self): 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_rand = self.random_state() + node_near = self.nearest_neighbor(self.node_list, node_rand) + node_new = self.new_state(node_near, node_rand) - if not self.check_collision(node_new, self.obs_circle, self.obs_rectangle): + if not self.check_collision(node_new): self.node_list.append(node_new) - 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) + if self.dis_to_goal(self.node_list[-1]) <= self.expand_len: + self.new_state(self.node_list[-1], self.xG) return self.extract_path(self.node_list) return None + def random_state(self): + if np.random.random() > self.goal_sample_rate: + 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 nearest_neighbor(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]))] + + def new_state(self, node_start, node_end): + node_new = Node((node_start.x, node_start.y)) + dist, theta = self.get_distance_and_angle(node_new, node_end) + + dist = min(self.expand_len, dist) + node_new.x += dist * math.cos(theta) + node_new.y += dist * math.sin(theta) + node_new.parent = node_start + + return node_new + def extract_path(self, nodelist): path = [(self.xG.x, self.xG.y)] node_now = nodelist[-1] @@ -59,62 +79,36 @@ class RRT: return path - def cal_dis_to_goal(self, node_cal): + def dis_to_goal(self, node_cal): return math.hypot(node_cal.x - self.xG.x, node_cal.y - self.xG.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_len: - expand_len = d - - 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) - - new_node.parent = node_start - - return new_node - - def generate_random_node(self): - if np.random.random() > self.goal_sample_rate: - 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, 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 - return math.hypot(dx, dy), math.atan2(dy, dx) - - @staticmethod - def check_collision(node_end, obs_circle, obs_rectangle): + def check_collision(self, node_end): if node_end is None: return True - for (ox, oy, r) in obs_circle: + for (ox, oy, r) in self.obs_circle: if math.hypot(node_end.x - ox, node_end.y - oy) <= r: return True - for (ox, oy, w, h) in obs_rectangle: + for (ox, oy, w, h) in self.obs_rectangle: + if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: + return True + + for (ox, oy, w, h) in self.obs_boundary: if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: return True return False + @staticmethod + def get_distance_and_angle(node_start, node_end): + dx = node_end.x - node_start.x + dy = node_end.y - node_start.y + return math.hypot(dx, dy), math.atan2(dy, dx) + if __name__ == '__main__': - x_Start = (15, 5) # Starting node - x_Goal = (45, 25) # Goal node + x_Start = (2, 2) # Starting node + x_Goal = (49, 28) # 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 e743b7320985ce2eccebe676e73f15122d4afa73..1c0531ac6772da9c7c19848a122fea8f6625eadd 100644 GIT binary patch delta 543 zcmY*Wy-EW?5Z=GL+$DFX(P#?6Xh8A&Nu!00Ac__iHfj+Ijb_6^kO&vCkcfg-g2k~f zAU=eRg-;+BcD}(@f}Nc+dl!hi?A+`(-#0Tm_v!q)!G`bW2`q2t=O`3OuuWH!gN0g4 z$eeWU$sIk+Nq3Kg`7gfJxolG8!on|uvk9kLOP;G1?~x~wyzK}6`Dyn!ZZ$7XI?-86 zm1(EexK4}qt!t0GLa_J3OMY`KwE!_lOT@Ywic07RGqe%5-( zHQqHQ&RDvNfa$=zit8B+P7E-XkWo?%zDTR;k_z3?^as&78)IS_QHlk)I6<0HuHVk33IiOB3w9bVsE z0{1DnO3DMuYNdWj>*29hHy0Id!&PWW6=1;NGc2z2ZRDFL;=oC#oOUf|T;S}$K8goH zAB5=Pw8*rdAy*kU;1gn`@?vF|VXA3my43&bXn_0V?Kh6`GhN)3%L}|kHBBItgvnzR z-pVDsS8M0K#gqJjR|J_F^Eu)JN{aSgru-}s-ykq)$LU8~9 diff --git a/Sampling-based Planning/__pycache__/env3D.cpython-37.pyc b/Sampling-based Planning/__pycache__/env3D.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..49568f816ef58eb67259d783cc11e917f8b3037b GIT binary patch literal 1528 zcmZux&2QX96rb_;I=h=D4Iv>2MR37~W~JsM0%HF7_qRWp zg#3Xj6CO)25pDe;`;mg}>5InCsQ!^K=2Uc~MSN|5n{;unYks}Jijtl~FNSQ^V> z!N}savYgiHDa&oG4X`xc2g`uWfwG3JwW+K-t+l1B?Z%4iD;kk2?ego2J|W*cztT(c zhF1D5z0xb~jaKRW?mnr=0RgnLM(?cA?i#(jMtco9w{GF_7x|Y&ovGbYXv6+E`t=tS z$B*EJN?H%`qhlqTU1+BRdtK;5zfJzA^Bi@aksp2k8s+DcHs`OC=v)bGZ$by%u8+H2 zUK`rK?>EVR-uERAWTljb2r03`;jDlx#M5gn&1;iM$riQ6@?50cGiq0gB1`6FoTasS zmSmF`LGO#RqGXdq6g7>1AaVfP|M`40&1Pb>oM!XUGnqw_%|c-_gE5T zGA^gHVsy%8d4j$(RtSFhtW6n-^yTrBqkK{KB2lVhMGp=y_COJBNZ)?8sX<-%Jf(6I zViQd0#0KEQDn0-uNI-$_8sFeRwHq4f+~GZ7yun=VCnV8j(J^~7Y$5I{xD66*~ zn-jQ=Id0kW`$-Fojw`zzIQBh9;tyToJ0)JKcFwYS%9&iKW&~E#y4r*>$+9-0j3u5^ z8-+;DYvXbjr-89hdI=gm>=Hewm!O;Daja^#a;R#AVVuTg8192rsDB!2(f45n+jQB7 zCLVPO>t8-{4VP*fGEF^_j@79imAl|-DyUkjOz&k}%vlmI#UVrs90Jjo13Z5n8GE*x z*niX(q^a4-ZA7@uGixUdQ#KP}Si503%lJG&+z-PGDA7bDK1aTX>g|Kq*O7N1fb1*q z`VMt%<-vnzEKb7^2xNyL&n9rLna*eVq8SU4Ql-iRK!KHtSMY?uJuO)&PIER9@-C)e lA8YzmZRRpg%m2GV?L2I5{s>uFV3Q2$XwK$v;J^Te^k2o}W5NIc literal 0 HcmV?d00001 diff --git a/Sampling-based Planning/__pycache__/plotting.cpython-37.pyc b/Sampling-based Planning/__pycache__/plotting.cpython-37.pyc index 59dd57e00357cf0ee8bde11666fa5d1896f602a1..efb61817edb6481bcf4b910636e2d82bf2875352 100644 GIT binary patch delta 1203 zcmah|&5IOA6t7oZUHvg1n$0M?Dk3VH4!bL;h>9rqdC+x*hzre{Fw@n}_M~@ax4LD= zwTB60Ko2U61<6fk{(&5F4&-mhwIK(Od&$9z;K^4#>n91R=Dqq=z4z+9dcW#NU%#kT zFIFlp!Ex*5>+grouo_=JkE@XQAc#)|2s$9XAu56ia{xXQTv)hchFsXf8IYgBw-CFc zRA5__zaw6C_{RY_JN$dz;u&etmZfYsGwpU7$qzv!gE-798p|+9>+PmmgojR11XRIQ zf5G(h)qv@uaOhdfC6wg!b9$6z#+WLP-T|h5qPzOJQ*TGB^)%|VWf`3&s-n?uR8<|P z_X*heF>s*_n^U*qPMSvT=KP1?fNCck4oV57$jE?@l#F0ZmT8anjP5CYq08j%>@w@I zp4sd3o|PFRBV{c!W0`r6W<0a7Z`e6uJmq5mvR+#vMIiqq@W0~NCvi70?l>m2Fz%aW zqey6PXR{~S`k9TXCP8MAY_NnCw;T*}g*iitna>bmX~Lf3g|@AwZEHzrtNYP9DrcZi z!h2UlNh3j!)bdT=Y{d28$J?IaJE3TXK_~90H^%gw#4eF7t$f-@whROwFF*Hv?rg7;q#WFVKjPcnkd zztN)^Z4v3FFe*^Xz_rnwV1}4AserM8U4xN1CM`a)G?9W0$e0$pwwUnaE|K=B?#bfZ z*&aPn9NhT-gV!5N6C(IcwP8MnOV9b0>K?39?n9MBHwSf>d%Sq|xq8ND_MJmRQbR?k zZV!ip`1oDeNfE0*`R@n5Mbj7Eee0f!qa+PFt83>M)X?fI&TFTC@s!8Vuz-^27ILyl z=j4`#9>^J-nh$HgU3;}TxZpAQp-wrg7VX1uU8VM63Rl&ywzMqQwvUu|sGZKCJ=)Qq Z!vA><|Ij#U%&*ncU?oiCUbWzyeFu!n{A~aL delta 1009 zcmZuw&1(}u6rVRcyF0u2Kx?o`ZA_!L%L~qw;0+D zJy_|fhcXurBFV{{f`5R2$X-PB(vx`h;+sv02+r_6=6&p&_nWuxa|dU#3z>{fa2>q; z_41*4lx;2C##5a*Ac#W+2)a)kO=yClWI`7PO1*9fQ}BKA8XOaKOW29dMQVu@(??&X z;QG9aZciB0GA!HY1jx>0wuHMil=YA1${{LKmz)A%t0 zX_^LxdkiSAkRh~88z`T5C+|Qpwi$8%Z zHq=a2Iww;8G>SadYwc|>bmXL2$lt)A;1n`K8BQ_>14bYXwp=nl!<01mB$VYP-hOfg zL;NYl{vA+%25n`eKtXP(i}-#X)gt&~C>p!Dj}N}>?3atoA70BZ%J2Ll+>zY6D6d&t mrok;fo=(XR)`c+@<&5CWZ{knYa#u^(LT$Ys_~Y`2HU9^Ia?{EH diff --git a/Sampling-based Planning/__pycache__/utils3D.cpython-37.pyc b/Sampling-based Planning/__pycache__/utils3D.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..2541f9168e81789019c237f1816ebd7e085909cc GIT binary patch literal 5351 zcmb_g&2J<}74NG4n4TVguXk>VtCSJtoZNZ*c zov)u`71m~lDNn}3K~$3TXcQOg8<6I<^7LqZ zf~{B;?6G1kQ;+K&THUjC5ACwmwa2`)PFZDDyvxt%Eu2lsaF27{&?7or@ocEvNXqD_ zV=FdN>`1vHk@4;qmYl+re{Zh!@2{(b7VWLcl(mC0NwL<|;gRyXart3%M4m4kG!|QBat7yJdM*0VqFXrh zqCbGpwZ?4pu(CF+UpyG|ik(^|JLNd+iVK+Fb1Yx5vkMIu_JxKPe#V&fY49^$~vb z@7jVaZ_bd=$2(5WkS62i%ypl}(eL`g7S1znf>U3GgzP#PMM4S}QiD=b_>h_y_e21R zu7)0xhXnRX*Y(cW*sfR~&t0*`4y@D?t2C7w#F{3Z;PqQS+qoR z5AiYfur6EpTH_&cr}g_aM#IvnoD+tq6Jx*Pm0S5M)*`%7`FPUgj!RbMp+5z^FZhy0 z65c^ScaOhiRkj($yapt{o(Jjbyifo2u~oX2b;3$--~TO>zpAX$2JYQhu_%JGd!=W3 zVg|MVI6ZU5P0aDDCe{#C!4pfg5Et{u?+5MnlYgigN6znSjyc?a}8OpD36BmIYrw!<*o_7Ig(~c zM;Wwvu2aZOvhtg6cGvaqPwyzUkA!lgbl8i+01y#BO+?u%!XOsicsEaTS#+9LQqV1W z(J)r-aV+yfxiZSSv2y6~lAatki@4lAjAdDbbI8_h++qTr>!QO9q)k=!|}ie3LmJLg!qY<7zBKJDGEcIo{PdqVYsyz3^eM z=JBl}j@!vz+bqAG?-XUUo5m)zwH+yxCHdEc@*PMYJ??P1Mk)#*pz1ow+BO80*A4Ur zbdPWt10ga}G7xEZ$uRVzgs7`{h)zi$!cX|vF4;tKMh~eZP>g8Ec^yFI_B}*^UAc%4 zB)?$$O?gjW;o;f_0L6hZ?+K1Y-}@NFgQDd4#mS;6$*}S=mIYWvQLlaz3-oox)o`zC z)#ZAXGdz+~&30o{{Zb~o+htsog<1eZ*skxk9Ys4kxXYz!sGchulavE!DJ7XIbc$YH zhD}mzKaJA#n(b6GDTwqSG=G5U#R3`&`4k|>W?)4MHQ4go(AL35cC3T#BXkVM=@XD^ z`@u?FwqZ#fcE}6Yg$x6 zS!l5-m>^(tY>`O{u{xmWwd#O+1szl;wtN9JT0UcUEUJzaQkA`EEm4JZur#*WaSYCW z4;RX}(I~$t<6)5;$Irblxl|GXP|gT=ef@o1TolVsFkTScS!@|jlN*q9F4ZqfMWObX ziGE_G+G94o-v96zh>rG{Gx3;H*%uzG_&y3bWd;gA$~W!@GVmq8$?xFN6O`0=C&>ku zj+4ZwWo?E|!Oy|v zhZxal0J!Y)eMG5Tq#XmmJkhGf@Wqa=+(;JSEjtNH2F^9dvw6?CA6!0}kE|FGO@|sO#n{s?2BU2IHRv#Ft z^VjV+fS7)tD5J@0AZ9-h4(<1zaR!=p+B&nCRZf9M(RI@&q~%>}-o70^uWRkAnajJv0+NBf<7o``m?8V73f(NoY~_Ro0X-i22G&I!S2V-=_IVdzB1$>3V@$oFXm3ti1>Lypby z3(#oRi3@{4B==F1fwtkSk!uPZa+zx;Z72z`5kou(io5^$;vawc$pbdl;f zc|QhuSlW>D8P`soQB?!J}Y+^ zVH1LlX)k};nKB@&k@%*#POVE9c@n zMa9b|^-uV>0PU4Cw#}D|3j5h><;?(fU|g;(_kFyWG2{UFC*)H=u-A~G56`dTRD5+eAx4&v=RmZB?4mK{P z6MHh`mcGFlhO49eK6QzfYd)#@lp!lroOLbIKG*BI5@=MHr2g9Dzct9kK^nh}FCkVz eVd_(mq0;L=_Z{E!ZT}l+8~&8<`)&|Gdg;GVLT0-F literal 0 HcmV?d00001 diff --git a/Sampling-based Planning/env.py b/Sampling-based Planning/env.py index 85a0aba..f095a59 100644 --- a/Sampling-based Planning/env.py +++ b/Sampling-based Planning/env.py @@ -3,7 +3,8 @@ class Env: self.x_range = (0, 50) self.y_range = (0, 30) self.obs_boundary = self.obs_boundary() - self.obs = self.obs_circle() + self.obs_circle = self.obs_circle() + self.obs_rectangle = self.obs_rectangle() @staticmethod def obs_boundary(): @@ -11,15 +12,25 @@ class Env: (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) + (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_rectangle(): + obs_rectangle = [ + (13, 10, 5, 3), + (18, 4, 5, 4), + (22, 13, 6, 3), + (33, 15, 5, 3), + (42, 6, 5, 3) + ] + return obs_rectangle + @staticmethod def obs_circle(): obs_cir = [ diff --git a/Sampling-based Planning/plotting.py b/Sampling-based Planning/plotting.py index de838bc..02b96b7 100644 --- a/Sampling-based Planning/plotting.py +++ b/Sampling-based Planning/plotting.py @@ -8,33 +8,44 @@ 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 + self.obs_circle = self.env.obs_circle + self.obs_rectangle = self.env.obs_rectangle - def animation(self, nodelist, path): + def animation(self, nodelist, path, animation=False): if path is None: print("No path found!") return - - self.plot_visited(nodelist) + self.plot_grid("RRT") + self.plot_visited(nodelist, animation) self.plot_path(path) def plot_grid(self, name): fig, ax = plt.subplots() - for x in self.obs_bound: + for (ox, oy, w, h) in self.obs_bound: ax.add_patch( patches.Rectangle( - (x[0], x[1]), x[2], x[3], + (ox, oy), w, h, edgecolor='black', facecolor='black', fill=True ) ) - for x in self.obs_circle: + for (ox, oy, w, h) in self.obs_rectangle: + ax.add_patch( + patches.Rectangle( + (ox, oy), w, h, + edgecolor='black', + facecolor='gray', + fill=True + ) + ) + + for (ox, oy, r) in self.obs_circle: ax.add_patch( patches.Circle( - (x[0], x[1]), x[2], + (ox, oy), r, edgecolor='black', facecolor='gray', fill=True @@ -47,13 +58,19 @@ class Plotting: plt.axis("equal") @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) + def plot_visited(nodelist, animation): + if animation: + for node in nodelist: + if node.parent: + plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) + plt.pause(0.001) + else: + for node in nodelist: + if node.parent: + plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") + @staticmethod def plot_path(path): diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index ac72159..2045d90 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -20,10 +20,6 @@ - - - - @@ -199,10 +195,11 @@