update

Search-based Planning/.idea/workspace.xml

@@ -21,6 +21,7 @@
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
       <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
+      <change beforePath="$PROJECT_DIR$/Search_3D/Dstar3D.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_3D/Dstar3D.py" afterDir="false" />
     </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
@@ -94,7 +95,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="Field_D_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="Dstar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -102,11 +103,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_3D" />
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/Field_D_star.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_3D/Dstar3D.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -115,7 +116,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="Potential_Field" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="Field_D_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -127,7 +128,7 @@
       <option name="IS_MODULE_SDK" value="true" />
       <option name="ADD_CONTENT_ROOTS" value="true" />
       <option name="ADD_SOURCE_ROOTS" value="true" />
-      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/Potential_Field.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/Field_D_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -203,17 +204,17 @@
       <item itemvalue="Python.dijkstra" />
       <item itemvalue="Python.D_star" />
       <item itemvalue="Python.Field_D_star" />
-      <item itemvalue="Python.Potential_Field" />
       <item itemvalue="Python.ReedsShepp" />
       <item itemvalue="Python.simulation" />
+      <item itemvalue="Python.Dstar3D" />
     </list>
     <recent_temporary>
       <list>
         <item itemvalue="Python.D_star" />
+        <item itemvalue="Python.Dstar3D" />
         <item itemvalue="Python.simulation" />
         <item itemvalue="Python.ReedsShepp" />
         <item itemvalue="Python.Field_D_star" />
-        <item itemvalue="Python.Potential_Field" />
       </list>
     </recent_temporary>
   </component>

Search-based Planning/Search_3D/Dstar3D.py

@@ -8,25 +8,26 @@ from collections import defaultdict
 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 StateSpace, getDist, getNearest, getRay, isinbound, isinball, isCollide, children, cost, initcost
+from Search_3D.utils3D import StateSpace, getDist, getNearest, getRay, isinbound, isinball, isCollide, children, cost, \
+    initcost
 from Search_3D.plot_util3D import visualization
 
 
 class D_star(object):
-    def __init__(self,resolution = 1):
+    def __init__(self, resolution=1):
         self.Alldirec = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1], [1, 1, 0], [1, 0, 1], [0, 1, 1], [1, 1, 1],
                                   [-1, 0, 0], [0, -1, 0], [0, 0, -1], [-1, -1, 0], [-1, 0, -1], [0, -1, -1],
                                   [-1, -1, -1],
                                   [1, -1, 0], [-1, 1, 0], [1, 0, -1], [-1, 0, 1], [0, 1, -1], [0, -1, 1],
                                   [1, -1, -1], [-1, 1, -1], [-1, -1, 1], [1, 1, -1], [1, -1, 1], [-1, 1, 1]])
-        self.env = env(resolution = resolution)
+        self.env = env(resolution=resolution)
         self.X = StateSpace(self.env)
         self.x0, self.xt = getNearest(self.X, self.env.start), getNearest(self.X, self.env.goal)
-        self.b = defaultdict(lambda: defaultdict(dict))# back pointers every state has one except xt.
-        self.OPEN = {} # OPEN list, here use a hashmap implementation. hash is point, key is value 
-        self.h = self.initH() # estimate from a point to the end point
-        self.tag = self.initTag() # set all states to new
-        self.V = set()# vertice in closed
+        self.b = defaultdict(lambda: defaultdict(dict))  # back pointers every state has one except xt.
+        self.OPEN = {}  # OPEN list, here use a hashmap implementation. hash is point, key is value
+        self.h = self.initH()  # estimate from a point to the end point
+        self.tag = self.initTag()  # set all states to new
+        self.V = set()  # vertice in closed
         # initialize cost set
         # self.c = initcost(self)
         # for visualization
@@ -34,8 +35,6 @@ class D_star(object):
         self.Path = []
         self.done = False
 
-        
-
     def initH(self):
         # h set, all initialzed h vals are 0 for all states.
         h = {}
@@ -47,7 +46,7 @@ class D_star(object):
         # tag , New point (never been in the OPEN list)
         #       Open point ( currently in OPEN )
         #       Closed (currently in CLOSED)
-        t = {} 
+        t = {}
         for xi in self.X:
             t[xi] = 'New'
         return t
@@ -57,7 +56,7 @@ class D_star(object):
         # -1 if it does not exist
         if self.OPEN:
             minv = np.inf
-            for v,k in enumerate(self.OPEN):
+            for v, k in enumerate(self.OPEN):
                 if v < minv: minv = v
             return minv
         return -1
@@ -68,46 +67,46 @@ class D_star(object):
         # it also removes this min value form the OPEN set.
         if self.OPEN:
             minv = np.inf
-            for v,k in enumerate(self.OPEN):
+            for v, k in enumerate(self.OPEN):
                 if v < minv: mink, minv = k, v
             return mink, self.OPEN.pop(mink)
         return None, -1
-    
+
     def insert(self, x, h_new):
         # inserting a key and value into OPEN list (x, kx)
         # depending on following situations
         if self.tag[x] == 'New':
             kx = h_new
         if self.tag[x] == 'Open':
-            kx = min(self.OPEN[x],h_new)
+            kx = min(self.OPEN[x], h_new)
         if self.tag[x] == 'Closed':
             kx = min(self.h[x], h_new)
         self.OPEN[x] = kx
-        self.h[x],self.tag[x] = h_new, 'Open'
-            
+        self.h[x], self.tag[x] = h_new, 'Open'
+
     def process_state(self):
         x, kold = self.min_state()
         self.tag[x] = 'Closed'
         self.V.add(x)
         if x == None: return -1
-        if kold < self.h[x]: # raised states
-            for y in children(self,x):
-                a = self.h[y] + cost(self,y,x)
+        if kold < self.h[x]:  # raised states
+            for y in children(self, x):
+                a = self.h[y] + cost(self, y, x)
                 if self.h[y] <= kold and self.h[x] > a:
-                    self.b[x], self.h[x] = y , a
-        elif kold == self.h[x]:# lower
-            for y in children(self,x):
-                bb = self.h[x] + cost(self,x,y)
+                    self.b[x], self.h[x] = y, a
+        elif kold == self.h[x]:  # lower
+            for y in children(self, x):
+                bb = self.h[x] + cost(self, x, y)
                 if self.tag[y] == 'New' or \
-                    (self.b[y] == x and self.h[y] != bb) or \
-                    (self.b[y] != x and self.h[y] > bb):
+                        (self.b[y] == x and self.h[y] != bb) or \
+                        (self.b[y] != x and self.h[y] > bb):
                     self.b[y] = x
                     self.insert(y, bb)
-        else: 
-            for y in children(self,x):
-                bb = self.h[x] + cost(self,x,y)
+        else:
+            for y in children(self, x):
+                bb = self.h[x] + cost(self, x, y)
                 if self.tag[y] == 'New' or \
-                    (self.b[y] == x and self.h[y] != bb):
+                        (self.b[y] == x and self.h[y] != bb):
                     self.b[y] = x
                     self.insert(y, bb)
                 else:
@@ -115,11 +114,11 @@ class D_star(object):
                         self.insert(x, self.h[x])
                     else:
                         if self.b[y] != x and self.h[y] > bb and \
-                            self.tag[y] == 'Closed' and self.h[y] == kold:
+                                self.tag[y] == 'Closed' and self.h[y] == kold:
                             self.insert(y, self.h[y])
         return self.get_kmin()
 
-    def modify_cost(self,x,y,cval):
+    def modify_cost(self, x, y, cval):
         # TODO: implement own function
         # self.c[x][y] = cval
         # if self.tag[x] == 'Closed': self.insert(x,self.h[x])
@@ -131,11 +130,12 @@ class D_star(object):
             kmin = self.process_state()
             if kmin >= self.h[x]: break
 
-    def path(self, goal = None):
+    def path(self, goal=None):
         path = []
         if not goal:
             x = self.x0
-        else: x = goal
+        else:
+            x = goal
         start = self.xt
         while x != start:
             path.append([np.array(x), np.array(self.b[x])])
@@ -147,7 +147,7 @@ class D_star(object):
         self.OPEN[self.xt] = 0
         # first run
         while True:
-            #TODO: self.x0 = 
+            # TODO: self.x0 =
             self.process_state()
             visualization(self)
             if self.tag[self.x0] == "Closed":
@@ -162,16 +162,15 @@ class D_star(object):
         while s != self.xt:
             if s == tuple(self.env.start):
                 s = self.b[self.x0]
-            else: 
+            else:
                 s = self.b[s]
-            self.modify(s)
-            self.env.move_block(a=[0,0,-0.1],s=0.5,block_to_move=1,mode='translation')
+            # self.modify(s)
+            self.env.move_block(a=[0, 0, -0.1], s=0.5, block_to_move=1, mode='translation')
             self.Path = self.path(s)
             visualization(self)
             self.ind += 1
-        
 
-        
+
 if __name__ == '__main__':
     D = D_star(1)
-    D.run()
+    D.run()