'AnyDstar'

Search-based Planning/Search_3D/Anytime_Dstar3D.py

@@ -0,0 +1,114 @@
+# check paper of 
+# [Likhachev2005]
+import numpy as np
+import matplotlib.pyplot as plt
+
+import os
+import sys
+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.utils3D import getDist, heuristic_fun, getNearest, isinbound, \
+     cost, children, StateSpace
+from Search_3D.plot_util3D import visualization
+from Search_3D import queue
+import time
+
+class Anytime_Dstar(object):
+
+    def __init__(self, resolution=1):
+        self.Alldirec = {(1, 0, 0): 1, (0, 1, 0): 1, (0, 0, 1): 1, \
+                        (-1, 0, 0): 1, (0, -1, 0): 1, (0, 0, -1): 1, \
+                        (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \
+                        (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \
+                        (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \
+                        (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \
+                        (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \
+                        (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \
+                        (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}
+        self.env = env(resolution=resolution)
+        self.settings = 'CollisionChecking' # for collision checking
+        self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
+        self.OPEN = queue.MinheapPQ()
+        self.km = 0
+        self.g = {} # all g initialized at inf
+        self.rhs = {self.xt:0} # rhs(x0) = 0
+        self.h = {}
+        self.OPEN.put(self.xt, self.key(self.xt))
+        self.INCONS = set()
+        self.CLOSED = set()
+        
+        # init children set:
+        self.CHILDREN = {}
+        # init cost set
+        self.COST = defaultdict(lambda: defaultdict(dict))
+        
+        # for visualization
+        self.V = set()  # vertice in closed
+        self.ind = 0
+        self.Path = []
+        self.done = False
+
+    def getcost(self, xi, xj):
+        # use a LUT for getting the costd
+        if xi not in self.COST:
+            for (xj,xjcost) in children(self, xi, settings=1):
+                self.COST[xi][xj] = cost(self, xi, xj, xjcost)
+        # this might happen when there is a node changed. 
+        if xj not in self.COST[xi]:
+            self.COST[xi][xj] = cost(self, xi, xj)
+        return self.COST[xi][xj]
+
+    def getchildren(self, xi):
+        if xi not in self.CHILDREN:
+            allchild = children(self, xi)
+            self.CHILDREN[xi] = set(allchild)
+        return self.CHILDREN[xi]
+
+    def geth(self, xi):
+        # when the heurisitic is first calculated
+        if xi not in self.h:
+            self.h[xi] = heuristic_fun(self, xi, self.x0)
+        return self.h[xi]
+
+    def getg(self, xi):
+        if xi not in self.g:
+            self.g[xi] = np.inf
+        return self.g[xi]
+
+    def getrhs(self, xi):
+        if xi not in self.rhs:
+            self.rhs[xi] = np.inf
+        return self.rhs[xi]
+
+#--------------main functions for Anytime D star
+
+    def key(self, s, epsilon=1):
+        if self.getg(s) > self.getrhs(s):
+            return [self.rhs[s] + epsilon * heuristic_fun(self, s, self.x0), self.rhs[s]]
+        else:
+            return [self.getg(s) + heuristic_fun(self, s, self.x0), self.getg(s)]
+
+    def UpdateState(self, s):
+        if s not in self.CLOSED:
+        # TODO if s is not visited before
+            self.g[s] = np.inf
+        if getDist(s, self.xt) <= self.env.resolution:
+            self.rhs[s] = min([self.getcost(s, s_p) + self.getg(s_p) for s_p in self.getchildren(s)]) 
+        self.OPEN.check_remove(s)
+        if self.getg(s) != self.getrhs(s):
+            if s not in self.CLOSED:
+                self.OPEN.put(s, self.key(s))
+            else:
+                self.INCONS.add(s)
+
+    def ComputeorImprovePath(self):
+        pass
+
+    def Main(self):
+        pass
+
+if __name__ == '__main__':
+    AD = Anytime_Dstar(resolution = 1)
+    AD.Main()

Search-based Planning/Search_3D/Astar3D.py

@@ -29,7 +29,7 @@ class Weighted_A_star(object):
                         (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \
                         (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \
                         (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}
-
+        self.settings = 'NonCollisionChecking'                
         self.env = env(resolution=resolution)
         self.start, self.goal = tuple(self.env.start), tuple(self.env.goal)
         self.g = {self.start:0,self.goal:np.inf}

Search-based Planning/Search_3D/Dstar3D.py

@@ -165,7 +165,7 @@ class D_star(object):
         # when the environemnt changes over time
 
         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.1, 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)

Search-based Planning/Search_3D/DstarLite3D.py

@@ -7,12 +7,10 @@ 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 getDist, getRay, g_Space, Heuristic, heuristic_fun, getNearest, isinbound, isinball, \
-    isCollide, cost, obstacleFree, children, StateSpace
+from Search_3D.utils3D import getDist, heuristic_fun, getNearest, isinbound, \
+     cost, children, StateSpace
 from Search_3D.plot_util3D import visualization
 from Search_3D import queue
-import pyrr
 import time
 
 class D_star_Lite(object):
@@ -23,13 +21,14 @@ class D_star_Lite(object):
                         (1, 1, 0): np.sqrt(2), (1, 0, 1): np.sqrt(2), (0, 1, 1): np.sqrt(2), \
                         (-1, -1, 0): np.sqrt(2), (-1, 0, -1): np.sqrt(2), (0, -1, -1): np.sqrt(2), \
                         (1, -1, 0): np.sqrt(2), (-1, 1, 0): np.sqrt(2), (1, 0, -1): np.sqrt(2), \
-                        (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2)}
-                        # (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \
-                        # (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \
-                        # (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}
+                        (-1, 0, 1): np.sqrt(2), (0, 1, -1): np.sqrt(2), (0, -1, 1): np.sqrt(2), \
+                        (1, 1, 1): np.sqrt(3), (-1, -1, -1) : np.sqrt(3), \
+                        (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \
+                        (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)}
         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.settings = 'CollisionChecking' # for collision checking
         self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal)
         # self.OPEN = queue.QueuePrior()
         self.OPEN = queue.MinheapPQ()
@@ -51,16 +50,6 @@ class D_star_Lite(object):
         self.Path = []
         self.done = False
 
-    def getcost(self, xi, xj):
-        # use a LUT for getting the costd
-        if xi not in self.COST:
-            for (xj,xjcost) in children(self, xi, settings=1):
-                self.COST[xi][xj] = cost(self, xi, xj, xjcost)
-        # this might happen when there is a node changed. 
-        if xj not in self.COST[xi]:
-            self.COST[xi][xj] = cost(self, xi, xj)
-        return self.COST[xi][xj]
-
     def updatecost(self,range_changed=None, new=None, old=None, mode=False):
         # scan graph for changed cost, if cost is changed update it
         CHANGED = set()
@@ -86,6 +75,16 @@ class D_star_Lite(object):
                         self.COST[xi][xj] = cost(self, xi, xj)
         return CHANGED
 
+    def getcost(self, xi, xj):
+        # use a LUT for getting the costd
+        if xi not in self.COST:
+            for (xj,xjcost) in children(self, xi, settings=1):
+                self.COST[xi][xj] = cost(self, xi, xj, xjcost)
+        # this might happen when there is a node changed. 
+        if xj not in self.COST[xi]:
+            self.COST[xi][xj] = cost(self, xi, xj)
+        return self.COST[xi][xj]
+
     def getchildren(self, xi):
         if xi not in self.CHILDREN:
             allchild = children(self, xi)

Search-based Planning/Search_3D/utils3D.py

@@ -39,15 +39,15 @@ def heuristic_fun(initparams, k, t=None):
         t = initparams.goal
     return max([abs(t[0] - k[0]), abs(t[1] - k[1]), abs(t[2] - k[2])])
 
-def isinbound(i, x, mode=False):
+def isinbound(i, x, mode=False, factor = 0):
     if mode == 'obb':
         return isinobb(i, x)
-    if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]:
+    if i[0] - factor <= x[0] < i[3] + factor and i[1] - factor <= x[1] < i[4] + factor and i[2] - factor <= x[2] < i[5] + factor:
         return True
     return False
 
-def isinball(i, x):
-    if getDist(i[0:3], x) <= i[3]:
+def isinball(i, x, factor = 0):
+    if getDist(i[0:3], x) <= i[3] + factor:
         return True
     return False
 
@@ -311,7 +311,7 @@ def obstacleFree(initparams, x):
 
 
 def cost(initparams, i, j, dist=None, settings='Euclidean'):
-    if initparams.env.resolution < 0.25:
+    if initparams.settings == 'NonCollisionChecking':
         if dist==None:
             dist = getDist(i,j)
         collide = False