From 36cc2ff837811754a61fb5ffa77a7e3c481c36da Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Mon, 3 Aug 2020 20:39:24 -0700 Subject: [PATCH] 'rrt_connect' --- .../rrt_3D/dynamic_rrt3D.py | 2 +- .../rrt_3D/rrt_connect3D.py | 44 ++++++++++++++++--- 2 files changed, 40 insertions(+), 6 deletions(-) diff --git a/Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py b/Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py index 1d3324c..92ffeab 100644 --- a/Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py +++ b/Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py @@ -139,7 +139,7 @@ class dynamic_rrt_3D(): t = 0 while True: # move the block while the robot is moving - new, old = self.env.move_block(a=[0, 0, -0.2], mode='translation') + new, _ = self.env.move_block(a=[0, 0, -0.2], mode='translation') self.InvalidateNodes(new) # if solution path contains invalid node self.done = True diff --git a/Sampling_based_Planning/rrt_3D/rrt_connect3D.py b/Sampling_based_Planning/rrt_3D/rrt_connect3D.py index 1da6ede..8466d17 100644 --- a/Sampling_based_Planning/rrt_3D/rrt_connect3D.py +++ b/Sampling_based_Planning/rrt_3D/rrt_connect3D.py @@ -16,6 +16,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_ba from rrt_3D.env3D import env from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset +from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent class rrt_connect(): def __init__(self): @@ -49,7 +50,7 @@ class rrt_connect(): qnear = tuple(self.NEAREST_NEIGHBOR(q, tree)) qnew, dist = steer(self, qnear, q) self.qnew = qnew # store qnew outside - if self.NEW_CONFIG(q, qnear, qnew, dist=dist): + if self.NEW_CONFIG(q, qnear, qnew, dist=None): tree.add_vertex(qnew) tree.add_edge(qnear, qnew) if qnew == q: @@ -93,29 +94,62 @@ class rrt_connect(): print(k) qrand = self.RANDOM_CONFIG() if self.EXTEND(Tree_A, qrand) != 'Trapped': - print('trapped') qnew = self.qnew # get qnew from outside if self.CONNECT(Tree_B, qnew) == 'Reached': print('reached') # return self.PATH(Tree_A, Tree_B) return + else: + print('not reached') Tree_A, Tree_B = self.SWAP(Tree_A, Tree_B) + self.visualization(Tree_A, Tree_B, k) print('Failure') return 'Failure' # def PATH(self, tree_a, tree_b): - - def SWAP(self, tree_a, tree_b): tree_a, tree_b = tree_b, tree_a return tree_a, tree_b + def visualization(self, tree_a, tree_b, index): + if (index % 10 == 0 and index != 0) or self.done: + # a_V = np.array(tree_a.V) + # b_V = np.array(tree_b.V) + start = self.env.start + goal = self.env.goal + a_edges, b_edges = [], [] + for i in tree_a.Parent: + a_edges.append([i,tree_a.Parent[i]]) + for i in tree_b.Parent: + b_edges.append([i,tree_b.Parent[i]]) + ax = plt.subplot(111, projection='3d') + ax.view_init(elev=8., azim=90.) + ax.clear() + 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, a_edges, visibility=0.75, color='g') + draw_line(ax, b_edges, visibility=0.75, color='y') + # draw_line(ax, Path, color='r') + 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') + 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) + ax.set_axis_off() + plt.pause(0.0001) + class Tree(): def __init__(self, node): self.V = [] self.Parent = {} self.V.append(node) - self.Parent[node] = None + # self.Parent[node] = None def add_vertex(self, node): if node not in self.V: