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

Search-based Planning/.idea/workspace.xml

@@ -20,12 +20,8 @@
   </component>
   <component name="ChangeListManager">
     <list default="true" id="025aff36-a6aa-4945-ab7e-b2c625055f47" name="Default Changelist" comment="">
-      <change beforePath="$PROJECT_DIR$/.idea/Search-based Planning.iml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/Search-based Planning.iml" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/.idea/misc.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/misc.xml" afterDir="false" />
-      <change beforePath="$PROJECT_DIR$/.idea/shelf/Uncommitted_changes_before_Update_at_6_27_20__12_26_AM__Default_Changelist_.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/shelf/Uncommitted_changes_before_Update_at_6_27_20__12_26_AM__Default_Changelist_.xml" afterDir="false" />
       <change beforePath="$PROJECT_DIR$/.idea/workspace.xml" beforeDir="false" afterPath="$PROJECT_DIR$/.idea/workspace.xml" afterDir="false" />
     </list>
-    <option name="EXCLUDED_CONVERTED_TO_IGNORED" value="true" />
     <option name="SHOW_DIALOG" value="false" />
     <option name="HIGHLIGHT_CONFLICTS" value="true" />
     <option name="HIGHLIGHT_NON_ACTIVE_CHANGELIST" value="false" />
@@ -53,24 +49,12 @@
     <property name="ASKED_ADD_EXTERNAL_FILES" value="true" />
     <property name="RunOnceActivity.OpenProjectViewOnStart" value="true" />
     <property name="RunOnceActivity.ShowReadmeOnStart" value="true" />
-    <property name="last_opened_file_path" value="$PROJECT_DIR$" />
+    <property name="last_opened_file_path" value="$PROJECT_DIR$/Search_2D/bfs.py" />
     <property name="restartRequiresConfirmation" value="false" />
-    <property name="settings.editor.selected.configurable" value="com.jetbrains.python.configuration.PyActiveSdkModuleConfigurable" />
+    <property name="settings.editor.selected.configurable" value="preferences.editor" />
   </component>
-  <component name="RunDashboard">
-    <option name="ruleStates">
-      <list>
-        <RuleState>
-          <option name="name" value="ConfigurationTypeDashboardGroupingRule" />
-        </RuleState>
-        <RuleState>
-          <option name="name" value="StatusDashboardGroupingRule" />
-        </RuleState>
-      </list>
-    </option>
-  </component>
-  <component name="RunManager" selected="Python.ara_star">
-    <configuration name="a_star" type="PythonConfigurationType" factoryName="Python" temporary="true">
+  <component name="RunManager" selected="Python.dijkstra">
+    <configuration name="a_star" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -78,11 +62,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$" />
+      <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$/a_star.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/a_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -99,11 +83,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$" />
+      <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$/ara_star.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/ara_star.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -120,11 +104,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$" />
+      <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$/bfs.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/bfs.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -141,11 +125,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$" />
+      <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$/dfs.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/dfs.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -154,7 +138,7 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
-    <configuration name="dijkstra" type="PythonConfigurationType" factoryName="Python" temporary="true">
+    <configuration name="dijkstra" type="PythonConfigurationType" factoryName="Python" temporary="true" nameIsGenerated="true">
       <module name="Search-based Planning" />
       <option name="INTERPRETER_OPTIONS" value="" />
       <option name="PARENT_ENVS" value="true" />
@@ -162,11 +146,11 @@
         <env name="PYTHONUNBUFFERED" value="1" />
       </envs>
       <option name="SDK_HOME" value="" />
-      <option name="WORKING_DIRECTORY" value="$PROJECT_DIR$" />
+      <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$/dijkstra.py" />
+      <option name="SCRIPT_NAME" value="$PROJECT_DIR$/Search_2D/dijkstra.py" />
       <option name="PARAMETERS" value="" />
       <option name="SHOW_COMMAND_LINE" value="false" />
       <option name="EMULATE_TERMINAL" value="false" />
@@ -175,13 +159,20 @@
       <option name="INPUT_FILE" value="" />
       <method v="2" />
     </configuration>
+    <list>
+      <item itemvalue="Python.ara_star" />
+      <item itemvalue="Python.bfs" />
+      <item itemvalue="Python.a_star" />
+      <item itemvalue="Python.dfs" />
+      <item itemvalue="Python.dijkstra" />
+    </list>
     <recent_temporary>
       <list>
-        <item itemvalue="Python.ara_star" />
+        <item itemvalue="Python.dijkstra" />
+        <item itemvalue="Python.dfs" />
         <item itemvalue="Python.a_star" />
         <item itemvalue="Python.bfs" />
