diff --git a/Search-based Planning/Search_3D/Dstar3D.py b/Search-based Planning/Search_3D/Dstar3D.py index fda8797..da76f7b 100644 --- a/Search-based Planning/Search_3D/Dstar3D.py +++ b/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() diff --git a/Search-based Planning/Search_3D/DstarLite3D.py b/Search-based Planning/Search_3D/DstarLite3D.py index c2dc364..c4977e2 100644 --- a/Search-based Planning/Search_3D/DstarLite3D.py +++ b/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,12 +37,13 @@ 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 = {} # init cost set self.COST = defaultdict(lambda: defaultdict(dict)) - + # for visualization self.V = set() # vertice in closed self.ind = 0 @@ -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: - # while getDist(s_start, self.xt) > self.env.resolution: - # newcost, allchild = [], [] - # for i in children(self, s_start): - # newcost.append(cost(self, i, s_start) + self.g[s_start]) - # allchild.append(i) - # s_start = allchild[np.argmin(newcost)] - # #TODO: move to s_start - # #TODO: scan graph or costs changes - # # self.km = self.km + heuristic_fun(self, s_start, s_last) - # # for all directed edges (u,v) with changed edge costs - # # update edge cost c(u,v) - # # updatevertex(u) - # self.ComputeShortestPath() + self.Path = self.path() + self.done = True + visualization(self) + plt.pause(0.5) + # plt.show() + # change the environment + print('running with map update ...') + for i in range(100): + range_changed1 = self.env.move_block(a=[0, 0, -0.1], s=0.5, block_to_move=0, mode='translation') + range_changed2 = self.env.move_block(a=[0.1, 0, 0], s=0.5, block_to_move=1, mode='translation') + range_changed3 = self.env.move_block(a=[0, 0.1, 0], s=0.5, block_to_move=2, mode='translation') + #range_changed = self.env.move_block(a=[0.1, 0, 0], s=0.5, block_to_move=1, mode='translation') + # update the edge cost of c(u,v) + 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)) \ No newline at end of file diff --git a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc index cbb9b74..49994b2 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc index d54e50d..126bb45 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/env3D.py b/Search-based Planning/Search_3D/env3D.py index fb503c5..ea16cff 100644 --- a/Search-based Planning/Search_3D/env3D.py +++ b/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