From d9c76565534a7c96f279248a3061279e848eecc2 Mon Sep 17 00:00:00 2001 From: zhm-real Date: Mon, 10 Aug 2020 16:33:39 -0700 Subject: [PATCH] update D* --- Search_based_Planning/Search_2D/D_star.py | 85 +++++++++++++----- .../Search_2D/__pycache__/env.cpython-37.pyc | Bin 1307 -> 1434 bytes .../__pycache__/plotting.cpython-37.pyc | Bin 5246 -> 5340 bytes Search_based_Planning/Search_2D/env.py | 3 + Search_based_Planning/Search_2D/plotting.py | 3 + 5 files changed, 71 insertions(+), 20 deletions(-) diff --git a/Search_based_Planning/Search_2D/D_star.py b/Search_based_Planning/Search_2D/D_star.py index a413c4f..300059b 100644 --- a/Search_based_Planning/Search_2D/D_star.py +++ b/Search_based_Planning/Search_2D/D_star.py @@ -50,10 +50,12 @@ class DStar: def run(self, s_start, s_end): self.init() self.insert(s_end, 0) + while True: self.process_state() if self.t[s_start] == 'CLOSED': break + self.path = self.extract_path(s_start, s_end) self.Plot.plot_grid("Dynamic A* (D*)") self.plot_path(self.path) @@ -66,22 +68,28 @@ class DStar: print("Please choose right area!") else: x, y = int(x), int(y) - print("Add obstacle at: s =", x, ",", "y =", y) - self.obs.add((x, y)) - plt.plot(x, y, 'sk') - s = self.s_start - self.visited = set() - self.count += 1 + if (x, y) not in self.obs: + print("Add obstacle at: s =", x, ",", "y =", y) + self.obs.add((x, y)) + self.Plot.update_obs(self.obs) - while s != self.s_goal: - if self.is_collision(s, self.PARENT[s]): - self.modify(s) - continue - s = self.PARENT[s] + s = self.s_start + self.visited = set() + self.count += 1 + + while s != self.s_goal: + if self.is_collision(s, self.PARENT[s]): + self.modify(s) + continue + s = self.PARENT[s] + + self.path = self.extract_path(self.s_start, self.s_goal) + + plt.cla() + self.Plot.plot_grid("Dynamic A* (D*)") + self.plot_visited(self.visited) + self.plot_path(self.path) - self.path = self.extract_path(self.s_start, self.s_goal) - self.plot_visited(self.visited) - self.plot_path(self.path) self.fig.canvas.draw_idle() def extract_path(self, s_start, s_end): @@ -94,42 +102,63 @@ class DStar: return path def process_state(self): - s = self.min_state() + s = self.min_state() # get node in OPEN set with min k value self.visited.add(s) if s is None: - return -1 + return -1 # OPEN set is empty - k_old = self.get_k_min() - self.delete(s) + k_old = self.get_k_min() # record the min k value of this iteration (min path cost) + self.delete(s) # move state s from OPEN set to CLOSED set + # k_min < h[s] --> s: RAISE state (increased cost) if k_old < self.h[s]: for s_n in self.get_neighbor(s): - if self.h[s_n] <= k_old and self.h[s] > self.h[s_n] + self.cost(s_n, s): + if self.h[s_n] <= k_old and \ + self.h[s] > self.h[s_n] + self.cost(s_n, s): + + # update h_value and choose parent self.PARENT[s] = s_n self.h[s] = self.h[s_n] + self.cost(s_n, s) + + # s: k_min >= h[s] -- > s: LOWER state (cost reductions) if k_old == self.h[s]: for s_n in self.get_neighbor(s): if self.t[s_n] == 'NEW' or \ (self.PARENT[s_n] == s and self.h[s_n] != self.h[s] + self.cost(s, s_n)) or \ (self.PARENT[s_n] != s and self.h[s_n] > self.h[s] + self.cost(s, s_n)): + + # Condition: + # 1) t[s_n] == 'NEW': not visited + # 2) s_n's parent: cost reduction + # 3) s_n find a better parent self.PARENT[s_n] = s self.insert(s_n, self.h[s] + self.cost(s, s_n)) else: for s_n in self.get_neighbor(s): if self.t[s_n] == 'NEW' or \ (self.PARENT[s_n] == s and self.h[s_n] != self.h[s] + self.cost(s, s_n)): + + # Condition: + # 1) t[s_n] == 'NEW': not visited + # 2) s_n's parent: cost reduction self.PARENT[s_n] = s self.insert(s_n, self.h[s] + self.cost(s, s_n)) else: - if self.PARENT[s_n] != s and self.h[s_n] > self.h[s] + self.cost(s, s_n): + if self.PARENT[s_n] != s and \ + self.h[s_n] > self.h[s] + self.cost(s, s_n): + + # Condition: LOWER happened in OPEN set (s), s should be explored again self.insert(s, self.h[s]) else: if self.PARENT[s_n] != s and \ self.h[s] > self.h[s_n] + self.cost(s_n, s) and \ self.t[s_n] == 'CLOSED' and \ self.h[s_n] > k_old: + + # Condition: LOWER happened in CLOSED set (s_n), s_n should be explored again self.insert(s_n, self.h[s_n]) + return self.get_k_min() def min_state(self): @@ -155,6 +184,12 @@ class DStar: return min([self.k[x] for x in self.OPEN]) def insert(self, s, h_new): + """ + insert node into OPEN set. + :param s: node + :param h_new: new or better cost to come value + """ + if self.t[s] == 'NEW': self.k[s] = h_new elif self.t[s] == 'OPEN': @@ -178,13 +213,23 @@ class DStar: self.OPEN.remove(s) def modify(self, s): + """ + start processing from state s. + :param s: is a node whose status is RAISE or LOWER. + """ + self.modify_cost(s) + while True: k_min = self.process_state() + if k_min >= self.h[s]: break def modify_cost(self, s): + # if node in CLOSED set, put it into OPEN set. + # Since cost may be changed between s - s.parent, calc cost(s, s.p) again + if self.t[s] == 'CLOSED': self.insert(s, self.h[self.PARENT[s]] + self.cost(s, self.PARENT[s])) diff --git a/Search_based_Planning/Search_2D/__pycache__/env.cpython-37.pyc b/Search_based_Planning/Search_2D/__pycache__/env.cpython-37.pyc index b0d7aae65f83abc731fc90981eb34abd9ea4fb13..945aa4dd30ff0b41097d4b473bf4321682bfd244 100644 GIT binary patch delta 233 zcmbQuHH(|qiIBn#jIQ-eAiY2UqCwahh>Jl6)-cvE#51HY08L=@(_}2-28wAi74ZNmFuRBs z!~zq1K-Mj;(t?!4lGOP8q+)&`zZh&VV-=rkUYQ<3Dwz=|2Lh}>%*Md5*`MhLBcsq{ i4;Fhyw#gkVT8sjd*RiN^0i_r?7&+KEm?z(1u?GNS#4a@e delta 92 zcmbQmJ)4WyiIcYe>={`nv1(1$XI0{00UE=>$iY52n$;cv@M09k diff --git a/Search_based_Planning/Search_2D/__pycache__/plotting.cpython-37.pyc b/Search_based_Planning/Search_2D/__pycache__/plotting.cpython-37.pyc index 2fdee55b728db65301d99f768cd3fa8244c8aa6f..8a41db2ceb6f599a4adf796c32cc3953227cf601 100644 GIT binary patch delta 1338 zcmZ`&OKjUl5ar4eEz+hwmg4vqxz5*&0T*sQj-8;X?bhxGFcK7j0bdA8OVdOtQC-oF z^8tK_PRS`)1S!yCPrbAUA;>kiXVz_*Vx!`U}Gvwu(j zYFo>eWoq>N_+rj^aNl~x_U`|AY02Wbwzg^TBfP+iZMJFh3NQ0R#0K~pfRd|dH7pZHp{CuP9yF;~7}*JLxbu%E&f^kBuiLT?NE-e$KoN8=j(!`5vk z`pIUeM3xrCg#o4j;fcA^uuo_?`m zA>6D*VI-U|XvN|%B1QlzzsiYC#3F~iZNSmnBTJv`G?61 zV~EZGl%84X4^&pvNuYFF?@CiAI8KHnQ;T*Qu{Z^;hhD>vxBt^=b%bAbo(+7)Dv+75 zfW|{L;@@cy5baYn{=Q92WuW0vRjrZX{XcAzr&ndd&WKe6rV=9z8s7DQlaMpW)vX`w zG+>6nzg7Ne?YEol%QO}TtyTqJokbAlr+W(TR;@Ivs?%Gc>u>5!H{N#kdh3m@>RxG> zan;wYPBk~r&a1LMhj9+@5rN$&z5rJ$&k{>=wMaC!!sxmus^N|&&dKNbh0*f}LKSo( z^WYc2nOH!(%|-fX!+KGETv-3~pns}qs4nTNyL-KMB5(J3&#V(mj8^PY=TBN zc%mEq2Syfl#m2;iKfvw8jT_wSneRL2%tz^W$~*4$x=j4{ z{%3do=C=1US=s(EoQY8%^;b#GNjqqWhI!f@kdE7?ZMeL)| zYbL1^R>@wTT_~JK+^i*hKCb|tEj0O%d1LRXDxW56+zS&HSr~^4dFip zdCF)is`)j6D=Y@@%wf1pA_j3AkUxHC@tFyc({g7eOV z6@`q{o;VYPq+rMOg3Q9Sn+DqPv=u?f>4UfKc#miVI+tv9Wuk#mP8imrR$10r68^Yj zR_s-_*5#QXag=8eMI!Z%3ypj7D0dKrm)@C;6h9^mNM*Mjmt>x4crwT!o4|u+ z!>KCE@@<9ci|jPks=LKVLg?kNa4)ix{j^>kYfjJ6q^!-N4|t# zghD+oLb0~%OOu34>3!B1NC>excAVSVSGLX4&7Fm?WZJxt8jWJ>8F+McE z4Cn46Kg40Z7dC@!@Fp;@u|F9x4s>EzN`*U(LE(){2xAE22nXGCBhkYl=TPP4{Jh5G zdNfxkF0mM959vs7Zk0dfI0qAfx!^0(557<;js)vMICQ0xb6YSQDxH|&k>M=svaHUj z71^*;rL@2bciA*1TvVeAGQ$kJ3SUFR?YK*at2J}(8H|N9K5Q`W9FBZa^25V$?{EY! zbRZl-IEsKPec*dV1BR!p7j&gqlJ!N6G8D*~;d?mY!2UUi!wT_8)+UxA2uqRGzashR AAOHXW diff --git a/Search_based_Planning/Search_2D/env.py b/Search_based_Planning/Search_2D/env.py index 0cd2424..753a8c8 100644 --- a/Search_based_Planning/Search_2D/env.py +++ b/Search_based_Planning/Search_2D/env.py @@ -12,6 +12,9 @@ class Env: (1, 0), (1, -1), (0, -1), (-1, -1)] self.obs = self.obs_map() + def update_obs(self, obs): + self.obs = obs + def obs_map(self): """ Initialize obstacles' positions diff --git a/Search_based_Planning/Search_2D/plotting.py b/Search_based_Planning/Search_2D/plotting.py index f952876..1cf98a3 100644 --- a/Search_based_Planning/Search_2D/plotting.py +++ b/Search_based_Planning/Search_2D/plotting.py @@ -19,6 +19,9 @@ class Plotting: self.env = env.Env() self.obs = self.env.obs_map() + def update_obs(self, obs): + self.obs = obs + def animation(self, path, visited, name): self.plot_grid(name) self.plot_visited(visited)