diff --git a/Sampling_based_Planning/rrt_3D/BIT_star3D.py b/Sampling_based_Planning/rrt_3D/BIT_star3D.py index fb1a551..a996176 100644 --- a/Sampling_based_Planning/rrt_3D/BIT_star3D.py +++ b/Sampling_based_Planning/rrt_3D/BIT_star3D.py @@ -319,7 +319,7 @@ class BIT_star: # ax.view_init(elev=0.+ 0.03*self.ind/(2*np.pi), azim=90 + 0.03*self.ind/(2*np.pi)) # ax.view_init(elev=0., azim=90.) - ax.view_init(elev=90., azim=0.) + ax.view_init(elev=90., azim=60.) # ax.view_init(elev=-8., azim=180) ax.clear() # drawing objects @@ -335,11 +335,11 @@ class BIT_star: if len(V) > 0: ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g', ) if len(Xsample) > 0: # plot the sampled points - ax.scatter3D(Xsample[:, 0], Xsample[:, 1], Xsample[:, 2], s=2, color='b', ) + ax.scatter3D(Xsample[:, 0], Xsample[:, 1], Xsample[:, 2], s=1, color='b',) 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 - ax.dist = 5 + ax.dist = 11 set_axes_equal(ax) make_transparent(ax) #plt.xlabel('s') 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 93843d0..5c3619e 100644 Binary files a/Sampling_based_Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc and b/Sampling_based_Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc differ diff --git a/Sampling_based_Planning/rrt_3D/plot_util3D.py b/Sampling_based_Planning/rrt_3D/plot_util3D.py index 44a2d8e..65a981f 100644 --- a/Sampling_based_Planning/rrt_3D/plot_util3D.py +++ b/Sampling_based_Planning/rrt_3D/plot_util3D.py @@ -106,7 +106,7 @@ def visualization(initparams): # 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=90., azim=0.) + ax.view_init(elev=65., azim=60.) # ax.view_init(elev=-8., azim=180) ax.clear() # drawing objects @@ -122,7 +122,7 @@ def visualization(initparams): 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 - ax.dist = 5 + ax.dist = 15 set_axes_equal(ax) make_transparent(ax) #plt.xlabel('s') diff --git a/Sampling_based_Planning/rrt_3D/rrt3D.py b/Sampling_based_Planning/rrt_3D/rrt3D.py index 096ea74..3d9fcf4 100644 --- a/Sampling_based_Planning/rrt_3D/rrt3D.py +++ b/Sampling_based_Planning/rrt_3D/rrt3D.py @@ -33,7 +33,7 @@ class rrt(): self.ind = 0 - self.fig = plt.figure(figsize=(10, 8)) + # self.fig = plt.figure(figsize=(10, 8)) def wireup(self, x, y): # self.E.add_edge([s, y]) # add edge @@ -55,7 +55,7 @@ class rrt(): self.Path, D = path(self) print('Total distance = ' + str(D)) break - # visualization(self) + visualization(self) self.i += 1 self.ind += 1 # if the goal is really reached diff --git a/Sampling_based_Planning/rrt_3D/rrt_star3D.py b/Sampling_based_Planning/rrt_3D/rrt_star3D.py index c59f279..bd86a34 100644 --- a/Sampling_based_Planning/rrt_3D/rrt_star3D.py +++ b/Sampling_based_Planning/rrt_3D/rrt_star3D.py @@ -68,7 +68,8 @@ class rrtstar(): if not collide: Xnear = near(self,xnew) self.V.append(xnew) # add point - # visualization(self) + visualization(self) + plt.title('rrt*') # minimal path and minimal cost xmin, cmin = xnearest, cost(self, xnearest) + getDist(xnearest, xnew) # connecting along minimal cost path