From 197fdf83a41fabea31a26aee1b97e523f840f54a Mon Sep 17 00:00:00 2001 From: yue qi <391311qy@gmail.com> Date: Sat, 25 Jul 2020 01:29:47 -0700 Subject: [PATCH] rrt --- .../rrt_3D/__pycache__/env3D.cpython-37.pyc | Bin 1908 -> 5161 bytes .../__pycache__/plot_util3D.cpython-37.pyc | Bin 4804 -> 5793 bytes Sampling-based Planning/rrt_3D/env3D.py | 141 ++++++++++++++++-- Sampling-based Planning/rrt_3D/plot_util3D.py | 49 +++++- Sampling-based Planning/rrt_3D/rrt3D.py | 2 +- 5 files changed, 172 insertions(+), 20 deletions(-) diff --git a/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc index 8dc318ed4080fc98860ae1c1d3a2ad080d9d4583..87f168dead362052eb8b3eef05d3db3bd39047b0 100644 GIT binary patch literal 5161 zcmb7I&2Jn@6|d^k)VQi%z50CL zue~-kqOW`+ zR9$(`g!01bk=?0b+)z!7n>=>al$yr6x|&h5=sk5t&8f3$UcIHxMb%BI&Z~uIveUr6 zMRfuDE*x2%W;mrTKDP9)!{(9bOe^b=Xk9uy2_}mc-(xRw`duMjun_t~i3oqt=@t?@OZgrEmQu_bfCaQkSNQ~`&6VPIcHrHH;?I#V{&6HN6(;}pwA8z5mzLX#Jx6ZJdY+69jOGIC7upvXDgc?mN`MNvC*ZK zRH(0@G84W4kit_eQVU|w3uRv<*kMd%0#jv-U0@=3GylZig;(~Lm~yYd@Lmanff>OY zSp;zph&&NawiNlI+PWsX3}@-#$7H_8%xIzk+>{S8`IAiEhh92y*x%F(w1I4StectZ z`@LZ=@%Ays zkG_D3cTidR-3pKYZ*kcpNQ>PRPNKes%F_BNfK)Qy5mS7bGN%&zg@am2#kRFwHEjd- zyDeq~sVPMQ(ul~R5%yJ?x3*DO{WcD5)$}*f=_P8uMGY}KtL~BcJ7_IsH95vt*>qiKA?vP-3= z6WOvauw~B`mYrI;&n;|LuD2{>q$D`_qVwpC0=>>G-P|m1w6Sc$F6X5FJ}{-AZ4AoC zkj#kt6igxtCzh%rq(ByVpjhHZE|^D}v}PhaEF(O?CFS9vzJ6@HGY*Arv^DZ4p)aC2 zCLheQEO|mAq^JB(PF*)?{}eT+#5fJy&rl18eu?~U(C@#=pPzJkE#zeJ&Q_?ySf8QO z-^5rx@)Np;C=u}8SiLh4ntrJM;xw%JCFAD}7y?-|IrhP0AWhB`d4gJhwBzG7^V z^SCC#42}D#&6=84GuT;Gvv}fMW+RO;?vEYeH*kx7iyAW9F2@D{)RWklz&`>=Yvd)2 z=A;(bsuMr*< z9g0OdGJnanysISwbivX_z}vu?b%R^sx###HGow`euYcM+!gnzxMSUVYPe?v=Mu zl4VZyIdP!#Dcw)qO?>TwCoXtm=|6l8Px6E@=Y}MnFmLECP9S+i92l=G6G5IMs`Hg! zLvOKrObt<5+&9Kf+uSmQfcQOyg=eg>{dAS_wkb|wSV|^{f+KBYhO&!(*hxO2* zWL+PEC=kH8u9y#uxpVrZ|!36qCIu z<$mmxO@hjft3|l0bVS|wWEo@0lUpG3rp#obp-&WjGR|Ys>$2oFtz4t#9yQ}I!w~saeI3ouQQ0-S(wlLs zF0=_4`p_8!y`k^7X0n>^t5Fx{JmiC~tYqi!%3?`KY>0IuMhL9qHO`)17f^v)-)d*4)~HJ^j#?{{ho)@5%rG literal 1908 zcma)6&2Ah;5bmD;9oB0wAvnZACWMGNWEaIZWSLR0!X;XeazI)s8I8Bc2m~*a8%*bNBV~szE z%yTlaBPX(-_0B0JOnXc`clnm^;;!I$GM-ZWz%W31AQ)aOL19=7 ziQ!@kLgC73Ry|m0%!QQ!wl~zOU$0{0jK<_lI~`VZpFFvHrkCWBR{9k^(<|*#`;Ht^ zw#Eh*PNnaYb9x0yo;>`TC@<}bLb~hY`1jv%IDQP7sL|{YKi*Mey)|8HC!4M5w%<5^ z*zSDX?s$Iu^G`5t-1p-{9Dm*SI|wuggfP4fBCdgoNkyLY&uB%zCpXCt+KKU){6aYT zd79LQpGf-5qtfM($kTb50o|Ymb>VM|@&W92pd;Y;TgAIaFll)g zBl9$70sD+tYdCdBacWC3i6|yv3%tst+rT7xsZ}+TdgIlbaAw0I!(U;T6~eK?3|Gkh zE|vdPe|71G0vu1{cmuCN0gq5na2sZ~Kw9j#5SlPe{{woC>2L=Xc3}hi#1_R-1oYy4 zw5*}R5N$XGy3?L~11v&4EOb+$4Okp&~5(kr7fD|kh&Xl37o zrV8}ZsO)nJGqbdsnT>U;s~^K_0X9eN7{CI(+37I?d70y;pnrVURkoR2Z(oBQ-*anb zcA!e> z)JxEHC{_NB1JVhCBumO5_yB>y5)x`b2Vfs;&}ARM9kwnV3lVJ;1txM!U73#C!Xv3( zlCbH!=ZTnyX|jwwuqw(CeYu9`cb2hdt4hA2@NMAlqS{1-HT6u)Ityn}5J)!&W;vUu zIPV9+352LE{0{nmgzDv$yLFnx!*j@rS4XJL{Bf#*s)2v|OabIoi%D;72L z@nKNaQ2zWbEGJeLeFOK9X2FxJWzZk_5L(o^6`26c32-LVJj4xs)I@0Xpv 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 c273b53778c539cfe49df0cfbae421e8c551ce68..227df59be5cadce6d15685fbeb1c0e3996d96308 100644 GIT binary patch delta 1806 zcmY*ZTW=dh6rMAC@$A}hV%M=_C%pkpu1w4Yp^FH#15JE_ZXTGyeDeP#z`R1H6XKvr@54$!- zv*SHIzQE`GK3F?ext|@NFYfQYI5D7ooxVd;S)I{6cf?e$F6*rBg{4ndbWZ2Na(as{ zz?awkx`=kmY(bY6rTI+LSy3JQ?!mVX#nDCg5b$&Ny`AHj<_hs-_$<^6>I7=@Dj*P@ z=x1x9y+JJkmm!6sZf(dGw{}Q%r%%)!aJM8{GL)FYnlvelG=qR@$|&Z~0b;d$eJNPD zy0jY88!?@V>0C^wt5$5S%*;%w6|+#AS<$Ppb7R&7O;h4=2@D9)7O{t@TH+dor>! zF}w`G!0l*a>(uzTsc&O*lKE9;hZtUGILyE^n=ytX4F8i3bf?Yq%^KvxxtqKJL2r^z zc`8ztMra>ps1G9YQOt>lf4B#ze4InGNvMK(ASp#MXQJ0sxxGgUoL*gMDhe$ZWwVGL z-IHcGkR1zGjyJJJ{AT!Nv?N=>Qm}BV>K??zt>1D&*-38*2q;tUVSQ?cg%hUgS!fUH zT$i}1RHt`CkO=x92vKjVdTtHoAfI$qeGB)Jgas`YJ@a+Yzv(GFirZX*su$<6Fv!7M zw~(QPI5$!2qn?!rg?j@bts!#JzDH&l3o3`y)o3*0?Wam~(HjMR|_t)fE;f*fu@5$+qhuW{FI< zPX1bmF8Rah9b@x+M?$_ zO6QN$$>`nu_U=(8FPl#>IXe?FI}}LTa;9ua{Qu7`#IlJ JyVOBN{{R*dYW@HK delta 882 zcmX|;&rcIU6vyXnx4YYIcej-7(v~VQ5y-~$V*C*kf`B2$AOs^K)TlrnLCR03B@x{f z6s{za`2%c^UL=O-A7DIq@Z_=aY(k6&4_=HRzG+r=GN1Xr?|buOXBwqX-S&`WnT)>Q zw~inDbocEX1pB>Hemk0&+=a4@E?Q`la?ru_E-R<84Kp;hA>3wu_T9&WIFtp~`iKYG zGz=WxGA}~DL~B-UxiT1FLax<3;KSl|GQV{Uj0LR15Ily;l2Y|3f>*%*)RcORwAz3W zXpDzSLOswc7HX)Yv8MnF^t#<(z-msQ zx8|jHM#!$UMPX(qilUA5O8Wvd02NH^#i-7uI!#{3Nfg5rS~8-Ivf^#(GI-*9szh|H z(jd)8B`<$Zs*XHUSZIWaD7H^BxmLyulTpU`pAj#u-l2{_M?0`xMs$dduV^{HkAU z#uitxsp8t|JU6I_pC>e7vI)cDn?0Hvklq#P8FOnJt2oOy`7niI+!@bK$mAV~Nr@?m zY4P5IDJ=4^p5}H)K<6up<^kU%CM6hj-JbW7>$EdQ&`W|4>+v@{z=%L`7mr zVp)v1=d{*#6C!Z0$7ji%5ue?q$_r9ug+HObN&RV4;VWFGc?ZE)`E#kaB(^1FJwwG1Sgwgd6rkKAC diff --git a/Sampling-based Planning/rrt_3D/env3D.py b/Sampling-based Planning/rrt_3D/env3D.py index 977aa8d..1204413 100644 --- a/Sampling-based Planning/rrt_3D/env3D.py +++ b/Sampling-based Planning/rrt_3D/env3D.py @@ -5,9 +5,21 @@ @author: yue qi """ import numpy as np +# from utils3D import OBB2AABB +def R_matrix(z_angle,y_angle,x_angle): + # x angle: row; y angle: pitch; z angle: yaw + # generate rotation matrix in SO3 + # RzRyRx = R, ZYX intrinsic rotation + # also (r1,r2,r3) in R3*3 in {W} frame + # used in obb.O + # [[R p] + # [0T 1]] gives transformation from body to world + return np.array([[np.cos(z_angle), -np.sin(z_angle), 0.0], [np.sin(z_angle), np.cos(z_angle), 0.0], [0.0, 0.0, 1.0]])@ \ + np.array([[np.cos(y_angle), 0.0, np.sin(y_angle)], [0.0, 1.0, 0.0], [-np.sin(y_angle), 0.0, np.cos(y_angle)]])@ \ + np.array([[1.0, 0.0, 0.0], [0.0, np.cos(x_angle), -np.sin(x_angle)], [0.0, np.sin(x_angle), np.cos(x_angle)]]) -def getblocks(resolution): +def getblocks(): # AABBs block = [[3.10e+00, 0.00e+00, 2.10e+00, 3.90e+00, 5.00e+00, 6.00e+00], [9.10e+00, 0.00e+00, 2.10e+00, 9.90e+00, 5.00e+00, 6.00e+00], @@ -19,31 +31,134 @@ def getblocks(resolution): Obstacles = [] for i in block: i = np.array(i) - Obstacles.append([j/resolution for j in i]) + Obstacles.append([j for j in i]) return np.array(Obstacles) -def getballs(resolution): - spheres = [[16,2.5,3,2],[10,2.5,1,1]] +def getAABB(blocks): + # used for Pyrr package for detecting collision + AABB = [] + for i in blocks: + AABB.append(np.array([np.add(i[0:3], -0), np.add(i[3:6], 0)])) # make AABBs alittle bit of larger + return AABB + +class aabb(object): + # make AABB out of blocks, + # P: center point + # E: extents + # O: Rotation matrix in SO(3), in {w} + def __init__(self,AABB): + self.P = [(AABB[3] + AABB[0])/2, (AABB[4] + AABB[1])/2, (AABB[5] + AABB[2])/2]# center point + self.E = [(AABB[3] - AABB[0])/2, (AABB[4] - AABB[1])/2, (AABB[5] - AABB[2])/2]# extents + self.O = [[1,0,0],[0,1,0],[0,0,1]] + +class obb(object): + # P: center point + # E: extents + # O: Rotation matrix in SO(3), in {w} + def __init__(self, P, E, O): + 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 + AABB = [] + for i in blocks: + AABB.append(aabb(i)) + return AABB + +def getballs(): + spheres = [[16,2.5,4,2],[10,2.5,1,1]] Obstacles = [] for i in spheres: - Obstacles.append([j/resolution for j in i]) + Obstacles.append([j for j in i]) return np.array(Obstacles) +def add_block(block = [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00]): + return block + class env(): def __init__(self, xmin=0, ymin=0, zmin=0, xmax=20, ymax=5, zmax=6, resolution=1): self.resolution = resolution - self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) / resolution - self.blocks = getblocks(resolution) - self.balls = getballs(resolution) + self.boundary = np.array([xmin, ymin, zmin, xmax, ymax, zmax]) + self.blocks = getblocks() + 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,2,1],R_matrix(0,0,45))]) + #self.OBB = np.squeeze(np.vstack([self.OBB,OBB2AABB(self.OBB[0])])) + #print(self.OBB) + # self.OBB = [] self.start = np.array([0.5, 2.5, 5.5]) self.goal = np.array([19.0, 2.5, 5.5]) + self.t = 0 # time - def visualize(self): - # fig = plt.figure() - # TODO: do visualizations - return + def New_block(self): + newblock = add_block() + self.blocks = np.vstack([self.blocks,newblock]) + self.AABB = getAABB2(self.blocks) + self.AABB_pyrr = getAABB(self.blocks) + + def move_start(self, x): + self.start = x + + def move_block(self, a = [0,0,0], s = 0, v = [0.1,0,0], theta = [0,0,0], block_to_move = 0, obb_to_move = 0, mode = 'uniform'): + # t is time , v is velocity in R3, a is acceleration in R3, s is increment ini time, + # R is an orthorgonal transform in R3*3, is the rotation matrix + # (x',t') = (x + tv, t) is uniform transformation + if mode == 'uniform': + ori = np.array(self.blocks[block_to_move]) + self.blocks[block_to_move] = \ + np.array([ori[0] + self.t * v[0],\ + ori[1] + self.t * v[1],\ + ori[2] + self.t * v[2],\ + ori[3] + self.t * v[0],\ + ori[4] + self.t * v[1],\ + ori[5] + self.t * v[2]]) + + self.AABB[block_to_move].P = \ + [self.AABB[block_to_move].P[0] + self.t * v[0], \ + self.AABB[block_to_move].P[1] + self.t * v[1], \ + self.AABB[block_to_move].P[2] + self.t * v[2]] + # return a range of block that the block might moved + a = self.blocks[block_to_move] + # return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \ + # a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]). \ + # np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \ + # ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution]) + return a,ori + # (x',t') = (x + a, t + s) is a translation + if mode == 'translation': + ori = np.array(self.blocks[block_to_move]) + self.blocks[block_to_move] = \ + np.array([ori[0] + a[0],\ + ori[1] + a[1],\ + ori[2] + a[2],\ + ori[3] + a[0],\ + ori[4] + a[1],\ + ori[5] + a[2]]) + + self.AABB[block_to_move].P = \ + [self.AABB[block_to_move].P[0] + a[0], \ + self.AABB[block_to_move].P[1] + a[1], \ + self.AABB[block_to_move].P[2] + a[2]] + self.t += s + # return a range of block that the block might moved + a = self.blocks[block_to_move] + return np.array([a[0] - self.resolution, a[1] - self.resolution, a[2] - self.resolution, \ + a[3] + self.resolution, a[4] + self.resolution, a[5] + self.resolution]), \ + np.array([ori[0] - self.resolution, ori[1] - self.resolution, ori[2] - self.resolution, \ + ori[3] + self.resolution, ori[4] + self.resolution, ori[5] + self.resolution]) + # return a,ori + # (x',t') = (Rx, t) + 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] + if __name__ == '__main__': newenv = env() - print(newenv.balls) diff --git a/Sampling-based Planning/rrt_3D/plot_util3D.py b/Sampling-based Planning/rrt_3D/plot_util3D.py index b164fd6..a05ca61 100644 --- a/Sampling-based Planning/rrt_3D/plot_util3D.py +++ b/Sampling-based Planning/rrt_3D/plot_util3D.py @@ -42,6 +42,34 @@ def draw_block_list(ax, blocks ,color=None,alpha=0.15): h = ax.add_collection3d(pc) return h +def obb_verts(obb): + # 0.017004013061523438 for 1000 iters + ori_body = np.array([[1,1,1],[-1,1,1],[-1,-1,1],[1,-1,1],\ + [1,1,-1],[-1,1,-1],[-1,-1,-1],[1,-1,-1]]) + # P + (ori * E) + ori_body = np.multiply(ori_body,obb.E) + # obb.O is orthornormal basis in {W}, aka rotation matrix in SO(3) + verts = (obb.O@ori_body.T).T + obb.P + return verts + + +def draw_obb(ax, OBB, color=None,alpha=0.15): + f = np.array([[0, 1, 5, 4], [1, 2, 6, 5], [2, 3, 7, 6], [3, 0, 4, 7], [0, 1, 2, 3], [4, 5, 6, 7]]) + n = OBB.shape[0] + vl = np.zeros((8 * n, 3)) + fl = np.zeros((6 * n, 4), dtype='int64') + for k in range(n): + vl[k * 8:(k + 1) * 8, :] = obb_verts(OBB[k]) + fl[k * 6:(k + 1) * 6, :] = f + k * 8 + if type(ax) is Poly3DCollection: + ax.set_verts(vl[fl]) + else: + pc = Poly3DCollection(vl[fl], alpha=alpha, linewidths=1, edgecolors='k') + pc.set_facecolor(color) + h = ax.add_collection3d(pc) + return h + + def draw_line(ax,SET,visibility=1,color=None): if SET != []: for i in SET: @@ -52,8 +80,8 @@ def draw_line(ax,SET,visibility=1,color=None): ax.add_line(line) def visualization(initparams): - if initparams.ind % 10 == 0 or initparams.done: - V = np.array(initparams.V) + if initparams.ind % 20 == 0 or initparams.done: + V = np.array(list(initparams.V)) E = initparams.E Path = np.array(initparams.Path) start = initparams.env.start @@ -61,15 +89,21 @@ def visualization(initparams): edges = E.get_edge() # generate axis objects ax = plt.subplot(111, projection='3d') - ax.view_init(elev=0., azim=90) + #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=180) ax.clear() # drawing objects draw_Spheres(ax, initparams.env.balls) draw_block_list(ax, initparams.env.blocks) + if initparams.env.OBB is not None: + draw_obb(ax,initparams.env.OBB) draw_block_list(ax, np.array([initparams.env.boundary]),alpha=0) draw_line(ax,edges,visibility=0.25) draw_line(ax,Path,color='r') - ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',) + if len(V) > 0: + ax.scatter3D(V[:, 0], V[:, 1], V[:, 2], s=2, color='g',) 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') # adjust the aspect ratio @@ -80,7 +114,7 @@ def visualization(initparams): ax.get_proj = make_get_proj(ax,1*dx, 1*dy, 2*dy) plt.xlabel('x') plt.ylabel('y') - plt.pause(0.001) + plt.pause(0.0001) def make_get_proj(self, rx, ry, rz): ''' @@ -134,4 +168,7 @@ def make_get_proj(self, rx, ry, rz): M0 = np.dot(viewM, np.dot(aspectM, worldM)) ## M = np.dot(perspM, M0) return M - return get_proj \ No newline at end of file + return get_proj + +if __name__ == '__main__': + pass \ No newline at end of file diff --git a/Sampling-based Planning/rrt_3D/rrt3D.py b/Sampling-based Planning/rrt_3D/rrt3D.py index 5c35ea5..0261b2b 100644 --- a/Sampling-based Planning/rrt_3D/rrt3D.py +++ b/Sampling-based Planning/rrt_3D/rrt3D.py @@ -45,7 +45,7 @@ class rrtstar(): if not isCollide(self, xnearest, xnew): self.V.append(xnew) # add point self.wireup(xnew, xnearest) - visualization(self) + # visualization(self) self.i += 1 self.ind += 1 if getDist(xnew, self.env.goal) <= 1: