update

Sampling_based_Planning/rrt_2D/optimal_bi_rrt.py

@@ -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)

Sampling_based_Planning/rrt_2D/rs_rrt_star.py

@@ -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]))]
-