-        <item itemvalue="Python.dfs" />
-        <item itemvalue="Python.dijkstra" />
+        <item itemvalue="Python.ara_star" />
       </list>
     </recent_temporary>
   </component>
@@ -203,40 +194,43 @@
       <map>
         <entry key="MAIN">
           <value>
-            <State>
-              <option name="COLUMN_ORDER" />
-            </State>
+            <State />
           </value>
         </entry>
       </map>
     </option>
+    <option name="oldMeFiltersMigrated" value="true" />
   </component>
   <component name="WindowStateProjectService">
-    <state x="819" y="314" width="424" height="482" key="FileChooserDialogImpl" timestamp="1592933974409">
-      <screen x="65" y="24" width="1855" height="1056" />
+    <state x="2377" y="197" key="#com.intellij.execution.impl.EditConfigurationsDialog" timestamp="1593282711181">
+      <screen x="1920" y="0" width="1920" height="1080" />
+    </state>
+    <state x="2377" y="197" key="#com.intellij.execution.impl.EditConfigurationsDialog/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282711181" />
+    <state x="2700" y="297" width="424" height="482" key="FileChooserDialogImpl" timestamp="1593282670147">
+      <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state x="2700" y="297" width="424" height="482" key="FileChooserDialogImpl/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592802293738" />
+    <state x="2700" y="297" width="424" height="482" key="FileChooserDialogImpl/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282670147" />
     <state x="819" y="314" key="FileChooserDialogImpl/65.24.1855.1056/1920.0.1920.1080@65.24.1855.1056" timestamp="1592933974409" />
-    <state width="1832" height="206" key="GridCell.Tab.0.bottom" timestamp="1593243345942">
+    <state width="1832" height="206" key="GridCell.Tab.0.bottom" timestamp="1593282755222">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="206" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593243345942" />
-    <state width="1832" height="206" key="GridCell.Tab.0.center" timestamp="1593243345941">
+    <state width="1832" height="206" key="GridCell.Tab.0.bottom/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282755222" />
+    <state width="1832" height="206" key="GridCell.Tab.0.center" timestamp="1593282755222">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="206" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593243345941" />
-    <state width="1832" height="206" key="GridCell.Tab.0.left" timestamp="1593243345941">
+    <state width="1832" height="206" key="GridCell.Tab.0.center/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282755222" />
+    <state width="1832" height="206" key="GridCell.Tab.0.left" timestamp="1593282755222">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="206" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593243345941" />
-    <state width="1832" height="206" key="GridCell.Tab.0.right" timestamp="1593243345941">
+    <state width="1832" height="206" key="GridCell.Tab.0.left/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282755222" />
+    <state width="1832" height="206" key="GridCell.Tab.0.right" timestamp="1593282755222">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state width="1832" height="206" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593243345941" />
-    <state x="2406" y="174" key="SettingsEditor" timestamp="1592801555194">
+    <state width="1832" height="206" key="GridCell.Tab.0.right/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282755222" />
+    <state x="2406" y="174" key="SettingsEditor" timestamp="1593282573348">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>
-    <state x="2406" y="174" key="SettingsEditor/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1592801555194" />
+    <state x="2406" y="174" key="SettingsEditor/65.24.1855.1056/1920.0.1920.1080@1920.0.1920.1080" timestamp="1593282573348" />
     <state x="2701" y="438" key="com.intellij.openapi.vcs.update.UpdateOrStatusOptionsDialogupdate-v2" timestamp="1593242832142">
       <screen x="1920" y="0" width="1920" height="1080" />
     </state>

Search-based Planning/Astar_3D/Astar3D.py

@@ -1,38 +0,0 @@
-# this is the three dimensional A* algo
-# !/usr/bin/env python3
-# -*- coding: utf-8 -*-
-"""
-@author: yue qi
-"""
-import numpy as np
-
-import os
-import sys
-
-sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
-from Astar_3D.env3D import env
-from Astar_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest
-import queue
-
-
-class Weighted_A_star(object):
-    def __init__(self):
-        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()
-        self.Space = StateSpace(self.env.boundary) # key is the point, store g value
-        self.OPEN = queue.QueuePrior() # store [point,priority]
-        self.start = getNearest(self.Space,self.env.start)
-        self.goal = getNearest(self.Space,self.env.goal)
-        self.h = Heuristic(self.Space,self.goal)
-        self.Parent = {}
-        self.CLOSED = {}
-        
-
-    def run(self):
-        pass
-if __name__ == '__main__':
-    Astar = Weighted_A_star()
-    

