From a1f051c3bf96e87fc433aa4dbc66b291d06fce74 Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Fri, 7 Aug 2020 21:51:44 -0700 Subject: [PATCH] 'FMTstar' --- Sampling_based_Planning/rrt_3D/FMT_star3D.py | 39 +++++++++++++----- .../rrt_3D/__pycache__/env3D.cpython-37.pyc | Bin 5165 -> 5165 bytes .../__pycache__/plot_util3D.cpython-37.pyc | Bin 6029 -> 6029 bytes .../rrt_3D/__pycache__/utils3D.cpython-37.pyc | Bin 12307 -> 12307 bytes 4 files changed, 29 insertions(+), 10 deletions(-) diff --git a/Sampling_based_Planning/rrt_3D/FMT_star3D.py b/Sampling_based_Planning/rrt_3D/FMT_star3D.py index 7a70c41..3ce1e49 100644 --- a/Sampling_based_Planning/rrt_3D/FMT_star3D.py +++ b/Sampling_based_Planning/rrt_3D/FMT_star3D.py @@ -30,6 +30,7 @@ class FMT_star: self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) # used for sample free self.n = 1000 # number of samples self.radius = 2.5 # radius of the ball + # self.radius = 40 * np.sqrt((np.log(self.n) / self.n)) # sets self.Vopen, self.Vopen_queue, self.Vclosed, self.V, self.Vunvisited, self.c = self.initNodeSets() # make space for save @@ -37,6 +38,7 @@ class FMT_star: # additional self.done = True self.Path = [] + self.Parent = {} def generateSampleSet(self, n): V = set() @@ -72,9 +74,16 @@ class FMT_star: def Save(self, V_associated, node): self.neighbors[node] = V_associated - def path(self, z, T): - V, E = T + def path(self, z, initT): path = [] + s = self.xgoal + i = 0 + while s != self.xinit: + path.append((s, self.Parent[s])) + s = self.Parent[s] + if i > self.n: + break + i += 1 return path def Cost(self, x, y): @@ -93,34 +102,44 @@ class FMT_star: ind = 0 while z != self.xgoal: Vopen_new = set() - Nz = self.Near(self.Vunvisited, z, rn) - Xnear = Nz.intersection(self.Vunvisited) + #Nz = self.Near(self.Vunvisited, z, rn) + #self.Save(Nz, z) + #Xnear = Nz.intersection(self.Vunvisited) + Xnear = self.Near(self.Vunvisited, z ,rn) + self.Save(Xnear, z) for x in Xnear: - Nx = self.Near(self.V.difference({x}), x, rn) - self.Save(Nx, x) - Ynear = list(Nx.intersection(self.Vopen)) + #Nx = self.Near(self.V.difference({x}), x, rn) + #self.Save(Nx, x) + #Ynear = list(Nx.intersection(self.Vopen)) + Ynear = list(self.Near(self.Vopen, x, rn)) + # self.Save(set(Ynear), x) ymin = Ynear[np.argmin([self.c[y] + self.Cost(y,x) for y in Ynear])] # DP programming equation collide, _ = isCollide(self, ymin, x) if not collide: E.add((ymin, x)) # straight line joining ymin and x is collision free Vopen_new.add(x) + self.Parent[x] = z self.Vunvisited = self.Vunvisited.difference({x}) self.c[x] = self.c[ymin] + self.Cost(ymin, x) # estimated cost-to-arrive from xinit in tree T = (VopenUVclosed, E) # update open set - print(len(self.Vopen)) self.Vopen = self.Vopen.union(Vopen_new).difference({z}) self.Vclosed.add(z) if len(self.Vopen) == 0: print('Failure') return ind += 1 - self.visualization(ind, E) + print(str(ind) + ' node expanded') + # self.visualization(ind, E) # update current node Vopenlist = list(self.Vopen) z = Vopenlist[np.argmin([self.c[y] for y in self.Vopen])] # creating the tree T = (self.Vopen.union(self.Vclosed), E) - return self.path(z, T) + self.done = True + self.Path = self.path(z, T) + self.visualization(ind, E) + plt.show() + # return self.path(z, T) def visualization(self, ind, E): if ind % 100 == 0 or self.done: diff --git a/Sampling_based_Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc b/Sampling_based_Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc index 39c891bad0ef38c86da3f346282471410e35e9dd..ff037369016fc0300b5cdd9918461371e2cee794 100644 GIT binary patch delta 92 zcmZ3hu~viIiIz>% diff --git a/Sampling_based_Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc b/Sampling_based_Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc index 0bf07cde12b0370cf9b76a8a6d73c2cc01a926d2..56cafc398013e1292e738a794966481c02378345 100644 GIT binary patch delta 20 acmeCx@73pa;^pOH0D`AWwKsA%i30#I`vmj= delta 20 ZcmeCx@73pa;^pOH0D@24nj5*B!~rd^1Z4mK diff --git a/Sampling_based_Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc b/Sampling_based_Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc index a0f9bb2802ff0c940db42691c0b2e86d174a8bd1..54bb7a2401d60d0e45014227cde917700f58f4a7 100644 GIT binary patch delta 1146 zcmZvb-AmJ96vutG`RkXP;MBPnUp8Onm_t|Lg*RR3EC>`SwPY#V+Ek(0`B+K@{EXTCW1{x{aRivm%lP`*42WbDndabI$KaVG0|2%FD}2 z#P8&V@13#M!h+^=&r$j4)5cKaK0FOx3HZYA7q`Gc?v_r_0W<$_WG~ zfj$ARj?dC-r=2C}$mybyBUhb{}G05UI$cq$IEg37`J}!@o zlhw56cCsD0iLdUcMK0F}uY?I!g08jR5L@1F&00qw^ayzMT*f|v@ECW>~Ru{C9@nB7jxVh^3faI09%YZRy=^?oE--1nM7{nl>j6v@ VFR<={E}h|r5T1)WS?QI3`WKl$0CxZY delta 1146 zcmZvb-%FEG7{@)^e0y^W{+iqTwfS?+p{wx18!!A(5GYcb7#|laKGK{6`I_(omae}30X{+RoSiI6YWb`JTNo?lqb!y=DDP)1PHmO1l{oxq zI;{L5jDxDQV6r-9(_vo&lE8J~230jVoKr%675^lzuQJuFs;VWgfKSm#latzABgGR4 zP5?auUK5YgOP7=BwC`#toQJ;v+yr#wELs#&VIrqV2$k_gs&{u3!j&m_5!3|DxG%D0 z`rux4Ed5#XDuyMhg(CbooH+{C^s-fYRr93%HVg;wH(2F&U?A3D-2pW|sgLT3h{o^2 zUIDs*d&Ejh=y9!|t5=VVfMT6?-J|p>#UectK2gr}3732u3H;nYoO80OKK!&z$j8n~D+O zE&69|#=GV^Iz;Sx1Q4)=nc6pn9AILmsu1!$F{@_2ndxa#nN=3!MsScuefE>Xkdr_k zASaJed0F=Cqcvon`#kLYQ^f87E?^Ci173&f1!U`%q2#OJh6(~{K%Q6%B`>f+)y7<*xZ2xP+9*Qs7MotuO5keH7&$p?lrB!_8 zX9Z)l&@$gQh?o%|0^qCWQ2|3qj9t~jDUL5K+lpnVRY2A#%iF-CtOA|p^VIEcvp<8x V;Rfq4Ovx1AgRv{_WTzMY$zR@d0u2BF