'DLite'

Search-based Planning/Search_3D/Dstar3D.py

@@ -187,5 +187,5 @@ class D_star(object):
 
 
 if __name__ == '__main__':
-    D = D_star(0.5)
+    D = D_star(1)
     D.run()

Search-based Planning/Search_3D/DstarLite3D.py

@@ -159,12 +159,12 @@ class D_star_Lite(object):
         ischanged = False
         while getDist(self.x0, self.xt) > 2*self.env.resolution:
             #---------------------------------- at 5th node, the environment is changed and cost is updated
-            if t == 1: 
+            if t % 2 == 0: 
                 # new1,old1 = self.env.move_block(a=[0, 0, -2], s=0.5, block_to_move=0, mode='translation')
-                new1,old1 = self.env.move_block(a=[0, 0, -2], s=0.5, block_to_move=0, mode='translation')
+                new1,old1 = self.env.move_block(a=[0, 0, -0.1], s=0.5, block_to_move=0, mode='translation')
+                #new2,old2 = self.env.move_block(a=[-0.3, 0, -0.1], s=0.5, block_to_move=1, mode='translation')
                 ischanged = True
                 self.Path = []
-                visualization(self)
             # if t == 5: 
             #     new1,old1 = self.env.move_block(a=[0, 0, -1], s=0.5, block_to_move=1, mode='translation')
             #     ischanged = True
@@ -183,6 +183,8 @@ class D_star_Lite(object):
                 self.km += heuristic_fun(self, self.x0, s_last)
                 s_last = self.x0
                 CHANGED = self.updatecost(True, new1, old1)
+                #CHANGED2 = self.updatecost(True, new2, old2)
+                #CHANGED = CHANGED.union(CHANGED2)
                 self.V = set()
                 for u in CHANGED:
                     self.UpdateVertex(u)

Search-based Planning/Search_3D/plot_util3D.py

@@ -61,7 +61,8 @@ def visualization(initparams):
         # edges = E.get_edge()
         # 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.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi))
+        ax.view_init(elev=0., azim=90)
         ax.clear()
         # drawing objects
         draw_Spheres(ax, initparams.env.balls)
@@ -69,7 +70,8 @@ def visualization(initparams):
         draw_block_list(ax, np.array([initparams.env.boundary]),alpha=0)
         # draw_line(ax,edges,visibility=0.25)
         draw_line(ax,Path,color='r')
-        ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',)
+        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