Search-based Planning/Astar_3D/utils3D.py

@@ -1,66 +0,0 @@
-import numpy as np
-
-def getRay(x, y):
-    direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]]
-    return np.array([x, direc])
-
-def getAABB(blocks):
-    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 getDist(pos1, pos2):
-    return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2]))
-
-def getNearest(Space,pt):
-    '''get the nearest point on the grid'''
-    mindis,minpt = 1000,None
-    for strpts in Space.keys(): 
-        pts = dehash(strpts)
-        dis = getDist(pts,pt)
-        if dis < mindis:
-            mindis,minpt = dis,pts
-    return minpt
-
-def Heuristic(Space,t):
-    '''Max norm distance'''
-    h = {}
-    for k in Space.keys():
-        h[k] = max(abs(t-dehash(k)))
-    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 StateSpace(boundary,factor=0):
-    '''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.'''
-    xmin,xmax = boundary[0]+factor,boundary[3]-factor
-    ymin,ymax = boundary[1]+factor,boundary[4]-factor
-    zmin,zmax = boundary[2]+factor,boundary[5]-factor
-    xarr = np.arange(xmin,xmax,1)
-    yarr = np.arange(ymin,ymax,1)
-    zarr = np.arange(zmin,zmax,1)
-    V = np.meshgrid(xarr,yarr,zarr)
-    VV = np.reshape(V,[3,len(xarr)*len(yarr)*len(zarr)]) # all points in 3D
-    Space = {}
-    for v in VV.T:
-        Space[hash3D(v)] = 0 # this hashmap initialize all g values at 0
-    return Space
-
-if __name__ == "__main__":
-    from env3D import env
-    env = env(resolution=1)
-    Space = StateSpace(env.boundary,0)
-    t = np.array([3.0,4.0,5.0])
-    h = Heuristic(Space,t)
-    print(h[hash3D(t)])

Search-based Planning/Search_3D/Astar3D.py

