From 5ccc649dc12442d92dbdf3bf2a903ffbbf944a77 Mon Sep 17 00:00:00 2001 From: zhm-real Date: Sat, 20 Jun 2020 12:46:49 -0700 Subject: [PATCH] update --- Search-based Planning/.idea/workspace.xml | 15 +++- .../__pycache__/env.cpython-37.pyc | Bin 794 -> 1192 bytes .../__pycache__/motion_model.cpython-37.pyc | Bin 285 -> 0 bytes .../__pycache__/plotting.cpython-37.pyc | Bin 0 -> 1911 bytes .../__pycache__/tools.cpython-37.pyc | Bin 2581 -> 0 bytes Search-based Planning/a_star.py | 69 +++++++++++------ Search-based Planning/bfs.py | 61 ++++++++++----- Search-based Planning/dfs.py | 61 ++++++++++----- Search-based Planning/dijkstra.py | 71 ++++++++++++------ Search-based Planning/env.py | 58 ++++++++------ Search-based Planning/motion_model.py | 7 -- Search-based Planning/plotting.py | 52 +++++++++++++ Search-based Planning/tools.py | 68 ----------------- 13 files changed, 279 insertions(+), 183 deletions(-) delete mode 100644 Search-based Planning/__pycache__/motion_model.cpython-37.pyc create mode 100644 Search-based Planning/__pycache__/plotting.cpython-37.pyc delete mode 100644 Search-based Planning/__pycache__/tools.cpython-37.pyc delete mode 100644 Search-based Planning/motion_model.py create mode 100644 Search-based Planning/plotting.py delete mode 100644 Search-based Planning/tools.py diff --git a/Search-based Planning/.idea/workspace.xml b/Search-based Planning/.idea/workspace.xml index 4a44399..7e11f97 100644 --- a/Search-based Planning/.idea/workspace.xml +++ b/Search-based Planning/.idea/workspace.xml @@ -1,7 +1,16 @@ - + + + + + + + + + + - + @@ -148,9 +157,9 @@ - + diff --git a/Search-based Planning/__pycache__/env.cpython-37.pyc b/Search-based Planning/__pycache__/env.cpython-37.pyc index 21dfed443fcc5758c7878b302dabaabb2061f88e..6e4ab821447bae5e8ca512c3a8b2858de9453be8 100644 GIT binary patch literal 1192 zcmZux&2G~`5Z+z?1^0(4Dy2fGSiOJ`?S)%}P(_RQIaEjmRfsx)%A zv=4y93$zj!-h-#`6$vR99)JtX*pAyuOtiD(otbZTXTQyhg$0{{e0}@-ps5q`6F+96 zfw2Wct$-4wOanOww(Sj7z9zz8TaF%-G!uD7+6~{ z)I(4{fn?HVC0JV%n$RyuTNefZnz{BeI_*-3L%|vLMYdt6C!hq)2!|bjh8+k<5@t__ zFIc(Mt_WDVAOK|TPM<>j^bshRO%Z4c1MR;c(kSd=l32lIrEQ{Z&t%B}cN@qw4*8W=9kb>)Xg--%p+%s+eEt z-P)YF>$c(H@E}Fo*5T|KIVKZm-7%f89+_yPwUlM_obHg1&553Ams+L^dc>|MB_|K| z$!NXyugv@z6Ks`KF)fHe40aO(E=RBY2Ng!?pJ4!fR!JT68DgB2;WeJKTRSl-ly7Af zVce=>)(Af;1VdOi5rkDo{Hh`v1;TQ97IA0h|3Xj9eUc%S?eTltspJiGJMh9_>^pGB zsn-pC_1H;b1i|C5ayi*O2qLN=G?8lXHa`6r3y7BIaQQ#4er+v5+G=Tck~3%1S*i72i!( z&dY2(ha9R%y$jq2x<%f==RCJLAH-r9qCL;~$o_JEY2JEv-oDw{@taP^1GKZ}e?I-C0KaXpGK}E}UGF0Z z@IjD(`oyP#ipE#)8=^UYo-=>w9r3A&Qq^bCG@iuCh|QyP>h);x5>FP75QgZbS9A-dZTeo!MCQeo5+p>yqHmbC0gE(ZB znJw(I7x>Un*ijb`EZnoOYvI7cj)i9ky=Lwxo{VJf&cfwKi)WckMDAWMH+nR0;9YVO z{{+>=Gk1gIesHFx(!pspejmploJf6T(k#fhiC8w~Nn-yDJ|3kiHqk@}1Ig7edU3(E z6zqLP2xRi*FuTrOtPx^ETUbFqK>)W&wn-O#kFM68GVapVs%%T|un9K|MLG;a)xiyA eyDd}Yeb?YVCi3>HNh+pe`P#uqHxZh&P5%Lb%Dl7y diff --git a/Search-based Planning/__pycache__/motion_model.cpython-37.pyc b/Search-based Planning/__pycache__/motion_model.cpython-37.pyc deleted file mode 100644 index 904996372837eb866d696f03dbdd1ebbc28acde5..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 285 zcmXwzv5LY#5QaBv)FWUg*rrY4J%Ah{YGHL8$TeYcS)ILXFv*79O>sWZfv=SIDiM4D z8=XY`VP?KzX83157z70Mb2)GRu>LpAOFqI3!wxY4i3xZ>p#y&6#~yUoBh($ diff --git a/Search-based Planning/__pycache__/plotting.cpython-37.pyc b/Search-based Planning/__pycache__/plotting.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..7e5f0a1918401b5bbe1cf7b884744db7e48a1ec5 GIT binary patch literal 1911 zcmbVN&2Jk;6rb5I+Z!h|ZAeLJ(Mp_*M5u%i$cjP*kwPyKQb3@Uiy7}s>`nG7Gvm}T z>r)~%7Y_Uf$&ns7@z?APams}Yx1M-!>^P~&r5)?d*L(Bc`@N6#SBr~2f$>@IpP&A8 z3HcijSHptM6Cm{vCQd@aDYp`uTA@t|x4CmlLWf(sq~(t9`%lr<>fPSjAfKb}-un zSMv;&;caXpAtuow4-a^{;9dMLB@HE8oTf}ogzMeYFI15)q7)Q7rxc4tD* zNJ*i3$Qt?0I&>hCRN|Z8Nz1CN^~V*BTePwPltdM;e*C)K`CbXBI^WFdKZN@4YzcLs z^P+>Wf+AsAhM}8BS9Nv;lhNSg9#et`-_8U&kP(eRUN4Sc`5Q^Bbd;yXCN`o%a~>H# znrCr+BgrF{s7=^hNxlufOT=f;v@D4oDlssC8$Jxvdx?E4YcM?mnt5WvBC)GOtEWP% z52Ox4R~!ymQqfPxwGY2|jK7NJEjw%^aVDO}Tn|Dw%AxW?FJcE;hjodu7E`Umz6)Z{~DiuId}3+OX-$SR}L1+ zwPz;Q>tJmb>$kFnskz0LT)w)$PU|PvrYb6TTFuInBF&FPeF{JO0ewJM z;kRT?4$O7>kTzj$k|o1kv3)8(fEO*ds&%_OkGkD(8NQc@H;H@DlDGpG%qmww)W}b> YhDC6lKd7uU=fgy7qKgXq$+4RDf0)F;cK`qY literal 0 HcmV?d00001 diff --git a/Search-based Planning/__pycache__/tools.cpython-37.pyc b/Search-based Planning/__pycache__/tools.cpython-37.pyc deleted file mode 100644 index 1febd5e99846d991a0135cb519d6cda99e719f05..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 2581 zcmb_eO>Y}T7@pZLeFZ?(Dd6 zqK%MfB`TyI_y@_ce~05|dzg3`e@3sixl_#e+EQ`TPBBRDX#j*+%40g#_1(>^sPL3q`?u{J<&fg0C=GZov<{ z@oHfe-U>VL3g?g&-Vx`laADchYQDUJ?O1AglRaAgo@uVF!g<1n^1 zk-i#h6-L_Eed!NF-S<0TE`^_^rbWpl)Nz*P{dnkST|do)%vbAXW^)*-aNv(0H2p0d zDotCX{?|J1HT`>8nEa;$b7iWV{e3X@)@V%;C)tt}a+(midm>&CodR%QC+65EN z3Pb6xFyS46OQw~ZR&H7=vsR~YMRoGE4L%DdYR@e*!xeSntYCCeHX<<6B)WBU70w~& z@1y9!)emP50FW+&U@Po5>ybk=Ry4<)b;ByH)oUe>8@#M!oxD8=ho(c(^?p@fqv%H2 zV7UGXElWN;wtp$_XS+0XbuG!FFv-_pOb5OVpqF!i)FOd9g#tiNlTD;Bc+`8aE+ZcU zne5{X8Um}62UZU`-)Zom+Raie;z$Rsd^8G^hF7{Ed{35kBGb|yCc1QnNv2Cz$2yUL z6OQA&;VFussy`Dgo;%h#{bujE<7 zR+r#TeTX9P2BF%KDp=Z)du_b55-)EX&rhmnlp;AHuv^GR3k$YAae7umxS#OU68L+7 zt6*=Gp3I|gD2;RIp^arrM~-6+A#u&RlX%W#sI0Cf;h-bJ_1jqgZ)CRr0r9#mGM&#e zY%=O4$<{2QP?=fy0=pM=11}2G$6+2U42DTN%Fo}i%@&j^)?xS2k;>l3fl YwLvCEiCiZgGfo|A(XL*oE>zF`3vLTka{vGU diff --git a/Search-based Planning/a_star.py b/Search-based Planning/a_star.py index a6d3af0..fad3386 100644 --- a/Search-based Planning/a_star.py +++ b/Search-based Planning/a_star.py @@ -5,22 +5,26 @@ """ import queue -import tools +import plotting import env -import motion_model class Astar: def __init__(self, x_start, x_goal, heuristic_type): - self.u_set = motion_model.motions # feasible input set self.xI, self.xG = x_start, x_goal - self.obs = env.obs_map() # position of obstacles - self.heuristic_type = heuristic_type - tools.show_map(self.xI, self.xG, self.obs, "a_star searching") + self.Env = env.Env() + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles + + [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG, heuristic_type) + + self.fig_name = "A* Algorithm" + plotting.animation(self.xI, self.xG, self.obs, + self.path, self.visited, self.fig_name) # animation generate - def searching(self): + def searching(self, xI, xG, heuristic_type): """ Searching using A_star. @@ -28,30 +32,53 @@ class Astar: """ q_astar = queue.QueuePrior() # priority queue - q_astar.put(self.xI, 0) - parent = {self.xI: self.xI} # record parents of nodes - action = {self.xI: (0, 0)} # record actions of nodes - cost = {self.xI: 0} + q_astar.put(xI, 0) + parent = {xI: xI} # record parents of nodes + action = {xI: (0, 0)} # record actions of nodes + visited = [] + cost = {xI: 0} while not q_astar.empty(): x_current = q_astar.get() - if x_current == self.xG: # stop condition + if x_current == xG: # stop condition break - if x_current != self.xI: - tools.plot_dots(x_current, len(parent)) + visited.append(x_current) for u_next in self.u_set: # explore neighborhoods of current node x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) if x_next not in self.obs: new_cost = cost[x_current] + self.get_cost(x_current, u_next) if x_next not in cost or new_cost < cost[x_next]: # conditions for updating cost cost[x_next] = new_cost - priority = new_cost + self.Heuristic(x_next, self.xG, self.heuristic_type) + priority = new_cost + self.Heuristic(x_next, xG, heuristic_type) q_astar.put(x_next, priority) # put node into queue using priority "f+h" - parent[x_next] = x_current - action[x_next] = u_next - [path_astar, actions_astar] = tools.extract_path(self.xI, self.xG, parent, action) + parent[x_next], action[x_next] = x_current, u_next - return path_astar, actions_astar + [path, policy] = self.extract_path(xI, xG, parent, action) + + return path, policy, visited + + + def extract_path(self, xI, xG, parent, policy): + """ + Extract the path based on the relationship of nodes. + + :param xI: Starting node + :param xG: Goal node + :param parent: Relationship between nodes + :param policy: Action needed for transfer between two nodes + :return: The planning path + """ + + path_back = [xG] + acts_back = [policy[xG]] + x_current = xG + while True: + x_current = parent[x_current] + path_back.append(x_current) + acts_back.append(policy[x_current]) + if x_current == xI: break + + return list(path_back), list(acts_back) def get_cost(self, x, u): @@ -88,6 +115,4 @@ class Astar: if __name__ == '__main__': x_Start = (5, 5) # Starting node x_Goal = (49, 5) # Goal node - astar = Astar(x_Start, x_Goal, "manhattan") - [path_astar, actions_astar] = astar.searching() - tools.showPath(x_Start, x_Goal, path_astar) \ No newline at end of file + astar = Astar(x_Start, x_Goal, "manhattan") \ No newline at end of file diff --git a/Search-based Planning/bfs.py b/Search-based Planning/bfs.py index 38483fa..0638105 100644 --- a/Search-based Planning/bfs.py +++ b/Search-based Planning/bfs.py @@ -5,21 +5,25 @@ """ import queue -import tools +import plotting import env -import motion_model class BFS: def __init__(self, x_start, x_goal): - self.u_set = motion_model.motions # feasible input set self.xI, self.xG = x_start, x_goal - self.obs = env.obs_map() # position of obstacles - tools.show_map(self.xI, self.xG, self.obs, "breadth-first searching") + self.Env = env.Env() + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles + [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG) + + self.fig_name = "Dijkstra's Algorithm" + plotting.animation(self.xI, self.xG, self.obs, + self.path, self.visited, self.fig_name) # animation generate - def searching(self): + def searching(self, xI, xG): """ Searching using BFS. @@ -27,30 +31,51 @@ class BFS: """ q_bfs = queue.QueueFIFO() # first-in-first-out queue - q_bfs.put(self.xI) - parent = {self.xI: self.xI} # record parents of nodes - action = {self.xI: (0, 0)} # record actions of nodes + q_bfs.put(xI) + parent = {xI: xI} # record parents of nodes + action = {xI: (0, 0)} # record actions of nodes + visited = [] while not q_bfs.empty(): x_current = q_bfs.get() - if x_current == self.xG: + if x_current == xG: break - if x_current != self.xI: - tools.plot_dots(x_current, len(parent)) + visited.append(x_current) for u_next in self.u_set: # explore neighborhoods of current node x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) if x_next not in parent and x_next not in self.obs: # node not visited and not in obstacles q_bfs.put(x_next) - parent[x_next] = x_current - action[x_next] = u_next - [path_bfs, action_bfs] = tools.extract_path(self.xI, self.xG, parent, action) # extract path + parent[x_next], action[x_next] = x_current, u_next - return path_bfs, action_bfs + [path, policy] = self.extract_path(xI, xG, parent, action) # extract path + + return path, policy, visited + + + def extract_path(self, xI, xG, parent, policy): + """ + Extract the path based on the relationship of nodes. + + :param xI: Starting node + :param xG: Goal node + :param parent: Relationship between nodes + :param policy: Action needed for transfer between two nodes + :return: The planning path + """ + + path_back = [xG] + acts_back = [policy[xG]] + x_current = xG + while True: + x_current = parent[x_current] + path_back.append(x_current) + acts_back.append(policy[x_current]) + if x_current == xI: break + + return list(path_back), list(acts_back) if __name__ == '__main__': x_Start = (5, 5) # Starting node x_Goal = (49, 5) # Goal node bfs = BFS(x_Start, x_Goal) - [path_bf, actions_bf] = bfs.searching() - tools.showPath(x_Start, x_Goal, path_bf) diff --git a/Search-based Planning/dfs.py b/Search-based Planning/dfs.py index 1ae5ea5..e37678a 100644 --- a/Search-based Planning/dfs.py +++ b/Search-based Planning/dfs.py @@ -5,21 +5,25 @@ """ import queue -import tools +import plotting import env -import motion_model class DFS: def __init__(self, x_start, x_goal): - self.u_set = motion_model.motions # feasible input set self.xI, self.xG = x_start, x_goal - self.obs = env.obs_map() # position of obstacles - tools.show_map(self.xI, self.xG, self.obs, "depth-first searching") + self.Env = env.Env() + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles + [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG) + + self.fig_name = "Dijkstra's Algorithm" + plotting.animation(self.xI, self.xG, self.obs, + self.path, self.visited, self.fig_name) # animation generate - def searching(self): + def searching(self, xI, xG): """ Searching using DFS. @@ -27,30 +31,51 @@ class DFS: """ q_dfs = queue.QueueLIFO() # last-in-first-out queue - q_dfs.put(self.xI) - parent = {self.xI: self.xI} # record parents of nodes - action = {self.xI: (0, 0)} # record actions of nodes + q_dfs.put(xI) + parent = {xI: xI} # record parents of nodes + action = {xI: (0, 0)} # record actions of nodes + visited = [] while not q_dfs.empty(): x_current = q_dfs.get() - if x_current == self.xG: + if x_current == xG: break - if x_current != self.xI: - tools.plot_dots(x_current, len(parent)) + visited.append(x_current) for u_next in self.u_set: # explore neighborhoods of current node x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) if x_next not in parent and x_next not in self.obs: # node not visited and not in obstacles q_dfs.put(x_next) - parent[x_next] = x_current - action[x_next] = u_next - [path_dfs, action_dfs] = tools.extract_path(self.xI, self.xG, parent, action) + parent[x_next], action[x_next] = x_current, u_next - return path_dfs, action_dfs + [path, policy] = self.extract_path(xI, xG, parent, action) + + return path, policy, visited + + + def extract_path(self, xI, xG, parent, policy): + """ + Extract the path based on the relationship of nodes. + + :param xI: Starting node + :param xG: Goal node + :param parent: Relationship between nodes + :param policy: Action needed for transfer between two nodes + :return: The planning path + """ + + path_back = [xG] + acts_back = [policy[xG]] + x_current = xG + while True: + x_current = parent[x_current] + path_back.append(x_current) + acts_back.append(policy[x_current]) + if x_current == xI: break + + return list(path_back), list(acts_back) if __name__ == '__main__': x_Start = (5, 5) # Starting node x_Goal = (49, 5) # Goal node dfs = DFS(x_Start, x_Goal) - [path_dfs, action_dfs] = dfs.searching() - tools.showPath(x_Start, x_Goal, path_dfs) diff --git a/Search-based Planning/dijkstra.py b/Search-based Planning/dijkstra.py index 78f00b8..1d50afa 100644 --- a/Search-based Planning/dijkstra.py +++ b/Search-based Planning/dijkstra.py @@ -6,51 +6,55 @@ import queue import env -import tools -import motion_model +import plotting class Dijkstra: def __init__(self, x_start, x_goal): - self.u_set = motion_model.motions # feasible input set self.xI, self.xG = x_start, x_goal - self.obs = env.obs_map() # position of obstacles - tools.show_map(self.xI, self.xG, self.obs, "dijkstra searching") + self.Env = env.Env() + self.u_set = self.Env.motions # feasible input set + self.obs = self.Env.obs # position of obstacles + [self.path, self.policy, self.visited] = self.searching(self.xI, self.xG) + + self.fig_name = "Dijkstra's Algorithm" + plotting.animation(self.xI, self.xG, self.obs, + self.path, self.visited, self.fig_name) # animation generate - def searching(self): + def searching(self, xI, xG): """ Searching using Dijkstra. :return: planning path, action in each node, visited nodes in the planning process """ - q_dijk = queue.QueuePrior() # priority queue - q_dijk.put(self.xI, 0) - parent = {self.xI: self.xI} # record parents of nodes - action = {self.xI: (0, 0)} # record actions of nodes - cost = {self.xI: 0} + q_dijk = queue.QueuePrior() # priority queue + q_dijk.put(xI, 0) + parent = {xI: xI} # record parents of nodes + action = {xI: (0, 0)} # record actions of nodes + visited = [] # record visited nodes + cost = {xI: 0} while not q_dijk.empty(): x_current = q_dijk.get() - if x_current == self.xG: # stop condition + if x_current == xG: # stop condition break - if x_current != self.xI: - tools.plot_dots(x_current, len(parent)) - for u_next in self.u_set: # explore neighborhoods of current node + visited.append(x_current) + for u_next in self.u_set: # explore neighborhoods of current node x_next = tuple([x_current[i] + u_next[i] for i in range(len(x_current))]) - if x_next not in self.obs: # node not visited and not in obstacles + if x_next not in self.obs: # node not visited and not in obstacles new_cost = cost[x_current] + self.get_cost(x_current, u_next) if x_next not in cost or new_cost < cost[x_next]: cost[x_next] = new_cost priority = new_cost - q_dijk.put(x_next, priority) # put node into queue using cost to come as priority - parent[x_next] = x_current - action[x_next] = u_next - [path_dijk, action_dijk] = tools.extract_path(self.xI, self.xG, parent, action) + q_dijk.put(x_next, priority) # put node into queue using cost to come as priority + parent[x_next], action[x_next] = x_current, u_next - return path_dijk, action_dijk + [path, policy] = self.extract_path(xI, xG, parent, action) + + return path, policy, visited def get_cost(self, x, u): @@ -66,9 +70,30 @@ class Dijkstra: return 1 + def extract_path(self, xI, xG, parent, policy): + """ + Extract the path based on the relationship of nodes. + + :param xI: Starting node + :param xG: Goal node + :param parent: Relationship between nodes + :param policy: Action needed for transfer between two nodes + :return: The planning path + """ + + path_back = [xG] + acts_back = [policy[xG]] + x_current = xG + while True: + x_current = parent[x_current] + path_back.append(x_current) + acts_back.append(policy[x_current]) + if x_current == xI: break + + return list(path_back), list(acts_back) + + if __name__ == '__main__': x_Start = (5, 5) # Starting node x_Goal = (49, 5) # Goal node dijkstra = Dijkstra(x_Start, x_Goal) - [path_dijk, actions_dijk] = dijkstra.searching() - tools.showPath(x_Start, x_Goal, path_dijk) diff --git a/Search-based Planning/env.py b/Search-based Planning/env.py index a128ecf..c8e517d 100644 --- a/Search-based Planning/env.py +++ b/Search-based Planning/env.py @@ -4,34 +4,44 @@ @author: huiming zhou """ -x_range, y_range = 51, 31 # size of background +class Env(): + def __init__(self): + self.x_range = 51 # size of background + self.y_range = 31 + self.motions = [(1, 0), (-1, 0), (0, 1), (0, -1)] + self.obs = self.obs_map() -def obs_map(): - """ - Initialize obstacles' positions - :return: map of obstacles - """ + def obs_map(self): + """ + Initialize obstacles' positions - obs = [] - for i in range(x_range): - obs.append((i, 0)) - for i in range(x_range): - obs.append((i, y_range - 1)) + :return: map of obstacles + """ - for i in range(y_range): - obs.append((0, i)) - for i in range(y_range): - obs.append((x_range - 1, i)) + x = self.x_range + y = self.y_range + obs = [] - for i in range(10, 21): - obs.append((i, 15)) - for i in range(15): - obs.append((20, i)) + for i in range(x): + obs.append((i, 0)) + for i in range(x): + obs.append((i, y - 1)) - for i in range(15, 30): - obs.append((30, i)) - for i in range(16): - obs.append((40, i)) + for i in range(y): + obs.append((0, i)) + for i in range(y): + obs.append((x - 1, i)) + + for i in range(10, 21): + obs.append((i, 15)) + for i in range(15): + obs.append((20, i)) + + for i in range(15, 30): + obs.append((30, i)) + for i in range(16): + obs.append((40, i)) + + return obs - return obs diff --git a/Search-based Planning/motion_model.py b/Search-based Planning/motion_model.py deleted file mode 100644 index 144bbcc..0000000 --- a/Search-based Planning/motion_model.py +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -@author: huiming zhou -""" - -motions = [(1, 0), (-1, 0), (0, 1), (0, -1)] # feasible motion sets \ No newline at end of file diff --git a/Search-based Planning/plotting.py b/Search-based Planning/plotting.py new file mode 100644 index 0000000..4a939b0 --- /dev/null +++ b/Search-based Planning/plotting.py @@ -0,0 +1,52 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +@author: huiming zhou +""" + +import matplotlib.pyplot as plt + + +def animation(xI, xG, obs, path, visited, name): + """ + generate animation for exploring process of algorithm + + :param xI: starting state + :param xG: goal state + :param obs: obstacle map + :param path: optimal path + :param visited: visited nodes + :param name: name of this figure + :return: animation + """ + + visited.remove(xI) + path.remove(xI) + path.remove(xG) + + # plot gridworld + obs_x = [obs[i][0] for i in range(len(obs))] + obs_y = [obs[i][1] for i in range(len(obs))] + plt.plot(xI[0], xI[1], "bs") + plt.plot(xG[0], xG[1], "gs") + plt.plot(obs_x, obs_y, "sk") + plt.title(name) + plt.axis("equal") + + # animation for the exploring order of visited nodes + count = 0 + for x in visited: + count += 1 + plt.plot(x[0], x[1], linewidth='3', color='#808080', marker='o') + plt.gcf().canvas.mpl_connect('key_release_event', + lambda event: [exit(0) if event.key == 'escape' else None]) + if count % 20 == 0: plt.pause(0.01) + + # plot optimal path + path_x = [path[i][0] for i in range(len(path))] + path_y = [path[i][1] for i in range(len(path))] + plt.plot(path_x, path_y, linewidth='3', color='r', marker='o') + + # show animation + plt.pause(0.01) + plt.show() diff --git a/Search-based Planning/tools.py b/Search-based Planning/tools.py deleted file mode 100644 index 7d4aede..0000000 --- a/Search-based Planning/tools.py +++ /dev/null @@ -1,68 +0,0 @@ -#!/usr/bin/env python3 -# -*- coding: utf-8 -*- -""" -@author: huiming zhou -""" - -import matplotlib.pyplot as plt - - -def extract_path(xI, xG, parent, actions): - """ - Extract the path based on the relationship of nodes. - - :param xI: Starting node - :param xG: Goal node - :param parent: Relationship between nodes - :param actions: Action needed for transfer between two nodes - :return: The planning path - """ - - path_back = [xG] - acts_back = [actions[xG]] - x_current = xG - while True: - x_current = parent[x_current] - path_back.append(x_current) - acts_back.append(actions[x_current]) - if x_current == xI: break - - return list(reversed(path_back)), list(reversed(acts_back)) - - -def show_map(xI, xG, obs_map, name): - obs_x = [obs_map[i][0] for i in range(len(obs_map))] - obs_y = [obs_map[i][1] for i in range(len(obs_map))] - - plt.plot(xI[0], xI[1], "bs") - plt.plot(xG[0], xG[1], "gs") - plt.plot(obs_x, obs_y, "sk") - plt.title(name, fontdict=None) - plt.axis("equal") - - -def showPath(xI, xG, path): - """ - Plot the path. - - :param xI: Starting node - :param xG: Goal node - :param path: Planning path - :return: A plot - """ - - path.remove(xI) - path.remove(xG) - path_x = [path[i][0] for i in range(len(path))] - path_y = [path[i][1] for i in range(len(path))] - plt.plot(path_x, path_y, linewidth='5', color='r', linestyle='-') - plt.pause(0.001) - plt.show() - - -def plot_dots(x, length): - plt.plot(x[0], x[1], linewidth='3', color='#808080', marker='o') - plt.gcf().canvas.mpl_connect('key_release_event', - lambda event: [exit(0) if event.key == 'escape' else None]) - if length % 15 == 0: plt.pause(0.001) -