Merge branch 'master' of https://github.com/zhm-real/PathPlanning

Search-based Planning/Search_3D/Dstar3D.py

@@ -187,5 +187,5 @@ class D_star(object):
 
 
 if __name__ == '__main__':
-    D = D_star(0.75)
+    D = D_star(0.5)
     D.run()

Search-based Planning/Search_3D/DstarLite3D.py

@@ -9,7 +9,7 @@ sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Search-base
 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, \
-    cost, obstacleFree, children, StateSpace
+    isCollide, cost, obstacleFree, children, StateSpace
 from Search_3D.plot_util3D import visualization
 import queue
 import pyrr
@@ -37,6 +37,7 @@ class D_star_Lite(object):
         self.rhs = {self.xt:0} # rhs(x0) = 0
         self.h = {}
         self.OPEN.put(self.xt, self.CalculateKey(self.xt))
+        self.CLOSED = set()
         
         # init children set:
         self.CHILDREN = {}
@@ -59,9 +60,38 @@ class D_star_Lite(object):
             self.COST[xi][xj] = cost(self, xi, xj)
         return self.COST[xi][xj]
 
-    def updatecost(self):
+    def updatecost(self,range_changed=None):
         # TODO: update cost when the environment is changed
-        pass
+        # chaged nodes
+        CHANGED = set()
+        for xi in self.CLOSED:
+            oldchildren = self.CHILDREN[xi]# A
+            # if you don't know where the change occured:
+            if range_changed is None:
+                newchildren = set(children(self,xi))# B
+                added = newchildren.difference(oldchildren)# B-A
+                removed = oldchildren.difference(newchildren)# A-B
+                self.CHILDREN[xi] = newchildren
+                if added or removed:
+                    CHANGED.add(xi)
+                for xj in removed:
+                    self.COST[xi][xj] = cost(self, xi, xj)  
+                for xj in added:
+                    self.COST[xi][xj] = cost(self, xi, xj)    
+            # if you do know where on the map changed, only update those changed around that area
+            else: 
+                if isinbound(range_changed, xi):
+                    newchildren = set(children(self,xi))# B
+                    added = newchildren.difference(oldchildren)# B-A
+                    removed = oldchildren.difference(newchildren)# A-B
+                    self.CHILDREN[xi] = newchildren
+                    if added or removed:
+                        CHANGED.add(xi)
+                    for xj in removed:
+                        self.COST[xi][xj] = cost(self, xi, xj)  
+                    for xj in added:
+                        self.COST[xi][xj] = cost(self, xi, xj)         
+        return CHANGED
 
     def getchildren(self, xi):
         if xi not in self.CHILDREN:
@@ -108,6 +138,7 @@ class D_star_Lite(object):
             kold = self.OPEN.top_key()
             u = self.OPEN.get()
             self.V.add(u)
+            self.CLOSED.add(u)
             if getDist(self.x0, u) <= self.env.resolution:
                 break
             # visualization(self)
@@ -120,32 +151,79 @@ class D_star_Lite(object):
                 self.UpdateVertex(u)
             for s in self.getchildren(u):
                 self.UpdateVertex(s)
-
             self.ind += 1
 
     def main(self):
         s_last = self.x0
         s_start = self.x0
+        print('first run ...')
         self.ComputeShortestPath()
