From b3f190ec3625ef6d2f4fc332438b11e981195c79 Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Tue, 4 Aug 2020 15:34:15 -0700 Subject: [PATCH] 'dynamic_rrt' --- .idea/PathPlanning.iml | 3 +- .idea/misc.xml | 2 +- .../rrt_2D/__pycache__/env.cpython-37.pyc | Bin 1261 -> 1298 bytes .../__pycache__/plotting.cpython-37.pyc | Bin 3765 -> 3766 bytes .../rrt_2D/__pycache__/rrt.cpython-37.pyc | Bin 3562 -> 3576 bytes .../rrt_2D/__pycache__/utils.cpython-37.pyc | Bin 3687 -> 3725 bytes Sampling_based_Planning/rrt_2D/rrt.py | 3 +- .../__pycache__/plot_util3D.cpython-37.pyc | Bin 6029 -> 6029 bytes .../rrt_3D/__pycache__/rrt3D.cpython-37.pyc | Bin 2107 -> 2107 bytes .../rrt_3D/__pycache__/utils3D.cpython-37.pyc | Bin 12290 -> 12307 bytes .../rrt_3D/dynamic_rrt3D.py | 107 ++++++++++-------- Sampling_based_Planning/rrt_3D/plot_util3D.py | 2 +- .../rrt_3D/rrt_connect3D.py | 67 +++++++---- Sampling_based_Planning/rrt_3D/utils3D.py | 1 + 14 files changed, 110 insertions(+), 75 deletions(-) diff --git a/.idea/PathPlanning.iml b/.idea/PathPlanning.iml index 7c9d48f..49df05d 100644 --- a/.idea/PathPlanning.iml +++ b/.idea/PathPlanning.iml @@ -2,11 +2,10 @@ - + - \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml index a2e120d..8161a60 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,4 +1,4 @@ - + \ No newline at end of file diff --git a/Sampling_based_Planning/rrt_2D/__pycache__/env.cpython-37.pyc b/Sampling_based_Planning/rrt_2D/__pycache__/env.cpython-37.pyc index 47fc5c8d35cfadf7c37d46fab840b62ff36115b9..90e600164b90c38a5c0daa0fa26ffe73498332da 100644 GIT binary patch delta 154 zcmaFMIf;wMiIejxn}0G&YQJNiEJU$uEdW&n(F( zO^PWBg S^gsx|qL|dYvdvc+C71!dJvM9r delta 98 zcmbQl^_G*ziIy xUl3D}SdyVokdv5~2a-z6NzX6JEXl|%jtNf8Eyw|i=_Vx>r=}=ue$J@P3;HXj2;oas_LAhu)_G*n<^N@;x?~R&H-|vmv%@Of? z)`$_;?YoXenB#H1F%*)bUrS&RxbQrvRY@W*jDq|gSvO01!Hn^Yo)mgk)1%H;Am{=h zlsL;r+r~DIwHdJoPs|8T(T+oHk9i!Yy2PGK747yJ!}5J>^9UnHs2F`YCd|yK$cXtk zDaP#)^L)XR+8(D!r_<7%+dY*CCMt01gxWrF9Y#647@mSanT+pLp(7mW_8o=D1S1c| zR2iqr=qZDTdxgBh%#LpwMp?d1h)n1lGLgjbD-dx_I!SAL8RV*lirL=S^;OjMI`q6- zarP~FWEj_FBE4trK#HMZB5r}MfHrs)bf^?O?#f|$_`D8sUKCUVa074?a0_r7a7ROb zjf!Hx6|m>x<%b8QkVUd>?dBJ`@FO2O0tf z(ugmdA&c{STIyq6(v$OQ`QxJ>XKxC<2bg7V~5?1nv??< zPMnbT#82RYgd(`WU*HeS4GC`i0>q&^P7kJ*_M2~aXTF)S$6x2a%%`?8nPf;lzy2+L zPNm4BdVJVqT%)`m8#YZ(b3TS>THgPCaeuw>lo^yYb_RCOcFl`*%NsPd4c6sezfrTf zHE31pJ9~|Nja&PU=DN~JbjY7P#`_{PgKSo`!Y`Ga_z^x|F35v&l%}Ks@@!eI6*`Sh zGYEmeaX!*gN)T;1n%;d<4nx9dn}+T=9)(0l(@$umV>=Fr57FW-5R*aFcs00U*oz3ao-pW5(kFPHP>_uitmLc&`~5}SC_CN2zP7~h&BY` z;TnN*2#N&?4c3Zrxp6yIRFnnrF3wVcK7^{)V@341A}qC1nnF5@(ox+TxEvy`*EUY8 z<;UOET}8Pr)Z|*_28JjS%7orTehaxm*N_KN;dWiP$=>tZNGC~=-9fmEuz_$7VH06X zg8Uk1ke`(R5yxtfqcnz>H-Aq49H>C7rrDbXRGT=_J>-7+fES)U zl)!Ki39---sZjbrQrXs`H0JA>Pqm*%Z#QIhHMtKogEd2hS>B=a^+)G_X+jI4lzF_F z#V~x)^c+GC!_sonxvlpMZh38*$MZP^oXSA6zFiA;)f2`y*r!n^?al hm%N#$3c>5N0Drr$se;5cnrf)Zb?>9RoTJ*)#wE diff --git a/Sampling_based_Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc b/Sampling_based_Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc index d22992d767440e3c9db3a11d55097784a1cbad24..4ab574ac7a0648adc285b2bdf2db3be1ab12240f 100644 GIT binary patch delta 729 zcmZ8e&ubGw7~Prd>~4N+Tcq~qEwv?Es3ncIR+M54;t%MdB@|&btZuqln{49j6jIrs z(1Q1lmx_nYQE%cO(3?lkvKPHc3yOyxJopZketHn~$uGGUF(eBuuFB-c~?2aQE3!SDD1b*NxI8Cq3q0kCH z{Ul%M%NU}B;*xzrl&_GySGEzSUkOmJo+6_i|`+O)@dT7)^~4#BSYivF)iV2o%m2FOF$ys11N-BMg- zQ?clJLS9(I3F~k_5OSNnmS^m-K7}8G7;;NHn_L3QbN|6ab_w97boJlluB;k2NJ$s!3FiYk5 zV|ZDL%nX|cT9xlIuU3~)TmeX!Bb;Cfe76QZl&IT~tt2dl|ywp=Kp72lEMLg5Ji zX{#<8ZI|OCI1a$^CT_{&>^!+3PqW?peGn{qf}JN503K{u-`BjiN4Vav$%EX?wLx+U z3Q5`t{1#-l1CD;42e8yc@7CZ-E)uO<^V&``NpUWCMSjaoOm2fE90E{?N>n;R462eb r$~WYQnIFd|CoI@)b@q4(IIkcS??v#?f&KP--+S+yc{4}3?>W0_+i4A#*eq^; zKC};D_F?Uo^C0v@=qz>pmLEL5xYcZTodqx4k?oGN=*wpJ$;{T$va{mK=E`$72(akG zPx#7SyhEqnzSJV^8N7l8?a}K2L|V)+)`u{_e{3M<5gW3Bj!`1g$J~evVPPg2o2VE= z7E;9H6mkIci_;NxW`@jR13pMZ93yK7s0&^G}!2yG_zONvzSsuYcT`4)$2pGu7gCDy2C_Rw!NR*IS-!wi|+m z5Oe@RJBexaF>?h@siRB}?y6lYYoa<{YTqj4#x6R>qm&!?E%a#zg6>Wf)hFvb)YK1a zGV7rbPEZ#ZV6c;5G6T*qv8pcE`Li^~hzH$Pr!UILMU7xeZP=Tzst)ZcUp8$M{sPGg BoLc|@ diff --git a/Sampling_based_Planning/rrt_2D/__pycache__/utils.cpython-37.pyc b/Sampling_based_Planning/rrt_2D/__pycache__/utils.cpython-37.pyc index 137a52c43fae1e51cf7bcebc48abe6dc9762903a..5e6e60b71a18a9840ed1fafa42dc1bdfcc0dd674 100644 GIT binary patch delta 172 zcmaDZ(<{sE#LLUY00fffH8yffGnqFhTg8MHrxq2*7+V?|8^*Y#7H5~_7sRAzmSmJB z#S|o#Wat*;BYml@fNfMypN xPj=&0fH8BpSHYMvJY522AO+?i0;+TyPd%f_WHa7WMzhItcjd4jW&MwI> zh;h#>$tX?Ijq>n~2}mr-2*^py%L6K!9LprXc_GtBHpW|zJ&vZ*}avYQRWP3jL$%=eA02$jd Avj6}9 diff --git a/Sampling_based_Planning/rrt_2D/rrt.py b/Sampling_based_Planning/rrt_2D/rrt.py index 1a1ecb2..4d86d37 100644 --- a/Sampling_based_Planning/rrt_2D/rrt.py +++ b/Sampling_based_Planning/rrt_2D/rrt.py @@ -41,6 +41,7 @@ class Rrt: self.obs_boundary = self.env.obs_boundary def planning(self): + print("z") for i in range(self.iter_max): node_rand = self.generate_random_node(self.goal_sample_rate) node_near = self.nearest_neighbor(self.vertex, node_rand) @@ -102,7 +103,7 @@ def main(): x_start = (2, 2) # Starting node x_goal = (49, 24) # Goal node - rrt = Rrt(x_start, x_goal, 0.5, 0.00, 10000) + rrt = Rrt(x_start, x_goal, 0.5, 0.05, 10000) path = rrt.planning() if path: diff --git a/Sampling_based_Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc b/Sampling_based_Planning/rrt_3D/__pycache__/plot_util3D.cpython-37.pyc index 558f38808522c3148d836fda047ab0c015ea066b..0bf07cde12b0370cf9b76a8a6d73c2cc01a926d2 100644 GIT binary patch delta 257 zcmeCx@73pZ;^pOH0D@24n(;3-@;>8bY6zSBmDh#w)?_Qb89?$f-(-P!pn{?V5Rn8T zk|&q(FJY|NEG=-0S*jQ$QUW4MK|~pdC~9h`2N6PVNxN5A6jCM==#u z0C6P{SKVSN$}P$TsTBtjnk+?KAa*y1=m8NR^+ggOA_qux0&y`9kYHfrVdMZo79p*o yoXtL>Y>d)*K;c7h1x!4QK*%J(D8N{hH#t|#nK5nhMzLD%8lVcWSwK-s@elwnlsP&8 delta 257 zcmeCx@73pZ;^pOH0D|S`HR4}x{8h`2LmPwo)O5A6mDM==#u z0C6P{SKVSN$}P$UsTBtjnk+>fAa*B+=mHTS^+ggOA`3`>#EW@=1OponBL@hw2x%2% yZT1mmW0cMT3LkuJiIuJiI5 z49O-AxA#I(nKydLh3U4$Z6@9>-k2=$;zTbs*@YKg7;k-_uaZTxCg}YOO$uHg{v~3 ziQW#MmA#+jvAtU5BR8qDveUYYC51A?JCc`xRQUkThcakg7_xHs)uj8JvO65kEmUuMv6oq&#)k&MYoBHh7 z8iY~6qa&tThXH+c=w#8HuT;mY3uQ}f!2BU#n5#X}R)0-d)_Bz_Da)=UnGdDtGT%CM zzfb#`5Mi#lF(XSjqz7dftOS?@OfiLmd@tP+9D0;Wh(;(+>IysQ8$&um?NFCgSJT^w z5b-#k3zn+cY7yRaY$Mv`_n8)Yn(t@Qw8ej9#u_2%$!UWUzH)9IoMT$mawEYx?L^Lp zd^)woQf=mCzSa|_tGcm2^i0W=$N=a*zQM|(S9%ppPel@J0&o?e?+Jm80k$|W zknKd)MbDBdRm(;HzthmD=M;|)#OY%+#N$%us~_2!lTph(v(MAG%E zo$L7u&GBqLLk@4}$6qW%f^UhM5pcW6QFa{>1|=Dvp_-BB|bWM hEb$d6Zgl8`W7svN1GxdKL?fg5`QY=^%Ff{7e*o?XD!u>! delta 1391 zcmZ`(U1*zS6wXP$WM9*yG;OjZ{Y%oubZOVr_No_7ot+M$Tak7XGD^}l-NyL z?5wNoM+X~qb4PFtH+P}CC@N?{@4Qpci9^Ag&)W*ZK)mo;JkOhr;m|-Hp7(v9^StMr z_at0CUk<+z4hKE*`T72y($UYuEwPB}ui3WcsA<`Cy=$ttJs4XhI>NVhFVYum#z*+M z_-nMqU&Z&5pLgQvj=ZTdqE;;c*ty)DcxHFIFpsyLOzNPQsNge%DT^D4FJmX=tOO%2-*ZZ6rd0V zDN1d)ZHaffKBeFIdiTdv=ZT)Z^gCNU7Xo#JuJ9i{dHRzRz4zpBr}t_4i(`Fr!tK5R zB7VQ`V5|Y50yuy*!0X&Qlt?ZM-I1@2f=x1grfC|%uv+H1p(O81&2>~EJ_VQ($a__t zKS?G1unh3`sZ9Gi&fWl=1vKCr*%0a{@%0Lk5;i!S-rtU0ZKEP!RbEX`cD6!h%G%>K zy&xd2 zfX7Crx_}M5uuBJu=5noGsy8Z@x`^$!0F!{XnWCL-AxBwOscy|H%XU)yQGbRm^SAxq z4Qs=)2=lv|BXb5pdTw<1aV!@A6&Uf4*+9Z}EK7|&E;UM1R3LSQ9}L_c*STs}-EH0I zKr>fl=K4=t%CXf7EOnMs4}Du{g5B%@ZSvpQV>#`h=Ltir;FULYhOTMVE6q}uc%wg& zY0fx1}-XnR9E%;|Mb*CQV&@x{4_wR@c6TQD0f(1-Wq+L!u()#&p$TKFth*w diff --git a/Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py b/Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py index 92ffeab..bf232a0 100644 --- a/Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py +++ b/Sampling_based_Planning/rrt_3D/dynamic_rrt3D.py @@ -12,14 +12,16 @@ import matplotlib.pyplot as plt import os import sys -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling-based_Planning/") +sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../../Sampling_based_Planning/") from rrt_3D.env3D import env -from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, cost, path, edgeset, isinbound, isinside +from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, cost, path, edgeset, isinbound, \ + isinside from rrt_3D.rrt3D import rrt from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent -class dynamic_rrt_3D(): - + +class dynamic_rrt_3D: + def __init__(self): self.env = env() self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) @@ -27,18 +29,20 @@ class dynamic_rrt_3D(): self.current = tuple(self.env.start) self.stepsize = 0.25 self.maxiter = 10000 - self.GoalProb = 0.05 # probability biased to the goal - self.WayPointProb = 0.05 # probability falls back on to the way points + self.GoalProb = 0.05 # probability biased to the goal + self.WayPointProb = 0.02 # probability falls back on to the way points + self.done = False + self.invalid = False - self.V = [] # vertices - self.Parent = {} # parent child relation - self.Edge = set() # edge relation (node, parent node) tuple + self.V = [] # vertices + self.Parent = {} # parent child relation + self.Edge = set() # edge relation (node, parent node) tuple self.Path = [] - self.flag = {}# flag dictionary + self.flag = {} # flag dictionary self.ind = 0 self.i = 0 -#--------Dynamic RRT algorithm + # --------Dynamic RRT algorithm def RegrowRRT(self): self.TrimRRT() self.GrowRRT() @@ -57,14 +61,14 @@ class dynamic_rrt_3D(): i += 1 self.CreateTreeFromNodes(S) print('trimming complete...') - + def InvalidateNodes(self, obstacle): Edges = self.FindAffectedEdges(obstacle) for edge in Edges: qe = self.ChildEndpointNode(edge) self.flag[qe] = 'Invalid' -#--------Extend RRT algorithm----- + # --------Extend RRT algorithm----- def initRRT(self): self.V.append(self.x0) self.flag[self.x0] = 'Valid' @@ -72,12 +76,11 @@ class dynamic_rrt_3D(): def GrowRRT(self): print('growing') qnew = self.x0 - tree = None distance_threshold = self.stepsize self.ind = 0 while self.ind <= self.maxiter: qtarget = self.ChooseTarget() - qnearest = self.Nearest(tree, qtarget) + qnearest = self.Nearest(qtarget) qnew, collide = self.Extend(qnearest, qtarget) if not collide: self.AddNode(qnearest, qnew) @@ -96,14 +99,14 @@ class dynamic_rrt_3D(): if len(self.V) == 1: i = 0 else: - i = np.random.randint(0, high = len(self.V) - 1) + i = np.random.randint(0, high=len(self.V) - 1) if 0 < p < self.GoalProb: return self.xt elif self.GoalProb < p < self.GoalProb + self.WayPointProb: return self.V[i] elif self.GoalProb + self.WayPointProb < p < 1: return tuple(self.RandomState()) - + def RandomState(self): # generate a random, obstacle free state xrand = sampleFree(self, bias=0) @@ -115,16 +118,16 @@ class dynamic_rrt_3D(): self.Edge.add((extended, nearest)) self.flag[extended] = 'Valid' - def Nearest(self, tree, target): + def Nearest(self, target): # TODO use kdTree to speed up search return nearest(self, target, isset=True) def Extend(self, nearest, target): - extended, dist = steer(self, nearest, target, DIST = True) + extended, dist = steer(self, nearest, target, DIST=True) collide, _ = isCollide(self, nearest, target, dist) return extended, collide -#--------Main function + # --------Main function def Main(self): # qstart = qgoal self.x0 = tuple(self.env.goal) @@ -132,33 +135,36 @@ class dynamic_rrt_3D(): self.xt = tuple(self.env.start) self.initRRT() self.GrowRRT() - self.Path, D = path(self) + self.Path, D = self.path() self.done = True - self.visualization() - plt.show() + self.visualization() t = 0 while True: # move the block while the robot is moving new, _ = self.env.move_block(a=[0, 0, -0.2], mode='translation') self.InvalidateNodes(new) + self.TrimRRT() # if solution path contains invalid node - self.done = True self.visualization() - plt.show() - invalid = self.PathisInvalid(self.Path) - if invalid: + self.invalid = self.PathisInvalid(self.Path) + if self.invalid: self.done = False self.RegrowRRT() self.Path = [] - self.Path, D = path(self) - + self.Path, D = self.path() + self.done = True + self.visualization() if t == 8: break + t += 1 + self.visualization() + plt.show() -#--------Additional utility functions + # --------Additional utility functions def FindAffectedEdges(self, obstacle): # scan the graph for the changed edges in the tree. # return the end point and the affected + print('finding affected edges') Affectededges = [] for e in self.Edge: child, parent = e @@ -171,44 +177,48 @@ class dynamic_rrt_3D(): return edge[0] def CreateTreeFromNodes(self, Nodes): - self.V = [] - Parent = {} - edges = set() - for v in Nodes: - self.V.append(v) - Parent[v] = self.Parent[v] - edges.add((v, Parent[v])) - self.Parent = Parent - self.Edge = edges + print('creating tree') + # self.Parent = {node: self.Parent[node] for node in Nodes} + self.V = [node for node in Nodes] + self.Edge = {(node, self.Parent[node]) for node in Nodes} + # if self.invalid: + # del self.Parent[self.xt] def PathisInvalid(self, path): for edge in path: if self.flag[tuple(edge[0])] == 'Invalid' or self.flag[tuple(edge[1])] == 'Invalid': return True - def path(self, Path=[], dist=0): + def path(self, dist=0): + Path=[] x = self.xt + i = 0 while x != self.x0: x2 = self.Parent[x] Path.append(np.array([x, x2])) dist += getDist(x, x2) x = x2 + if i > 10000: + print('Path is not found') + return + i+= 1 return Path, dist - -#--------Visualization specialized for dynamic RRT + + # --------Visualization specialized for dynamic RRT def visualization(self): if self.ind % 100 == 0 or self.done: V = np.array(self.V) Path = np.array(self.Path) start = self.env.start goal = self.env.goal - edges = [] - for i in self.Parent: - edges.append([i,self.Parent[i]]) + # edges = [] + # for i in self.Parent: + # edges.append([i, self.Parent[i]]) + edges = np.array([list(i) for i in self.Edge]) ax = plt.subplot(111, projection='3d') # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi)) # ax.view_init(elev=0., azim=90.) - ax.view_init(elev=8., azim=120.) + ax.view_init(elev=0., azim=90.) ax.clear() # drawing objects draw_Spheres(ax, self.env.balls) @@ -229,13 +239,12 @@ class dynamic_rrt_3D(): dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy) make_transparent(ax) - #plt.xlabel('x') - #plt.ylabel('y') + # plt.xlabel('x') + # plt.ylabel('y') ax.set_axis_off() plt.pause(0.0001) - if __name__ == '__main__': rrt = dynamic_rrt_3D() rrt.Main() diff --git a/Sampling_based_Planning/rrt_3D/plot_util3D.py b/Sampling_based_Planning/rrt_3D/plot_util3D.py index 58f62fd..1c4cb33 100644 --- a/Sampling_based_Planning/rrt_3D/plot_util3D.py +++ b/Sampling_based_Planning/rrt_3D/plot_util3D.py @@ -106,7 +106,7 @@ def visualization(initparams): # ax.view_init(elev=0.+ 0.03*initparams.ind/(2*np.pi), azim=90 + 0.03*initparams.ind/(2*np.pi)) # ax.view_init(elev=0., azim=90.) - ax.view_init(elev=8., azim=120.) + ax.view_init(elev=8., azim=90.) # ax.view_init(elev=-8., azim=180) ax.clear() # drawing objects diff --git a/Sampling_based_Planning/rrt_3D/rrt_connect3D.py b/Sampling_based_Planning/rrt_3D/rrt_connect3D.py index 8466d17..08d9be6 100644 --- a/Sampling_based_Planning/rrt_3D/rrt_connect3D.py +++ b/Sampling_based_Planning/rrt_3D/rrt_connect3D.py @@ -18,6 +18,23 @@ from rrt_3D.env3D import env from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path, edgeset from rrt_3D.plot_util3D import make_get_proj, draw_block_list, draw_Spheres, draw_obb, draw_line, make_transparent + +class Tree(): + def __init__(self, node): + self.V = [] + self.Parent = {} + self.V.append(node) + # self.Parent[node] = None + + def add_vertex(self, node): + if node not in self.V: + self.V.append(node) + + def add_edge(self, parent, child): + # here edge is defined a tuple of (parent, child) (qnear, qnew) + self.Parent[child] = parent + + class rrt_connect(): def __init__(self): self.env = env() @@ -33,6 +50,7 @@ class rrt_connect(): self.qgoal = tuple(self.env.goal) self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) self.qnew = None + self.done = False self.ind = 0 self.fig = plt.figure(figsize=(10, 8)) @@ -78,7 +96,7 @@ class rrt_connect(): collide, _ = isCollide(self, qnear, qnew, dist = dist) return not collide - #----------RRT connect algorithm +#----------RRT connect algorithm def CONNECT(self, Tree, q): print('in connect') while True: @@ -97,13 +115,14 @@ class rrt_connect(): qnew = self.qnew # get qnew from outside if self.CONNECT(Tree_B, qnew) == 'Reached': print('reached') - # return self.PATH(Tree_A, Tree_B) + self.done = True + self.Path = self.PATH(Tree_A, Tree_B) + self.visualization(Tree_A, Tree_B, k) + plt.show() return - else: - print('not reached') + # return Tree_A, Tree_B = self.SWAP(Tree_A, Tree_B) self.visualization(Tree_A, Tree_B, k) - print('Failure') return 'Failure' # def PATH(self, tree_a, tree_b): @@ -111,10 +130,29 @@ class rrt_connect(): tree_a, tree_b = tree_b, tree_a return tree_a, tree_b + def PATH(self, tree_a, tree_b): + qnew = self.qnew + patha = [] + pathb = [] + while True: + patha.append((tree_a.Parent[qnew], qnew)) + qnew = tree_a.Parent[qnew] + if qnew == self.qinit or qnew == self.qgoal: + break + qnew = self.qnew + while True: + pathb.append((tree_b.Parent[qnew], qnew)) + qnew = tree_b.Parent[qnew] + if qnew == self.qinit or qnew == self.qgoal: + break + return patha + pathb + +#----------RRT connect algorithm def visualization(self, tree_a, tree_b, index): - if (index % 10 == 0 and index != 0) or self.done: + if (index % 20 == 0 and index != 0) or self.done: # a_V = np.array(tree_a.V) # b_V = np.array(tree_b.V) + Path = self.Path start = self.env.start goal = self.env.goal a_edges, b_edges = [], [] @@ -132,32 +170,19 @@ class rrt_connect(): draw_block_list(ax, np.array([self.env.boundary]), alpha=0) draw_line(ax, a_edges, visibility=0.75, color='g') draw_line(ax, b_edges, visibility=0.75, color='y') - # draw_line(ax, Path, color='r') + draw_line(ax, Path, color='r') ax.plot(start[0:1], start[1:2], start[2:], 'go', markersize=7, markeredgecolor='k') ax.plot(goal[0:1], goal[1:2], goal[2:], 'ro', markersize=7, markeredgecolor='k') xmin, xmax = self.env.boundary[0], self.env.boundary[3] ymin, ymax = self.env.boundary[1], self.env.boundary[4] zmin, zmax = self.env.boundary[2], self.env.boundary[5] - dx, dy, dz = xmax - xmin, ymax - ymin, zmax - zmin + dx, dy, _ = xmax - xmin, ymax - ymin, zmax - zmin ax.get_proj = make_get_proj(ax, 1 * dx, 1 * dy, 2 * dy) make_transparent(ax) ax.set_axis_off() plt.pause(0.0001) -class Tree(): - def __init__(self, node): - self.V = [] - self.Parent = {} - self.V.append(node) - # self.Parent[node] = None - def add_vertex(self, node): - if node not in self.V: - self.V.append(node) - - def add_edge(self, parent, child): - # here edge is defined a tuple of (parent, child) (qnear, qnew) - self.Parent[child] = parent if __name__ == '__main__': p = rrt_connect() diff --git a/Sampling_based_Planning/rrt_3D/utils3D.py b/Sampling_based_Planning/rrt_3D/utils3D.py index 02b3287..0214f0c 100644 --- a/Sampling_based_Planning/rrt_3D/utils3D.py +++ b/Sampling_based_Planning/rrt_3D/utils3D.py @@ -196,6 +196,7 @@ def steer(initparams, x, y, DIST=False): if np.equal(x, y).all(): return x, 0.0 dist, step = getDist(y, x), initparams.stepsize + step = min(dist, step) increment = ((y[0] - x[0]) / dist * step, (y[1] - x[1]) / dist * step, (y[2] - x[2]) / dist * step) xnew = (x[0] + increment[0], x[1] + increment[1], x[2] + increment[2]) # direc = (y - x) / np.linalg.norm(y - x)