'D*'

Search-based Planning/Search_3D/Dstar3D.py

@@ -66,7 +66,7 @@ class D_star(object):
         # get the minimum of the k val in OPEN
         # -1 if it does not exist
         if self.OPEN:
-            return min([x for x in self.OPEN.values()])
+            return min(self.OPEN.values())
         return -1
 
     def min_state(self):
@@ -74,12 +74,10 @@ class D_star(object):
         # if empty, returns None and -1
         # it also removes this min value form the OPEN set.
         if self.OPEN:
-            mink = -1
-            minv = np.inf
-            for v, k in enumerate(self.OPEN):
-                if v < minv:
-                    mink, minv = k, v
-            return mink, self.OPEN.pop(mink)
+            minvalue = min(self.OPEN.values())
+            for k in self.OPEN.keys():
+                if self.OPEN[k] == minvalue:
+                    return k, self.OPEN.pop(k)
         return None, -1
 
     def insert(self, x, h_new):
@@ -130,12 +128,9 @@ class D_star(object):
         return self.get_kmin()
 
     def modify_cost(self, x):
-        # TODO: implement own function
-        # self.c[x][y] = cval
         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()
@@ -164,7 +159,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
@@ -175,20 +170,18 @@ 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)
+        for i in range(5):
+            self.env.move_block(a=[0.25, 0, 0], s=0.5, block_to_move=1, mode='translation')
+            self.env.move_block(a=[0, 0, -0.25], s=0.5, block_to_move=0, mode='translation')
+            # travel from end to start
             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 there is a change of cost, or a collision.
                 if cost(self, s, sparent) == np.inf:
-                    # print(s, "   ", sparent)
                     self.modify(s)
                     continue
                 self.ind += 1
@@ -199,5 +192,5 @@ class D_star(object):
 
 
 if __name__ == '__main__':
-    D = D_star(0.75)
+    D = D_star(1)
     D.run()

Search-based Planning/Search_3D/LP_Astar3D.py

@@ -7,7 +7,7 @@ import sys
 sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
 from Search_3D.env3D import env
 from Search_3D import Astar3D
-from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isinbound, isinball, hash3D, dehash, \
+from Search_3D.utils3D import getDist, getRay, g_Space, Heuristic, getNearest, isinbound, isinball, \
     cost, obstacleFree
 from Search_3D.plot_util3D import visualization
 import queue
@@ -89,7 +89,7 @@ class Lifelong_Astar(object):
         ray , dist = getRay(x, child) ,  getDist(x, child)
         if not isinbound(self.env.boundary,child):
             return True, dist
-        for i in self.env.AABB:
+        for i in self.env.AABB_pyrr:
             shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i)
             if shot is not None:
                 dist_wall = getDist(x, shot)
@@ -177,7 +177,7 @@ class Lifelong_Astar(object):
 
 if __name__ == '__main__':
     sta = time.time()
-    Astar = Lifelong_Astar(1)
+    Astar = Lifelong_Astar(0.5)
     Astar.ComputePath()
     Astar.change_env()
     Astar.ComputePath()

Search-based Planning/Search_3D/env3D.py

@@ -22,12 +22,12 @@ def getblocks():
         Obstacles.append([j for j in i])
     return np.array(Obstacles)
 
-# def getAABB(blocks):
-#     # used for Pyrr package for detecting collision
-#     AABB = []
-#     for i in blocks:
-#         AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)]))  # make AABBs alittle bit of larger
-#     return AABB
+def getAABB(blocks):
+    # used for Pyrr package for detecting collision
+    AABB = []
+    for i in blocks:
+        AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)]))  # make AABBs alittle bit of larger
+    return AABB
 
 class aabb(object):
     def __init__(self,AABB):
@@ -57,6 +57,7 @@ class env():
         self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) 
         self.blocks = getblocks()
         self.AABB = getAABB2(self.blocks)
+        self.AABB_pyrr = getAABB(self.blocks)
         self.balls = getballs()
         self.start = np.array([0.5, 2.5, 5.5])
         self.goal = np.array([19.0, 2.5, 5.5])
@@ -66,6 +67,7 @@ class env():
         newblock = add_block()
         self.blocks = np.vstack([self.blocks,newblock])
         self.AABB = getAABB2(self.blocks)
+        self.AABB_pyrr = getAABB(self.blocks)
 
     def move_start(self, x):
         self.start = x