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 0000000..ac7a5c7 Binary files /dev/null and b/Sampling_based_Planning/rrt_3D/__pycache__/queue.cpython-37.pyc differ 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 e11383b..748cceb 100644 Binary files a/Search_based_Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc and b/Search_based_Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc differ diff --git a/Search_based_Planning/Search_3D/__pycache__/env3D.cpython-37.pyc b/Search_based_Planning/Search_3D/__pycache__/env3D.cpython-37.pyc index 4b7036f..01f0334 100644 Binary files a/Search_based_Planning/Search_3D/__pycache__/env3D.cpython-37.pyc and b/Search_based_Planning/Search_3D/__pycache__/env3D.cpython-37.pyc differ 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 af8baf3..63be0ba 100644 Binary files a/Search_based_Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc and b/Search_based_Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc differ diff --git a/Search_based_Planning/Search_3D/__pycache__/queue.cpython-37.pyc b/Search_based_Planning/Search_3D/__pycache__/queue.cpython-37.pyc index f0e6250..4c7b1cd 100644 Binary files a/Search_based_Planning/Search_3D/__pycache__/queue.cpython-37.pyc and b/Search_based_Planning/Search_3D/__pycache__/queue.cpython-37.pyc differ diff --git a/Search_based_Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc b/Search_based_Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc index 1db5f8f..8a2759e 100644 Binary files a/Search_based_Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc and b/Search_based_Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc differ