From bb945dddf4fd6f4124e439e98cf4314f296f48fd Mon Sep 17 00:00:00 2001 From: zhm-real Date: Tue, 23 Jun 2020 19:37:10 -0700 Subject: [PATCH] update format --- Sampling-based Planning/.idea/.gitignore | 3 + .../.idea/Sampling-based Planning.iml | 8 + .../inspectionProfiles/profiles_settings.xml | 6 + Sampling-based Planning/.idea/misc.xml | 4 + Sampling-based Planning/.idea/modules.xml | 8 + Sampling-based Planning/.idea/vcs.xml | 6 + .../rrt_2D/__pycache__/env.cpython-37.pyc | Bin 0 -> 1280 bytes .../__pycache__/plotting.cpython-37.pyc | Bin 0 -> 2616 bytes Sampling-based Planning/rrt_2D/env.py | 45 ++++ Sampling-based Planning/rrt_2D/plotting.py | 80 +++++++ Sampling-based Planning/rrt_2D/rrt*.py | 200 ++++++++++++++++++ Sampling-based Planning/rrt_2D/rrt.py | 114 ++++++++++ .../rrt_3D/__pycache__/env3D.cpython-37.pyc | Bin 0 -> 1537 bytes .../rrt_3D/__pycache__/utils3D.cpython-37.pyc | Bin 0 -> 5364 bytes Sampling-based Planning/rrt_3D/env3D.py | 44 ++++ Sampling-based Planning/rrt_3D/rrt3D.py | 59 ++++++ Sampling-based Planning/rrt_3D/rrtstar3D.py | 79 +++++++ Sampling-based Planning/rrt_3D/utils3D.py | 177 ++++++++++++++++ 18 files changed, 833 insertions(+) create mode 100644 Sampling-based Planning/.idea/.gitignore create mode 100644 Sampling-based Planning/.idea/Sampling-based Planning.iml create mode 100644 Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml create mode 100644 Sampling-based Planning/.idea/misc.xml create mode 100644 Sampling-based Planning/.idea/modules.xml create mode 100644 Sampling-based Planning/.idea/vcs.xml create mode 100644 Sampling-based Planning/rrt_2D/__pycache__/env.cpython-37.pyc create mode 100644 Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc create mode 100644 Sampling-based Planning/rrt_2D/env.py create mode 100644 Sampling-based Planning/rrt_2D/plotting.py create mode 100644 Sampling-based Planning/rrt_2D/rrt*.py create mode 100644 Sampling-based Planning/rrt_2D/rrt.py create mode 100644 Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc create mode 100644 Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc create mode 100644 Sampling-based Planning/rrt_3D/env3D.py create mode 100644 Sampling-based Planning/rrt_3D/rrt3D.py create mode 100644 Sampling-based Planning/rrt_3D/rrtstar3D.py create mode 100644 Sampling-based Planning/rrt_3D/utils3D.py diff --git a/Sampling-based Planning/.idea/.gitignore b/Sampling-based Planning/.idea/.gitignore new file mode 100644 index 0000000..26d3352 --- /dev/null +++ b/Sampling-based Planning/.idea/.gitignore @@ -0,0 +1,3 @@ +# Default ignored files +/shelf/ +/workspace.xml diff --git a/Sampling-based Planning/.idea/Sampling-based Planning.iml b/Sampling-based Planning/.idea/Sampling-based Planning.iml new file mode 100644 index 0000000..5965bde --- /dev/null +++ b/Sampling-based Planning/.idea/Sampling-based Planning.iml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml b/Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/Sampling-based Planning/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/Sampling-based Planning/.idea/misc.xml b/Sampling-based Planning/.idea/misc.xml new file mode 100644 index 0000000..0e7ac62 --- /dev/null +++ b/Sampling-based Planning/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/Sampling-based Planning/.idea/modules.xml b/Sampling-based Planning/.idea/modules.xml new file mode 100644 index 0000000..caf2a07 --- /dev/null +++ b/Sampling-based Planning/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Sampling-based Planning/.idea/vcs.xml b/Sampling-based Planning/.idea/vcs.xml new file mode 100644 index 0000000..6c0b863 --- /dev/null +++ b/Sampling-based Planning/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ 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 new file mode 100644 index 0000000000000000000000000000000000000000..d6169c034fe4903af079d60653efa2648f37c594 GIT binary patch literal 1280 zcmZuwJ#W-N5M6&e7hgya9|aNuBqWTKSV~2KP((n8<`k6f3Rcdo@tx$4W9^H`3996W zgp%LDKXFU7Ep$}OTicgQ#9Fg{`*vnMZ+858I2IU7Ft+AzEEp8d-#Fjcmb?;wUr~g)y~H zQ)o&YrXB)2jrWDixJZ-6JL^DDo>cs#TofXfXUzbs$xKeNq!|LsWKvT=mV`!QS4sBi z2a}t0*541&w9J!eIV~5_kt|ag=g~Z_r@QkkE{a)^?#5YK%2_?ltLQk+=NY(@xJtyM zqs}FgvgZ45qNF(8o1Zm4=d)r~bB>HFeMyE(&w!p~KsDOy9agu1V$mJ8#o8u->Z9(d z{f){*<#0x!txM3>0Ra*bJqs*xk#ehL@oEFs*%j7q`UG8v(2yPiP?L5FfWU2#wJI#r zz~j2hZKznU_%$UCMI0gu-PbD-PU>`8T+*V3w5L!U1$TwSh((0bT6PG@J1u(;_!hQY zAUKilV-R4a_U+i`AauZr?G-}FO@-j@lU2~mI)$XsT<-K5SG{vhBr?$=k>iy};ihgU z(;J0Wn;LGk&DxS0-z6d?qY)g0mcm*gjm}Fw0ipBNC9u>}Y(7}?*o6l+722$mIQF{6 zzbVrvADmgCEv{seP;0r>3vGXxQRt(Ei#Sg>Zw8#_rC4OZ0q36=an|lgTwoJabzILT zc~VbH(YBDz8sA&XLARCeC^Rmtf`@PTUa%ExN-!hpLA?JL*7z^mEMCFm4;lLhG7RdD literal 0 HcmV?d00001 diff --git a/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..c055c0c7f811e29af02102937b0ff66ab5f67db1 GIT binary patch literal 2616 zcmb7G&u<$=6rPz~uh))~l8^!cR22>dqb5OBRUs5oL_;g2Vnt0wq*cmly|Y=bv)*-P zH>qRnQ*xvSF5HlkV=w#z{1+VehB)O2+$s)yZ|t~f6bTsX&Ago-Z|1%Cz4!dZ^mLV> zIluh*?N1fPey5MKW`Ow|l-)rmndC9+a7XYFZ*)w@)VzpG9m}yed%&b2%_AnwL*bMq z-(dC9DbBJ+ofk%s?ECa_)+XXtF%vL{OXdj4r8r`aA*ZD&OGn%>r6tRlDFv3aW#xz+ za;FTwDyKAW%NnkkDeUz)&GRS;eUeL!MxuM5>{E0DJ`jQpm`lWSacrCrCkCu7+9tS? z<}7d^%$32FC4+q4sN03;eFb`;1Y{M)m&txnOPiV7O!pG$se__2`6{Z!`jPVEpqM6J z1%B=&VI0(r!pwqrTbYoeO6Z<5Rm1S&o8?y836}ecGrJ?dV%Uv&TEp}rsNub%H z7l)~e@>VBX-taoznC6;Z7Rc-C<1NcdV8COz>;zxn1Wat403qOwfu7ao zGaCJwMtYsJxxP*2dap1ZJ$hU>i&9rbNnTXw!EUG`S=2PUA7xP<$ngRltea{IZz${} zl|dY3d12CYQSp+fu3}2ZC7xrDoYFS14e6s z$ub4cGpI9Oof@xB>DB7X)#KWLlO$+8EvJan9;`K-QZx4aoyT>7u|aqO7D?SX>X*eP|6XlADrg~$LE05*6U$-L!=FhfM|?s+jn*XYK%V`qC!@_eQU zrcT9^(uIHhR#DbNkU5n{XF;S0#fm)TSocOzX42u8K~A3~78Z^nG}Yfi7(l}z6xfk3}*ekKuX-aQ(4Wj=7vHk~>_| z4^(ql`3#iNJ|+ku7JnakrpjLv#P_dW`rcOzIDx_nWFSa&3ORx)*HH1`SX9L^kGd$W zQ79h^Ssn)~YS}nc?Z!kfd4)2#_6)r)gpen-As?XFF0dcOu1UpBJ40&)kBFs@)koNR zLCY7_6-v5(+Uc$~u$+->i}i23sv4!_^L{ux^1q)%fj54VS|t9GH>o8ooPUg1RHyVV zsI-6~{BHi|2fy`??xJ=~+^%k7#WAy1`V`x);^Ts*>UGRL1ZCtCCT?63$)qO!)Z7QI zP+lpmxNcE(-A*d^V&ZGAOWAF-qGljR14vO$DQa1cg5`IDyp>7~AARS!cTmMrx` self.goal_sample_rate: + return Node((np.random.uniform(self.x_range[0], self.x_range[1]), + np.random.uniform(self.y_range[0], self.y_range[1]))) + return self.xG + + def nearest_neighbor(self, node_list, n): + return self.node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y) + for nd in node_list]))] + + def new_state(self, node_start, node_goal): + node_new = Node((node_start.x, node_start.y)) + dist, theta = self.get_distance_and_angle(node_new, node_goal) + dist = min(self.expand_len, dist) + + node_new.x += dist * math.cos(theta) + node_new.y += dist * math.sin(theta) + node_new.parent = node_start + + return node_new + + def find_near_neighbor(self, node_new): + n = len(self.node_list) + 1 + r = min(self.connect_dist * math.sqrt((math.log(n) / n)), self.expand_len) + + dist_table = [math.hypot(nd.x - node_new.x, nd.y - node_new.y) for nd in self.node_list] + node_index = [dist_table.index(d) for d in dist_table if d <= r] + + return node_index + + def choose_parent(self, node_new, neighbor_index): + if not neighbor_index: + return None + + cost = [] + + for i in neighbor_index: + node_near = self.node_list[i] + node_mid = self.new_state(node_near, node_new) + + if node_mid and not self.check_collision(node_mid): + cost.append(self.update_cost(node_near, node_mid)) + else: + cost.append(float("inf")) + + if min(cost) != float('inf'): + index = int(np.argmin(cost)) + neighbor_min = neighbor_index[index] + node_new = self.new_state(self.node_list[neighbor_min], node_new) + node_new.cost = min(cost) + return node_new + + return None + + def search_best_goal_node(self): + dist_to_goal_list = [self.dis_to_goal(n) for n in self.node_list] + goal_inds = [dist_to_goal_list.index(i) for i in dist_to_goal_list if i <= self.expand_len] + + return goal_inds[0] + # safe_goal_inds = [] + # for goal_ind in goal_inds: + # t_node = self.new_state(self.node_list[goal_ind], self.xG) + # if self.check_collision(t_node): + # safe_goal_inds.append(goal_ind) + # + # if not safe_goal_inds: + # print('hahhah') + # return None + # + # min_cost = min([self.node_list[i].cost for i in safe_goal_inds]) + # for i in safe_goal_inds: + # if self.node_list[i].cost == min_cost: + # self.xG.parent = self.node_list[i] + + def rewire(self, node_new, neighbor_index): + for i in neighbor_index: + node_near = self.node_list[i] + node_edge = self.new_state(node_new, node_near) + if not node_edge: + continue + + node_edge.cost = self.update_cost(node_new, node_near) + collision = self.check_collision(node_edge) + improved_cost = node_near.cost > node_edge.cost + + if not collision and improved_cost: + self.node_list[i] = node_edge + self.propagate_cost_to_leaves(node_new) + + def update_cost(self, node_start, node_end): + dist, theta = self.get_distance_and_angle(node_start, node_end) + return node_start.cost + dist + + def propagate_cost_to_leaves(self, parent_node): + for node in self.node_list: + if node.parent == parent_node: + node.cost = self.update_cost(parent_node, node) + self.propagate_cost_to_leaves(node) + + def extract_path(self): + path = [[self.xG.x, self.xG.y]] + node = self.xG + while node.parent is not None: + path.append([node.x, node.y]) + node = node.parent + path.append([node.x, node.y]) + + return path + + def dis_to_goal(self, node_cal): + return math.hypot(node_cal.x - self.xG.x, node_cal.y - self.xG.y) + + def check_collision(self, node_end): + if node_end is None: + return True + + for (ox, oy, r) in self.obs_circle: + if math.hypot(node_end.x - ox, node_end.y - oy) <= r: + return True + + for (ox, oy, w, h) in self.obs_rectangle: + if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: + return True + + for (ox, oy, w, h) in self.obs_boundary: + if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: + return True + + return False + + @staticmethod + def get_distance_and_angle(node_start, node_end): + dx = node_end.x - node_start.x + dy = node_end.y - node_start.y + return math.hypot(dx, dy), math.atan2(dy, dx) + + +if __name__ == '__main__': + x_Start = (2, 2) # Starting node + x_Goal = (49, 28) # Goal node + + rrt = RRT(x_Start, x_Goal) \ No newline at end of file diff --git a/Sampling-based Planning/rrt_2D/rrt.py b/Sampling-based Planning/rrt_2D/rrt.py new file mode 100644 index 0000000..5609603 --- /dev/null +++ b/Sampling-based Planning/rrt_2D/rrt.py @@ -0,0 +1,114 @@ +from rrt_2D import env +from rrt_2D import plotting + +import numpy as np +import math + + +class Node: + def __init__(self, n): + self.x = n[0] + self.y = n[1] + self.parent = None + + +class RRT: + def __init__(self, xI, xG): + self.xI = Node(xI) + self.xG = Node(xG) + self.expand_len = 0.4 + self.goal_sample_rate = 0.05 + self.iterations = 5000 + self.node_list = [self.xI] + + self.env = env.Env() + self.plotting = plotting.Plotting(xI, xG) + + self.x_range = self.env.x_range + self.y_range = self.env.y_range + self.obs_circle = self.env.obs_circle + self.obs_rectangle = self.env.obs_rectangle + self.obs_boundary = self.env.obs_boundary + + self.path = self.planning() + self.plotting.animation(self.node_list, self.path) + + def planning(self): + for i in range(self.iterations): + node_rand = self.random_state() + node_near = self.nearest_neighbor(self.node_list, node_rand) + node_new = self.new_state(node_near, node_rand) + + if not self.check_collision(node_new): + self.node_list.append(node_new) + + if self.dis_to_goal(self.node_list[-1]) <= self.expand_len: + self.new_state(self.node_list[-1], self.xG) + return self.extract_path(self.node_list) + + return None + + def random_state(self): + if np.random.random() > self.goal_sample_rate: + return Node((np.random.uniform(self.x_range[0], self.x_range[1]), + np.random.uniform(self.y_range[0], self.y_range[1]))) + return self.xG + + def nearest_neighbor(self, node_list, n): + return self.node_list[int(np.argmin([math.hypot(nd.x - n.x, nd.y - n.y) + for nd in node_list]))] + + def new_state(self, node_start, node_end): + node_new = Node((node_start.x, node_start.y)) + dist, theta = self.get_distance_and_angle(node_new, node_end) + + dist = min(self.expand_len, dist) + node_new.x += dist * math.cos(theta) + node_new.y += dist * math.sin(theta) + node_new.parent = node_start + + return node_new + + def extract_path(self, nodelist): + path = [(self.xG.x, self.xG.y)] + node_now = nodelist[-1] + + while node_now.parent is not None: + node_now = node_now.parent + path.append((node_now.x, node_now.y)) + + return path + + def dis_to_goal(self, node_cal): + return math.hypot(node_cal.x - self.xG.x, node_cal.y - self.xG.y) + + def check_collision(self, node_end): + if node_end is None: + return True + + for (ox, oy, r) in self.obs_circle: + if math.hypot(node_end.x - ox, node_end.y - oy) <= r: + return True + + for (ox, oy, w, h) in self.obs_rectangle: + if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: + return True + + for (ox, oy, w, h) in self.obs_boundary: + if 0 <= (node_end.x - ox) <= w and 0 <= (node_end.y - oy) <= h: + return True + + return False + + @staticmethod + def get_distance_and_angle(node_start, node_end): + dx = node_end.x - node_start.x + dy = node_end.y - node_start.y + return math.hypot(dx, dy), math.atan2(dy, dx) + + +if __name__ == '__main__': + x_Start = (2, 2) # Starting node + x_Goal = (49, 28) # Goal node + + rrt = RRT(x_Start, x_Goal) \ No newline at end of file diff --git a/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..9aafad5acf5ca73f4e427beb2f63ecc55c9e1e8c GIT binary patch literal 1537 zcmZuxPjBNy6rb_`xJkP0mhP6WP{m&GVH;^HaX^S#P=UlDtdx~N7GO;?Q^%}hyJKf* zBKOpO3HG!-@(~bU0WKVULPA1{#EBy(-W%It54`BTynplF%zMA&^0{t_>O>W&1Zrx~sAxv(+(B!AWxFLZhY+-%p z0QVr~iY?*ZP)fKqA)d3^aecNZXPNxqXt@wam$AEs7Gyl7_yuGD_klkFE`9@nt+6dO zj4W;|%W18ivfS3%0!!n4unfo?C~MeS+seAvT06?xZLG+?rV+W;uD-758TsbXjQ}1=B zyf$?3)NhjieCkVF$XW>vHH5?(m$NoOczUg+d2KQ&*|N4+o{N-wM(s*bWXYn8v$QtP zlWh7T=zVcslx&)aqNeeWL=NEiKa$aipxN(~6W?T{B(G^JTLZ1f@!qcsj6B@jW~ta3>`z#Uy7_ qA@5@fF0!Uy)n+c^wEVv-)Xs;^%|F8D6*we=I=Ztn96GRoA^jJimu5Zy literal 0 HcmV?d00001 diff --git a/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..42deba7dc939337eb092a38187b880368279bef1 GIT binary patch literal 5364 zcmb_gO>^W%8CJJGMl<8@_4;FXlMH(ZnN(sY>l7>ylHHB7kN}Ix22!O8xMa6Hqp?Ps zv}Es$OG809;kH%$1iLDi{09zH@ee3Yec-~$7pmYyQ5<;Q?y>Ckh8voi*4uBl)av)+ zc^~P)^mN0*vGj+3{^~D_mi0I4oF6uXd-&&nL?bN05-VUh^Mv>9z-C701dbka`(9Af zIy>?E^`Ne0Cu#JXL31+K?@tBOkX_+@ZUr-d* z^EELq7I0n^*Tf>u*Tr?Qg!7VUi50Q>ISZD>4RI5i6>&?vfb(hx&$h?Ki{d4W-Vp4u z)&9mAc40kgGv&!>&<_ie?hm7UV-wQcR+fx5Hs8yVB-$gs zbac?(X;|<5LkJyf%!c<$Yt#DW-7zoOiB+%@j=x=U0ZV)VD+qRauHnKy*U64XuJq9;I}`#|@ph7IcRvrD`}ZF_P%PGVG0ikht6?iN zG>1-37h23=4R)8w>yRs7lEGRZmcOanJ74$on{i$&pAdWZay&?$bE>9beeOdCEZ_2K9S5VxOW{JQQDzDs zQXS)-Xh5Q?p@-xlkA2d0yi+!|OV-1>tJc_om71dY1;_*oF0N zF;zN?mZ6h~V*coZMhnN|gB_V6@6xDsKqAEiah1iik8MhzWY%H2ycjb5%OMuX^#BJPax z*90J?fSe^AWznNEoknh)7T?+p1ni#X%PTCCH2@>QcrsglAv7Cp@XAgOUy}}mQ z5}!e;zh=*{Ilc&;d<`=y5Tk^m)YM7;5CSF736j2N_jEWDL``}Vz&hn)Mj=qLV=nK) z-3W%q1QxYYr@XK(r9IhNlGWS3NqZ0G5m{f=4$^oplYM2wsmkBU4pI@yk@Dj_PV-nq zs*w{|M-OEbX}Ci%XvAq;3_?JBF6(q>ssI;AC-0#90RIWfy$qD|xz<)_L5NCT(Hs69 zgyC|3&tI)u^aSNJ!X^AU8;l{v0PM6{DRhnVAC zo+E0HHR1~&2CE+}$m+PdW5CXPt zkgRP3#`2tj-h_@yl`#+^BSEA{((aOB=%AFSN_v1!K~Tbv`PeSlL~@4rs4!5BXyADR zaOL(qM1Wnohz~%3VEc7>TVLVf+8SWRfiZ6jj!obG1SN!`$L0A1Lw?zSC8b<#(X1f{pB0 z1>48y7{b#dIM?=rmAGudk~-{=MJNx*Eq;tba%{umR?q8^RXLSaVWS{9G#mFpAvi@r zxs+?P8rYf>4&!G5iv2?GP*SohJBd=|!QP-n%1J2&y-;@gaoTn-ndo~omk(tCwgr=3 zHAXFsLJ3=MV_HsGXt61npuy(YB9r&guL6o5s|u)B(Ls%3E9XF?87TiK;3}{nu39$8e;{*`cC)-_?H#20?c<`{Yu~kNAi_vjLO}&ThoF_- z7NRpHdRa*2(A63~F5{yDbJCNpgo^wT?lZ!Ff{}B~VXrc+t*Rykd_|QDq}8RoW)s<_ z9G}R@RD`$H14ionb$c}+rr#stXtElJ*=q=gUjCkP2C{b2JhhlrOo2?%b<@YBYJW(!}OG z??QJ&ERintA>X99&>U;|(OHH-3^-^c3mOBN^~~#e2U>lc5rWa{Rg^@BffF9Z{VT~M z-=`TYa5b+DINp?BfJXC9T-fM`avwDrXdBELxgx;}4Ieoh(e0JxGe6mLKd45-P-par6X5JT8( zL|hG1b}@591TIn?C-27~4;!0-Pvn}21YRWd9rE3<0HeT=Gfc4cH*kZ%89@wu;^app zdBY^!iITt@>9cYNAznoAdfLsNw5JTnY9#(CEx$z#J+u57H7`>`>;a`R5BMlo{3urr zJ;W7QQPYQUeh?<{QAl4==||*YMiU7`CRR|PR!~i6G{rEzMfBG29pVnV zrGHd!8*GY8a&uL}sMb=A(+u|z0)h>IJ+_Aeq>W$*a82NyCy*ULGK$&+tVu?oYv3mN z9Jm4QCtgtl>99-Z{A)$Yi#qj>`L_Y>r8Bn8_lpv5wO7iU0qTZvxwhO7@Lu@outRWLdHw7x@}G8A4G~ zxeRW6-O!qjRr5kvJD*OJ(2!gD5@Q&yj`BOyC0efeq~=qGtWa^*wMhG1@9R>aQC*Vy sn~(o?KNAN@^bUQk$|+2J3NloB{j+`7ulbJuO`ncwzwUdDCZt#X3z1Q3Q~&?~ literal 0 HcmV?d00001 diff --git a/Sampling-based Planning/rrt_3D/env3D.py b/Sampling-based Planning/rrt_3D/env3D.py new file mode 100644 index 0000000..118452b --- /dev/null +++ b/Sampling-based Planning/rrt_3D/env3D.py @@ -0,0 +1,44 @@ +# this is the three dimensional configuration space for rrt +# !/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +@author: yue qi +""" +import numpy as np + + +def getblocks(resolution): + # 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], + [1.51e+01, 0.00e+00, 2.10e+00, 1.59e+01, 5.00e+00, 6.00e+00], + [1.00e-01, 0.00e+00, 0.00e+00, 9.00e-01, 5.00e+00, 3.90e+00], + [6.10e+00, 0.00e+00, 0.00e+00, 6.90e+00, 5.00e+00, 3.90e+00], + [1.21e+01, 0.00e+00, 0.00e+00, 1.29e+01, 5.00e+00, 3.90e+00], + [1.81e+01, 0.00e+00, 0.00e+00, 1.89e+01, 5.00e+00, 3.90e+00]] + Obstacles = [] + for i in block: + i = np.array(i) + Obstacles.append((i[0] / resolution, i[1] / resolution, i[2] / resolution, i[3] / resolution, i[4] / resolution, + i[5] / resolution)) + return np.array(Obstacles) + + +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.start = np.array([0.5, 2.5, 5.5]) + self.goal = np.array([19.0, 2.5, 5.5]) + + def visualize(self): + # fig = plt.figure() + # TODO: do visualizations + return + + +if __name__ == '__main__': + newenv = env() + X = StateSpace(newenv.boundary, newenv.resolution) + print(X) diff --git a/Sampling-based Planning/rrt_3D/rrt3D.py b/Sampling-based Planning/rrt_3D/rrt3D.py new file mode 100644 index 0000000..0700580 --- /dev/null +++ b/Sampling-based Planning/rrt_3D/rrt3D.py @@ -0,0 +1,59 @@ + +""" +This is rrt star code for 3D +@author: yue qi +""" +import numpy as np +from numpy.matlib import repmat +from rrt_3D.env3D import env +from collections import defaultdict +import pyrr as pyrr +from utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path +import time + + +class rrtstar(): + def __init__(self): + self.env = env() + self.Parent = defaultdict(lambda: defaultdict(dict)) + self.V = [] + self.E = [] + self.i = 0 + self.maxiter = 10000 + self.stepsize = 0.5 + self.Path = [] + + def wireup(self,x,y): + self.E.append([x,y]) # add edge + self.Parent[str(x[0])][str(x[1])][str(x[2])] = y + + def removewire(self,xnear): + xparent = self.Parent[str(xnear[0])][str(xnear[1])][str(xnear[2])] + a = np.array([xnear,xparent]) + self.E = [xx for xx in self.E if not (xx==a).all()] # remove and replace old the connection + + def run(self): + self.V.append(self.env.start) + ind = 0 + xnew = self.env.start + while ind < self.maxiter and getDist(xnew,self.env.goal) > 1: + xrand = sampleFree(self) + xnearest = nearest(self,xrand) + xnew = steer(self,xnearest,xrand) + if not isCollide(self,xnearest,xnew): + self.V.append(xnew) # add point + self.wireup(xnew,xnearest) + #visualization(self) + self.i += 1 + ind += 1 + if getDist(xnew,self.env.goal) <= 1: + self.wireup(self.env.goal,xnew) + self.Path,D = path(self) + print('Total distance = '+str(D)) + visualization(self) + +if __name__ == '__main__': + p = rrtstar() + starttime = time.time() + p.run() + print('time used = ' + str(time.time()-starttime)) \ No newline at end of file diff --git a/Sampling-based Planning/rrt_3D/rrtstar3D.py b/Sampling-based Planning/rrt_3D/rrtstar3D.py new file mode 100644 index 0000000..bcd6418 --- /dev/null +++ b/Sampling-based Planning/rrt_3D/rrtstar3D.py @@ -0,0 +1,79 @@ + +""" +This is rrt star code for 3D +@author: yue qi +""" +import numpy as np +from numpy.matlib import repmat +from rrt_3D.env3D import env +from collections import defaultdict +import pyrr as pyrr +from rrt_3D.utils3D import getDist, sampleFree, nearest, steer, isCollide, near, visualization, cost, path +import time + + +class rrtstar(): + def __init__(self): + self.env = env() + self.Parent = defaultdict(lambda: defaultdict(dict)) + self.V = [] + self.E = [] + self.i = 0 + self.maxiter = 10000 + self.stepsize = 0.5 + self.Path = [] + + def wireup(self,x,y): + self.E.append([x,y]) # add edge + self.Parent[str(x[0])][str(x[1])][str(x[2])] = y + + def removewire(self,xnear): + xparent = self.Parent[str(xnear[0])][str(xnear[1])][str(xnear[2])] + a = np.array([xnear,xparent]) + self.E = [xx for xx in self.E if not (xx==a).all()] # remove and replace old the connection + + def run(self): + self.V.append(self.env.start) + ind = 0 + xnew = self.env.start + while ind < self.maxiter and getDist(xnew,self.env.goal) > 1: + xrand = sampleFree(self) + xnearest = nearest(self,xrand) + xnew = steer(self,xnearest,xrand) + if not isCollide(self,xnearest,xnew): + Xnear = near(self,xnew) + self.V.append(xnew) # add point + # visualization(self) + # minimal path and minimal cost + xmin,cmin = xnearest,cost(self,xnearest) + getDist(xnearest,xnew) + # connecting along minimal cost path + if self.i == 0: + c1 = cost(self,Xnear) + getDist(xnew,Xnear) + if not isCollide(self,xnew,Xnear) and c1 < cmin: + xmin,cmin = Xnear,c1 + self.wireup(xnew,xmin) + else: + for xnear in Xnear: + c1 = cost(self,xnear) + getDist(xnew,xnear) + if not isCollide(self,xnew,xnear) and c1 < cmin: + xmin,cmin = xnear,c1 + self.wireup(xnew,xmin) + # rewire + for xnear in Xnear: + c2 = cost(self,xnew) + getDist(xnew,xnear) + if not isCollide(self,xnew,xnear) and c2 < cost(self,xnear): + self.removewire(xnear) + self.wireup(xnear,xnew) + self.i += 1 + ind += 1 + if getDist(xnew,self.env.goal) <= 1: + self.wireup(self.env.goal,xnew) + self.Path,D = path(self) + print('Total distance = '+str(D)) + visualization(self) + +if __name__ == '__main__': + p = rrtstar() + starttime = time.time() + p.run() + print('time used = ' + str(time.time()-starttime)) \ No newline at end of file diff --git a/Sampling-based Planning/rrt_3D/utils3D.py b/Sampling-based Planning/rrt_3D/utils3D.py new file mode 100644 index 0000000..4aa8290 --- /dev/null +++ b/Sampling-based Planning/rrt_3D/utils3D.py @@ -0,0 +1,177 @@ +import numpy as np +from numpy.matlib import repmat +import pyrr as pyrr +# plotting +import matplotlib.pyplot as plt +from mpl_toolkits.mplot3d import Axes3D +from mpl_toolkits.mplot3d.art3d import Poly3DCollection +import mpl_toolkits.mplot3d as plt3d + + +def getRay(x, y): + direc = [y[0] - x[0], y[1] - x[1], y[2] - x[2]] + return np.array([x, direc]) + + +def getAABB(blocks): + 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 + + +def getDist(pos1, pos2): + return np.sqrt(sum([(pos1[0] - pos2[0]) ** 2, (pos1[1] - pos2[1]) ** 2, (pos1[2] - pos2[2]) ** 2])) + + +def draw_block_list(ax, blocks): + ''' + Subroutine used by draw_map() to display the environment blocks + ''' + v = np.array([[0, 0, 0], [1, 0, 0], [1, 1, 0], [0, 1, 0], [0, 0, 1], [1, 0, 1], [1, 1, 1], [0, 1, 1]], + dtype='float') + 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]]) + # clr = blocks[:,6:]/255 + n = blocks.shape[0] + d = blocks[:, 3:6] - blocks[:, :3] + vl = np.zeros((8 * n, 3)) + fl = np.zeros((6 * n, 4), dtype='int64') + # fcl = np.zeros((6*n,3)) + for k in range(n): + vl[k * 8:(k + 1) * 8, :] = v * d[k] + blocks[k, :3] + fl[k * 6:(k + 1) * 6, :] = f + k * 8 + # fcl[k*6:(k+1)*6,:] = clr[k,:] + + if type(ax) is Poly3DCollection: + ax.set_verts(vl[fl]) + else: + pc = Poly3DCollection(vl[fl], alpha=0.15, linewidths=1, edgecolors='k') + # pc.set_facecolor(fcl) + h = ax.add_collection3d(pc) + return h + + +''' The following utils can be used for rrt or rrt*, + required param initparams should have + env, environement generated from env3D + V, node set + E, edge set + i, nodes added + maxiter, maximum iteration allowed + stepsize, leaf growth restriction + +''' + + +def sampleFree(initparams): + x = np.random.uniform(initparams.env.boundary[0:3], initparams.env.boundary[3:6]) + if isinside(initparams, x): + return sampleFree(initparams) + else: + return np.array(x) + + +def isinside(initparams, x): + '''see if inside obstacle''' + for i in initparams.env.blocks: + if i[0] <= x[0] < i[3] and i[1] <= x[1] < i[4] and i[2] <= x[2] < i[5]: + return True + return False + + +def isCollide(initparams, x, y): + '''see if line intersects obstacle''' + ray = getRay(x, y) + dist = getDist(x, y) + for i in getAABB(initparams.env.blocks): + shot = pyrr.geometric_tests.ray_intersect_aabb(ray, i) + if shot is not None: + dist_wall = getDist(x, shot) + if dist_wall <= dist: # collide + return True + return False + + +def nearest(initparams, x): + V = np.array(initparams.V) + if initparams.i == 0: + return initparams.V[0] + xr = repmat(x, len(V), 1) + dists = np.linalg.norm(xr - V, axis=1) + return initparams.V[np.argmin(dists)] + + +def steer(initparams, x, y): + direc = (y - x) / np.linalg.norm(y - x) + xnew = x + initparams.stepsize * direc + return xnew + + +def near(initparams, x, r=2): + # TODO: r = min{gamma*log(card(V)/card(V)1/d),eta} + V = np.array(initparams.V) + if initparams.i == 0: + return initparams.V[0] + xr = repmat(x, len(V), 1) + inside = np.linalg.norm(xr - V, axis=1) < r + nearpoints = V[inside] + return np.array(nearpoints) + + +def cost(initparams, x): + '''here use the additive recursive cost function''' + if all(x == initparams.env.start): + return 0 + xparent = initparams.Parent[str(x[0])][str(x[1])][str(x[2])] + return cost(initparams, xparent) + getDist(x, xparent) + + +def visualization(initparams): + V = np.array(initparams.V) + E = np.array(initparams.E) + Path = np.array(initparams.Path) + start = initparams.env.start + goal = initparams.env.goal + ax = plt.subplot(111, projection='3d') + ax.view_init(elev=0., azim=90) + ax.clear() + draw_block_list(ax, initparams.env.blocks) + if E != []: + for i in E: + xs = i[0][0], i[1][0] + ys = i[0][1], i[1][1] + zs = i[0][2], i[1][2] + line = plt3d.art3d.Line3D(xs, ys, zs) + ax.add_line(line) + + if Path != []: + for i in Path: + xs = i[0][0], i[1][0] + ys = i[0][1], i[1][1] + zs = i[0][2], i[1][2] + line = plt3d.art3d.Line3D(xs, ys, zs, color='r') + ax.add_line(line) + + 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') + ax.scatter3D(V[:, 0], V[:, 1], V[:, 2]) + plt.xlim(initparams.env.boundary[0], initparams.env.boundary[3]) + plt.ylim(initparams.env.boundary[1], initparams.env.boundary[4]) + ax.set_zlim(initparams.env.boundary[2], initparams.env.boundary[5]) + plt.xlabel('x') + plt.ylabel('y') + if not Path != []: + plt.pause(0.001) + else: + plt.show() + + +def path(initparams, Path=[], dist=0): + x = initparams.env.goal + while not all(x == initparams.env.start): + x2 = initparams.Parent[str(x[0])][str(x[1])][str(x[2])] + Path.append(np.array([x, x2])) + dist += getDist(x, x2) + x = x2 + return Path, dist +