""" 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 rrt_2D import env from rrt_2D import plotting from rrt_2D import utils class Node: def __init__(self, n): self.x = n[0] self.y = n[1] self.parent = None class RrtStarSmart: def __init__(self, x_start, x_goal, step_len, goal_sample_rate, search_radius, iter_max): self.x_start = 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.env = env.Env() self.plotting = plotting.Plotting(x_start, x_goal) 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.x_start] self.beacons = [] self.beacons_radius = 2 self.path = None def planning(self): n = 0 b = 3 for k in range(self.iter_max): if (k - n) % b == 0: x_rand = self.Sample(self.beacons) else: x_rand = self.Sample() x_nearest = self.Nearest(self.V, x_rand) x_new = self.Steer(x_nearest, x_rand) if x_new and not self.utils.is_collision(x_nearest, x_new): X_near = self.Near(self.V, x_new) c_min = self.Cost(x_new) self.V.append(x_new) for x_near in X_near: c_new = self.Cost(x_near) + self.Line(x_near, x_new) if c_new < c_min: x_new.parent = x_near c_min = c_new for x_near in X_near: c_near = self.Cost(x_near) c_new = c_min + self.Line(x_new, x_near) if c_new < c_near: x_near.parent = x_new if self.InGoalRegion(x_new): self.X_soln.add(x_new) new_cost = self.Cost(x_new) + self.Line(x_new, self.x_goal) if new_cost < c_best: c_best = new_cost x_best = x_new if k % 20 == 0: self.animation(x_center=x_center, c_best=c_best, dist=dist, theta=theta) self.path = self.ExtractPath(x_best) self.animation(x_center=x_center, c_best=c_best, dist=dist, theta=theta) plt.plot([x for x, _ in self.path], [y for _, y in self.path], '-r') plt.pause(0.01) plt.show() def Steer(self, x_start, x_goal): dist, theta = self.get_distance_and_angle(x_start, x_goal) dist = min(self.step_len, dist) node_new = Node((x_start.x + dist * math.cos(theta), x_start.y + dist * math.sin(theta))) node_new.parent = x_start return node_new def Near(self, nodelist, node): n = len(nodelist) + 1 r = 50 * math.sqrt((math.log(n) / n)) dist_table = [(nd.x - node.x) ** 2 + (nd.y - node.y) ** 2 for nd in nodelist] X_near = [nodelist[ind] for ind in range(len(dist_table)) if dist_table[ind] <= r ** 2 and not self.utils.is_collision(node, nodelist[ind])] return X_near def Sample(self, goal=None): if goal in None: delta = self.utils.delta goal_sample_rate = self.goal_sample_rate 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 self.x_goal else: R = self.beacons_radius r = random.uniform(0, R) theta = random.uniform(0, 2 * math.pi) ind = random.randint(0, len(goal) - 1) return Node((goal[ind][0] + r * math.cos(theta), goal[ind][1] + r * math.sin(theta))) def SampleFreeSpace(self): delta = self.delta if np.random.random() > self.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 self.x_goal def ExtractPath(self, node): path = [[self.x_goal.x, self.x_goal.y]] while node.parent: path.append([node.x, node.y]) node = node.parent path.append([self.x_start.x, self.x_start.y]) return path def InGoalRegion(self, node): if self.Line(node, self.x_goal) < self.step_len: return True return False @staticmethod def RotationToWorldFrame(x_start, x_goal, L): a1 = np.array([[(x_start.x - x_start.x) / L], [(x_goal.y - x_start.y) / L], [0.0]]) e1 = np.array([[1.0], [0.0], [0.0]]) M = a1 @ e1.T U, _, V_T = np.linalg.svd(M, True, True) C = U @ np.diag([1.0, 1.0, np.linalg.det(U) * np.linalg.det(V_T.T)]) @ V_T return C @staticmethod def SampleUnitNBall(): theta, r = random.uniform(0.0, 2 * math.pi), random.random() x = r * math.cos(theta) y = r * math.sin(theta) return np.array([[x], [y], [0.0]]) @staticmethod def Nearest(nodelist, n): return nodelist[int(np.argmin([(nd.x - n.x) ** 2 + (nd.y - n.y) ** 2 for nd in nodelist]))] @staticmethod def Line(x_start, x_goal): return math.hypot(x_goal.x - x_start.x, x_goal.y - x_start.y) @staticmethod def Cost(node): cost = 0.0 if node.parent is None: return cost 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) def animation(self, x_center=None, c_best=None, dist=None, theta=None): plt.cla() self.plot_grid("Informed rrt*, N = " + str(self.iter_max)) plt.gcf().canvas.mpl_connect( 'key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if c_best != np.inf: self.draw_ellipse(x_center, c_best, dist, theta) for node in self.V: if node.parent: plt.plot([node.x, node.parent.x], [node.y, node.parent.y], "-g") plt.pause(0.01) def plot_grid(self, name): for (ox, oy, w, h) in self.obs_boundary: self.ax.add_patch( patches.Rectangle( (ox, oy), w, h, edgecolor='black', facecolor='black', fill=True ) ) for (ox, oy, w, h) in self.obs_rectangle: self.ax.add_patch( patches.Rectangle( (ox, oy), w, h, edgecolor='black', facecolor='gray', fill=True ) ) for (ox, oy, r) in self.obs_circle: self.ax.add_patch( patches.Circle( (ox, oy), r, edgecolor='black', facecolor='gray', fill=True ) ) plt.plot(self.x_start.x, self.x_start.y, "bs", linewidth=3) plt.plot(self.x_goal.x, self.x_goal.y, "rs", linewidth=3) plt.title(name) plt.axis("equal") @staticmethod def draw_ellipse(x_center, c_best, dist, theta): a = math.sqrt(c_best ** 2 - dist ** 2) / 2.0 b = c_best / 2.0 angle = math.pi / 2.0 - theta cx = x_center[0] cy = x_center[1] t = np.arange(0, 2 * math.pi + 0.1, 0.1) x = [a * math.cos(it) for it in t] y = [b * math.sin(it) for it in t] rot = Rot.from_euler('z', -angle).as_dcm()[0:2, 0:2] fx = rot @ np.array([x, y]) px = np.array(fx[0, :] + cx).flatten() py = np.array(fx[1, :] + cy).flatten() plt.plot(cx, cy, ".b") plt.plot(px, py, linestyle='--', color='darkorange', linewidth=2) def main(): x_start = (18, 8) # Starting node x_goal = (37, 18) # Goal node rrt = RrtStarSmart(x_start, x_goal, 1.0, 0.10, 0, 1000) rrt.planning() if __name__ == '__main__': main()