bi_A*3D fin

Search-based Planning/Search_3D/bidirectionalAstar3D.py

@@ -6,6 +6,7 @@
 """
 import numpy as np
 import matplotlib.pyplot as plt
+from collections import defaultdict
 
 import os
 import sys
@@ -27,14 +28,14 @@ class Weighted_A_star(object):
         self.Space = StateSpace(self) # key is the point, store g value
         self.start, self.goal = getNearest(self.Space,self.env.start), getNearest(self.Space,self.env.goal)
         self.AABB = getAABB(self.env.blocks)
-        self.Space[hash3D(getNearest(self.Space,self.start))] = 0 # set g(x0) = 0
-        self.Space[hash3D(getNearest(self.Space,self.goal))] = 0 # set g(x0) = 0
+        self.Space[hash3D(self.start)] = 0 # set g(x0) = 0
+        self.Space[hash3D(self.goal)] = 0 # set g(x0) = 0
         self.OPEN1 = queue.QueuePrior() # store [point,priority]
         self.OPEN2 = queue.QueuePrior()
         self.h1 = Heuristic(self.Space,self.goal) # tree NO.1
         self.h2 = Heuristic(self.Space,self.start) # tree NO.2
-        self.Parent = {}
-        self.CLOSED = {}
+        self.Parent1, self.Parent2 = {}, {}
+        self.CLOSED1, self.CLOSED2 = {}, {}
         self.V = []
         self.done = False
         self.Path = []
@@ -52,11 +53,11 @@ class Weighted_A_star(object):
         self.OPEN1.put(x0, self.Space[x0] + self.h1[x0]) # item, priority = g + h
         self.OPEN2.put(xt, self.Space[xt] + self.h2[xt]) # item, priority = g + h
         self.ind = 0
-        while not any(check in self.OPEN1.enumerate() for check in self.OPEN2.enumerate()): # while xt not reached and open is not empty
+        while not any(check in self.CLOSED1 for check in self.CLOSED2): # while xt not reached and open is not empty
             strxi1, strxi2 = self.OPEN1.get(), self.OPEN2.get() 
             xi1, xi2 = dehash(strxi1), dehash(strxi2)
-            self.CLOSED[strxi1] = [] # add the point in CLOSED set
-            self.CLOSED[strxi2] = []
+            self.CLOSED1[strxi1] = [] # add the point in CLOSED set
+            self.CLOSED2[strxi2] = []
             self.V.append(xi1)
             self.V.append(xi2)
             visualization(self)
@@ -65,6 +66,7 @@ class Weighted_A_star(object):
             self.evaluation(allchild2,strxi2,xi2,conf=2)
             if self.ind % 100 == 0: print('iteration number = '+ str(self.ind))
             self.ind += 1
+        self.common = set(self.CLOSED1).intersection(self.CLOSED2)
         self.done = True
         self.Path = self.path()
         visualization(self)
@@ -73,33 +75,45 @@ class Weighted_A_star(object):
     def evaluation(self, allchild, strxi, xi, conf):
         for xj in allchild:
             strxj = hash3D(xj)
-            if strxj not in self.CLOSED:
-                gi, gj = self.Space[strxi], self.Space[strxj]
-                a = gi + cost(xi,xj)
-                if a < gj:
-                    self.Space[strxj] = a
-                    self.Parent[strxj] = xi
-                    if conf == 1:
+            if conf == 1:
+                if strxj not in self.CLOSED1:
+                    gi, gj = self.Space[strxi], self.Space[strxj]
+                    a = gi + cost(xi,xj)
+                    if a < gj:
+                        self.Space[strxj] = a
+                        self.Parent1[strxj] = xi
                         if (a, strxj) in self.OPEN1.enumerate():
                             self.OPEN1.put(strxj, a+1*self.h1[strxj])
                         else:
                             self.OPEN1.put(strxj, a+1*self.h1[strxj])
-                    elif conf == 2:
+            if conf == 2:
+                if strxj not in self.CLOSED2:
+                    gi, gj = self.Space[strxi], self.Space[strxj]
+                    a = gi + cost(xi,xj)
+                    if a < gj:
+                        self.Space[strxj] = a
+                        self.Parent2[strxj] = xi
                         if (a, strxj) in self.OPEN2.enumerate():
                             self.OPEN2.put(strxj, a+1*self.h2[strxj])
                         else:
                             self.OPEN2.put(strxj, a+1*self.h2[strxj])
-        
+            
     def path(self):
+        # TODO: fix path
         path = []
-        strx = hash3D(self.goal)
+        strgoal = hash3D(self.goal)
         strstart = hash3D(self.start)
+        strx = list(self.common)[0]
         while strx != strstart:
-            path.append([dehash(strx),self.Parent[strx]])
-            strx = hash3D(self.Parent[strx])
+            path.append([dehash(strx),self.Parent1[strx]])
+            strx = hash3D(self.Parent1[strx])
+        strx = list(self.common)[0]
+        while strx != strgoal:
+            path.append([dehash(strx),self.Parent2[strx]])
+            strx = hash3D(self.Parent2[strx])
         path = np.flip(path,axis=0)
         return path
 
 if __name__ == '__main__':
-    Astar = Weighted_A_star(1)
+    Astar = Weighted_A_star(0.5)
     Astar.run()