update

Search-based Planning/.idea/workspace.xml

@@ -22,6 +22,7 @@
     <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" />
+      <change beforePath="$PROJECT_DIR$/Search_3D/utils3D.py" beforeDir="false" afterPath="$PROJECT_DIR$/Search_3D/utils3D.py" afterDir="false" />
     </list>
     <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
@@ -73,8 +74,8 @@
       </list>
     </option>
   </component>
-  <component name="RunManager" selected="Python.Dstar3D">
-    <configuration name="D_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
+  <component name="RunManager" selected="Python.Astar3D">
+    <configuration name="Astar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -82,11 +83,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/D_star.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_3D/Astar3D.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -95,7 +96,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="Dstar3D" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="D_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -103,11 +104,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_3D" />
+      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$/Search_2D" />
       <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_3D/Dstar3D.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/D_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -116,7 +117,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" />
@@ -124,11 +125,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" />
@@ -203,18 +204,18 @@
     <list>
       <item itemvalue="Python.dijkstra" />
       <item itemvalue="Python.D_star" />
-      <item itemvalue="Python.Field_D_star" />
       <item itemvalue="Python.ReedsShepp" />
       <item itemvalue="Python.simulation" />
       <item itemvalue="Python.Dstar3D" />
+      <item itemvalue="Python.Astar3D" />
     </list>
     <recent_temporary>
       <list>
+        <item itemvalue="Python.Astar3D" />
         <item itemvalue="Python.Dstar3D" />
         <item itemvalue="Python.D_star" />
         <item itemvalue="Python.simulation" />
         <item itemvalue="Python.ReedsShepp" />
-        <item itemvalue="Python.Field_D_star" />
       </list>
     </recent_temporary>
   </component>

Search-based Planning/Search_3D/Dstar3D.py

@@ -74,17 +74,12 @@ class D_star(object):
         # if empty, returns None and -1
         # it also removes this min value form the OPEN set.
         if self.OPEN:
-            mink = min(self.OPEN, key=self.OPEN.get)
-            minv = self.OPEN[mink]
-            _ = self.OPEN.pop(mink)
-        #     #
-        #     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)
-            return mink, minv
+            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)
         return None, -1
 
     def insert(self, x, h_new):
@@ -167,7 +162,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
@@ -178,29 +173,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(1)
+    D = D_star(0.5)
     D.run()

Search-based Planning/Search_3D/utils3D.py

@@ -2,72 +2,76 @@ import numpy as np
 import pyrr
 from collections import defaultdict
 
+
 def getRay(x, y):
     direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]]
     return np.array([x, direc])
 
+
 def getDist(pos1, pos2):
     return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2]))
 
+
 def getManDist(pos1, pos2):
-    return sum([abs(pos1[0] - pos2[0]),abs(pos1[1] - pos2[1]),abs(pos1[2] - pos2[2])])
+    return sum([abs(pos1[0] - pos2[0]), abs(pos1[1] - pos2[1]), abs(pos1[2] - pos2[2])])
 
-def getNearest(Space,pt):
+
+def getNearest(Space, pt):
     '''get the nearest point on the grid'''
-    mindis,minpt = 1000,None
-    for pts in Space: 
-        dis = getDist(pts,pt)
+    mindis, minpt = 1000, None
+    for pts in Space:
+        dis = getDist(pts, pt)
         if dis < mindis:
-            mindis,minpt = dis,pts
+            mindis, minpt = dis, pts
     return minpt
 
-def Heuristic(Space,t):
+
+def Heuristic(Space, t):
     '''Max norm distance'''
     h = {}
     for k in Space.keys():
-        h[k] = max(abs(np.array([t[0]-k[0],t[1]-k[1],t[2]-k[2]])))
+        h[k] = max(abs(np.array([t[0] - k[0], t[1] - k[1], t[2] - k[2]])))
     return h
 
-def hash3D(x):
-    return str(x[0])+' '+str(x[1])+' '+str(x[2])
-
-def dehash(x):
-    return np.array([float(i) for i in x.split(' ')])
 
 def isinbound(i, x):
     if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]:
         return True
     return False
 
+
 def isinball(i, x):
     if getDist(i[0:3], x) <= i[3]:
         return True
     return False
 
-def lineSphere(p0,p1,ball):
+
+def lineSphere(p0, p1, ball):
     # https://cseweb.ucsd.edu/classes/sp19/cse291-d/Files/CSE291_13_CollisionDetection.pdf
-    c, r= ball[0:3],ball[-1]
+    c, r = ball[0:3], ball[-1]
     line = [p1[0] - p0[0], p1[1] - p0[1], p1[2] - p0[2]]
     d1 = [c[0] - p0[0], c[1] - p0[1], c[2] - p0[2]]
-    t = (1 / (line[0]*line[0] + line[1]*line[1] + line[2]*line[2])) * (line[0]*d1[0] + line[1]*d1[1] + line[2]*d1[2])
-    if t <= 0: 
+    t = (1 / (line[0] * line[0] + line[1] * line[1] + line[2] * line[2])) * (
+                line[0] * d1[0] + line[1] * d1[1] + line[2] * d1[2])
+    if t <= 0:
         if (d1[0] * d1[0] + d1[1] * d1[1] + d1[2] * d1[2]) <= r ** 2: return True
-    elif t >= 1: 
+    elif t >= 1:
         d2 = [c[0] - p1[0], c[1] - p1[1], c[2] - p1[2]]
         if (d2[0] * d2[0] + d2[1] * d2[1] + d2[2] * d2[2]) <= r ** 2: return True
-    elif 0 < t < 1: 
+    elif 0 < t < 1:
         x = [p0[0] + t * line[0], p0[1] + t * line[1], p0[2] + t * line[2]]
         k = [c[0] - x[0], c[1] - x[1], c[2] - x[2]]
