From d57aea0758fc7c51a66d05eb01f8e99ff4d21223 Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Sat, 18 Jul 2020 23:37:52 -0700 Subject: [PATCH] 'OBB_line' --- .../Search_3D/DstarLite3D.py | 41 ++++++------ .../__pycache__/env3D.cpython-37.pyc | Bin 4982 -> 5167 bytes .../__pycache__/plot_util3D.cpython-37.pyc | Bin 5722 -> 5722 bytes .../__pycache__/utils3D.cpython-37.pyc | Bin 10136 -> 10371 bytes Search-based Planning/Search_3D/env3D.py | 4 +- .../Search_3D/plot_util3D.py | 2 +- Search-based Planning/Search_3D/utils3D.py | 61 +++++++++++------- 7 files changed, 62 insertions(+), 46 deletions(-) diff --git a/Search-based Planning/Search_3D/DstarLite3D.py b/Search-based Planning/Search_3D/DstarLite3D.py index d7fc9ec..7742dc6 100644 --- a/Search-based Planning/Search_3D/DstarLite3D.py +++ b/Search-based Planning/Search_3D/DstarLite3D.py @@ -28,9 +28,9 @@ class D_star_Lite(object): # (1, -1, -1): np.sqrt(3), (-1, 1, -1): np.sqrt(3), (-1, -1, 1): np.sqrt(3), \ # (1, 1, -1): np.sqrt(3), (1, -1, 1): np.sqrt(3), (-1, 1, 1): np.sqrt(3)} self.env = env(resolution=resolution) - self.X = StateSpace(self.env) - self.x0, self.xt = getNearest(self.X, self.env.start), getNearest(self.X, self.env.goal) - #self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) + #self.X = StateSpace(self.env) + #self.x0, self.xt = getNearest(self.X, self.env.start), getNearest(self.X, self.env.goal) + self.x0, self.xt = tuple(self.env.start), tuple(self.env.goal) # self.OPEN = queue.QueuePrior() self.OPEN = queue.MinheapPQ() self.km = 0 @@ -64,7 +64,7 @@ class D_star_Lite(object): def updatecost(self,range_changed=None, new=None, old=None, mode=False): # scan graph for changed cost, if cost is changed update it CHANGED = set() - for xi in self.X: + for xi in self.CLOSED: if xi in self.CHILDREN: oldchildren = self.CHILDREN[xi]# A if isinbound(old, xi, mode) or isinbound(new, xi, mode): @@ -77,7 +77,6 @@ class D_star_Lite(object): for xj in intersection.union(added): self.COST[xi][xj] = cost(self, xi, xj) CHANGED.add(xi) - self.CHILDREN[xi] = newchildren else: if isinbound(old, xi, mode) or isinbound(new, xi, mode): CHANGED.add(xi) @@ -116,8 +115,10 @@ class D_star_Lite(object): def UpdateVertex(self, u): # if still in the hunt if not getDist(self.xt, u) <= self.env.resolution: # originally: u != s_goal - cdren = self.getchildren(u) - self.rhs[u] = min([self.getcost(s, u) + self.getg(s) for s in self.getchildren(u)]) + if u in self.CHILDREN and len(self.CHILDREN[u]) == 0: + self.rhs[u] = np.inf + else: + self.rhs[u] = min([self.getcost(s, u) + self.getg(s) for s in self.getchildren(u)]) # if u is in OPEN, remove it self.OPEN.check_remove(u) # if rhs(u) not equal to g(u) @@ -130,10 +131,10 @@ class D_star_Lite(object): u = self.OPEN.get() self.V.add(u) self.CLOSED.add(u) - # if not self.done: # first time running, we need to stop on this condition - # if getDist(self.x0,u) < 1.5*self.env.resolution: - # self.x0 = u - # break + if not self.done: # first time running, we need to stop on this condition + if getDist(self.x0,u) < 1*self.env.resolution: + self.x0 = u + break if kold < self.CalculateKey(u): self.OPEN.put(u, self.CalculateKey(u)) if self.getg(u) > self.getrhs(u): @@ -143,6 +144,7 @@ class D_star_Lite(object): self.UpdateVertex(u) for s in self.getchildren(u): self.UpdateVertex(s) + # visualization(self) self.ind += 1 def main(self): @@ -159,17 +161,14 @@ class D_star_Lite(object): ischanged = False self.V = set() while getDist(self.x0, self.xt) > 2*self.env.resolution: - #---------------------------------- at 5th node, the environment is changed and cost is updated - if t % 1 == 0: - # new1,old1 = self.env.move_block(a=[0, 0, -2], s=0.5, block_to_move=0, mode='translation') + #---------------------------------- at specific times, the environment is changed and cost is updated + if t % 2 == 0: + new0,old0 = self.env.move_block(a=[-0.1, 0, -0.2], s=0.5, block_to_move=1, mode='translation') new1,old1 = self.env.move_block(a=[0, 0, -0.2], s=0.5, block_to_move=0, mode='translation') new2,old2 = self.env.move_block(theta = [0,0,0.1*t], mode='rotation') #new2,old2 = self.env.move_block(a=[-0.3, 0, -0.1], s=0.5, block_to_move=1, mode='translation') ischanged = True self.Path = [] - # if t == 5: - # new1,old1 = self.env.move_block(a=[0, 0, -1], s=0.5, block_to_move=1, mode='translation') - # ischanged = True #----------------------------------- traverse the route as originally planned if t == 0: children_new = [i for i in self.CLOSED if getDist(self.x0, i) <= self.env.resolution*np.sqrt(3)] @@ -184,16 +183,16 @@ class D_star_Lite(object): if ischanged: self.km += heuristic_fun(self, self.x0, s_last) s_last = self.x0 - CHANGED = self.updatecost(True, new1, old1) + CHANGED = self.updatecost(True, new0, old0) + CHANGED1 = self.updatecost(True, new1, old1) CHANGED2 = self.updatecost(True, new2, old2, mode='obb') - CHANGED = CHANGED.union(CHANGED2) + CHANGED = CHANGED.union(CHANGED1, CHANGED2) # self.V = set() for u in CHANGED: self.UpdateVertex(u) self.ComputeShortestPath() ischanged = False - # self.Path = self.path(s_start) self.Path = self.path(self.x0) visualization(self) t += 1 @@ -217,7 +216,7 @@ class D_star_Lite(object): children = [i for i in self.CLOSED if getDist(s, i) <= self.env.resolution*np.sqrt(3)] else: children = list(self.CHILDREN[s]) - snext = children[np.argmin([cost(self,s,s_p) + self.getg(s_p) for s_p in children])] + snext = children[np.argmin([self.getcost(s,s_p) + self.getg(s_p) for s_p in children])] path.append([s, snext]) s = snext if ind > 100: diff --git a/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/env3D.cpython-37.pyc index e109d7c55c7555348267a634f036116ef378a5a3..41eb6c0462f46e992dc0d4566d89e6ace8cf791a 100644 GIT binary patch delta 756 zcmY+CTWb?R6oBWYK2XF05>|mB}gyi!Jmg z%pXV*pL{6duh0k4*9iR)KKLk}Gby$1aK1g4nb~vM-&YRmLC5zUhEMlfC90kM6wH}B zCYsAA!~2LFau%_iBOw<^o{3QbrI@pnr^bXX!#69jgzrNSgrw!RT=j=hcT7q4G#MPzP{;fdNdTc(9u7#Qlk`L(i$>QFm) z0UQWH&Z}MP5LQ)WC#O~@s;h!3P`nDVseanK{u)K-TMT4NJ$L@}im^yU=Envub5HDv z1a#6t1|uF@F^}y;XwQ`PI43-%7|V~!x;$4MxR(mkc(eNM4yNZ(kySzsF}9bMI-Tg% zTPZP-JKodUH453spk(D`#6W%Y-p#bgq%>unaGLP{jq-+i=r>_q?f5;oslNDiNYpRC z!>^0a3&962glwzBQnR12k%Qvme9YXavD8kzle(VVBvL$@AIB0ow&iGklXH=$yz&`X zRsX}VlJS6$r#`CNjYu&$&*=A7CuKQmHPhg~JY73iy8I0IMJR@k_` zNIN=&2ZVnu(wgp58v~C}^bcTxGi4bTmHQ*?mUNezbW-=Qt!69tT*opkvnqmt1%Cm< CVviO8 delta 576 zcmY+BF-#Oe6o%)`aJ#d6w+nZxu$L~I7DW&*$Xe->4ux~d-}8G4;9 zwWYeS$Em_T?8?MVWTLyub_Q+7`=>LMWN2sJX7(|gyMa@=cBw+Ao0w4~9D9vMv)Os| z#u&=5lY3H}WXrn*=QLx4Hs*r2#nvFWBxQKUMNMX@edgbkZrG7bEmZ%z7kluf`CZ(PHl(R|j^u6AP;wl!P;EYoeE diff --git a/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/plot_util3D.cpython-37.pyc index 8e23f3c472c51c5fb20a32fbde80a6f255d4f42e..af8baf399a72f2b2dfb3918584cefb970a4ad84c 100644 GIT binary patch delta 28 icmcbmb4!QYiIj;AL`%+RVxq$^iguBL^7( diff --git a/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc b/Search-based Planning/Search_3D/__pycache__/utils3D.cpython-37.pyc index bc2b6df649fedd8add6ec612d8781d15f5bab77b..d55e468bf1280c77826d6404da0c73eb9d847a7d 100644 GIT binary patch delta 2506 zcmaJCOKclObY}MBjopScag)&eoWyZr(x^?+lr$}k8$y3-K24BXtxAoZ**g9vj5ldo zt#PT#0jZ)Xd*O3wIdeeDg##yqgg}5eufzo;Brd3ML0k~;%{oC8!L0VpdvD&n|IEC* zP`f-dvwwe(z*zsGCHdJXAwMFqI|6_t;4g2|heHdYDZZaK-lJxSpL}@t%*ZJo;k`hH zc^~h;N6dpf$_HR>sPi!~ zqgxScfS;(*S@KHQj9CY8-Rid@HI;UeZTi~9E5sV~g{^2isZk0JEtB~4P9yzQuc{C& zM*Sj2eGPy^a>S;Om_s>z%$i7r0Y~BNSrWoEh0{Gu*~4HpxVKuzU^PfGdPT8SM{!t< zfl+l{C}2ZS1Za9Vu6gujp#kdYsr7ZM#69|whyxv0#UP5Icj6!bcd{YUpaVi!GfnJR z4Gs-?bWw}|$D<4GlVBGebsIyibi_?Hw;VPWW@g6DoSB(91p?)ExiLs}{MdPlUz4Pw zR2<*op%9FTn zWv#GLEG5f!Y9%kQ{e?^zdLIv5{ySMBfopvLJVjV#Q(Y_YMN>(Y$^tK0AB1o@fH{K$ zxdB93UcfBM!YsygD$eaZX#PHM2`k~-8N(ggZA+|`Jx!!a8B5kG-sGuk>GUX=NvFre zF_fZdlP#v;Pm2jj&v-%GEs;f4*ifFHF08HO%ko0?!#ey<|48!^3gFcyO`{v` zbbo?wx_A1sO;ULo7vl(4-9P&0=}q^1G+IT^@qx%eQ|5_?ItnLwCc15SF7S%4z(Y~d zO3k)xUq|-cHQ-bfydXjHlB0?9wwlxMPJ&Sy02*&onXrJwv{!t6)vn2BoIm+G_s|v zy|Pxe=iFCg2dj5b$VeuOscb2kj5E(z0^AnaEqP`#-+YZjP1Ivl6Y_P;y#F4KFQ?@T z`7<7i%(xtZMCOJZe@ zSaz*gM4mhg7Gg4YGR~VwVzA;401Tj%Dq6{;r`p+~Rsp1;nC|IY_^YW~l8tfd_jV7bnXfr}aCV0RI b8g0Q=BNz-D4MC_#6OFJ|sFOh(X_);5C}srL delta 2202 zcmaJ?O>7fK6rMM`Ua!|7xFJB&{1BW!yKNu|phBP}aS0G0KULsjkgQGCIF9WN8oM+J zyI3?-s;Wv&G#3s%lpcEHQ2A7;$DVp>PnA|`5AC^Cd#!rtd$Y!b1GP18-n@D9y*KY? zcIW*ki>b_k0~W(y;m3~LzyxEz)8hG0LuMLB^*6ScT8xi72b}iDkc~SNdmqh=O*#Y4 zaa?vfNoVje%N}-yoMD`gIH#NuXB2miIw>cOk}l`;-d{&d@v8&NEPbZ&75r_~;|LtZ z@g5oY8cB_P2{^dN*0ps#q;ZCGB*6;&DLCPu7j#S*SKT&x*zckKkA!)%+tMyXLwQ1^kN0Ww`!(mdD;#~|Mdj{ z=&qpX2pvf}CL%Zm#{Fu|UM&S}s~hDHDsC=Z!D0Y616T?S``*30>jW?>(u|#F>9`mr z2nvEYLF6!@=#`?ospwXg_-*9vO_JD~B(XQQKeWVpf^S19Ymi=ZW|ULprRX~N=7UIDA_N^Ue^Lm-qY zSc*W?NC>M?RRqHL?00RS`_O#~z1RmE0xADmzAGawST0V>pA*M2FKVO%6N*LnE>7Yi zl0bKg)tWd*tX3&vrOpL>t-PU!xJfLhdJ_*7^gQ>5w1 z2sw`3izV<<_g)c$=s?<3?JM3yUeHBU^MoC#f;CREYor-@5iD_;q)8$tC?Gvi3JCVo zHj1hy5(dYXhfYh8pWIHc5szF59ZXqvJE)rRNV`XJR(Md5yTCkGk zOE!)babB%60o+H6-@g7mEH@tZJOtw=%Ec|o`#yub{Jif6BTs_cGS}Y+OXQl|?eB%P z#3qI;BxvFo>+!<#JU}?*|%QMqEb@uRVa1dZ>cP>RPevh|BWd zV^_g#%nn=vsK`%`KkD5e9eUByAS%SuhegoKA@0f1?V5*<=id#P4XCI^Xh1`flT z(iX%6#LLF!ZFkMC)oeGd3##p~uueX9fr4MZgk?x1>9F9p=v~xgjgn- zDzr7?)Mb@_RQ}4zmNnO| H%!K~{Y!JaV diff --git a/Search-based Planning/Search_3D/env3D.py b/Search-based Planning/Search_3D/env3D.py index bebf1d0..1204413 100644 --- a/Search-based Planning/Search_3D/env3D.py +++ b/Search-based Planning/Search_3D/env3D.py @@ -59,6 +59,7 @@ class obb(object): self.P = P self.E = E self.O = O + self.T = np.vstack([np.column_stack([self.O.T,-self.O.T@self.P]),[0,0,0,1]]) def getAABB2(blocks): # used in lineAABB @@ -85,7 +86,7 @@ class env(): self.AABB = getAABB2(self.blocks) self.AABB_pyrr = getAABB(self.blocks) self.balls = getballs() - self.OBB = np.array([obb([2.6,2.5,1],[0.2,1,1],R_matrix(0,0,45))]) + self.OBB = np.array([obb([2.6,2.5,1],[0.2,2,1],R_matrix(0,0,45))]) #self.OBB = np.squeeze(np.vstack([self.OBB,OBB2AABB(self.OBB[0])])) #print(self.OBB) # self.OBB = [] @@ -154,6 +155,7 @@ class env(): if mode == 'rotation': # this makes an OBB rotate ori = [self.OBB[obb_to_move]] self.OBB[obb_to_move].O = R_matrix(z_angle=theta[0],y_angle=theta[1],x_angle=theta[2]) + self.OBB[obb_to_move].T = np.vstack([np.column_stack([self.OBB[obb_to_move].O.T,-self.OBB[obb_to_move].O.T@self.OBB[obb_to_move].P]),[0,0,0,1]]) return self.OBB[obb_to_move], ori[0] diff --git a/Search-based Planning/Search_3D/plot_util3D.py b/Search-based Planning/Search_3D/plot_util3D.py index 1293e5c..6bf565b 100644 --- a/Search-based Planning/Search_3D/plot_util3D.py +++ b/Search-based Planning/Search_3D/plot_util3D.py @@ -91,7 +91,7 @@ def visualization(initparams): 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=105.) + ax.view_init(elev=8., azim=120.) #ax.view_init(elev=-8., azim=180) ax.clear() # drawing objects diff --git a/Search-based Planning/Search_3D/utils3D.py b/Search-based Planning/Search_3D/utils3D.py index 22ffc2f..64269d2 100644 --- a/Search-based Planning/Search_3D/utils3D.py +++ b/Search-based Planning/Search_3D/utils3D.py @@ -52,12 +52,9 @@ def isinball(i, x): return False def isinobb(i, x): - # pt = i.O.T@np.array(x) # transform the point from {W} to {body} - # minx,miny,minz,maxx,maxy,maxz = i.P[0] - i.E[0],i.P[1] - i.E[1],i.P[2] - i.E[2],i.P[0] + i.E[0],i.P[1] + i.E[1],i.P[2] + i.E[2] - T = np.vstack([np.column_stack([i.O.T,-i.O.T@i.P]),[0,0,0,1]]) - pt = T@np.append(x,1) # transform the point from {W} to {body} - minx,miny,minz,maxx,maxy,maxz = - i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2] - block = [minx,miny,minz,maxx,maxy,maxz] + # transform the point from {W} to {body} + pt = i.T@np.append(x,1) + block = [- i.E[0],- i.E[1],- i.E[2],+ i.E[0],+ i.E[1],+ i.E[2]] return isinbound(block, pt) def OBB2AABB(obb): @@ -100,30 +97,33 @@ def lineAABB(p0, p1, dist, aabb): mid = [(p0[0] + p1[0]) / 2, (p0[1] + p1[1]) / 2, (p0[2] + p1[2]) / 2] # mid point I = [(p1[0] - p0[0]) / dist, (p1[1] - p0[1]) / dist, (p1[2] - p0[2]) / dist] # unit direction hl = dist / 2 # radius - P = aabb.P # center of the AABB - E = aabb.E # extents of AABB - T = [P[0] - mid[0], P[1] - mid[1], P[2] - mid[2]] + T = [aabb.P[0] - mid[0], aabb.P[1] - mid[1], aabb.P[2] - mid[2]] # do any of the principal axis form a separting axis? - if abs(T[0]) > (E[0] + hl * abs(I[0])): return False - if abs(T[1]) > (E[1] + hl * abs(I[1])): return False - if abs(T[2]) > (E[2] + hl * abs(I[2])): return False + if abs(T[0]) > (aabb.E[0] + hl * abs(I[0])): return False + if abs(T[1]) > (aabb.E[1] + hl * abs(I[1])): return False + if abs(T[2]) > (aabb.E[2] + hl * abs(I[2])): return False # I.cross(x axis) ? - r = E[1] * abs(I[2]) + E[2] * abs(I[1]) + r = aabb.E[1] * abs(I[2]) + aabb.E[2] * abs(I[1]) if abs(T[1] * I[2] - T[2] * I[1]) > r: return False # I.cross(y axis) ? - r = E[0] * abs(I[2]) + E[2] * abs(I[0]) + r = aabb.E[0] * abs(I[2]) + aabb.E[2] * abs(I[0]) if abs(T[2] * I[0] - T[0] * I[2]) > r: return False # I.cross(z axis) ? - r = E[0] * abs(I[1]) + E[1] * abs(I[0]) + r = aabb.E[0] * abs(I[1]) + aabb.E[1] * abs(I[0]) if abs(T[0] * I[1] - T[1] * I[0]) > r: return False return True def lineOBB(p0, p1, dist, obb): - T = np.vstack([np.column_stack([obb.O.T,-obb.O.T@obb.P]),[0,0,0,1]]) - p0 = T@np.append(p0,1) # transform the points to the box - p1 = T@np.append(p1,1) - return lineAABB(p0[0:3],p1[0:3],dist,obb) + # transform points to obb frame + res = obb.T@np.column_stack([np.array([p0,p1]),[1,1]]).T + # record old position and set the position to origin + oldP, obb.P= obb.P, [0,0,0] + # calculate segment-AABB testing + ans = lineAABB(res[0:3,0],res[0:3,1],dist,obb) + # reset the position + obb.P = oldP + return ans def OBBOBB(obb1, obb2): # https://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?print=1 @@ -311,7 +311,12 @@ def obstacleFree(initparams, x): def cost(initparams, i, j, dist=None, settings='Euclidean'): - collide, dist = isCollide(initparams, i, j, dist) + if initparams.env.resolution < 0.25: + if dist==None: + dist = getDist(i,j) + collide = False + else: + collide, dist = isCollide(initparams, i, j, dist) # collide, dist= False, getDist(i, j) if settings == 'Euclidean': if collide: @@ -335,7 +340,17 @@ def initcost(initparams): return c if __name__ == "__main__": - pass - # obb1 = obb([0,0,0],[1,1,1],[[1,0,0],[0,1,0],[0,0,1]]) + import time + from env3D import R_matrix, obb + obb1 = obb([2.6,2.5,1],[0.2,2,2],R_matrix(0,0,45)) # obb2 = obb([1,1,0],[1,1,1],[[1/np.sqrt(3)*1,1/np.sqrt(3)*1,1/np.sqrt(3)*1],[np.sqrt(3/2)*(-1/3),np.sqrt(3/2)*2/3,np.sqrt(3/2)*(-1/3)],[np.sqrt(1/8)*(-2),0,np.sqrt(1/8)*2]]) - # print(OBBOBB(obb1, obb2)) + p0, p1 = [2.9,2.5,1],[1.9,2.5,1] + dist = getDist(p0,p1) + start = time.time() + for i in range(3000*27): + lineAABB(p0,p1,dist,obb1) + #lineOBB(p0,p1,dist,obb1) + print(time.time() - start) + + +