update plotting

Sampling_based_Planning/rrt_3D/FMT_star3D.py

@@ -129,7 +129,7 @@ class FMT_star:
                 return 
             ind += 1
             print(str(ind) + ' node expanded')
-            # self.visualization(ind, E)
+            self.visualization(ind, E)
             # update current node
             Vopenlist = list(self.Vopen)
             z = Vopenlist[np.argmin([self.c[y] for y in self.Vopen])]
@@ -160,7 +160,8 @@ class FMT_star:
             
             # 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.dist = 15
             # ax.view_init(elev=-8., azim=180)
             ax.clear()
             # drawing objects

Sampling_based_Planning/rrt_3D/plot_util3D.py

@@ -40,9 +40,7 @@ def draw_block_list(ax, blocks, color=None, alpha=0.15):
     if type(ax) is Poly3DCollection:
         ax.set_verts(vl[fl])
     else:
-        pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k')
-        pc.set_facecolor(color)
-        h = ax.add_collection3d(pc)
+        h = ax.add_collection3d(Poly3DCollection(vl[fl], facecolors='black', alpha=alpha, linewidths=1, edgecolors='k'))
         return h
 
 
@@ -68,9 +66,7 @@ def draw_obb(ax, OBB, color=None, alpha=0.15):
     if type(ax) is Poly3DCollection:
         ax.set_verts(vl[fl])
     else:
-        pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k')
-        pc.set_facecolor(color)
-        h = ax.add_collection3d(pc)
+        h = ax.add_collection3d(Poly3DCollection(vl[fl], facecolors='black', alpha=alpha, linewidths=1, edgecolors='k'))
         return h