From 84a030d4b34e3ef8d9c19788feb4ac62ef50cbe6 Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Thu, 6 Aug 2020 00:40:24 -0600 Subject: [PATCH] 'FMT' --- Sampling_based_Planning/rrt_3D/FMT_star3D.py | 154 ++++++++++++++---- .../rrt_3D/__pycache__/queue.cpython-37.pyc | Bin 0 -> 2126 bytes Sampling_based_Planning/rrt_3D/queue.py | 53 ++++++ .../__pycache__/Astar3D.cpython-37.pyc | Bin 3539 -> 3521 bytes .../__pycache__/env3D.cpython-37.pyc | Bin 5175 -> 5175 bytes .../__pycache__/plot_util3D.cpython-37.pyc | Bin 5722 -> 5722 bytes .../__pycache__/queue.cpython-37.pyc | Bin 4837 -> 4837 bytes .../__pycache__/utils3D.cpython-37.pyc | Bin 10919 -> 10919 bytes 8 files changed, 177 insertions(+), 30 deletions(-) create mode 100644 Sampling_based_Planning/rrt_3D/__pycache__/queue.cpython-37.pyc create mode 100644 Sampling_based_Planning/rrt_3D/queue.py diff --git a/Sampling_based_Planning/rrt_3D/FMT_star3D.py b/Sampling_based_Planning/rrt_3D/FMT_star3D.py index b718600..7a70c41 100644 --- a/Sampling_based_Planning/rrt_3D/FMT_star3D.py +++ b/Sampling_based_Planning/rrt_3D/FMT_star3D.py @@ -17,66 +17,160 @@ import sys sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from rrt_3D.env3D import env from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide +from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent +from rrt_3D.queue import MinheapPQ class FMT_star: def __init__(self): self.env = env() - # note that the xgoal could be a region since this algorithm is a multiquery method + # init start and goal + # note that the xgoal could be a region since this algorithm is a multiquery method self.xinit, self.xgoal = tuple(self.env.start), tuple(self.env.goal) - self.n = 100 # number of samples + 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 # sets - self.V = self.generateSampleSet(self.n - 2) # set of all nodes - self.Vopen = set(self.xinit) # open set - self.Vclosed = set() # closed set - self.Vunvisited = copy.deepcopy(self.V) # unvisited set - self.Vunvisited.add(self.xgoal) - # cost to come - self.c = {} + self.Vopen, self.Vopen_queue, self.Vclosed, self.V, self.Vunvisited, self.c = self.initNodeSets() + # make space for save + self.neighbors = {} + # additional + self.done = True + self.Path = [] def generateSampleSet(self, n): V = set() for i in range(n): - V.add(sampleFree(self, bias = 0.0)) + V.add(tuple(sampleFree(self, bias = 0.0))) return V - def Near(self, nodeset, node, range): - newSet = set() - return newSet + def initNodeSets(self): + # open set + Vopen = {self.xinit} # open set + # closed set + closed = set() + # V, Vunvisited set + V = self.generateSampleSet(self.n - 2) # set of all nodes + Vunvisited = copy.deepcopy(V) # unvisited set + Vunvisited.add(self.xgoal) + V.add(self.xinit) + V.add(self.xgoal) + # initialize all cost to come at inf + c = {node : np.inf for node in V} + c[self.xinit] = 0 + # use a min heap to speed up + Vopen_queue = MinheapPQ() + Vopen_queue.put(self.xinit, c[self.xinit]) # priority organized as the cost to come + return Vopen, Vopen_queue, closed, V, Vunvisited, c - def Path(self, T): + def Near(self, nodeset, node, rn): + if node in self.neighbors: + return self.neighbors[node] + validnodes = {i for i in nodeset if getDist(i, node) < rn} + return validnodes + + def Save(self, V_associated, node): + self.neighbors[node] = V_associated + + def path(self, z, T): V, E = T path = [] return path def Cost(self, x, y): - pass + # collide, dist = isCollide(self, x, y) + # if collide: + # return np.inf + # return dist + return getDist(x, y) def FMTrun(self): - z = copy.deepcopy(self.xinit) + z = self.xinit + rn = self.radius Nz = self.Near(self.Vunvisited, z, rn) E = set() - # Save(Nz, z) + self.Save(Nz, z) + ind = 0 while z != self.xgoal: Vopen_new = set() + Nz = self.Near(self.Vunvisited, z, rn) Xnear = Nz.intersection(self.Vunvisited) for x in Xnear: - Nx = self.Near(self.V.difference(set(x)), x, rn) - # Save(Nx, x) - Ynear = Nx.intersection(self.Vopen) + Nx = self.Near(self.V.difference({x}), x, rn) + self.Save(Nx, x) + Ynear = list(Nx.intersection(self.Vopen)) ymin = Ynear[np.argmin([self.c[y] + self.Cost(y,x) for y in Ynear])] # DP programming equation - collide, _ = self.isCollide(ymin, x) + collide, _ = isCollide(self, ymin, x) if not collide: - E = E.add((ymin, x)) # straight line joining ymin and x is collision free + E.add((ymin, x)) # straight line joining ymin and x is collision free Vopen_new.add(x) - self.Vunvisited = self.Vunvisited.difference(set(x)) - self.c[x] = self.c[ymin] + self.Cost(ymin, x) # cost-to-arrive from xinit in tree T = (VopenUVclosed, E) - self.Vopen = (self.Vopen.union(Vopen_new)).difference(set(z)) - self.Vclosed = self.Vclosed.union(set(z)) - if len(self.Vopen) > 0: - return 'Failure' - z = np.argmin([self.c[y] for y in self.Vopen]) - return self.Path(z, T = (self.Vopen.union(self.Vclosed), E)) + 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) + # 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) + + def visualization(self, ind, E): + if ind % 100 == 0 or self.done: + #----------- list structure + # V = np.array(list(initparams.V)) + # E = initparams.E + #----------- end + # edges = initparams.E + Path = np.array(self.Path) + start = self.env.start + goal = self.env.goal + # edges = E.get_edge() + #----------- list structure + edges = np.array(list(E)) + #----------- end + # generate axis objects + ax = plt.subplot(111, projection='3d') + + # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi)) + # ax.view_init(elev=0., azim=90.) + ax.view_init(elev=8., azim=90.) + # ax.view_init(elev=-8., azim=180) + ax.clear() + # drawing objects + draw_Spheres(ax, self.env.balls) + draw_block_list(ax, self.env.blocks) + if self.env.OBB is not None: + draw_obb(ax, self.env.OBB) + draw_block_list(ax, np.array([self.env.boundary]), alpha=0) + draw_line(ax, edges, visibility=0.75, color='g') + draw_line(ax, Path, color='r') + # if len(V) > 0: + # ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', ) + ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') + ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') + # adjust the aspect ratio + xmin, xmax = self.env.boundary[0], self.env.boundary[3] + ymin, ymax = self.env.boundary[1], self.env.boundary[4] + zmin, zmax = self.env.boundary[2], self.env.boundary[5] + dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin + ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy) + make_transparent(ax) + #plt.xlabel('x') + #plt.ylabel('y') + ax.set_axis_off() + plt.pause(0.0001) + +if __name__ == '__main__': + A = FMT_star() + A.FMTrun() diff --git a/Sampling_based_Planning/rrt_3D/__pycache__/queue.cpython-37.pyc b/Sampling_based_Planning/rrt_3D/__pycache__/queue.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..ac7a5c708861436159da6690ebfb0090a6cbe0e4 GIT binary patch literal 2126 zcma)7UvC>l5Z}E!`_7J&Hf<>a2{?&9^Z?t6!b__V6`BYnh+7f?l4a?1x!u@T-@ojx zNo-wSNI!`4*w4Z@xL2O~6?lP}Jv&YiRBSS{dpo;3JHPqO`kVFjCV{qg@VWncosd89 zW7$j?JOh!RfntQwnDncj_No3h`o_0}8O(f5n0aaRO-7H1XT62>q!*%IHUkFFK;(Cz zIKlmWgHhO_clRx3vD$0WudzC_p|3NCHK4ayldVDTuohc~zQH!w9q60vE_m838wXK3 z;z9oM$Jv{v3+-z+7f~i6HFZxXe8S!1Kyv10shdQpi%Y+7&qh%=a#b+q(tYtb&W5Rn z3wX>Eo+?-HB>Odp^>HJma{08|WmzaY`BaUvw3CTpce@)$$088ZE*^Q>8L1@xbm_UR zJ-4GIuTBh9l%?%7Q(WppLkb{Z!yb`&4k90e8j^zeMA1v^jR>`=taVw=UeXJ*FbcDv zL-WEaEL>O69fH`yxL#OS28A5Fvgb~eTBjR%%^u%O~nS4CDqVVl)B4>TiS2)>lZvst{?34Sk|K2a#Wb zT7YPa4J0B0RY7@yKpF)U^$yOOb87)qMc21~L4zynkj*^H1!l5}re4MFnK~fx01XdO zVa8Y27kA+EBaj6Nn$)5#B*DYg)o)^XU1msv>ll83kMmu;x7by5j<+UPbYNc43j;`C z;@!s$f%8^jeuc5r=JmovvJ~bKIW>ME%4YPE{zjPrrD)Ff4)Mj{kYWaphnk2~XFtx) zxKt|=AVE6rVGv2~e$S`-LI7K$6PG8sns#O`fa0ErEYVcKxz$!(3_Z`OD6k13l(weD z5=VqpgDE~lqq98UvnovALpw5~LiaYXr8(s45f~S|aOgdN)d6swTfBf(*O=aVx{ywY zBcmT^(GQfsB_7pRh7P&ipB~>A2(fq{6;^@3G7ul5`UI7WYL!$y!ipCNstqFVf^sa2I=0i) rVmhX_dRDm>W^v3z{FRad$t7@$w*K3prL$Arb{C6EVj>)1ievl@#z*&A literal 0 HcmV?d00001 diff --git a/Sampling_based_Planning/rrt_3D/queue.py b/Sampling_based_Planning/rrt_3D/queue.py new file mode 100644 index 0000000..1bd43fb --- /dev/null +++ b/Sampling_based_Planning/rrt_3D/queue.py @@ -0,0 +1,53 @@ +# min heap used in the FMT* + +import collections +import heapq +import itertools + +class MinheapPQ: + """ + A priority queue based on min heap, which takes O(logn) on element removal + https://docs.python.org/3/library/heapq.html#priority-queue-implementation-notes + """ + def __init__(self): + self.pq = [] # lis of the entries arranged in a heap + self.nodes = set() + self.entry_finder = {} # mapping of the item entries + self.counter = itertools.count() # unique sequence count + self.REMOVED = '' + + def put(self, item, priority): + '''add a new task or update the priority of an existing item''' + if item in self.entry_finder: + self.check_remove(item) + count = next(self.counter) + entry = [priority, count, item] + self.entry_finder[item] = entry + heapq.heappush(self.pq, entry) + self.nodes.add(item) + + def check_remove(self, item): + if item not in self.entry_finder: + return + entry = self.entry_finder.pop(item) + entry[-1] = self.REMOVED + self.nodes.remove(item) + + def get(self): + """Remove and return the lowest priority task. Raise KeyError if empty.""" + while self.pq: + priority, count, item = heapq.heappop(self.pq) + if item is not self.REMOVED: + del self.entry_finder[item] + self.nodes.remove(item) + return item + raise KeyError('pop from an empty priority queue') + + def top_key(self): + return self.pq[0][0] + + def enumerate(self): + return self.pq + + def allnodes(self): + return self.nodes diff --git a/Search_based_Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc b/Search_based_Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc index e11383bec79f751c1c08c716b0b9d562f6df76ff..748cceb3d1ed6ec92e9f2a72816c8691aa3b79ca 100644 GIT binary patch delta 636 zcmZWlO-oxr6m{NLVwzVFloH}ss}n9!icq>qg)sUaNhE!F+&86W<*r+m zS#(wE)}`nV@B^W^>7pB%U(-4Bq|){=?=bhCbI+Z7e-$r_e#Q55J^CyBoUBis`qL~^ z-{wge)X!eAieuIKRh>u zb04Q>^42?G?Xy~@%2-o=&lcQ33?2{3o9xT|VS288X&f-ALD`K^&*SpFr`6Sm}HtuG+ zd8qNEl4Bfc2}Nv=3LyUWq9yqwcV0E~7&-&ZpCvsEMZ+Mlu9B6yuKwdDFCI0<%Sf&P z(3D;S9y+EY$T#^a8<2}4Rw>WWL{q9ziv!`a7624sJzbhZnz`pF}&81 Vch?ia30vi9uNR>HYxB)t{0;SCkoy1t delta 607 zcmZWlJ4+)`6rP)TV)xf&l)5L~0;Bc`@hY-F+yCLvCSGgnj)Ei5fWZeg>9 zTiOVsY_W}4X>TFiU*r8I7zGFBDJBwDq+UP<5C>!d zVtuQD-qG%*4@LI3;|~45ZjBXN0CyKpQ)xG>Fisi$Fz=3j+rt1<4U);oP+wRabiYovNr6S-{(;O=b$r9@nC&_GT zUhy*02-KmDf@aH%xJzEGje;kGl83Q3M>E|K1OVO(3i$v4 diff --git a/Search_based_Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc b/Search_based_Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc index af8baf399a72f2b2dfb3918584cefb970a4ad84c..63be0bad526e1a23ac94b77a76e37f049297080a 100644 GIT binary patch delta 45 tcmcbmb4!QYiIWMYp`N-R!IiJzR!BoAY6p2&1k7yuXS4Q>Dc delta 45 tcmcbmb4!QYiI