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 c055c0c..d91e451 100644 Binary files a/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc and b/Sampling-based Planning/rrt_2D/__pycache__/plotting.cpython-37.pyc differ 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 0000000..b1a9d01 Binary files /dev/null and b/Sampling-based Planning/rrt_2D/__pycache__/rrt.cpython-37.pyc differ 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 15349fd..9aafad5 100644 Binary files a/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc and b/Sampling-based Planning/rrt_3D/__pycache__/env3D.cpython-37.pyc differ diff --git a/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc b/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc index 90cfeab..42deba7 100644 Binary files a/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc and b/Sampling-based Planning/rrt_3D/__pycache__/utils3D.cpython-37.pyc differ