'D*'

Search-based Planning/Search_3D/Astar3D.py

@@ -61,7 +61,7 @@ class Weighted_A_star(object):
             if xi not in self.CLOSED:
                 self.V.append(np.array(xi))
             self.CLOSED.add(xi)  # add the point in CLOSED set
-            # visualization(self)
+            visualization(self)
             allchild = children(self,xi)
             for xj in allchild:
                 if xj not in self.CLOSED:
@@ -88,9 +88,9 @@ class Weighted_A_star(object):
         if xt in self.CLOSED:
             self.done = True
             self.Path = self.path()
-            # if N is None:
-            #     visualization(self)
-            #     plt.show()
+            if N is None:
+                visualization(self)
+                plt.show()
             return True
 
         return False

Search-based Planning/Search_3D/Dstar3D.py

@@ -3,12 +3,13 @@ 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 import Astar3D
 from Search_3D.utils3D import StateSpace, getDist, getNearest, getRay, isinbound, isinball, isCollide, children, cost, initcost
-import pyrr
+from Search_3D.plot_util3D import visualization
 
 
 class D_star(object):
@@ -21,16 +22,19 @@ class D_star(object):
         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 = {} # back pointers every state has one except xt.
+        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)
+        # self.c = initcost(self)
+        # for visualization
+        self.ind = 0
+        self.Path = []
+        self.done = False
 
-        # put G (ending state) into the OPEN list
-        self.OPEN[self.xt] = 0
+        
 
     def initH(self):
         # h set, all initialzed h vals are 0 for all states.
@@ -53,7 +57,7 @@ class D_star(object):
         # -1 if it does not exist
         if self.OPEN:
             minv = np.inf
-            for k,v in enumerate(self.OPEN):
+            for v,k in enumerate(self.OPEN):
                 if v < minv: minv = v
             return minv
         return -1
@@ -64,7 +68,7 @@ class D_star(object):
         # it also removes this min value form the OPEN set.
         if self.OPEN:
             minv = np.inf
-            for k,v 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
@@ -84,15 +88,16 @@ class D_star(object):
     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] + self.c[y][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] + self.c[x][y]
+                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):
@@ -100,7 +105,7 @@ class D_star(object):
                     self.insert(y, bb)
         else: 
             for y in children(self,x):
-                bb = self.h[x] + self.c[x][y]
+                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
@@ -115,14 +120,51 @@ class D_star(object):
         return self.get_kmin()
 
     def modify_cost(self,x,y,cval):
-        self.c[x][y] = cval # set the new cost to the cval 
-        if self.tag[x] == 'Closed': self.insert(x,self.h[x])
-        return self.get_kmin()
+        # TODO: implement own function
+        # self.c[x][y] = cval
+        # if self.tag[x] == 'Closed': self.insert(x,self.h[x])
+        # return self.get_kmin()
+        pass
+
+    def path(self):
+        path = []
+        x = self.x0
+        start = self.xt
+        while x != start:
+            path.append([np.array(x), np.array(self.b[x])])
+            x = self.b[x]
+        return path
 
     def run(self):
-        # TODO: implementation of changing obstable in process
-        pass
+        # put G (ending state) into the OPEN list
+        self.OPEN[self.xt] = 0
+        # first run
+        while True:
+            #TODO: self.x0 = 
+            self.process_state()
+            visualization(self)
+            if self.tag[self.x0] == "Closed":
+                break
+            self.ind += 1
+        self.Path = self.path()
+        self.done = True
+        visualization(self)
+        # plt.show()
+        # when the environemnt changes over time
+        s = tuple(self.env.start)
+        while s != self.xt:
+            if s == tuple(self.env.start):
+                s = self.b[self.x0]
+            else: 
+                s = self.b[s]
+            self.process_state()
+            self.env.move_block(a=[0,0,0.1],s=0.5,mode='translation')
+            self.Path = self.path()
+            visualization(self)
+            self.ind += 1
+        
 
         
 if __name__ == '__main__':
-    D = D_star(1)
+    D = D_star(1)
+    D.run()

Search-based Planning/Search_3D/LP_Astar3D.py

@@ -167,7 +167,7 @@ class Lifelong_Astar(object):
         plt.pause(2)
 
     def change_env(self):
-        self.env.change()
+        self.env.New_block()
         self.done = False
         self.Path = []
         self.CLOSED = set()

Search-based Planning/Search_3D/env3D.py

@@ -22,12 +22,12 @@ def getblocks():
         Obstacles.append([j for j in i])
     return np.array(Obstacles)
 
-def getAABB(blocks):
-    # used for Pyrr package for detecting collision
-    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 getAABB(blocks):
+#     # used for Pyrr package for detecting collision
+#     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
 
 class aabb(object):
     def __init__(self,AABB):
@@ -60,11 +60,56 @@ class env():
         self.balls = getballs()
         self.start = np.array([0.5, 2.5, 5.5])
         self.goal = np.array([19.0, 2.5, 5.5])
+        self.t = 0 # time 
 
-    def change(self):
+    def New_block(self):
         newblock = add_block()
         self.blocks = np.vstack([self.blocks,newblock])
-        self.AABB = getAABB(self.blocks)
+        self.AABB = getAABB2(self.blocks)
+
+    def move_start(self, x):
+        self.start = x
+
+    def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], G = None, block_to_move = 0, mode = 'uniform'):
+        # t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time, 
+        # G is an orthorgonal transform in R3*3, in the Galilean transformation
+        # (x',t') = (x + tv, t) is uniform transformation
+        if mode == 'uniform':
+            ori = self.blocks[block_to_move]
+            self.blocks[block_to_move] = \
+                np.array([ori[0] + self.t * v[0],\
+                    ori[1] + self.t * v[1],\
+                    ori[2] + self.t * v[2],\
+                    ori[3] + self.t * v[0],\
+                    ori[4] + self.t * v[1],\
+                    ori[5] + self.t * v[2]])
+
+            self.AABB[block_to_move].P = \
+            [self.AABB[block_to_move].P[0] + self.t * v[0], \
+            self.AABB[block_to_move].P[1] + self.t * v[1], \
+            self.AABB[block_to_move].P[2] + self.t * v[2]]
+        # (x',t') = (x + a, t + s) is a translation
+        if mode == 'translation':
+            ori = self.blocks[block_to_move]
+            self.blocks[block_to_move] = \
+                np.array([ori[0] + a[0],\
+                    ori[1] + a[1],\
+                    ori[2] + a[2],\
+                    ori[3] + a[0],\
+                    ori[4] + a[1],\
+                    ori[5] + a[2]])
+
+            self.AABB[block_to_move].P = \
+            [self.AABB[block_to_move].P[0] + a[0], \
+            self.AABB[block_to_move].P[1] + a[1], \
+            self.AABB[block_to_move].P[2] + a[2]]
+            self.t += s
+        # (x',t') = (Gx, t)
+        if mode == 'rotation': # this makes AABB become a OBB
+            #TODO: implement this with rotation matrix
+            pass
+          
+
 
 if __name__ == '__main__':
     newenv = env()

Search-based Planning/Search_3D/plot_util3D.py

@@ -53,7 +53,7 @@ def draw_line(ax,SET,visibility=1,color=None):
 
 def visualization(initparams):
     if initparams.ind % 20 == 0 or initparams.done:
-        V = np.array(initparams.V)
+        V = np.array(list(initparams.V))
         # E = initparams.E
         Path = np.array(initparams.Path)
         start = initparams.env.start