From 9e01addfc48eeebeb3f9b77583e4e6fb6d77874b Mon Sep 17 00:00:00 2001 From: zhm-real Date: Fri, 31 Jul 2020 00:23:05 -0700 Subject: [PATCH] update --- .../rrt_2D/adaptively_informed_trees.py | 0 .../rrt_2D/advanced_batch_informed_trees.py | 0 .../rrt_2D/batch_informed_trees.py | 0 .../rrt_2D/fast_marching_trees.py | 0 .../rrt_2D/optimal_bi_rrt.py | 127 ------------------ Sampling_based_Planning/rrt_2D/rrt_sharp.py | 0 Sampling_based_Planning/rrt_2D/rs_rrt_star.py | 98 -------------- 7 files changed, 225 deletions(-) create mode 100644 Sampling_based_Planning/rrt_2D/adaptively_informed_trees.py create mode 100644 Sampling_based_Planning/rrt_2D/advanced_batch_informed_trees.py create mode 100644 Sampling_based_Planning/rrt_2D/batch_informed_trees.py create mode 100644 Sampling_based_Planning/rrt_2D/fast_marching_trees.py delete mode 100644 Sampling_based_Planning/rrt_2D/optimal_bi_rrt.py create mode 100644 Sampling_based_Planning/rrt_2D/rrt_sharp.py delete mode 100644 Sampling_based_Planning/rrt_2D/rs_rrt_star.py diff --git a/Sampling_based_Planning/rrt_2D/adaptively_informed_trees.py b/Sampling_based_Planning/rrt_2D/adaptively_informed_trees.py new file mode 100644 index 0000000..e69de29 diff --git a/Sampling_based_Planning/rrt_2D/advanced_batch_informed_trees.py b/Sampling_based_Planning/rrt_2D/advanced_batch_informed_trees.py new file mode 100644 index 0000000..e69de29 diff --git a/Sampling_based_Planning/rrt_2D/batch_informed_trees.py b/Sampling_based_Planning/rrt_2D/batch_informed_trees.py new file mode 100644 index 0000000..e69de29 diff --git a/Sampling_based_Planning/rrt_2D/fast_marching_trees.py b/Sampling_based_Planning/rrt_2D/fast_marching_trees.py new file mode 100644 index 0000000..e69de29 diff --git a/Sampling_based_Planning/rrt_2D/optimal_bi_rrt.py b/Sampling_based_Planning/rrt_2D/optimal_bi_rrt.py deleted file mode 100644 index 59630fb..0000000 --- a/Sampling_based_Planning/rrt_2D/optimal_bi_rrt.py +++ /dev/null @@ -1,127 +0,0 @@ -""" -Optimal_Bidirectional_RRT 2D -@author: huiming zhou -""" - -import os -import sys -import math -import numpy as np -import matplotlib.pyplot as plt -import matplotlib.patches as patches - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Sampling_based_Planning/") - -from Sampling_based_Planning.rrt_2D import env, plotting, utils, queue - - -class Node: - def __init__(self, n): - self.x = n[0] - self.y = n[1] - self.parent = None - - -class OBiRrt: - def __init__(self, x_start, x_goal, step_len, - goal_sample_rate, search_radius, iter_max): - self.x_init = Node(x_start) - self.x_goal = Node(x_goal) - self.step_len = step_len - self.goal_sample_rate = goal_sample_rate - self.search_radius = search_radius - self.iter_max = iter_max - - self.Ta = [self.x_init] - self.Tb = [self.x_goal] - self.c_best = np.inf - self.sigma_best = {} - self.path = [] - self.visited = [] - - self.env = env.Env() - self.plotting = plotting.Plotting(x_start, x_goal) - self.utils = utils.Utils() - # self.fig, self.ax = plt.subplots() - - 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_max): - x_rand = self.generate_random_node(self.x_goal, self.goal_sample_rate) - x_nearest = self.nearest_neighbor(self.Ta, x_rand) - x_new = self.steer(x_nearest, x_rand) - X_near_ind = self.nearest_neighbor(self.Ta, x_new) - L_near = [] - - for x_near_ind in X_near_ind: - x_near = self.Ta[x_near_ind] - sigma_near = self.steer(x_near, x_new) - c_near = self.cost(x_near) + self.cost(sigma_near) - L_near.append((c_near, x_near, sigma_near)) - - L_near.sort() - - def cost_to_go(self, x_start, x_goal): - return math.hypot(x_goal.x - x_start.x, x_goal.y - x_start.y) - - def generate_random_node(self, goal_point, goal_sample_rate): - delta = self.utils.delta - - if np.random.random() > goal_sample_rate: - return Node((np.random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), - np.random.uniform(self.y_range[0] + delta, self.y_range[1] - delta))) - - return goal_point - - @staticmethod - def nearest_neighbor(V, node): - return V[int(np.argmin([math.hypot(nd.x - node.x, nd.y - node.y) for nd in V]))] - - def steer(self, node_start, node_goal): - dist, theta = self.get_distance_and_angle(node_start, node_goal) - - dist = min(self.step_len, dist) - node_new = Node((node_start.x + dist * math.cos(theta), - node_start.y + dist * math.sin(theta))) - - node_new.parent = node_start - - return node_new - - def find_near_neighbor(self, V, node): - n = len(V) + 1 - r = min(self.search_radius * math.sqrt((math.log(n) / n)), self.step_len) - - dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in V] - dist_table_index = [ind for ind in range(len(dist_table)) if dist_table[ind] <= r and - not self.utils.is_collision(node, V[ind])] - - return dist_table_index - - def get_new_cost(self, node_start, node_end): - dist, _ = self.get_distance_and_angle(node_start, node_end) - - return self.cost(node_start) + dist - - @staticmethod - def cost(node_p): - node = node_p - cost = 0.0 - - while node.parent: - cost += math.hypot(node.x - node.parent.x, node.y - node.parent.y) - node = node.parent - - return cost - - @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) diff --git a/Sampling_based_Planning/rrt_2D/rrt_sharp.py b/Sampling_based_Planning/rrt_2D/rrt_sharp.py new file mode 100644 index 0000000..e69de29 diff --git a/Sampling_based_Planning/rrt_2D/rs_rrt_star.py b/Sampling_based_Planning/rrt_2D/rs_rrt_star.py deleted file mode 100644 index fbbe5ff..0000000 --- a/Sampling_based_Planning/rrt_2D/rs_rrt_star.py +++ /dev/null @@ -1,98 +0,0 @@ -""" -RS_RRT_STAR_SMART 2D -@author: huiming zhou -""" - -import os -import sys -import math -import random -import numpy as np -import matplotlib.pyplot as plt -from scipy.spatial.transform import Rotation as Rot -import matplotlib.patches as patches - -sys.path.append(os.path.dirname(os.path.abspath(__file__)) + - "/../../Sampling_based_Planning/") - -from Sampling_based_Planning.rrt_2D import env, plotting, utils -import CurvesGenerator.reeds_shepp as rs - - -class Node: - def __init__(self, x, y, yaw): - self.x = x - self.y = y - self.yaw = yaw - self.path_x = [] - self.path_y = [] - self.paty_yaw = [] - self.parent = None - self.cost = 0.0 - - -class RrtStarSmart: - def __init__(self, sx, sy, syaw, gx, gy, gyaw, step_len, - goal_sample_rate, search_radius, iter_max): - self.s_start = Node(sx, sy, syaw) - self.s_goal = Node(gx, gy, gyaw) - self.step_len = step_len - self.goal_sample_rate = goal_sample_rate - self.search_radius = search_radius - self.iter_max = iter_max - self.curv = 1.0 - - self.env = env.Env() - self.utils = utils.Utils() - - self.fig, self.ax = plt.subplots() - self.delta = self.utils.delta - 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.V = [self.s_start] - self.path = None - - def planning(self): - - for k in range(self.iter_max): - node_rand = self.Sample() - node_nearest = self.Nearest(self.V, node_rand) - node_new = self.Steer(node_nearest, node_rand) - - def Steer(self, node_start, node_end): - sx, sy, syaw = node_start.x, node_start.y, node_start.yaw - gx, gy, gyaw = node_end.x, node_end.y, node_end.yaw - maxc = self.curv - - path = rs.calc_optimal_path(sx, sy, syaw, gx, gy, gyaw, maxc, step_size=0.2) - - if not path: - return None - - node_new = Node(path.x[-1], path.y[-1], path.yaw[-1]) - node_new.path_x = path.x - node_new.path_y = path.y - node_new.path_yaw = path.yaw - node_new.cost = path.L - node_new.parent = node_start - - return node_new - - def Sample(self): - delta = self.utils.delta - - rnd = Node(random.uniform(self.x_range[0] + delta, self.x_range[1] - delta), - random.uniform(self.y_range[0] + delta, self.y_range[1] - delta), - random.uniform(-math.pi, math.pi)) - - return rnd - - @staticmethod - def Nearest(nodelist, n): - return nodelist[int(np.argmin([(nd.x - n.x) ** 2 + (nd.y - n.y) ** 2 - for nd in nodelist]))] -