def generate_cw_csc_test_1(): turning_radius = 5 sqrt_2 = np.sqrt(2) start = np.array([-5 / sqrt_2, -5 / sqrt_2, to_rad(135)]) end = np.array([30 + 5 / sqrt_2, -5 / sqrt_2, to_rad(225)]) dc = dubin.DubinsCurve(start, end, turning_radius) return dc
def generate_ccc_test_2(): turning_radius = 5 sqrt_3 = np.sqrt(3) start = np.array([5 / 2 * sqrt_3, 5 / 2, to_rad(120)]) end = np.array([5, 0, to_rad(270)]) dc = dubin.DubinsCurve(start, end, turning_radius) return dc
def generate_ccw_csc_test_2(): turning_radius = 5 sqrt_2 = np.sqrt(2) start = np.array([0, 5, to_rad(180)]) end = np.array([30 / sqrt_2, -30 / sqrt_2 - 5, to_rad(0)]) dc = dubin.DubinsCurve(start, end, turning_radius) return dc
def _heuristics(self, start_config, end_config): # Dubin heuristics dubin = dc.DubinsCurve(start_config, end_config, self.turning_radius) dubin_path, dubin_length = dubin.compute() # Euclidean heuristics distance = np.linalg.norm( np.array(start_config[0:2]) - np.array(end_config[0:2])) return np.fmax(dubin_length, distance)
def generate_random_test(): turning_radius = 5 x_lower = 2 * turning_radius x_high = 50 - 2 * turning_radius y_lower = 2 * turning_radius y_high = 50 - 2 * turning_radius start = np.array([ random.uniform(x_lower, x_high), random.uniform(y_lower, y_high), random.uniform(0, 2 * np.pi) ]) end = np.array([ random.uniform(x_lower, x_high), random.uniform(y_lower, y_high), random.uniform(0, 2 * np.pi) ]) dc = dubin.DubinsCurve(start, end, turning_radius) return dc
def _connect_using_dubin(self, start, end): min_turning_radius = self.motion_model.get_min_turning_radius() dubins_curve = dcrv.DubinsCurve(start, end, min_turning_radius) return dubins_curve.compute()