From 0f3c44566465b9b33fee068e34193c9b1cb82cd4 Mon Sep 17 00:00:00 2001 From: zhm-real Date: Wed, 24 Jun 2020 12:55:13 -0700 Subject: [PATCH] update RRT_2D --- .../__pycache__/plotting.cpython-37.pyc | Bin 2616 -> 2574 bytes .../rrt_2D/__pycache__/rrt.cpython-37.pyc | Bin 0 -> 3673 bytes Sampling-based Planning/rrt_2D/plotting.py | 11 +- Sampling-based Planning/rrt_2D/rrt*.py | 200 ------------------ Sampling-based Planning/rrt_2D/rrt.py | 74 +++---- Sampling-based Planning/rrt_2D/rrt_star.py | 169 +++++++++++++++ .../rrt_3D/__pycache__/env3D.cpython-37.pyc | Bin 1569 -> 1537 bytes .../rrt_3D/__pycache__/utils3D.cpython-37.pyc | Bin 5396 -> 5364 bytes 8 files changed, 211 insertions(+), 243 deletions(-) create mode 100644 Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc delete mode 100644 Sampling-based Planning/rrt_2D/rrt*.py create mode 100644 Sampling-based Planning/rrt_2D/rrt_star.py diff --git a/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc index c055c0c7f811e29af02102937b0ff66ab5f67db1..d91e451286184f35c4f15295b16aa166a4341349 100644 GIT binary patch delta 405 zcmdlX(kH^}#LLUY00g&ZeU3}m$eYV3dW*dxzPKc@sN@z~MSOaGV$Mp2BEHETjEjs^ zfa+=(7BHkRE@Wf`u^4L@YnT@@f!RzoOd!z|hF}IwCch}=pr8;<=E?0$?y`J9ZAD;$ zAH)&^5+Ft~8<1ew{EASsC|e~ZH=CowlECDG1l@;bIg z>02y$`6;OoD-iYw0S&R9?8Saa8ziR5bc;#PphyeIL{s>S%O)qYxFk70x4_POvN=bF zDnxye9!M<~4MqAu3Gc~AIGoVkEew?KoGiuZ%P258gHufa>=*>WKY1Rf7%MA>IuGCE Mot(j#(lT5!0NLA2%K!iX delta 488 zcmeAZ*&)K~#LLUY00dFHKgG@6$eYW^SHuG37O?_}l?+8ZllvJL#aaM4HB2cC*&Ib8 zB@8Ky&5Vo;DGb33H4FQlrW2SH5*#H0l diff --git a/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc b/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..b1a9d01d7d45bbb750377dd44da07e69cd385ec4 GIT binary patch literal 3673 zcmai1O>Y~=8Qz)w;F2OG+EHD_O;e>GX;{=&ng(rw3bLD|4vGeXQWrHqI9bq~6+cAo z(z7d@B9=W=M%zO#y%=au)TzjS=#S{NbM47RZ@uNz=b5D_Sqac3X6Bu*cV^!Ad7l}+ z*J#une0Tr$-%tP3aGZa!a{jpx?jmKMB2$hMBWEnyLQs~FWm{U=9l31}ZSKf#`yV>W zQ~onY`KPiSpcbeKY8BKfDpb{HPCJaNs&*uGI}T4BXmIa4t+f|e!)XcQ#>o>D;bfHN z`5@`-ciD0NNeFk5vQ1>M(-z8UOC^+qTGuM_jJKbvxXY3JSTenjl)a9u5K6r26i!4* zS_xWEm&(CL+IJiwYsA!u6BA52I!^MIG+q{u4vk1YbMzWUeE#uHKOM(AvwnKK zbD-0n?u>UPoxFc*GU_A=_J6B0>ZN*+_s7}J{mytYV%wum7OU-p<&Yh%^XQ!qSlphR zm@tY4$smuSDi+8BWR9reH*4@9-Ja_~%r>F9MRZk}O=*-NHle6+UvL zq3Cf`kXIJr8U0wvtBYD83V9|`t71fDxCRLcQXLW!qy{7;Nb8W0AZQ|`?4_Mil+mAYq&spY%n8rHZ!Po8L+ z(u4;~Oq!mOjd&?m%}(v1w4&aW}^hRO_rhN{4y6Bg8vfVY0qxzkX1USf#4I` z#59u_5uD{w5)XR)N2%6dWpg!&pO(YSMzhUz!oo*g6@LQGAL1F;yh*$boEk$m^gtiq68;@Fzh^{z`+*fkP z8(1QvI~~y!b+In$5=mUA&)~JvTjzscVmx}xdg~(yIXtxxhz)V%>3g}fb$Djzl}v~d zJa~4|?l0T@vi%Uzcc1? z!1+z|{{Sf?8#y@IP4T*zH3`Mbp6oJ+__5>xS!SG!tvR{K<-+}ga|n9Jbr2SS=BYRX zL|-jweT8O5C8u*+*;~kZQ1Hq296YlSg&4{|IzzbyYYkmU?iMWbXZhH>U&a$y*Bisn z#_yj@(p;0bG}Ym9khkB4K~zgpvt@Hc(V5s^jO9X#Oy5|@hUQJ?~c-L zXO!(hIG@z=O1!#*B-d|41JJ2^<3ZALOSF|xTkWQqaWk|LH0P{Xv$0hYV;N_NVEuiJ z(6>0t&-*c0^9ugHjjo>}W$VZoYE4-eTlfWX)+nvCWQkRb(Tc0v>OE8lK^&R6Jd{21 zAdED9vv7y5a*nEcmlDh-!HzCQx*SLDd%X1k_gi^%zhspartIPLo6rvgkoWZ+WX2=W zw3kGZKD~M#V)`CZM$bFqy6|wGK-`dJOkHNH)q#j$E^Gb$ZwS+Oc^J_9_a3@)5c%B8 z1xOx2QIx@fV=y!d4?Lf#(rO+yaaj?%@hl7pGXA%=jH#Xzu`4dr-SPjKX2m z$<;G3{34|CDGDgn#q1_`{pA_ZUe<%| zIL`a2vJro28#Z1oja|c7>dmM(6yde783v&ju7$O5L$kMwWIx(&g=mJT5_9}-A-*{U0it#L9v^UTfT*}hZH?hVGSyBpz*y76Mol^aaBTmISD;|tOH0;rJihr3=`e-Ji8@jphw8K3|F literal 0 HcmV?d00001 diff --git a/Sampling-based Planning/rrt_2D/plotting.py b/Sampling-based Planning/rrt_2D/plotting.py index 24ec50b..d8f136c 100644 --- a/Sampling-based Planning/rrt_2D/plotting.py +++ b/Sampling-based Planning/rrt_2D/plotting.py @@ -5,17 +5,14 @@ from rrt_2D import env class Plotting: - def __init__(self, xI, xG): - self.xI, self.xG = xI, xG + def __init__(self, x_start, x_goal): + self.xI, self.xG = x_start, x_goal self.env = env.Env() self.obs_bound = self.env.obs_boundary self.obs_circle = self.env.obs_circle self.obs_rectangle = self.env.obs_rectangle def animation(self, nodelist, path, animation=False): - if path is None: - print("No path found!") - return self.plot_grid("RRT") self.plot_visited(nodelist, animation) self.plot_path(path) @@ -55,6 +52,7 @@ class Plotting: plt.plot(self.xI[0], self.xI[1], "bs", linewidth=3) plt.plot(self.xG[0], self.xG[1], "gs", linewidth=3) + plt.title(name) plt.axis("equal") @@ -72,9 +70,8 @@ class Plotting: if node.parent: plt.plot([node.parent.x, node.x], [node.parent.y, node.y], "-g") - @staticmethod def plot_path(path): plt.plot([x[0] for x in path], [x[1] for x in path], '-r', linewidth=2) plt.pause(0.01) - plt.show() \ No newline at end of file + plt.show() diff --git a/Sampling-based Planning/rrt_2D/rrt*.py b/Sampling-based Planning/rrt_2D/rrt*.py deleted file mode 100644 index e105b7f..0000000 --- a/Sampling-based Planning/rrt_2D/rrt*.py +++ /dev/null @@ -1,200 +0,0 @@ -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.cost = 0.0 - self.parent = None - - -class RRT: - def __init__(self, xI, xG): - self.xI = Node(xI) - self.xG = Node(xG) - self.expand_len = 1 - self.goal_sample_rate = 0.05 - self.connect_dist = 10 - 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, False) - - def planning(self): - for k 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): - neighbor_index = self.find_near_neighbor(node_new) - node_new = self.choose_parent(node_new, neighbor_index) - if node_new: - self.node_list.append(node_new) - self.rewire(node_new, neighbor_index) - - # 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() - - index = self.search_best_goal_node() - self.xG.parent = self.node_list[index] - return self.extract_path() - - 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_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 index 5609603..d449bfe 100644 --- a/Sampling-based Planning/rrt_2D/rrt.py +++ b/Sampling-based Planning/rrt_2D/rrt.py @@ -12,17 +12,17 @@ class Node: 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] +class Rrt: + def __init__(self, x_start, x_goal, expand_len, goal_sample_rate, iter_limit): + self.xI = Node(x_start) + self.xG = Node(x_goal) + self.expand_len = expand_len + self.goal_sample_rate = goal_sample_rate + self.iter_limit = iter_limit + self.vertex = [self.xI] self.env = env.Env() - self.plotting = plotting.Plotting(xI, xG) + self.plotting = plotting.Plotting(x_start, x_goal) self.x_range = self.env.x_range self.y_range = self.env.y_range @@ -30,33 +30,31 @@ class RRT: 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) + for i in range(self.iter_limit): + node_rand = self.random_state(self.goal_sample_rate) + node_near = self.nearest_neighbor(self.vertex, 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 node_new and not self.check_collision(node_new): + self.vertex.append(node_new) + dist, _ = self.get_distance_and_angle(node_new, self.xG) - 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) + if dist <= self.expand_len: + self.new_state(node_new, self.xG) + return self.extract_path(node_new) return None - def random_state(self): - if np.random.random() > self.goal_sample_rate: + def random_state(self, goal_sample_rate): + if np.random.random() > 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]))] + return self.vertex[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)) @@ -69,9 +67,9 @@ class RRT: return node_new - def extract_path(self, nodelist): + def extract_path(self, node_end): path = [(self.xG.x, self.xG.y)] - node_now = nodelist[-1] + node_now = node_end while node_now.parent is not None: node_now = node_now.parent @@ -79,13 +77,7 @@ class RRT: 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 @@ -107,8 +99,18 @@ class RRT: return math.hypot(dx, dy), math.atan2(dy, dx) -if __name__ == '__main__': - x_Start = (2, 2) # Starting node - x_Goal = (49, 28) # Goal node +def 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 + rrt = Rrt(x_start, x_goal, 0.4, 0.05, 2000) + path = rrt.planning() + + if path: + rrt.plotting.animation(rrt.vertex, path) + else: + print("No Path Found!") + + +if __name__ == '__main__': + main() diff --git a/Sampling-based Planning/rrt_2D/rrt_star.py b/Sampling-based Planning/rrt_2D/rrt_star.py new file mode 100644 index 0000000..3a0e091 --- /dev/null +++ b/Sampling-based Planning/rrt_2D/rrt_star.py @@ -0,0 +1,169 @@ +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.cost = 0.0 + self.parent = None + + +class RrtStar: + def __init__(self, x_start, x_goal, expand_len, + goal_sample_rate, search_radius, iter_limit): + self.xI = Node(x_start) + self.xG = Node(x_goal) + self.expand_len = expand_len + self.goal_sample_rate = goal_sample_rate + self.search_radius = search_radius + self.iter_limit = iter_limit + self.vertex = [self.xI] + + self.env = env.Env() + self.plotting = plotting.Plotting(x_start, x_goal) + + 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 + + def planning(self): + for k in range(self.iter_limit): + node_rand = self.random_state(self.goal_sample_rate) + node_near = self.nearest_neighbor(self.vertex, node_rand) + node_new = self.new_state(node_near, node_rand) + + if node_new and not self.check_collision(node_new): + neighbor_index = self.find_near_neighbor(node_new) + if neighbor_index: + node_new = self.choose_parent(node_new, neighbor_index) + self.vertex.append(node_new) + self.rewire(node_new, neighbor_index) + + index = self.search_goal_parent() + return self.extract_path(self.vertex[index]) + + def random_state(self, goal_sample_rate): + if np.random.random() > 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.vertex[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.vertex) + 1 + r = min(self.search_radius * 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.vertex] + + return [dist_table.index(d) for d in dist_table if d <= r] + + def choose_parent(self, node_new, neighbor_index): + cost = [] + + for i in neighbor_index: + node_neighbor = self.vertex[i] + cost.append(self.get_new_cost(node_neighbor, node_new)) + + cost_min_index = neighbor_index[int(np.argmin(cost))] + node_new = self.new_state(self.vertex[cost_min_index], node_new) + node_new.cost = min(cost) + + return node_new + + def search_goal_parent(self): + dist_list = [math.hypot(n.x - self.xG.x, n.y - self.xG.y) for n in self.vertex] + node_index = [dist_list.index(i) for i in dist_list if i <= self.expand_len] + + if node_index: + cost_list = [dist_list[i] + self.vertex[i].cost for i in node_index] + return node_index[int(np.argmin(cost_list))] + + return None + + def rewire(self, node_new, neighbor_index): + for i in neighbor_index: + node_neighbor = self.vertex[i] + new_cost = self.get_new_cost(node_new, node_neighbor) + + if node_neighbor.cost > new_cost: + self.vertex[i] = self.new_state(node_new, node_neighbor) + self.propagate_cost_to_leaves(node_new) + + def get_new_cost(self, node_start, node_end): + dist, _ = 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.vertex: + if node.parent == parent_node: + node.cost = self.get_new_cost(parent_node, node) + self.propagate_cost_to_leaves(node) + + def extract_path(self, node_end): + path = [[self.xG.x, self.xG.y]] + node = node_end + while node.parent is not None: + path.append([node.x, node.y]) + node = node.parent + path.append([node.x, node.y]) + + return path + + def check_collision(self, node_end): + 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) + + +def main(): + x_start = (2, 2) # Starting node + x_goal = (49, 28) # Goal node + + rrt_star = RrtStar(x_start, x_goal, 1, 0.1, 10, 5000) + path = rrt_star.planning() + + if path: + rrt_star.plotting.animation(rrt_star.vertex, path) + else: + print("No Path Found!") + + +if __name__ == '__main__': + main() 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 15349fded17479e47e7e17efa9c49849b517e0be..9aafad5acf5ca73f4e427beb2f63ecc55c9e1e8c 100644 GIT binary patch delta 45 zcmZ3;)5yc^#LLUY00gskf7-}h%p@GFpOK%Ns$Z3nU#cHal%HOdn7er+QvovoAt??w delta 56 zcmZqVS;)if#LLUY00cKGK5yhMW|At_&&bbB)lbYxtu8n^8DaKO;XkRlh1Dzf?b