'updated'

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')

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')

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

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