-        # while s_start != self.xt:
+        self.Path = self.path()
-        # while getDist(s_start, self.xt) > self.env.resolution:
+        self.done = True
-        #     newcost, allchild = [], []
+        visualization(self)
-        #     for i in children(self, s_start):
+        plt.pause(0.5)
-        #         newcost.append(cost(self, i, s_start) + self.g[s_start])
+        # plt.show()
-        #         allchild.append(i)
+        # change the environment 
-        #     s_start = allchild[np.argmin(newcost)]
+        print('running with map update ...')
-        #     #TODO: move to s_start
+        for i in range(100):
-        #     #TODO: scan graph or costs changes 
+            range_changed1 = self.env.move_block(a=[0, 0, -0.1], s=0.5, block_to_move=0, mode='translation')
-        #     # self.km = self.km + heuristic_fun(self, s_start, s_last)
+            range_changed2 = self.env.move_block(a=[0.1, 0, 0], s=0.5, block_to_move=1, mode='translation')
-        #     # for all directed edges (u,v) with changed edge costs
+            range_changed3 = self.env.move_block(a=[0, 0.1, 0], s=0.5, block_to_move=2, mode='translation')
-        #     #   update edge cost c(u,v)
+            #range_changed = self.env.move_block(a=[0.1, 0, 0], s=0.5, block_to_move=1, mode='translation')
-        #     #   updatevertex(u)
+                #   update the edge cost of c(u,v) 
-        #     self.ComputeShortestPath()
+            CHANGED1 = self.updatecost(range_changed1)
+            CHANGED2 = self.updatecost(range_changed2)
+            CHANGED3 = self.updatecost(range_changed3)
+            CHANGED2 = CHANGED2.union(CHANGED1)
+            CHANGED = CHANGED3.union(CHANGED2)
+            while getDist(s_start, self.xt) > 2*self.env.resolution:
+                if s_start == self.x0:
+                    children = [i for i in self.CLOSED if getDist(s_start, i) <= self.env.resolution*np.sqrt(3)]
+                else:
+                    children = list(self.CHILDREN[s_start])
+                s_start = children[np.argmin([cost(self,s_start,s_p) + self.g[s_p] for s_p in children])]
+                
+                #   for all directed edges (u,v) with changed costs
+                if CHANGED:
+                    self.km = self.km + heuristic_fun(self, s_start, s_last)
+                    for u in CHANGED:
+                        self.UpdateVertex(u)
+                    s_last = s_start
+                    self.ComputeShortestPath()
+            self.Path = self.path()
+            visualization(self)
+        plt.show()
+
+    def path(self):
+        '''After ComputeShortestPath()
+        returns, one can then follow a shortest path from s_start to
+        s_goal by always moving from the current vertex s, starting
+        at s_start. , to any successor s' that minimizes c(s,s') + g(s') 
+        until s_goal is reached (ties can be broken arbitrarily).'''
+        path = []
+        s_goal = self.xt
+        s = self.x0
+        ind = 0
+        while s != s_goal:
+            if s == self.x0:
+                children = [i for i in self.CLOSED if getDist(s, i) <= self.env.resolution*np.sqrt(3)]
+            else: 
+                children = list(self.CHILDREN[s])
+            snext = children[np.argmin([cost(self,s,s_p) + self.g[s_p] for s_p in children])]
+            path.append([s, snext])
+            s = snext
+            if ind > 100:
+                break
+            ind += 1
+        return path
 
 if __name__ == '__main__':
-    a = time.time()
+    
     D_lite = D_star_Lite(1)
-    # D_lite.UpdateVertex(D_lite.x0)
+    #D_lite.ComputeShortestPath()
+    a = time.time()
+    #range_changed = D_lite.env.move_block(a=[0, 0, 1], s=0.5, block_to_move=1, mode='translation')
+    #CHANGED = D_lite.updatecost(range_changed)
     D_lite.main()
     print('used time (s) is ' + str(time.time() - a))
             

Search-based Planning/Search_3D/env3D.py

@@ -90,6 +90,8 @@ class env():
             [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]]
+            # return a range of block that the block might moved
+            return self.blocks[block_to_move] + self.t * 2* max([abs(i) for i in v]) 
         # (x',t') = (x + a, t + s) is a translation
         if mode == 'translation':
             ori = self.blocks[block_to_move]
@@ -106,6 +108,8 @@ class env():
             self.AABB[block_to_move].P[1] + a[1], \
             self.AABB[block_to_move].P[2] + a[2]]
             self.t += s
+            # return a range of block that the block might moved
+            return self.blocks[block_to_move] + 2* max([abs(i) for i in a]) 
         # (x',t') = (Gx, t)
         if mode == 'rotation': # this makes AABB become a OBB
             #TODO: implement this with rotation matrix