@@ -0,0 +1,94 @@
+# this is the three dimensional A* algo
+# !/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+@author: yue qi
+"""
+import numpy as np
+
+import os
+import sys
+
+sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-based Planning/")
+from Search_3D.env3D import env
+from Search_3D.utils3D import getAABB, getDist, getRay, StateSpace, Heuristic, getNearest, isCollide, hash3D, dehash, cost
+from Search_3D.plot_util3D import visualization
+import queue
+
+
+class Weighted_A_star(object):
+    def __init__(self,resolution=0.2):
+        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.Space = StateSpace(self) # key is the point, store g value
+        self.start = getNearest(self.Space,self.env.start)
+        self.goal = getNearest(self.Space,self.env.goal)
+        self.Space[hash3D(getNearest(self.Space,self.start))] = 0 # set g(x0) = 0
+        self.OPEN = queue.QueuePrior() # store [point,priority]
+        self.h = Heuristic(self.Space,self.goal)
+        self.Parent = {}
+        self.CLOSED = {}
+        self.V = []
+        self.done = False
+        self.Path = []
+
+    def children(self,x):
+        allchild = []
+        for j in self.Alldirec:
+            collide,child = isCollide(self,x,j)
+            if not collide:
+                allchild.append(child)
+        return allchild
+
+    def run(self):
+        x0 = hash3D(self.start)
+        xt = hash3D(self.goal)
+        self.OPEN.put(x0,self.Space[x0] + self.h[x0]) # item, priority = g + h
+        self.ind = 0
+        while xt not in self.CLOSED and self.OPEN: # while xt not reached and open is not empty
+            strxi = self.OPEN.get()           
+            xi = dehash(strxi)
+            self.V.append(xi)
+            visualization(self)
+            self.CLOSED[strxi] = [] # add the point in CLOSED set
+            allchild = self.children(xi)
+            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 strxj in self.OPEN.enumerate():
+                            #TODO: update priority of xj
+                            # self.OPEN.put(strxj, a+1*self.h[strxj])
+                            pass
+                        else:
+                            #TODO: add xj in to OPEN set
+                            self.OPEN.put(strxj, a+1*self.h[strxj])
+            if self.ind % 100 == 0: print('iteration number = '+ str(self.ind))
+            self.ind += 1
+        self.done = True
+        #self.Path = self.path()
+        #visualization(self)
+
+    def path(self):
+        path = [self.goal]
+        strx = hash3D(self.goal)
+        strstart = hash3D(self.start)
+        while strx != strstart:
+            path.append(self.Parent[strx])
+            strx = hash3D(self.Parent[strx])
+        path = np.array(np.flip(path,axis=0))
+        return path
+
+
+if __name__ == '__main__':
+    Astar = Weighted_A_star(1)
+    Astar.run()
+    PATH = Astar.path()
+    print(PATH)

Search-based Planning/Astar_3D/env3D.py → Search-based Planning/Search_3D/env3D.py

@@ -7,7 +7,7 @@
 import numpy as np
 
 
-def getblocks(resolution):
+def getblocks():
     # AABBs
     block = [[3.10e+00, 0.00e+00, 2.10e+00, 3.90e+00, 5.00e+00, 6.00e+00],
              [9.10e+00, 0.00e+00, 2.10e+00, 9.90e+00, 5.00e+00, 6.00e+00],
@@ -19,22 +19,22 @@ def getblocks(resolution):
     Obstacles = []
     for i in block:
         i = np.array(i)
-        Obstacles.append([j/resolution for j in i])
+        Obstacles.append([j for j in i])
     return np.array(Obstacles)
 
-def getballs(resolution):
+def getballs():
     spheres = [[16,2.5,3,2],[10,2.5,1,1]]
     Obstacles = []
     for i in spheres:
-        Obstacles.append([j/resolution for j in i])
+        Obstacles.append([j for j in i])
     return np.array(Obstacles)
 
 class env():
     def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1):
         self.resolution = resolution
-        self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) / resolution
-        self.blocks = getblocks(resolution)
-        self.balls = getballs(resolution)
+        self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) 
+        self.blocks = getblocks()
+        self.balls = getballs()
         self.start = np.array([0.5, 2.5, 5.5])
         self.goal = np.array([19.0, 2.5, 5.5])
 

Search-based Planning/Astar_3D/plot_util3D.py → Search-based Planning/Search_3D/plot_util3D.py

@@ -54,11 +54,11 @@ def draw_line(ax,SET,visibility=1,color=None):
 def visualization(initparams):
     if initparams.ind % 10 == 0 or initparams.done:
         V = np.array(initparams.V)
-        E = initparams.E
+        # E = initparams.E
         Path = np.array(initparams.Path)
         start = initparams.env.start
         goal = initparams.env.goal
-        edges = E.get_edge()
+        # edges = E.get_edge()
         # generate axis objects
         ax = plt.subplot(111, projection='3d')
         ax.view_init(elev=0., azim=90)
@@ -67,7 +67,7 @@ def visualization(initparams):
         draw_Spheres(ax, initparams.env.balls)
         draw_block_list(ax, initparams.env.blocks)
         draw_block_list(ax, np.array([initparams.env.boundary]),alpha=0)
-        draw_line(ax,edges,visibility=0.25)
+        # draw_line(ax,edges,visibility=0.25)
         draw_line(ax,Path,color='r')
         ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',)
         ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k')

Search-based Planning/Search_3D/utils3D.py

@@ -0,0 +1,90 @@
+import numpy as np
+import pyrr
+
+def getRay(x, y):
+    direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]]
+    return np.array([x, direc])
+
+def getAABB(blocks):
+    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 getDist(pos1, pos2):
+    return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2]))
+
+def getNearest(Space,pt):
+    '''get the nearest point on the grid'''
+    mindis,minpt = 1000,None
+    for strpts in Space.keys(): 
+        pts = dehash(strpts)
+        dis = getDist(pts,pt)
+        if dis < mindis:
+            mindis,minpt = dis,pts
+    return minpt
+
+def Heuristic(Space,t):
+    '''Max norm distance'''
+    h = {}
+    for k in Space.keys():
+        h[k] = max(abs(t-dehash(k)))
+    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 StateSpace(initparams,factor=0):
+    '''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.'''
+    boundary = initparams.env.boundary
+    resolution = initparams.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)
+    V = np.meshgrid(xarr,yarr,zarr)
+    VV = np.reshape(V,[3,len(xarr)*len(yarr)*len(zarr)]) # all points in 3D
+    Space = {}
+    for v in VV.T:
+        Space[hash3D(v)] = np.inf # this hashmap initialize all g values at inf
+    return Space
+
+def isCollide(initparams, x, direc):
+    '''see if line intersects obstacle'''
+    resolution = initparams.env.resolution
+    child = np.array(list(map(np.add,x,np.multiply(direc,resolution))))
+    ray = getRay(x, direc)
+    dist = getDist(x, child)
+    if not isinbound(initparams.env.boundary,child):
+        return True, child
+    for i in getAABB(initparams.env.blocks):
+        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, child
+    for i in initparams.env.balls:
+        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, child
+    return False, child
+
+def cost(i,j):
+    return getDist(i,j)
+    
+
+if __name__ == "__main__":
+    from env3D import env