-        if (k[0] * k[0] + k[1] * k[1] + k[2] * k[2]) <= r**2: return True
+        if (k[0] * k[0] + k[1] * k[1] + k[2] * k[2]) <= r ** 2: return True
     return False
-    
-def lineAABB(p0,p1,dist,aabb):
-    #https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1
-    mid = [(p0[0] + p1[0]) / 2, (p0[1] + p1[1]) / 2, (p0[2] + p1[2]) / 2] # mid point
-    I = [(p1[0] - p0[0]) / dist, (p1[1] - p0[1]) / dist, (p1[2] - p0[2]) / dist] # unit direction
-    hl = dist / 2 # radius
-    P = aabb.P#center of the AABB
-    E = aabb.E# extents of AABB
+
+
+def lineAABB(p0, p1, dist, aabb):
+    # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1
+    mid = [(p0[0] + p1[0]) / 2, (p0[1] + p1[1]) / 2, (p0[2] + p1[2]) / 2]  # mid point
+    I = [(p1[0] - p0[0]) / dist, (p1[1] - p0[1]) / dist, (p1[2] - p0[2]) / dist]  # unit direction
+    hl = dist / 2  # radius
+    P = aabb.P  # center of the AABB
+    E = aabb.E  # extents of AABB
     T = [P[0] - mid[0], P[1] - mid[1], P[2] - mid[2]]
     # do any of the principal axis form a separting axis?
     if abs(T[0]) > (E[0] + hl * abs(I[0])): return False
@@ -84,46 +88,48 @@ def lineAABB(p0,p1,dist,aabb):
     if abs(T[0] * I[1] - T[1] * I[0]) > r: return False
 
     return True
-    
 
-def StateSpace(env, factor = 0):
+
+def StateSpace(env, factor=0):
     boundary = env.boundary
     resolution = env.resolution
-    xmin,xmax = boundary[0]+factor*resolution,boundary[3]-factor*resolution
-    ymin,ymax = boundary[1]+factor*resolution,boundary[4]-factor*resolution
-    zmin,zmax = boundary[2]+factor*resolution,boundary[5]-factor*resolution
-    xarr = np.arange(xmin,xmax,resolution).astype(float)
-    yarr = np.arange(ymin,ymax,resolution).astype(float)
-    zarr = np.arange(zmin,zmax,resolution).astype(float)
+    xmin, xmax = boundary[0] + factor * resolution, boundary[3] - factor * resolution
+    ymin, ymax = boundary[1] + factor * resolution, boundary[4] - factor * resolution
+    zmin, zmax = boundary[2] + factor * resolution, boundary[5] - factor * resolution
+    xarr = np.arange(xmin, xmax, resolution).astype(float)
+    yarr = np.arange(ymin, ymax, resolution).astype(float)
+    zarr = np.arange(zmin, zmax, resolution).astype(float)
     Space = set()
     for x in xarr:
         for y in yarr:
             for z in zarr:
-                Space.add((x,y,z))
+                Space.add((x, y, z))
     return Space
 
+
 def g_Space(initparams):
     '''This function is used to get nodes and discretize the space.
        State space is by x*y*z,3 where each 3 is a point in 3D.'''
     g = {}
     Space = StateSpace(initparams.env)
     for v in Space:
-        g[v] = np.inf # this hashmap initialize all g values at inf
+        g[v] = np.inf  # this hashmap initialize all g values at inf
     return g
 
+
 def isCollide(initparams, x, child):
     '''see if line intersects obstacle'''
     dist = getDist(x, child)
-    if not isinbound(initparams.env.boundary,child): return True, dist
+    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 lineAABB(x, child, dist, i): return True, dist
     for i in initparams.env.balls:
-        if isinball(i, child):return True, dist
+        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]
@@ -132,43 +138,52 @@ def isCollide(initparams, x, child):
         if lineSphere(x, child, i): return True, dist
     return False, dist
 
+
 def children(initparams, x):
     # get the neighbor of a specific state
     allchild = []
     resolution = initparams.env.resolution
     for direc in initparams.Alldirec:
-        child = tuple(map(np.add,x,np.multiply(direc,resolution)))
-        if isinbound(initparams.env.boundary,child):
+        child = tuple(map(np.add, x, np.multiply(direc, resolution)))
+        if isinbound(initparams.env.boundary, child):
             allchild.append(child)
     return allchild
 
-def obstacleFree(initparams,x):
+
+def obstacleFree(initparams, x):
     for i in initparams.env.blocks:
-        if isinbound(i,x):
+        if isinbound(i, x):
             return False
     for i in initparams.env.balls:
-        if isinball(i,x):
+        if isinball(i, x):
             return False
     return True
 
-def cost(initparams, i,j,settings=0):
-    collide, dist = isCollide(initparams,i,j)
+
+def cost(initparams, i, j, settings=0):
+    collide, dist = isCollide(initparams, i, j)
     if settings == 0:
-        if collide: return np.inf
-        else: return dist
+        if collide:
+            return np.inf
+        else:
+            return dist
     if settings == 1:
-        if collide: return np.inf
-        else: return getManDist(i,j)
+        if collide:
+            return np.inf
+        else:
+            return getManDist(i, j)
+
 
 def initcost(initparams):
     # initialize cost dictionary, could be modifed lateron
-    c = defaultdict(lambda: defaultdict(dict)) # two key dicionary
+    c = defaultdict(lambda: defaultdict(dict))  # two key dicionary
     for xi in initparams.X:
         cdren = children(initparams, xi)
         for child in cdren:
             c[xi][child] = cost(initparams, xi, child)
     return c
-    
+
+
 if __name__ == "__main__":
     a = '()'
-    print(list(a))
+    print(list(a))