Merge branch 'master' of https://github.com/zhm-real/path-planning-algorithms

Search-based Planning/Search_3D/Dstar3D.py

@@ -135,11 +135,13 @@ class D_star(object):
         xparent = self.b[x]
         if self.tag[x] == 'Closed':
             self.insert(x, self.h[xparent] + cost(self, x, xparent))
-
+            # self.insert(x, self.h[xparent])
     def modify(self, x):
         self.modify_cost(x)
+        self.V = set()
         while True:
             kmin = self.process_state()
+            # visualization(self)
             if kmin >= self.h[x]:
                 break
 
@@ -162,7 +164,7 @@ class D_star(object):
         while True:
             # TODO: self.x0 =
             self.process_state()
-            # visualization(self)
+            visualization(self)
             if self.tag[self.x0] == "Closed":
                 break
             self.ind += 1
@@ -173,29 +175,29 @@ class D_star(object):
         # plt.show()
         # when the environemnt changes over time
 
-        # for i in range(2):
-        #     self.env.move_block(a=[0, 0, -1], s=0.5, block_to_move=1, mode='translation')
-        #     visualization(self)
-        #     s = tuple(self.env.start)
-        #
-        #     while s != self.xt:
-        #         if s == tuple(self.env.start):
-        #             sparent = self.b[self.x0]
-        #         else:
-        #             sparent = self.b[s]
-        #         # self.update_obs()
-        #
-        #         if cost(self, s, sparent) == np.inf:
-        #             # print(s, "   ", sparent)
-        #             self.modify(s)
-        #             continue
-        #         self.ind += 1
-        #         s = sparent
-        #     self.Path = self.path()
-        #     visualization(self)
+        for i in range(2):
+            self.env.move_block(a=[0, 0, -1], s=0.5, block_to_move=1, mode='translation')
+            visualization(self)
+            s = tuple(self.env.start)
+        
+            while s != self.xt:
+                if s == tuple(self.env.start):
+                    sparent = self.b[self.x0]
+                else:
+                    sparent = self.b[s]
+                # self.update_obs()
+        
+                if cost(self, s, sparent) == np.inf:
+                    # print(s, "   ", sparent)
+                    self.modify(s)
+                    continue
+                self.ind += 1
+                s = sparent
+            self.Path = self.path()
+            visualization(self)
         plt.show()
 
 
 if __name__ == '__main__':
-    D = D_star(0.5)
+    D = D_star(0.75)
     D.run()

Search-based Planning/Search_3D/utils3D.py

@@ -120,22 +120,18 @@ def g_Space(initparams):
 def isCollide(initparams, x, child):
     '''see if line intersects obstacle'''
     dist = getDist(x, child)
-    if not isinbound(initparams.env.boundary, child): return True, dist
-    for i in initparams.env.AABB:
-        # shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i)
-        # if shot is not None:
-        #     dist_wall = getDist(x, shot)
-        #     if dist_wall <= dist:  # collide
-        #         return True, dist
-        if lineAABB(x, child, dist, i): return True, dist
+    if not isinbound(initparams.env.boundary, child): 
+        return True, dist
+    for i in range(len(initparams.env.AABB)):
+        # if isinbound(initparams.env.blocks[i], child): 
+        #     return True, dist
+        if lineAABB(x, child, dist, initparams.env.AABB[i]): 
+            return True, dist
     for i in initparams.env.balls:
-        if isinball(i, child): return True, dist
-        # shot = pyrr.geometric_tests.ray_intersect_sphere(ray, i)
-        # if shot != []:
-        #     dists_ball = [getDist(x, j) for j in shot]
-        #     if all(dists_ball <= dist):  # collide
-        #         return True, dist
-        if lineSphere(x, child, i): return True, dist
+        # if isinball(i, child): 
+        #     return True, dist
+        if lineSphere(x, child, i): 
+            return True, dist
     return False, dist
 
 
@@ -145,6 +141,10 @@ def children(initparams, x):
     resolution = initparams.env.resolution
     for direc in initparams.Alldirec:
         child = tuple(map(np.add, x, np.multiply(direc, resolution)))
+        if any([isinball(i ,child) for i in initparams.env.balls]):
+            continue
+        if any([isinbound(i ,child) for i in initparams.env.blocks]):
+            continue
         if isinbound(initparams.env.boundary, child):
             allchild.append(child)
     return allchild