From f3e43eed4f96c1ec336f081337cceaa59dbc66f3 Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Fri, 24 Jul 2020 15:43:42 -0700 Subject: [PATCH] anytimeD --- .../Search_3D/Anytime_Dstar3D.py | 33 ++++++++++++++---- .../__pycache__/env3D.cpython-37.pyc | Bin 5167 -> 5163 bytes .../__pycache__/plot_util3D.cpython-37.pyc | Bin 5722 -> 5718 bytes .../__pycache__/queue.cpython-37.pyc | Bin 4585 -> 4619 bytes .../__pycache__/utils3D.cpython-37.pyc | Bin 10429 -> 10425 bytes 5 files changed, 26 insertions(+), 7 deletions(-) diff --git a/Search-based Planning/Search_3D/Anytime_Dstar3D.py b/Search-based Planning/Search_3D/Anytime_Dstar3D.py index 30a50c0..80219a8 100644 --- a/Search-based Planning/Search_3D/Anytime_Dstar3D.py +++ b/Search-based Planning/Search_3D/Anytime_Dstar3D.py @@ -51,6 +51,7 @@ class Anytime_Dstar(object): # epsilon in the key caculation self.epsilon = 1 + self.increment = 0.1 def getcost(self, xi, xj): # use a LUT for getting the costd @@ -120,7 +121,7 @@ class Anytime_Dstar(object): 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: + if s != self.xt: 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): @@ -130,11 +131,13 @@ class Anytime_Dstar(object): self.INCONS.add(s) def ComputeorImprovePath(self): - while self.key(self.OPEN.top_key()) < self.key(self.x0) or self.rhs[self.x0] != self.g[self.x0]: + while self.OPEN.top_key() < self.key(self.x0) or self.rhs[self.x0] != self.g[self.x0]: s = self.OPEN.get() + visualization(self) if self.g[s] > self.rhs[s]: self.g[s] = self.rhs[s] self.CLOSED.add(s) + self.V.add(s) for s_p in self.getchildren(s): self.UpdateState(s_p) else: @@ -142,17 +145,22 @@ class Anytime_Dstar(object): self.UpdateState(s) for s_p in self.getchildren(s): self.UpdateState(s_p) + self.ind += 1 def Main(self): epsilon = self.epsilon + increment = self.increment ischanged = False + islargelychanged = False self.ComputeorImprovePath() #TODO publish current epsilon sub-optimal solution while True: # change environment new2,old2 = self.env.move_block(theta = [0,0,0.1*t], mode='rotation') ischanged = True + # islargelychanged = True self.Path = [] + # update cost with changed environment if ischanged: CHANGED = self.updatecost(True, new2, old2, mode='obb') @@ -161,13 +169,24 @@ class Anytime_Dstar(object): self.ComputeorImprovePath() ischanged = False - # if the environment is largely changed - # epsilon += increment or replan from scratch - # else if - # epsilon -= increment + if islargelychanged: + epsilon += increment # or replan from scratch + elif epsilon > 1: + epsilon -= increment + # move states from the INCONS to OPEN + # update priorities in OPEN + Allnodes = self.INCONS.union(set(self.OPEN.enumerate())) + for node in Allnodes: + self.OPEN.put(node, self.key(node, epsilon)) + self.INCONS = set() + self.CLOSED = set() + self.ComputeorImprovePath() + # publish current epsilon sub optimal solution + # if epsilon == 1: + # wait for change to occur pass if __name__ == '__main__': - AD = Anytime_Dstar(resolution = 1) + AD = Anytime_Dstar(resolution = 0.5) AD.Main() \ 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 41eb6c0462f46e992dc0d4566d89e6ace8cf791a..682db3060fbb679b4197991142592a5e3b7aeef6 100644 GIT binary patch delta 122 zcmZ3lv08)MiI4J Sa~3b4C6hl0i%c#O-U|R)_$RUe delta 138 zcmZ3jv0j7QiIIV{ByUl5a?S&~tj z6jP8`lA&9WlbDy6nU}7cn3JAg1eC}vjtNdpEK1JMO-d|IO;HGdDu|hE#iYs&m5Voa ciP@aR)WpwtYw}ycMj)9l^qkRWa=7pn0H0McE&u=k 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 af8baf399a72f2b2dfb3918584cefb970a4ad84c..847155f2b828e797646a4086ef731ab4c2d46aaa 100644 GIT binary patch delta 116 zcmcbmb4`caiIyU!b3!S&~tjq+gI& zlA&9WlbDy6nU}7cn3JAg1eC}v)(=iiEK1JMO-d|IO;HGdD$t)?!lcR#m5Voa(cip; HX`3(r`GF`+ delta 85 zcmcbnb4!QYiIIV{ByUl5a?S&~tj m6f@b8NrEwEas!hnkSt(Q<&FtXO)N^zh&OhL*}Q;hn=k-fIvUFW 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 eb24b07f97254f69d81c04316e0c8e9cf2f7ddf3..aaa3e20598b0d52bfa0d12a7d3c4420ef70a4537 100644 GIT binary patch delta 155 zcmaE<+^xdv#LLUY00ghENyZ0l)tn O($vz_&7T;*^8)}#RyPm; delta 81 zcmeBHd8y3n#LLUY00d0|yzx&q@^&!F$0u9Ggche36~`D`8X6nMxTF?mm*f}3q-U07 jlqSVY-oPlq7&G}jqbPSwaB5;vaz?zdOU!0|ra$}u=kFQ* 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 df0f89fb254c8e2db7339f0b273c9b7db4f87304..a43f9484525a3152370745991f29ce2b646da7f2 100644 GIT binary patch delta 188 zcmdlRxHFK?iI&Nwlp+0jB!aV&MwI>h)K^Z z$tX>VnVi5R!5A}nK9eYr>}OKtjtNdpEK1IZH+G5Hyr0QgiSgECX4Qp^)svU1egu-! i)%+M6CqGl03KXwVuVJeO+E&y!`JcKpWAkJwjrjmQ!YtJQ