diff --git a/Search-based Planning/Search_3D/Dstar3D.py b/Search-based Planning/Search_3D/Dstar3D.py index 3c4f870..9390fe4 100644 --- a/Search-based Planning/Search_3D/Dstar3D.py +++ b/Search-based Planning/Search_3D/Dstar3D.py @@ -135,11 +135,13 @@ class D_star(object): xparent = self.b[x] if self.tag[x] == 'Closed': self.insert(x, self.h[xparent] + cost(self, x, xparent)) - + # self.insert(x, self.h[xparent]) def modify(self, x): self.modify_cost(x) + self.V = set() while True: kmin = self.process_state() + # visualization(self) if kmin >= self.h[x]: break @@ -162,7 +164,7 @@ class D_star(object): while True: # TODO: self.x0 = self.process_state() - # visualization(self) + visualization(self) if self.tag[self.x0] == "Closed": break self.ind += 1 @@ -173,29 +175,29 @@ class D_star(object): # plt.show() # when the environemnt changes over time - # for i in range(2): - # self.env.move_block(a=[0, 0, -1], s=0.5, block_to_move=1, mode='translation') - # visualization(self) - # s = tuple(self.env.start) - # - # while s != self.xt: - # if s == tuple(self.env.start): - # sparent = self.b[self.x0] - # else: - # sparent = self.b[s] - # # self.update_obs() - # - # if cost(self, s, sparent) == np.inf: - # # print(s, " ", sparent) - # self.modify(s) - # continue - # self.ind += 1 - # s = sparent - # self.Path = self.path() - # visualization(self) + for i in range(2): + self.env.move_block(a=[0, 0, -1], s=0.5, block_to_move=1, mode='translation') + visualization(self) + s = tuple(self.env.start) + + while s != self.xt: + if s == tuple(self.env.start): + sparent = self.b[self.x0] + else: + sparent = self.b[s] + # self.update_obs() + + if cost(self, s, sparent) == np.inf: + # print(s, " ", sparent) + self.modify(s) + continue + self.ind += 1 + s = sparent + self.Path = self.path() + visualization(self) plt.show() if __name__ == '__main__': - D = D_star(0.5) + D = D_star(0.75) D.run() diff --git a/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc index 58807f3..a140348 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/Astar3D.cpython-37.pyc differ 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 0c432ca..6d8859c 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 6ae7677..d54e50d 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/__pycache__/queue.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc index 7c5dea4..6871e7f 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/queue.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc index d902d0c..69a2b14 100644 Binary files a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc and b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc differ diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search-based Planning/Search_3D/utils3D.py index 3b74101..c6f7c44 100644 --- a/Search-based Planning/Search_3D/utils3D.py +++ b/Search-based Planning/Search_3D/utils3D.py @@ -120,22 +120,18 @@ def g_Space(initparams): def isCollide(initparams, x, child): '''see if line intersects obstacle''' dist = getDist(x, child) - if not isinbound(initparams.env.boundary, child): return True, dist - for i in initparams.env.AABB: - # 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, dist - if lineAABB(x, child, dist, i): return True, dist + if not isinbound(initparams.env.boundary, child): + return True, dist + for i in range(len(initparams.env.AABB)): + # if isinbound(initparams.env.blocks[i], child): + # return True, dist + if lineAABB(x, child, dist, initparams.env.AABB[i]): + return True, dist for i in initparams.env.balls: - if isinball(i, child): return True, dist - # 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, dist - if lineSphere(x, child, i): return True, dist + # if isinball(i, child): + # return True, dist + if lineSphere(x, child, i): + return True, dist return False, dist @@ -145,6 +141,10 @@ def children(initparams, x): resolution = initparams.env.resolution for direc in initparams.Alldirec: child = tuple(map(np.add, x, np.multiply(direc, resolution))) + if any([isinball(i ,child) for i in initparams.env.balls]): + continue + if any([isinbound(i ,child) for i in initparams.env.blocks]): + continue if isinbound(initparams.env.boundary, child): allchild.append(child) return allchild