def random_state(self) -> DubinsConfig: seed = np.random.rand(3, 1) state = self.origin + DubinsConfig( float(seed[0]) * self.space_width, float(seed[1]) * self.space_height, float(seed[2]) * self.phi_span) return state
def __init__(self, phi_max=math.pi / 2, s=1, rou_min=0.5, N=1000, K=15, cspace=None, ob_center: list = None, ob_radio: list = None): if cspace is None: self.cspace = [(-3, 3), (-1, 1)] else: self.cspace = [(-3, 3), (-1, 1)] self.origin = DubinsConfig(self.cspace[0][0], self.cspace[1][0], -phi_max) self.space_width = self.cspace[0][1] - self.cspace[0][0] self.space_height = self.cspace[1][1] - self.cspace[1][0] self.obs = dict() self.N = N self.K = K for index, ct in enumerate(ob_center): self.obs[ct] = ob_radio[index] self.speed = s self.turnning_radius = rou_min self.phi_max = phi_max self.phi_span = phi_max * 2
def check_collision_edge(self, edge, collision_margin=0): configurations = DubinsConfig.from_list(edge.configurations[1:]) for config in configurations: if self.is_out_boundary(config): return True for ct, r in self.obs.items(): if config.line_distance_point(ct) - r <= collision_margin: return True return False
def random_state(self, p_thres, qG): p = np.random.random() if p < p_thres: return qG else: seed = np.random.rand(3, 1) state = self.origin + DubinsConfig( float(seed[0]) * self.space_width, float(seed[1]) * self.space_height, float(seed[2]) * self.phi_span) return state
x1 = 10 y1 = 9.9 theta1 = 0 q0 = (-1.2199559242110303, -0.8651945309735829, 6.074952869120775) q1 = (-0.7607923756239869, -0.7272899164210449, 0.7917675619411886) q0 = (-2, -0.5, 0) q1 = (1.195952357338463, 0.26702924459657296, 0.9756583154337937) q2 = (-0.9676497498599093, -0.9790110699545767, 4.2723201582020955) turning_radius = 0.5 step_size = 0.01 # path = dubins.shortest_path(q0, q1, turning_radius) # e = path.path_length() # configurations, d = path.sample_many(step_size) conf0 = DubinsConfig.from_tuple(q0) conf1 = DubinsConfig.from_tuple(q1) conf, length = DubinsConfig.shortest_path(conf0, conf1, turning_radius, step_size) nd0 = Node(conf0) nd1 = Node(conf1) ed = Edge(nd0, nd1, turning_radius) s = 1 phi_max = 1 rou_min = 0.5 ob_center = [(0, -1), (0, 1)] dt = 0.2 ob_r = [1 - dt, 1 - dt]
obs.append( generate_circle(ob_center[0], ob_r[0], th0=0, th1=np.pi, num=100)) obs.append( generate_circle(ob_center[1], ob_r[1], th0=0, th1=-np.pi, num=100)) obs_ct = Point.point_from_list(ob_center) path = [] rrt = RRt(phi_max=phi_max, s=s, rou_min=rou_min, cspace=cspace, ob_center=obs_ct, ob_radio=ob_r) qI = (-2, -0.5, 0) qG = (2, -0.5, math.pi / 2) qi = DubinsConfig(qI[0], qI[1], qI[2]) qg = DubinsConfig(qG[0], qG[1], qG[2]) G, path_nd = rrt.generate_graph(qi, qg, K=100, goal_margin=0, p_thres=0.1) for indx in range(len(path_nd) - 1): ed = G.edgedict[(path_nd[indx], path_nd[indx + 1])] config = ed.configurations for conf in config: path.append(conf) fig, ax = plt.subplots() #G.draw(ax) obstacles = [] dc.draw(ax, cspace, obs, qI, qG, G, path, title="RRT planning") plt.show(ax)
def is_achieved(self, x: Node, xG: DubinsConfig, margin=0): config = x.item if xG.is_close(config): return True return False