From e417553dc63f1fd69180d19cbac5d171b61af956 Mon Sep 17 00:00:00 2001 From: Mineinjava <65673396+Mineinjava@users.noreply.github.com> Date: Sun, 31 Mar 2024 12:40:30 -0700 Subject: [PATCH 01/13] tune noise --- .../__pycache__/bresenhams.cpython-39.pyc | Bin 3809 -> 3809 bytes .../__pycache__/cubicSpline.cpython-39.pyc | Bin 2416 -> 2416 bytes pathPlanner/__pycache__/node.cpython-39.pyc | Bin 1761 -> 1761 bytes pathPlanner/__pycache__/point.cpython-39.pyc | Bin 2793 -> 2793 bytes pathPlanner/theta_star.py | 17 ++++++++++------- 5 files changed, 10 insertions(+), 7 deletions(-) diff --git a/pathPlanner/__pycache__/bresenhams.cpython-39.pyc b/pathPlanner/__pycache__/bresenhams.cpython-39.pyc index f5a710e7e314327668235646d8e12d7ce3e56e5a..0a7a00de302b3f7ef166b9c2be1c5f91a9a8f415 100644 GIT binary patch delta 599 zcmaDT`%soUk(ZZ?0SIE^K5gVSXJL$*?98IaC^or>WhSGIt z#x2IoTZ|<|qClY{VGtn(C&VWQv8gi3POf14A`jNWky=qwlvq$wTqF%w1XeCSxtra9 zQDO5Q_Jd5$aHFD_K}v;yN~2gSic3-pqL@=sDnUd=kswHvGqtEFzo?)nwd@vSJc`8! zxSAPNC%bTGn1ak-$#jcJ&j8{^B?ZQi+fp)2GavdK3W??T7(+fsscj z&Xdty(6aKHVwQP3aEAmV0vB^Kgc+86AO&$At3p|LvF|dcbT&-S#jfK{uoOT(UJwuQ zI#lNG@lT~4SW%eK9nOq;6W4S);eP*~#lvlmZkmWTI%wVzT&)!TNK8Q!XX>!Q{;vut zMt#}ipZ4SpC7RY~;`Z#ZbMh?DUvh@9CKhVz)F=1(yqbeG8DkjhQV0yJ@*2{^6dQN( z#zcztY|BOx*U6T)$K=0WGv7k(ZZ?0SK1v;!NAfbCnGMKMV!b delta 21 bcmaFJ`;eC>k(ZZ?0SKDsiKK4ixyl9rJ#_`3 diff --git a/pathPlanner/__pycache__/point.cpython-39.pyc b/pathPlanner/__pycache__/point.cpython-39.pyc index c8c9aebda6876a86ac290b6464c818ef677f67c9..c346e4ef92016e6fdf41b1d3c008d2be412e9ce2 100644 GIT binary patch delta 21 bcmaDU`cjl9k(ZZ?0SK1v;!NAfbC(MML68Oj delta 21 bcmaDU`cjl9k(ZZ?0SKDsiKK4ixyuCrKlugB diff --git a/pathPlanner/theta_star.py b/pathPlanner/theta_star.py index 0d35ae7..42e2e8c 100644 --- a/pathPlanner/theta_star.py +++ b/pathPlanner/theta_star.py @@ -1,7 +1,10 @@ +import time + import point from node import node import numpy as np import bresenhams +from time import perf_counter import traceback from perlin_noise import PerlinNoise HERUISTIC_WEIGHT = 1 @@ -50,12 +53,9 @@ def theta_star(start:node, goal:point, grid): for neighbor in s.neighbors: if neighbor in closedSet: continue - #if neighbor.x >= len(grid) or neighbor.y >= len(grid[0]): - # continue - if True:# grid[neighbor.x-1][neighbor.y-1] == 0: - update_vertex(s, neighbor, goal, grid) - if neighbor not in openSet and neighbor.parent != None: - openSet.append(neighbor) + update_vertex(s, neighbor, goal, grid) + if neighbor not in openSet and neighbor.parent != None: + openSet.append(neighbor) return None @@ -66,7 +66,7 @@ def theta_star(start:node, goal:point, grid): from cubicSpline import interpolate_xy WIDTH = 50 HEIGHT = 50 - FILL_PCT = 0.15 + FILL_PCT = 0.3 start = node(0, 0) goal = node(WIDTH-1, HEIGHT-1) grid = [] @@ -92,7 +92,10 @@ def theta_star(start:node, goal:point, grid): grid[0][0] = 0 grid[HEIGHT-1][WIDTH-1] = 0 + start_time = time.perf_counter() path = theta_star(start, goal, grid) + end_time = time.perf_counter() + print("Time taken: ", (end_time - start_time) * 1000) print(path) splinex = [] spliney = [] From 93261cff5fe439b0292179f5cf4fd13030597841 Mon Sep 17 00:00:00 2001 From: Mineinjava <65673396+Mineinjava@users.noreply.github.com> Date: Sun, 31 Mar 2024 12:41:13 -0700 Subject: [PATCH 02/13] tune noise --- pathPlanner/theta_star.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/pathPlanner/theta_star.py b/pathPlanner/theta_star.py index 42e2e8c..80e9a18 100644 --- a/pathPlanner/theta_star.py +++ b/pathPlanner/theta_star.py @@ -6,6 +6,7 @@ import bresenhams from time import perf_counter import traceback +from queue import PriorityQueue from perlin_noise import PerlinNoise HERUISTIC_WEIGHT = 1 @@ -29,7 +30,7 @@ def update_vertex(s:node, neighbor:node, goal:node, grid): neighbor.set_shortestDist(s.parent.shortestDist + s.parent.distance(neighbor)) neighbor.set_heuristic_distance(goal) else: - if neighbor.line_of_sight(s, grid): + if True: #neighbor.line_of_sight(s, grid): if s.shortestDist + s.distance(neighbor) < neighbor.shortestDist: neighbor.parent = s neighbor.set_shortestDist(s.shortestDist + s.distance(neighbor)) From f14e93549eff9397580b2fc856a1682be536904c Mon Sep 17 00:00:00 2001 From: Mineinjava <65673396+Mineinjava@users.noreply.github.com> Date: Sun, 31 Mar 2024 12:43:35 -0700 Subject: [PATCH 03/13] found it --- pathPlanner/theta_star.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pathPlanner/theta_star.py b/pathPlanner/theta_star.py index 80e9a18..7b6248c 100644 --- a/pathPlanner/theta_star.py +++ b/pathPlanner/theta_star.py @@ -30,7 +30,7 @@ def update_vertex(s:node, neighbor:node, goal:node, grid): neighbor.set_shortestDist(s.parent.shortestDist + s.parent.distance(neighbor)) neighbor.set_heuristic_distance(goal) else: - if True: #neighbor.line_of_sight(s, grid): + if neighbor.line_of_sight(s, grid): if s.shortestDist + s.distance(neighbor) < neighbor.shortestDist: neighbor.parent = s neighbor.set_shortestDist(s.shortestDist + s.distance(neighbor)) From b1f9ce5617109c92b792dd88002e0a8e707489be Mon Sep 17 00:00:00 2001 From: Mineinjava <65673396+Mineinjava@users.noreply.github.com> Date: Sun, 31 Mar 2024 12:57:49 -0700 Subject: [PATCH 04/13] small optimization --- pathPlanner/__pycache__/node.cpython-39.pyc | Bin 1761 -> 1761 bytes pathPlanner/__pycache__/point.cpython-39.pyc | Bin 2793 -> 2969 bytes pathPlanner/point.py | 8 ++++++++ 3 files changed, 8 insertions(+) diff --git a/pathPlanner/__pycache__/node.cpython-39.pyc b/pathPlanner/__pycache__/node.cpython-39.pyc index b9ee0228dc35d006e9fb8ea5de3f53e6fc862372..251e8a367847a983dfbf94ff7a560eb21934fc7c 100644 GIT binary patch delta 19 ZcmaFJ`;eC_k(ZZ?0SJosZ{)hp1^_e*1vUTx delta 19 ZcmaFJ`;eC_k(ZZ?0SK1v+Q@aC4FEMB1!DjJ diff --git a/pathPlanner/__pycache__/point.cpython-39.pyc b/pathPlanner/__pycache__/point.cpython-39.pyc index c346e4ef92016e6fdf41b1d3c008d2be412e9ce2..71d32931f7257bdf8022e390036b35e4b922dd9e 100644 GIT binary patch delta 517 zcmYk2KT88a5XEP9_x6&5gn)lS8VQP`RUpP7f?|yn5{o&&&s;BhbRK<<=cFhh z736Hohbp+L28|*U{t@`1DqNL=DZcKr> zEd_4Ns)@#nFHDq0K8i)wyf?Gi)vkR7XR@Pw=vZC$&FqQ-Tw+uDvZH|D+dMw4#&M;7NIzftRoUiDw14JlrUYjR-_t5{#s zDGgz1C`M$F>PXb+0)1)aP+E)tnh;H?Mu!J|WmQYXrcJ#oNBWJIOg&_V`IJ<`a_!o|7-# zYc{$^{Hb>)m68BfpC^9{7E1 ix}?Cf!85k_J$S?pcft=8cssLXO6gI>xlEDIGsQm;wNAtU diff --git a/pathPlanner/point.py b/pathPlanner/point.py index 3aff454..38100e4 100644 --- a/pathPlanner/point.py +++ b/pathPlanner/point.py @@ -21,6 +21,14 @@ def line_of_sight(self, other: 'point2d', grid) -> bool: return True if self is None or other is None: return False + + if self.x < 0 or self.y < 0 or self.x >= grid.shape[0] or self.y >= grid.shape[1]: + return False + if other.x < 0 or other.y < 0 or other.x >= grid.shape[0] or other.y >= grid.shape[1]: + return False + if grid[self.x][self.y] == 1 or grid[other.x][other.y] == 1: + return False + intersected_points = [point2d(x, y) for x, y in bresenhams.supercover(self.as_tuple(), other.as_tuple())] From d2209d06d4bf1fa5f9093e14a953e786c3b02087 Mon Sep 17 00:00:00 2001 From: Mineinjava <65673396+Mineinjava@users.noreply.github.com> Date: Sun, 31 Mar 2024 13:02:26 -0700 Subject: [PATCH 05/13] use line instead of supercover --- pathPlanner/__pycache__/point.cpython-39.pyc | Bin 2969 -> 2967 bytes pathPlanner/point.py | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/pathPlanner/__pycache__/point.cpython-39.pyc b/pathPlanner/__pycache__/point.cpython-39.pyc index 71d32931f7257bdf8022e390036b35e4b922dd9e..f9261e55cc73cff6ffca493222e4a9b47bf8b5e0 100644 GIT binary patch delta 37 rcmbO!K3$wQk(ZZ?0SFima;8;mt$8|seB5# delta 39 tcmbO(K2w}Gk(ZZ?0SJ^2aHdsl bool: return False intersected_points = [point2d(x, y) for x, y in - bresenhams.supercover(self.as_tuple(), + bresenhams.get_line(self.as_tuple(), other.as_tuple())] intersected_points.append(other) intersected_points.append(self) From ae17e55630b8ffc1b48c34ba2bfbd4c5d70df0d9 Mon Sep 17 00:00:00 2001 From: Mineinjava <65673396+Mineinjava@users.noreply.github.com> Date: Sun, 31 Mar 2024 13:44:07 -0700 Subject: [PATCH 06/13] add jump --- .../__pycache__/bresenhams.cpython-39.pyc | Bin 3809 -> 3877 bytes pathPlanner/__pycache__/node.cpython-39.pyc | Bin 1761 -> 1795 bytes pathPlanner/__pycache__/point.cpython-39.pyc | Bin 2967 -> 3001 bytes pathPlanner/bresenhams.py | 3 +++ pathPlanner/node.py | 17 ++++++++++++++++- pathPlanner/point.py | 2 +- pathPlanner/theta_star.py | 3 +++ 7 files changed, 23 insertions(+), 2 deletions(-) diff --git a/pathPlanner/__pycache__/bresenhams.cpython-39.pyc b/pathPlanner/__pycache__/bresenhams.cpython-39.pyc index 0a7a00de302b3f7ef166b9c2be1c5f91a9a8f415..8a1e255e2136eabc0a91f7f6a373c8a76d4c88ca 100644 GIT binary patch delta 1270 zcma)5%}*0S6yKTM?Z>v;Z`uL^N+hsiD&j{3k&_oaa1j&Tm~58LP>Q>4Gut*b9ymbs zM55WF7d#l{%$0w@lg68K<48RE5BO$E3KC7&&Hm=io8NoyH*aQ-#y*Xku4x(yJa50o z*Sqh{v8}sEQHVm2r})T5NTG;ec-37@G@|b)+sMWwLZUm0t&$jt!>AF1Bw*BOgd|_$ zcZkhH2GfiBF*}Y#Hq$fg1WDORlD18ne1Ywhou(#?89PfcO;U3k+c~Pzt-hJ3`iZkZ zhiHyuXntD-G4ahJ$&%cTLURv63dx_b3uK5CNoiL<;g#iDUh;;|*dt{0FWx1&R*<|g z=gefBOq?as6wTByP19@*lW}sHT-nt`slC!i1$JpZQLM>h(Mt<)t+82euUgoSt-C?n zciS6Wue#L@da!~%V9ZeXOMQdNA=92FR0s(cMPMrYp)H`1Fsc9C(qO(~>0I^Q9*e+) z#Q?a@nwnVt+krLO^Nf&6r7ZRw1A9&B7hz7Xrl%KP@SVTD+&LQfbsx_!Qz@n zS?d8o676G-D|3BC=Ne%wdG(-OZThX{&%pc*pTa8P zQR3d5Xmel3^?@~z#VX(v5$py4M-?^|K1#06%nqpk3;H61pu(LBo4X)e&`RNqxp0B} z`-A*z7s%gdLkLGrc@;UTK}Tm$c zB8j?3FPMl4(Tm~i-{6_OdE~0`?#z^wAJNEe_M30te($|+US{_Of0XR1ZJUH&%a4BI z-M4mWbC41O1n4$#2`HfQm4XI3UloWzl&>mkU_4dcP&NQDFzb3F;zlWv+;C%HxhBM2 z3(aN4jk`8ld`-AXBxs_wrMM}i$1IKNG0R{drohI{^p;8qM%S{CfaEs8)IG9AA$5wU zAp?Do-O-PAIr%Lub@@}g9|~u5m*lsM)D4{CMYw#T8(GU@91}GKZA{h_C_)JacQnyz zH}`??U78ETDIdI|?-k{U1Xs0}G#`A{mMlIu;?Ia#+JU7{(?W1xKim}2c}EY`MzzB< z{=y<0LY;Z*O<+;Zow%#DJbzN+6tjdTF2EL$5Rm2IM(WtCwV#A^HKc8p6Y)L)`5yFh z7HV~5En&mYaL5wXpa#|G0FCptz;9LxzDB-3R7IL_=V)C^Dz7gMg%Rwuu?ddaZEWyY zR&BL?Runns`XyJq4P+JEz-Fc4F?n&u`)KU<$d}o-a zghr*(sIE3E73pT4n~<&qILeXu0~ifgv!K??wrqU*_^Gf`t233(VF|n zmB08y*jVt*zIAh-%9(aeT^I4QxonKbL=qe45Yh>DC3q2EyyoHp=sVxAFFPaeXM*7!&#MU5#APM#G6F9{eDFdQr<@PlAW_S$5%F#`(&*R=FuUq#b&#M^vxxO#A?|R-0+U%{&%@c!I z3GO&Y4l@22#3uOGAef@AbyOnGz7dpqCk;11%1+N?@wdB;)~Z^Bft0F*DUDChni=B( zRyL}zh7Zh`5ztNy8d?LuDO7!GBba#*P#DXLtwIDs0+Bo;LUsL%d7D{IJq`0xhS z#C`!x0GI&N8kF74vP9X5&RnP{H)mN+nYd&P=zC$zEq|z2o@y7!h(Rir6a; zJOVRMz$-9E46yMOfG?$`NI3Fe=iL2&y1Q=v-85a(%u3?0XXE_7XU?NqZ*8HpFZr^s z+)2JdP**-xef3DPwO+XR^stLGFi?ZPqOz5NP*2|=S+3w?)fA!1&+roe#2Z^^C9OEe-*5#P{0pzwQ}47W73Wnjdw%Q+M4&X+M1Q*Rl zL|KA@1YM5K#y8cQm7h)ufLIn{qrUs^4rrCXs3*6uJX!)NgJ6(ZA&Jt6qA*cH(OpVO z^fry!gvQsj3VG9UywGLLBVp(`BTbA00yafq2t`~5OVQD4CNGOmD_R((xkFpyk6NXc YK1K+?yVkd_BEJ)o9RwI)<&cHH0Ta)2umAu6 diff --git a/pathPlanner/__pycache__/point.cpython-39.pyc b/pathPlanner/__pycache__/point.cpython-39.pyc index f9261e55cc73cff6ffca493222e4a9b47bf8b5e0..a77c2b0df8df094f7ba6d1bf41e12e1c12048e20 100644 GIT binary patch delta 863 zcmZuv&ui2`6wV|w+1Vdyx7J#S+OA^RW-F^+JQcx%iU=ymf-Hi~cCy<@Hi?r&tOz3Y zD8f33|A3c0ieAJ&z?1*LT)gT*Jb8CsBJPixz&CH+eDA&Q&6~HYpH?l`vMQST@RxG` z)yO)DE{?X2UuGIJ0z4#Lg0yQI!;HMp7&*|pCe!a|j`;&Z99_~|aq35*8%WX#m2uNS z5{D1`bd>zEP2d<3CtlF!CAHyH!C2!}@f6)gCGi!#X;s0cbI`yS0Kic|O<9sMKS|wi z*Q<(`dJ`>*k9xaR1`ebjqat!X$W892SsZwaA8N*TgqFl_yxjzngE)c3kb3+4MAURu zahF^*3%TMQIkmE)_|kOa*bA9M{wfucY*eSJii-K!{?{Wvs$ynxelrL5Iyg@Q96H`9 zJzoc1Q;LqvrFcy{C!lZDK@*PV&&5O9Y32lV<$G?jHC1Ya}Dl0kR3g;1biZMyzL(EKj@5=|9TcGMJC^U*oYAaikM{baLGvs@f z!7~fUMo!2FC2^qU%$LEPUpVeXQNY(=e+FO!Aa8sVtQNpIfC~VVN#qv1oP5skMv`L+ zD5_%{FX;6(YN&sgu0tTm;YsI((PfyvcS89 N6fJ>@bwp9k_ycc*pY{L% delta 808 zcmZuvKTq306!-b=%-PgYC{RQwX%VHVg9v;8)UBW{1qo&8QW2oNjL z39)v~+@)2g4wd)}w{>fmcIX%2-noK=)Runt?!Djpe_y7qrd;239YcNSo4vc=b!Sn% zyEwD7ZUi_Wr$#_d%(o;kw+xT|0-9$^wvp!TB=#doo3Rq!bkF}(DA=$hSs3kdOI`Sg zf}X)0ae+3GC4QjGIwb0u+y=7?0Ez-el_Xiv&T>EA2_5m#tf7*)HXC&Y90=b-h39=I zKYN#V(kLu_dWwG{R2DW}tpUkHoWOyl*;u(Nsz0lU=$ciOD?Zb?$qB`mmY=3!9C-9! zrQ$bT812g{s@B)aZI66h`3%JTYqcs>P|g7ine&n3Cs0>kMPv6vOt9u-Xk4}M9Be~` zGuEtWf|~iBpY8Pp8%l`m12vUHduI8zXz9F;gK+;9=Lz?UjY!%C)j%iT?UNUE$iE1+ z;9?$2N|yA4A9cb3a_Y2>9sn6@g5HTFkyiBvYjLmpN0E8z%u~dVhtn@paHM~ za5r=5ZN%g^sy;EuB2WdNQaT6S&EItjOr~jYWlC8c^ Qf=(2^M2eOHt&AAH0f_{UJ^%m! diff --git a/pathPlanner/bresenhams.py b/pathPlanner/bresenhams.py index aeb660d..2eaf04c 100644 --- a/pathPlanner/bresenhams.py +++ b/pathPlanner/bresenhams.py @@ -1,3 +1,6 @@ +from numba import njit + +@njit(fastmath=True, cache=True) def get_line(start, end): """Bresenham's Line Algorithm diff --git a/pathPlanner/node.py b/pathPlanner/node.py index 6c910c4..292ed87 100644 --- a/pathPlanner/node.py +++ b/pathPlanner/node.py @@ -1,5 +1,21 @@ from point import point2d from typing import Optional, Union +from numba import njit + +@njit(fastmath=True, cache=True) +def jump(pos, dx, dy, grid): + """go in a direction until we are near a wall + inspired by JPS but more simple""" + x, y = pos + while True: + if x < 0 or y < 0 or x >= grid.shape[0] or y >= grid.shape[1]: + return (x - dx, y - dy) + # calculate sum of all point around the current point + sum_ = grid[x-1:x+2, y-1:y+2].sum() + if sum_ > 0: + return (x, y) + x += dx + y += dy class node(point2d): def __init__(self, x:float, y:float, shortestDist:Optional[float]=float('inf'), parent:Optional['node']=None): @@ -20,7 +36,6 @@ def set_heuristic_distance(self, goal:point2d): self.heuristic_distance = self.distance(goal) return self.heuristic_distance - def get_neighbors(self, diagonal:bool=True)->list: neighbors:list = [] for i in range(-1, 1+1): diff --git a/pathPlanner/point.py b/pathPlanner/point.py index 77902de..0995875 100644 --- a/pathPlanner/point.py +++ b/pathPlanner/point.py @@ -1,5 +1,6 @@ from typing import Optional, Union import bresenhams +from numba import njit class point2d: @@ -13,7 +14,6 @@ def distance(self, other: Union['point2d', tuple, list]) -> float: def as_tuple(self) -> tuple: return (self.x, self.y) - def line_of_sight(self, other: 'point2d', grid) -> bool: """Returns true if there is a line of sight between self and other. """ diff --git a/pathPlanner/theta_star.py b/pathPlanner/theta_star.py index 7b6248c..000e1e5 100644 --- a/pathPlanner/theta_star.py +++ b/pathPlanner/theta_star.py @@ -3,6 +3,7 @@ import point from node import node import numpy as np +from numba import njit import bresenhams from time import perf_counter import traceback @@ -10,6 +11,7 @@ from perlin_noise import PerlinNoise HERUISTIC_WEIGHT = 1 + def reconstruct_path(goal:node): path = [] current = goal @@ -92,6 +94,7 @@ def theta_star(start:node, goal:point, grid): plt.show() grid[0][0] = 0 grid[HEIGHT-1][WIDTH-1] = 0 + theta_star(start, goal, grid) start_time = time.perf_counter() path = theta_star(start, goal, grid) From b4c62633ec11b4e33277f54adcba86a0d1efa67b Mon Sep 17 00:00:00 2001 From: Mineinjava <65673396+Mineinjava@users.noreply.github.com> Date: Sun, 31 Mar 2024 13:44:51 -0700 Subject: [PATCH 07/13] update gitignore --- .gitignore | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.gitignore b/.gitignore index be592b5..016aca3 100644 --- a/.gitignore +++ b/.gitignore @@ -14,6 +14,9 @@ build # Ignore the Visual Studio Code workspace directory .vscode/ +# ignore pycache files +__pycache__/ + # Ignore local.properties file bin/ local.properties From ee37d9e8956d859550409ef5eec1a82be403a9d8 Mon Sep 17 00:00:00 2001 From: Mineinjava <65673396+Mineinjava@users.noreply.github.com> Date: Sun, 31 Mar 2024 14:26:39 -0700 Subject: [PATCH 08/13] JPS WORKS!!! --- pathPlanner/__pycache__/node.cpython-39.pyc | Bin 1795 -> 3129 bytes pathPlanner/node.py | 44 ++++++++++++++------ pathPlanner/theta_star.py | 6 +-- 3 files changed, 35 insertions(+), 15 deletions(-) diff --git a/pathPlanner/__pycache__/node.cpython-39.pyc b/pathPlanner/__pycache__/node.cpython-39.pyc index d303af037a1b26224066159eb22cf6dcb3c0e125..eacd10f11010054908b92e60f89165cc147138a9 100644 GIT binary patch literal 3129 zcmb7G&2J<}6|buPnDKaKJofrCyM*Q|Fj2BzA_M}hRuQ{^guq1FghMM)t36d?_qeCK zUDd;`t)5H9QF2)%?ki*;_lgjI38%ht!4fxaT;TU=JhnG`qFVLp`_+5(-uu0dJDnCo zdGPn8;UAV6`v(nf7Y~CEQO&6FOrzbnVzly|EwqoGD+r_`DZF z`m%AxW;|^Cipi#2I%9H4@tG4g)sk!-3H?v-Sx#HwvTfVrPT0lyKZcj{Fx=9&3Tl|O^xm5*B-{bz9UEC+DhVpEJNfz~??q=zT*PxlV; zAjyI_kcn0UdW&F^l}Q>rQ$eg%kf~T>^;w*ztpFv-Oo5#;*gpw={n684e^LhH92-qC zE>fjAp5k;vJ`dtd2J$2r#95Y?!M+L}T+-U50NLxib?f(WI#IvSI@h&l4&y@Aj+u;W zuh1|~zfn6yZfYT)qn_06flg%2pV#~(bg?aZ#q>Hj{pZ8oPmI!LcO2`%#2m$w;cy&h zyHAukD)VBu(D?{zncd?_oTR%2tocz2k(A!ea;feYU_F|Q3o@nI1YvHEH+fUM$9v)j zyhVL{nwAE%e7pEp1M@jLiiKRVP$)<88JA*+9zIvWrplFUhe_w9`z0H43Z%16=)vjz zms~bv^UMu>oLj=EC6_U3AQ0LZbz~Q#CApzmYFYMP3b`uR&b+X#I`WQOw}IIGlDRBg zAqR)=LaEwGvLT*WuR$z*2c&MB!(5ljlur^<)_wuYX61mCUKHw+e>0-<}dh7 ztQgCjpRU-OZ`N=(Df<82{XLvm+)bzF*IEHc?Yc+&Z`< z2wB=b!aZ~CgE|3@THxp0TXVuiZHA<&G zAtzA!N5Dt4CMXe&z(jxM05jclQ956-5oWd+TFh@DEnOh-ls?VV+z{wn`IVTt(?->} znFZPgm2)nt2JJr%j@N9gd(uB^Rt}}BsZ%y82iQOo{H1cIh$KX9WAF06D0&Gh%;D2b z-U~(pLd!)VAES&z!ED|EIe^7Jdwa@D$~(XoP-GHlgPijfo4J1y$1Zp~mZ7F+(iAQ* z+7_DH8>V?&BFBmQ59;P365+A|?Y!}~*25H`JIKeyqn}{q->B^L9c#7&n0Rr}+<%zn zgE%#h&{-VlH)~JF*@4m>oT~k}C{!lx{kI~WfVFl~mGztR%L#k*D$OokvkzDH>w?c+3!;wWB#WZGzPKcG z);}jgE_g}OT@nCXlC#4ZuZ{iMLzBRiiyosIa$D=|y(zNM2%bLTLoxSnYjv=>PITzs z$A0`o&>!B{U`hWFOTR_c9}uAt{a3^%1d)SrT=ehhU(yQQU;E`rkz@z+7bR(GFPn__ zV@;XT{twJu`DcqI{7Q(ds8{jFSfqKGCj0oipdRw=ymgASti4mEb2ImF4{1@C+CnIK z33ig)W*#xlX;ReUIevmpOxQ4kxGa%i{bUSye}!d&nblSVMl$CU+KGp8W0j-BT*3~pbA3PC3iLs)}6C=1rcXa zn1&K<3lfy+pyVGQQPOQo6$wTB0$|3*A+y@IJ2P+QcHVtinUq_8spK-WmtURUJIUA& za?YAS=Qi%-=G#_l`5p^IAYZdUhA^>qa)JGbc{yE&mVRP=vJJd2aHyBWn_C5}QPkSF z-6$ONpY6sf@uU$+IP9rBWt}-i5%~y=)9vmJ{UqsjkMs%e*s6?iTmKYIDCh;bRV`A* zGJ?m|0^JMxfn0-!McnCY;yr z?K`hHGUkS=FnVtXchW=XvoTLu00E!yyX@)1v3164k40t$)^o0|W@2_8)H5p+7-?sa zv=8d&^AC{njO|;QRbzOHOw`zfe*|EdFYja=brsvkf6$KUF$xe+KB@eu9~#>qjlw8U zHl%eE)g^kuISIsI)}&lyR*?YvNw$VR9TdzpQmaB7YL?P)bVD-ZX6Gd7^42u z?j}D{@hDX3p=#*s?&>vq+3X86AL*Y^vpj6($`&qJt#FvmzU}3W9L0ku)ty395M7@X M8ZKRmI8~AV1q`{96#xJL diff --git a/pathPlanner/node.py b/pathPlanner/node.py index 292ed87..6116847 100644 --- a/pathPlanner/node.py +++ b/pathPlanner/node.py @@ -6,16 +6,19 @@ def jump(pos, dx, dy, grid): """go in a direction until we are near a wall inspired by JPS but more simple""" + if dx == 0 and dy == 0: + raise ValueError("dx and dy cannot be 0") x, y = pos while True: if x < 0 or y < 0 or x >= grid.shape[0] or y >= grid.shape[1]: return (x - dx, y - dy) # calculate sum of all point around the current point sum_ = grid[x-1:x+2, y-1:y+2].sum() - if sum_ > 0: + print(pos, sum_) + if sum_ != 0: return (x, y) - x += dx - y += dy + x = x + dx + y = y + dy class node(point2d): def __init__(self, x:float, y:float, shortestDist:Optional[float]=float('inf'), parent:Optional['node']=None): @@ -36,20 +39,24 @@ def set_heuristic_distance(self, goal:point2d): self.heuristic_distance = self.distance(goal) return self.heuristic_distance - def get_neighbors(self, diagonal:bool=True)->list: + def get_neighbors(self, diagonal:bool=True, grid=None)->list: neighbors:list = [] + backupneighbors = [] for i in range(-1, 1+1): for j in range(-1, 1+1): - if i == 0 and j == 0: - continue - if (i==j) and not diagonal: - continue - - else: - neighbors.append(node(self.x + i, self.y + j)) + if not (i == 0 and j == 0): + print("i, j", i, j) + backupneighbors.append(node(self.x + i, self.y + j)) + jumppoint = jump((self.x, self.y), i, j, grid) + if jumppoint != (self.x, self.y): + neighbors.append(jumppoint) #neighbors = [neighbor for neighbor in neighbors if neighbor.x >= 0 and neighbor.y >= 0] - + neighbors = [node(neighbor[0], neighbor[1], float('inf'), self) for neighbor in neighbors] + if len(neighbors) == 0: + neighbors = backupneighbors + # make sure neighbors are within the grid + # neighbors = [neighbor for neighbor in neighbors if neighbor.x >= 0 and neighbor.y >= 0 and neighbor.x < grid.shape[0] and neighbor.y < grid.shape[1]] return neighbors @property @@ -58,3 +65,16 @@ def neighbors(self): self._neighbors = self.get_neighbors() return self._neighbors +if __name__ == "__main__": + import numpy as np + import matplotlib.pyplot as plt + grid = np.zeros((10, 10)) + grid[3:6, 3:6] = 1 + start = node(0, 0) + goal = node(9, 9) + points = start.get_neighbors(diagonal=True, grid=grid) + print(points) + xs, ys = zip(*[(point.x, point.y) for point in points]) + plt.scatter(xs, ys) + plt.imshow(grid, cmap='gray') + plt.show() \ No newline at end of file diff --git a/pathPlanner/theta_star.py b/pathPlanner/theta_star.py index 000e1e5..44d5219 100644 --- a/pathPlanner/theta_star.py +++ b/pathPlanner/theta_star.py @@ -53,7 +53,7 @@ def theta_star(start:node, goal:point, grid): if s == goal: return reconstruct_path(s) closedSet.append(s) - for neighbor in s.neighbors: + for neighbor in s.get_neighbors(diagonal=True, grid=grid): if neighbor in closedSet: continue update_vertex(s, neighbor, goal, grid) @@ -69,7 +69,7 @@ def theta_star(start:node, goal:point, grid): from cubicSpline import interpolate_xy WIDTH = 50 HEIGHT = 50 - FILL_PCT = 0.3 + FILL_PCT = 0.35 start = node(0, 0) goal = node(WIDTH-1, HEIGHT-1) grid = [] @@ -81,7 +81,7 @@ def theta_star(start:node, goal:point, grid): grid.append(arr) ''' - noise = PerlinNoise(octaves=10) + noise = PerlinNoise(octaves=5) xpix, ypix = WIDTH, HEIGHT pic = [[noise([i/xpix, j/ypix]) for j in range(xpix)] for i in range(ypix)] From 0862dd73bb13926d333e1f4dd493d95c6de2047d Mon Sep 17 00:00:00 2001 From: Mineinjava <65673396+Mineinjava@users.noreply.github.com> Date: Sun, 31 Mar 2024 14:26:56 -0700 Subject: [PATCH 09/13] JPS WORKS!!! --- pathPlanner/node.py | 1 - 1 file changed, 1 deletion(-) diff --git a/pathPlanner/node.py b/pathPlanner/node.py index 6116847..ec20e78 100644 --- a/pathPlanner/node.py +++ b/pathPlanner/node.py @@ -14,7 +14,6 @@ def jump(pos, dx, dy, grid): return (x - dx, y - dy) # calculate sum of all point around the current point sum_ = grid[x-1:x+2, y-1:y+2].sum() - print(pos, sum_) if sum_ != 0: return (x, y) x = x + dx From 4591025d642596a8adb66e0f3fa21bfabca28dfa Mon Sep 17 00:00:00 2001 From: Mineinjava <65673396+Mineinjava@users.noreply.github.com> Date: Sun, 31 Mar 2024 14:52:08 -0700 Subject: [PATCH 10/13] its fast --- pathPlanner/__pycache__/node.cpython-39.pyc | Bin 3129 -> 3087 bytes pathPlanner/node.py | 1 - 2 files changed, 1 deletion(-) diff --git a/pathPlanner/__pycache__/node.cpython-39.pyc b/pathPlanner/__pycache__/node.cpython-39.pyc index eacd10f11010054908b92e60f89165cc147138a9..7b62055c99f7525551bcf659ab140222ccf34b89 100644 GIT binary patch delta 1177 zcmZ8g&1=*^6wk~g+3c=cW2^4AYF%~JMqBNIwSE+_&>lo6et?K5G2LXhwP_M3sZhuw zvVtB2597g$P%nbs3cdIjh~B--e^Bt?e^B4ccH3fx{AT8T{NBuaZ@yRFS4vK)R8ZiX z{+(}5td|xD`6gbFH%6+vWQp+)Vn6*j$YkMxpf0r7|4 z89In8s!0rD&{1O25nRFGhr~m+p&tM(?-%{*ySYJJRsdwd4!p(I!hFbMOOtBs`Ayz6 z?3r*h{z_!twi~_^$F|J}P>0d@QGl}eqt?ivIG~NzhfsJDz@q#(uCqv){f?6?#9m@Y z4)=m&SOnTIsi5y<=3Cq2iwEq++VwTjH(m&N;>F3#Vw{{1e>HaueIevAn55x?*a;e5 z?+j=D;}Ng)Bcvui>5pE11650DGx*g+afwSibVhj+wY5FFr)@peT((5{C8!C^aWz%f zNJ8H#&seIJ6`0jjFDo7TnGhw>P>!-DO|?`{X-946(%h~Ts#;Ci8cC;f=_#fiJr0!$-;g~)iz7neZ|t}Hn-s8dAt$<5I=KwZwQ!nibg#r;aU<=9PRVakUzHNxfS#)Noe)F*RN<4(}>r0ztK7q}VSo?bYF?9BH9 z%jj}hAmFC>Vbtpx?M2*USIANeiqs1oqg{*zQiuwp4~8ku=O3TjF4{0?_!WeVA>-OP zC!YYtlvvN-dO3bA#N%!12v{Mg z$##-93fJyoAdu|5E?AF&NNQ2I7$pBSIv)qsZTNW|0e4buac9*cQf(|dk#&^Mikro= zMXXD=R#yctUapyFs=*833C}toQ!Di>NDRR^1QWlCr_J3&ce5?FipQC0j*1ieD*piA Ck^ou& delta 1162 zcmZ8hTTc@~6yCY)r7dj=mbOqX1-V#44fO)i5FQMY_@EJa^OB}?mr^Kgnca|Jw&9@~ zj4?50UVKmji7!5C2#NngA6(-P;72H)vy_YMWWJg6oy+W;b9Ub*za_(ka5$*IGx0sJ zwDBdpOvt|6Aln@da}`%>QUYj_RVH?%O5YPMcW6@g)Ve5F9eF^9NKF2s4}0PmgGY%$ z3_4Dt^a=^$9)^g^yKG*kJuZn8@FBZ9Zyaiv%ZXgn$N@w(6*d>y&$Zr(=VT>eew^_PWhh0=RCX6 z{eou{J?-V1a4g%IE!)nZ{H>Z<^o0ZaVDStS=t8w+tv%j4098q8 zFqqs?Ok&a|omHN>4fUKK+<8g(+)AYt3n|d6oT(khrLo(fJrE@0-sQt zbdM0l(U3=OiK4wro4ldBdW&5E8fut!*)DZ;bXZTXcWf(5#8ge~h2gSUrAZph(OuRg z{wsWSd4suZLCNS34v*jw0lXdoc+Sh?=@t1&pEr^q!u1pKkKUU~qrti4Pgfv8#oAhZ zW=h^Pa?^&VibAzyi7*&?TA^OIs-{Sw*I6TACwZ)5RWrtMUB^)0EwfGBl(8ke za0zNuwaTUC#hS3u&h7zF2!|1Y5hZQ%$ap%|s#-tj#4Q9r#{U%;lc1Q9F9Y|svS`8( zu<67Q!UzII$N+eJu~w^i953ic^o+Ww)h*#{`fbO zV309A!eFQ!F-{32bUqj#xP!(9z#+cPp@x7}fQ+}YvZu%7hv3K~j1BB*8exAGDELro z@S$F4Zjl4keVF1J0!kh)iNa=vc)Yk)sAqb_oQ#D=Ls*RC0^OE#p~=B^G*zMNg(LiK t$836list: for i in range(-1, 1+1): for j in range(-1, 1+1): if not (i == 0 and j == 0): - print("i, j", i, j) backupneighbors.append(node(self.x + i, self.y + j)) jumppoint = jump((self.x, self.y), i, j, grid) if jumppoint != (self.x, self.y): From 3cabfae52e187e7b0fd2f657f56bf47aa60671ae Mon Sep 17 00:00:00 2001 From: Mineinjava <65673396+Mineinjava@users.noreply.github.com> Date: Wed, 1 May 2024 23:08:21 -0700 Subject: [PATCH 11/13] kind of works but not really --- pathPlanner/theta_star_fast.py | 328 +++++++++++++++++++++++++++++++++ 1 file changed, 328 insertions(+) create mode 100644 pathPlanner/theta_star_fast.py diff --git a/pathPlanner/theta_star_fast.py b/pathPlanner/theta_star_fast.py new file mode 100644 index 0000000..29342a7 --- /dev/null +++ b/pathPlanner/theta_star_fast.py @@ -0,0 +1,328 @@ +# Copyright (C) Marcus Kauffman 2023-Present + +# This work would not have been possible without the work of many +# contributors, most notably Colin Montigel. See ACKNOWLEDGEMENT.md for +# more details. + +# This file is part of Quail. + +# Quail is free software: you can redistribute it and/or modify it under +# the terms of the GNU General Public License as published by the Free +# Software Foundation, version 3. + +# Quail is distributed in the hope that it will be useful, but WITHOUT +# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or +# FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License +# for more details. + +# You should have received a copy of the GNU General Public License +# along with Quail. If not, see . + +""" +self-contained theta* implementation, optimized for speed +Goal: get it fast enough to be run on every loop cycle. +""" +import copy +# Grid is a 1d tuple of integers. The array represents the x-axis and +# each integer represents the y-axis. + +# Nodes are represented as a list +# [heuristic_distance, (x, y), parent, shortestDist] +# [float, tuple, tuple, float] + +from typing import Tuple, List +from numba import njit +import heapq +from heapq import heappush, heappop + +USE_JUMP = False +GRID_SHAPE = (50, 50) + + +def reconstruct_path(goal: List) -> \ + list[list]: + """Reconstructs the path from the goal node to the start node. + returns a list of nodes on the path""" + path = [] + current = goal + while current[2][1] != current[1]: # the parent is not the current + path.append(current) + current = current[2] + path.append(current) + path.reverse() + return path + + +def get_grid_value(grid: Tuple[int], x: int, y: int) -> int: + """Returns the value of the grid at the given coordinates, or 1 if + out of bounds""" + if x < 0 or y < 0 or x >= GRID_SHAPE[0] or y >= GRID_SHAPE[1]: + return 1 + return (grid[x] >> y) & 1 + + +def get_line(start, end): + """Bresenham's Line Algorithm + Produces a list of tuples from start and end + >>> points1 = get_line((0, 0), (3, 4)) + >>> points2 = get_line((3, 4), (0, 0)) + >>> assert(set(points1) == set(points2)) + >>> print points1 + [(0, 0), (1, 1), (1, 2), (2, 3), (3, 4)] + >>> print points2 + [(3, 4), (2, 3), (1, 2), (1, 1), (0, 0)] + """ + # Setup initial conditions + x1, y1 = start + x2, y2 = end + dx = x2 - x1 + dy = y2 - y1 + + # Determine how steep the line is + is_steep = abs(dy) > abs(dx) + + # Rotate line + if is_steep: + x1, y1 = y1, x1 + x2, y2 = y2, x2 + + # Swap start and end points if necessary and store swap state + swapped = False + if x1 > x2: + x1, x2 = x2, x1 + y1, y2 = y2, y1 + swapped = True + + # Recalculate differentials + dx = x2 - x1 + dy = y2 - y1 + + # Calculate error + error = int(dx / 2.0) + ystep = 1 if y1 < y2 else -1 + + # Iterate over bounding box generating points between start and end + y = y1 + points = [] + for x in range(x1, x2 + 1): + coord = (y, x) if is_steep else (x, y) + points.append(coord) + error -= abs(dy) + if error < 0: + y += ystep + error += dx + + # Reverse the list if the coordinates were swapped + # if swapped: + # we are iterating over all the points so the order + # doesn't matter. + # points.reverse() + return points + + +def line_of_sight(s: List, + neighbor: List, + grid: Tuple[int]) -> bool: + """Returns true if there is a line of sight between self and other. + """ + if s[1] == neighbor[1]: + return True + if s is None or neighbor is None: + return False + intersected_points: List[Tuple[int, int]] = get_line(s[1], + neighbor[1]) + intersected_points.append(neighbor[1]) + intersected_points.append(s[1]) + + for point in intersected_points: + if point[0] < 0 or point[1] < 0 or point[0] >= GRID_SHAPE[0] \ + or point[1] >= GRID_SHAPE[1]: + print("F") + return False + elif get_grid_value(grid, point[0], point[1]) == 1: + print("F") + return False + + return True + + +def point_distance(s: Tuple[int, int], neighbor: Tuple[int, int]) \ + -> float: + return ((s[0] - neighbor[0]) ** 2 + ( + s[1] - neighbor[1]) ** 2) ** 0.5 + + +def node_distance(s: List, + neighbor: List) -> float: + return ((s[1][0] - neighbor[1][0]) ** 2 + ( + s[1][1] - neighbor[1][1]) ** 2) ** 0.5 + + +def heuristic_distance(s: List, + goal: List, + shortest_distance: float) -> float: + """Returns the heuristic distance to the goal. + distance is the distance from the start node to s.""" + if shortest_distance is None: + return node_distance(s, goal) + return node_distance(s, goal) + shortest_distance + + +def update_vertex(s: List, + neighbor: List, + goal: List, + grid: Tuple[int]): + if line_of_sight(s[2], neighbor, grid): # if line of sight + # between parent of s and neighbor + if (s[3] + node_distance(s[2], neighbor)) < neighbor[3]: + # if the shortest known distance to s + distance to neighbor + # is less than the shortest known distance to neighbor + neighbor[0] = heuristic_distance(neighbor, goal, s[3]) + neighbor[2] = s[2], # parent of neighbor is parent of s + neighbor[3] = s[2][3] + node_distance(s[2], neighbor) + # shortest distance to neighbor is shortest + # distance to s + distance to neighbor + + else: + if line_of_sight(s, neighbor, grid): # if line of sight between + # s and neighbor + if s[3] + node_distance(s, neighbor) < neighbor[3]: + # if the shortest known distance to s + distance + # to neighbor is less than the shortest known distance to neighbor + neighbor[0] = heuristic_distance(neighbor, goal, s[3]) + neighbor[2] = s, # parent of neighbor is s + neighbor[3] = s[3] + node_distance(s, neighbor) + + +def jump(pos: Tuple[int, int], dx: int, dy: int, grid: Tuple[int]) \ + -> Tuple[int, int]: + """go in a direction until we are near a wall + inspired by JPS but simple""" + if dx == 0 and dy == 0: + raise ValueError("dx and dy cannot be 0") + x, y = pos + while True: + if x < 0 or y < 0 or x >= GRID_SHAPE[0] or y >= GRID_SHAPE[1]: + return (x - dx, y - dy) + # calculate sum of all point around the current point + sum_ = 0 + for i in range(-1, 1 + 1): + for j in range(-1, 1 + 1): + sum_ += get_grid_value(grid, x + i, y + j) + if sum_ != 0: + print("sum") + return (x-dx, y-dy) + x = x + dx + y = y + dy + + +def get_neighbors(s: List, + grid: Tuple[int], + goal: List) \ + -> List[List]: + neighbors: List[List] = [] + backup_neighbors: List[List] = [] + + for i in range(-1, 2): + for j in range(-1, 2): + if not (i == 0 and j == 0): + backup_neighbors.append([None, + (s[1][0] + i, s[1][1] + j), + s, + None]) + jumppoint = jump(s[1], i, j, grid) + if jumppoint != s[1]: + neighbors.append([None, jumppoint, s, None]) + + if len(neighbors) == 0: + neighbors = backup_neighbors + + new_neighbors = [] + for neighbor in neighbors: + if neighbor[1][0] < 0 or neighbor[1][1] < 0 or neighbor[1][0] >= \ + GRID_SHAPE[0] or neighbor[1][1] >= GRID_SHAPE[1]: + continue + elif get_grid_value(grid, neighbor[1][0], neighbor[1][1]) == 1: + continue + new_neighbors.append([heuristic_distance(neighbor, goal, s[3]), + neighbor[1], # keep pos the same + neighbor[2], # keep parent the same + s[3] + node_distance(s, neighbor)]) + + return new_neighbors + + +def theta_star(start_pose: Tuple[int, int], goal_pose: Tuple[int, int], + grid: Tuple[int]) -> list[tuple]: + start = [0, start_pose, None, 0] + goal_node = [0, goal_pose, None, 0] + start[2] = copy.deepcopy(start) + # the start node is the first node in the open set, so it will be + # added to the closed set first, so [0] and [1] don't really matter + + open_set: List[List] = [start] + + closed_set = [] + + while len(open_set) > 0: + s = heappop(open_set) + if s[1] == goal_pose: + return reconstruct_path(s) + closed_set.append(s) + for neighbor in get_neighbors(s, grid=grid, goal=goal_node): + if neighbor in closed_set: + continue + update_vertex(s, neighbor, goal_node, grid) + if neighbor not in open_set and neighbor[2] is not None: + heappush(open_set, neighbor) + return None + + +if __name__ == "__main__": + import matplotlib.pyplot + import numpy as np + from perlin_noise import PerlinNoise + + FILL_PCT = 0.1 + SWAP_XY = True + start = (0, 0) + goal = (GRID_SHAPE[0] - 1, GRID_SHAPE[1] - 1) + grid = [] + + noise = PerlinNoise(octaves=3, seed=999) + xpix, ypix = GRID_SHAPE + pic = [[noise([i / xpix, j / ypix]) for j in range(xpix)] for i in + range(ypix)] + grid = np.array(pic) + grid = np.where(grid < FILL_PCT, 0, 1) + + pathing_grid = np.array([0 for x in range(GRID_SHAPE[0])]) + + # convert the grid to a 1d tuple + for x in range(GRID_SHAPE[0]): + for y in range(GRID_SHAPE[1]): + pathing_grid[x] = pathing_grid[x] + (grid[x][y] << -y) + + import time + + theta_star(start, goal, tuple(pathing_grid)) # precompile for njit + startT = time.perf_counter_ns() + path = theta_star(start, goal, tuple(pathing_grid)) + endT = time.perf_counter_ns() + + print("Time taken: ", (endT - startT) / 1e6) + + xs, ys = [], [] + + import matplotlib.pyplot as plt + + for point in path: + xs.append(point[1][0]) + ys.append(point[1][1]) + + plt.plot(xs, ys, 'ro-') + + plt.imshow(grid, cmap='gray', interpolation='none', origin='lower') + plt.show() + + print(path) From 47f0e4fb550fe30801c8a676eb4eb9b52a0c9816 Mon Sep 17 00:00:00 2001 From: Mineinjava <65673396+Mineinjava@users.noreply.github.com> Date: Fri, 3 May 2024 09:25:29 -0700 Subject: [PATCH 12/13] still doesn't work --- .../__pycache__/bresenhams.cpython-39.pyc | Bin 3877 -> 3877 bytes .../__pycache__/cubicSpline.cpython-39.pyc | Bin 2416 -> 2416 bytes pathPlanner/__pycache__/node.cpython-39.pyc | Bin 3087 -> 3087 bytes pathPlanner/__pycache__/point.cpython-39.pyc | Bin 3001 -> 3001 bytes pathPlanner/theta_star_fast.py | 24 +++++++++--------- 5 files changed, 12 insertions(+), 12 deletions(-) diff --git a/pathPlanner/__pycache__/bresenhams.cpython-39.pyc b/pathPlanner/__pycache__/bresenhams.cpython-39.pyc index 8a1e255e2136eabc0a91f7f6a373c8a76d4c88ca..fc916a243b123d0e17c53b4845e4666199867963 100644 GIT binary patch delta 607 zcmZ1~w^WWhk(ZZ?0SGiE8*St+U}3y9xrRlLQF8KXmYIywn_XGu7&XL!(zn=hQuESF zGHx+u-eN2%k^l-7iGc`7I3YE;n@yEbaq>pCFY+KQQ5>liB}It^CB;SZa7AF{Qj?Fc z8!)PD{>pxk$r*0cEoP8XQIJm7isF*gf+*&clu8g$Q6vHq{D{ zPp)Q0t;x0A$dLTVuV{neBq4Zlh^QR3Lt_R5{8qX@;LwiFT;HV delta 607 zcmZ1~w^WWhk(ZZ?0SMYpa&F`aO9pS+Rni#%8hM`}e$QDQ+!agi)s5m>q8d3{olrR2s!vQCyN*5XGF5QVAj|iiAO;oT)`c`9%dqsb#kq<54XB z$<@rLKDm}VSsG;iN~T*(dIk_TB0KLlwBP@f#a7tSa)c@3YY03w(nVL16Ip928!i+bn) diff --git a/pathPlanner/__pycache__/cubicSpline.cpython-39.pyc b/pathPlanner/__pycache__/cubicSpline.cpython-39.pyc index 38b46d26ceb09e784873d577c0b9cebcc7095bc7..09a325ad200720ca1b1bd49cb40ecada61169450 100644 GIT binary patch delta 21 bcmew$^g)Oxk(ZZ?0SGiE8>MaJN#O(lJwOE$ delta 21 bcmew$^g)Oxk(ZZ?0SIE^KBaEtN#O(lLFfh1 diff --git a/pathPlanner/__pycache__/node.cpython-39.pyc b/pathPlanner/__pycache__/node.cpython-39.pyc index 7b62055c99f7525551bcf659ab140222ccf34b89..558ee6d4b2bc8961041c8e581c8735769a0bd580 100644 GIT binary patch delta 20 ZcmeB|=$GJ5 \ @@ -58,6 +58,7 @@ def get_grid_value(grid: Tuple[int], x: int, y: int) -> int: out of bounds""" if x < 0 or y < 0 or x >= GRID_SHAPE[0] or y >= GRID_SHAPE[1]: return 1 + value = grid[x] >> y & 1 return (grid[x] >> y) & 1 @@ -137,10 +138,8 @@ def line_of_sight(s: List, for point in intersected_points: if point[0] < 0 or point[1] < 0 or point[0] >= GRID_SHAPE[0] \ or point[1] >= GRID_SHAPE[1]: - print("F") return False elif get_grid_value(grid, point[0], point[1]) == 1: - print("F") return False return True @@ -210,7 +209,6 @@ def jump(pos: Tuple[int, int], dx: int, dy: int, grid: Tuple[int]) \ for j in range(-1, 1 + 1): sum_ += get_grid_value(grid, x + i, y + j) if sum_ != 0: - print("sum") return (x-dx, y-dy) x = x + dx y = y + dy @@ -232,7 +230,8 @@ def get_neighbors(s: List, None]) jumppoint = jump(s[1], i, j, grid) if jumppoint != s[1]: - neighbors.append([None, jumppoint, s, None]) + pass + #neighbors.append([None, jumppoint, s, None]) if len(neighbors) == 0: neighbors = backup_neighbors @@ -244,7 +243,8 @@ def get_neighbors(s: List, continue elif get_grid_value(grid, neighbor[1][0], neighbor[1][1]) == 1: continue - new_neighbors.append([heuristic_distance(neighbor, goal, s[3]), + else: + new_neighbors.append([heuristic_distance(neighbor, goal, s[3]), neighbor[1], # keep pos the same neighbor[2], # keep parent the same s[3] + node_distance(s, neighbor)]) @@ -268,9 +268,9 @@ def theta_star(start_pose: Tuple[int, int], goal_pose: Tuple[int, int], s = heappop(open_set) if s[1] == goal_pose: return reconstruct_path(s) - closed_set.append(s) + closed_set.append(s[1]) for neighbor in get_neighbors(s, grid=grid, goal=goal_node): - if neighbor in closed_set: + if neighbor[1] in closed_set: continue update_vertex(s, neighbor, goal_node, grid) if neighbor not in open_set and neighbor[2] is not None: @@ -289,7 +289,7 @@ def theta_star(start_pose: Tuple[int, int], goal_pose: Tuple[int, int], goal = (GRID_SHAPE[0] - 1, GRID_SHAPE[1] - 1) grid = [] - noise = PerlinNoise(octaves=3, seed=999) + noise = PerlinNoise(octaves=3, seed=980) xpix, ypix = GRID_SHAPE pic = [[noise([i / xpix, j / ypix]) for j in range(xpix)] for i in range(ypix)] @@ -301,7 +301,7 @@ def theta_star(start_pose: Tuple[int, int], goal_pose: Tuple[int, int], # convert the grid to a 1d tuple for x in range(GRID_SHAPE[0]): for y in range(GRID_SHAPE[1]): - pathing_grid[x] = pathing_grid[x] + (grid[x][y] << -y) + pathing_grid[x] = pathing_grid[x] + (grid[x][y] << y) import time @@ -317,8 +317,8 @@ def theta_star(start_pose: Tuple[int, int], goal_pose: Tuple[int, int], import matplotlib.pyplot as plt for point in path: - xs.append(point[1][0]) - ys.append(point[1][1]) + xs.append(point[1][1]) + ys.append(point[1][0]) plt.plot(xs, ys, 'ro-') From 559ac27a266a4612af787a21721f49bda538c2bf Mon Sep 17 00:00:00 2001 From: Mineinjava <65673396+Mineinjava@users.noreply.github.com> Date: Wed, 24 Jul 2024 22:35:33 -0700 Subject: [PATCH 13/13] no clue --- pathPlanner/theta_star_fast.py | 422 ++++++++++----------------------- 1 file changed, 123 insertions(+), 299 deletions(-) diff --git a/pathPlanner/theta_star_fast.py b/pathPlanner/theta_star_fast.py index 070d666..3c80023 100644 --- a/pathPlanner/theta_star_fast.py +++ b/pathPlanner/theta_star_fast.py @@ -1,328 +1,152 @@ -# Copyright (C) Marcus Kauffman 2023-Present - -# This work would not have been possible without the work of many -# contributors, most notably Colin Montigel. See ACKNOWLEDGEMENT.md for -# more details. - -# This file is part of Quail. - -# Quail is free software: you can redistribute it and/or modify it under -# the terms of the GNU General Public License as published by the Free -# Software Foundation, version 3. - -# Quail is distributed in the hope that it will be useful, but WITHOUT -# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or -# FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License -# for more details. - -# You should have received a copy of the GNU General Public License -# along with Quail. If not, see . - -""" -self-contained theta* implementation, optimized for speed -Goal: get it fast enough to be run on every loop cycle. -""" -import copy -# Grid is a 1d tuple of integers. The array represents the x-axis and -# each integer represents the y-axis. - -# Nodes are represented as a list -# [heuristic_distance, (x, y), parent, shortestDist] -# [float, tuple, tuple, float] - -from typing import Tuple, List +import numpy as np from numba import njit -import heapq -from heapq import heappush, heappop - -USE_JUMP = False -GRID_SHAPE = (15, 15) - - -def reconstruct_path(goal: List) -> \ - list[list]: - """Reconstructs the path from the goal node to the start node. - returns a list of nodes on the path""" - path = [] - current = goal - while current[2][1] != current[1]: # the parent is not the current - path.append(current) - current = current[2] - path.append(current) - path.reverse() - return path - - -def get_grid_value(grid: Tuple[int], x: int, y: int) -> int: - """Returns the value of the grid at the given coordinates, or 1 if - out of bounds""" - if x < 0 or y < 0 or x >= GRID_SHAPE[0] or y >= GRID_SHAPE[1]: - return 1 - value = grid[x] >> y & 1 - return (grid[x] >> y) & 1 - - -def get_line(start, end): - """Bresenham's Line Algorithm - Produces a list of tuples from start and end - >>> points1 = get_line((0, 0), (3, 4)) - >>> points2 = get_line((3, 4), (0, 0)) - >>> assert(set(points1) == set(points2)) - >>> print points1 - [(0, 0), (1, 1), (1, 2), (2, 3), (3, 4)] - >>> print points2 - [(3, 4), (2, 3), (1, 2), (1, 1), (0, 0)] - """ - # Setup initial conditions - x1, y1 = start - x2, y2 = end - dx = x2 - x1 - dy = y2 - y1 - - # Determine how steep the line is - is_steep = abs(dy) > abs(dx) - - # Rotate line - if is_steep: - x1, y1 = y1, x1 - x2, y2 = y2, x2 - - # Swap start and end points if necessary and store swap state - swapped = False - if x1 > x2: - x1, x2 = x2, x1 - y1, y2 = y2, y1 - swapped = True - - # Recalculate differentials - dx = x2 - x1 - dy = y2 - y1 - - # Calculate error - error = int(dx / 2.0) - ystep = 1 if y1 < y2 else -1 - - # Iterate over bounding box generating points between start and end - y = y1 - points = [] - for x in range(x1, x2 + 1): - coord = (y, x) if is_steep else (x, y) - points.append(coord) - error -= abs(dy) - if error < 0: - y += ystep - error += dx - - # Reverse the list if the coordinates were swapped - # if swapped: - # we are iterating over all the points so the order - # doesn't matter. - # points.reverse() - return points - - -def line_of_sight(s: List, - neighbor: List, - grid: Tuple[int]) -> bool: +from numba import int32, float32, uint32 +from numba.experimental import jitclass +from typing import Optional +import bresenhams + + +class Node: + def __init__(self, x, y, parent=None): + self.x: int = x + self.y: int = y + self.parent: Optional[Node] = parent + self.gScore: float = 0 + self.heuristic: float = 0 + self.f: float = 0 + + def neighbors(self): + _neighbors = [] + for i in range(-1, 2): + for j in range(-1, 2): + if i == 0 and j == 0: + continue + _neighbors.append(Node(self.x + i, self.y + j, self)) + return _neighbors + def __eq__(self, other): + return self.x == other.x and self.y == other.y + + @property + def pos(self): + return (self.x, self.y) + + def __le__(self, other): + return self.f <= other.f + + def __lt__(self, other): + return self.f < other.f + + def __hash__(self): + return hash(self.pos) + + +def line_of_sight(grid: np.ndarray, a, b) -> bool: """Returns true if there is a line of sight between self and other. - """ - if s[1] == neighbor[1]: + """ + if a == b: return True - if s is None or neighbor is None: + if a is None or b is None: + return False + + if a.x < 0 or a.y < 0 or a.x >= grid.shape[0] or a.y >= grid.shape[1]: return False - intersected_points: List[Tuple[int, int]] = get_line(s[1], - neighbor[1]) - intersected_points.append(neighbor[1]) - intersected_points.append(s[1]) + if b.x < 0 or b.y < 0 or b.x >= grid.shape[0] or b.y >= grid.shape[1]: + return False + if grid[a.x][a.y] == 1 or grid[b.x][b.y] == 1: + return False + + intersected_points = [(x, y) for x, y in + bresenhams.get_line(a.as_tuple(), + b.as_tuple())] + intersected_points.append(b) + intersected_points.append(a) for point in intersected_points: - if point[0] < 0 or point[1] < 0 or point[0] >= GRID_SHAPE[0] \ - or point[1] >= GRID_SHAPE[1]: + if point[0] < 0 or point[0] < 0 or point[1] >= grid.shape[ + 0] or point[1] >= grid.shape[1]: return False - elif get_grid_value(grid, point[0], point[1]) == 1: + elif grid[point[0]][point[1]] == 1: return False return True +def euclidian_node_distance(pose: Node, goal: Node): + return np.sqrt((pose.x - goal.x) ** 2 + (pose.y - goal.y) ** 2) -def point_distance(s: Tuple[int, int], neighbor: Tuple[int, int]) \ - -> float: - return ((s[0] - neighbor[0]) ** 2 + ( - s[1] - neighbor[1]) ** 2) ** 0.5 +def euclidian_tuple_distance(pose: tuple, goal: tuple): + return np.sqrt((pose[0] - goal[0]) ** 2 + (pose[1] - goal[1]) ** 2) -def node_distance(s: List, - neighbor: List) -> float: - return ((s[1][0] - neighbor[1][0]) ** 2 + ( - s[1][1] - neighbor[1][1]) ** 2) ** 0.5 +def update_vertex(s: Node, neighbor: Node, grid: np.ndarray): + if line_of_sight(grid, s.parent, neighbor): + if s.gScore + euclidian_node_distance(s.parent, neighbor) < neighbor.gScore: + neighbor.gScore = s.parent.gScore + euclidian_node_distance(s, neighbor) + neighbor.f = neighbor.gScore + neighbor.heuristic + neighbor.parent = s.parent -def heuristic_distance(s: List, - goal: List, - shortest_distance: float) -> float: - """Returns the heuristic distance to the goal. - distance is the distance from the start node to s.""" - if shortest_distance is None: - return node_distance(s, goal) - return node_distance(s, goal) + shortest_distance + elif (s.gScore + euclidian_node_distance(s, neighbor) < neighbor.gScore) \ + and line_of_sight(grid, s, neighbor): + neighbor.gScore = s.gScore + euclidian_node_distance(s, neighbor) + neighbor.f = neighbor.gScore + neighbor.heuristic + neighbor.parent = s -def update_vertex(s: List, - neighbor: List, - goal: List, - grid: Tuple[int]): - if line_of_sight(s[2], neighbor, grid): # if line of sight - # between parent of s and neighbor - if (s[3] + node_distance(s[2], neighbor)) < neighbor[3]: - # if the shortest known distance to s + distance to neighbor - # is less than the shortest known distance to neighbor - neighbor[0] = heuristic_distance(neighbor, goal, s[3]) - neighbor[2] = s[2], # parent of neighbor is parent of s - neighbor[3] = s[2][3] + node_distance(s[2], neighbor) - # shortest distance to neighbor is shortest - # distance to s + distance to neighbor - - else: - if line_of_sight(s, neighbor, grid): # if line of sight between - # s and neighbor - if s[3] + node_distance(s, neighbor) < neighbor[3]: - # if the shortest known distance to s + distance - # to neighbor is less than the shortest known distance to neighbor - neighbor[0] = heuristic_distance(neighbor, goal, s[3]) - neighbor[2] = s, # parent of neighbor is s - neighbor[3] = s[3] + node_distance(s, neighbor) - - -def jump(pos: Tuple[int, int], dx: int, dy: int, grid: Tuple[int]) \ - -> Tuple[int, int]: - """go in a direction until we are near a wall - inspired by JPS but simple""" - if dx == 0 and dy == 0: - raise ValueError("dx and dy cannot be 0") - x, y = pos - while True: - if x < 0 or y < 0 or x >= GRID_SHAPE[0] or y >= GRID_SHAPE[1]: - return (x - dx, y - dy) - # calculate sum of all point around the current point - sum_ = 0 - for i in range(-1, 1 + 1): - for j in range(-1, 1 + 1): - sum_ += get_grid_value(grid, x + i, y + j) - if sum_ != 0: - return (x-dx, y-dy) - x = x + dx - y = y + dy - - -def get_neighbors(s: List, - grid: Tuple[int], - goal: List) \ - -> List[List]: - neighbors: List[List] = [] - backup_neighbors: List[List] = [] - - for i in range(-1, 2): - for j in range(-1, 2): - if not (i == 0 and j == 0): - backup_neighbors.append([None, - (s[1][0] + i, s[1][1] + j), - s, - None]) - jumppoint = jump(s[1], i, j, grid) - if jumppoint != s[1]: - pass - #neighbors.append([None, jumppoint, s, None]) - - if len(neighbors) == 0: - neighbors = backup_neighbors - - new_neighbors = [] - for neighbor in neighbors: - if neighbor[1][0] < 0 or neighbor[1][1] < 0 or neighbor[1][0] >= \ - GRID_SHAPE[0] or neighbor[1][1] >= GRID_SHAPE[1]: - continue - elif get_grid_value(grid, neighbor[1][0], neighbor[1][1]) == 1: - continue - else: - new_neighbors.append([heuristic_distance(neighbor, goal, s[3]), - neighbor[1], # keep pos the same - neighbor[2], # keep parent the same - s[3] + node_distance(s, neighbor)]) - - return new_neighbors - - -def theta_star(start_pose: Tuple[int, int], goal_pose: Tuple[int, int], - grid: Tuple[int]) -> list[tuple]: - start = [0, start_pose, None, 0] - goal_node = [0, goal_pose, None, 0] - start[2] = copy.deepcopy(start) - # the start node is the first node in the open set, so it will be - # added to the closed set first, so [0] and [1] don't really matter - - open_set: List[List] = [start] - +def theta_star(grid: np.ndarray, start: tuple, goal: tuple) -> Optional[ + list]: + start_node = Node(start[0], start[1]) + goal_node = Node(goal[0], goal[1]) + start_node.heuristic = euclidian_node_distance(start_node, goal_node) + start_node.f = start_node.heuristic + open_set = [start_node] closed_set = [] - - while len(open_set) > 0: - s = heappop(open_set) - if s[1] == goal_pose: - return reconstruct_path(s) - closed_set.append(s[1]) - for neighbor in get_neighbors(s, grid=grid, goal=goal_node): - if neighbor[1] in closed_set: + while open_set: + open_set.sort() + s = open_set.pop(0) + if s == goal_node: + path = [] + while s.parent: + path.append(s.pos) + s = s.parent + path.append(s.pos) + return path[::-1] + closed_set.append(s) + for neighbor in s.neighbors(): + if neighbor in closed_set or not grid[neighbor.x][ + neighbor.y]: continue - update_vertex(s, neighbor, goal_node, grid) - if neighbor not in open_set and neighbor[2] is not None: - heappush(open_set, neighbor) + if neighbor not in open_set: + neighbor.heuristic = euclidian_node_distance(neighbor, goal_node) + neighbor.gScore = float('inf') + neighbor.f = float('inf') + neighbor.parent = None + update_vertex(s, neighbor, grid) + if neighbor not in open_set: + open_set.append(neighbor) return None -if __name__ == "__main__": - import matplotlib.pyplot - import numpy as np +if __name__ == '__main__': from perlin_noise import PerlinNoise - - FILL_PCT = 0.1 - SWAP_XY = True - start = (0, 0) - goal = (GRID_SHAPE[0] - 1, GRID_SHAPE[1] - 1) - grid = [] - - noise = PerlinNoise(octaves=3, seed=980) - xpix, ypix = GRID_SHAPE - pic = [[noise([i / xpix, j / ypix]) for j in range(xpix)] for i in - range(ypix)] - grid = np.array(pic) - grid = np.where(grid < FILL_PCT, 0, 1) - - pathing_grid = np.array([0 for x in range(GRID_SHAPE[0])]) - - # convert the grid to a 1d tuple - for x in range(GRID_SHAPE[0]): - for y in range(GRID_SHAPE[1]): - pathing_grid[x] = pathing_grid[x] + (grid[x][y] << y) - - import time - - theta_star(start, goal, tuple(pathing_grid)) # precompile for njit - startT = time.perf_counter_ns() - path = theta_star(start, goal, tuple(pathing_grid)) - endT = time.perf_counter_ns() - - print("Time taken: ", (endT - startT) / 1e6) - - xs, ys = [], [] - import matplotlib.pyplot as plt - for point in path: - xs.append(point[1][1]) - ys.append(point[1][0]) + noise = PerlinNoise(octaves=10, seed=1) + w, h = 10, 10 + grid = np.zeros((w, h)) + for i in range(w): + for j in range(h): + grid[i][j] = noise([i / w, j / h]) + # threshold the grid + grid = np.where(grid > 1.5, 1, 0) + grid = np.zeros((w,h)) - plt.plot(xs, ys, 'ro-') - - plt.imshow(grid, cmap='gray', interpolation='none', origin='lower') + start = (0, 0) + goal = (w - 1, h - 1) + path = theta_star(grid, start, goal) + print(path) + plt.imshow(grid) + plt.show() + xs, ys = zip(*path) + plt.plot(ys, xs, 'r') plt.show() - print(path)