def __init__(self, world=None, NGEN=1000, n_ind=100, n_elite=10, fitness_thresh=0.1, margin_on=False, verbose=True): self.trajectory = [world.start] self.history = [] self.pop = [] self.best_ind = [] self.gtot = 0 self.n_ind = n_ind # The number of individuals in a population. self.n_elite = n_elite self.NGEN = NGEN # The number of generation loop. self.fitness_thresh = fitness_thresh self.verbose = verbose # --- Generate Collision Checker self.world = world if margin_on: self.world_margin = world.mcopy(rate=1.05) else: self.world_margin = world self.cc = CollisionChecker(world) self.ccm = CollisionChecker(self.world_margin)
def __init__(self): self.world_size = GlobalVars.get_world_size() self.bird_state = BirdState() self.background_state = BackgroundState() self.collision_checker = CollisionChecker(self.background_state, self.bird_state) self.is_alive = True self.score = 0
def __init__(self, num_paths, path_offset, circle_offsets, circle_radii, path_select_weight, time_gap, a_max, coasting_speed, stop_line_buffer): self.num_paths = num_paths self.path_offset = path_offset self.path_optimizer = PathOptimizer() self.collision_checker = CollisionChecker(circle_offsets, circle_radii, path_select_weight) self.velocity_planner = VelocityPlanner(time_gap, a_max, coasting_speed, stop_line_buffer)
def __init__(self, world, branch_length=0.1, resampling_length=0.05, verbose=False): # set RRT parameter self.verbose = verbose self.rrt_length = branch_length self.resampling_length = resampling_length self.rrt_size = 5000 self.world = world self.path = [] self.smoothed_path = [] self.resampled_path = [] self.tree = [] self.cc = CollisionChecker(world)
def shortcut(path_list, world): cc = CollisionChecker(world) q0 = np.array(path_list[0]) res_path_list = [q0] i = 1 while i < len(path_list): q1 = np.array(path_list[i]) if cc.line_validation(q0, q1): i += 1 else: q0 = np.array(path_list[i - 1]) res_path_list.append(q0) i += 1 res_path_list.append(path_list[-1]) return np.array(res_path_list)
def locate(self, scale_minmax): frame = np.array(self.world.frame) xmax = np.max(frame[:, 0]) xmin = np.min(frame[:, 0]) ymax = np.max(frame[:, 1]) ymin = np.min(frame[:, 1]) def collision_check(target): res = [ cc.path_validation_new(target[i], target[i + 1]) for i in range(len(target) - 1) ] res_start = [ cc.crossing_number_algorithm(self.world.start, target) ] res_goal = [cc.crossing_number_algorithm(self.world.goal, target)] return all(res + res_start + res_goal) new_objects = [] for obj in self.world.objects: self.world.objects = new_objects cc = CollisionChecker(self.world, 0.05) for iter in range(10): # set random value # x, y: translate distance, theta: rotate angle, scale: scale x = np.random.rand() * (xmax - xmin) + xmin y = np.random.rand() * (ymax - ymin) + ymin theta = np.random.rand() * 2.0 * pi scale = np.random.rand() * \ (scale_minmax[1] - scale_minmax[0]) + scale_minmax[0] # Rotate, scale, translate rot_mat = np.array([[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]]) rslt = np.dot(rot_mat, obj.T) * scale + \ np.array([x, y]).reshape(-1, 1) # Check collisions with the other objects. if collision_check(rslt.T): new_objects.append(rslt.T) break self.world.objects = new_objects.copy()
def __init__(self, world, n, k, delta=0.01): # --- Collision Checker self.cc = CollisionChecker(world, delta) # --- World self.world = world # --- parameter self.n = n self.d = 2 self.k = k self.V = np.array([]) self.E = [] self.roadmap = [] self.resampling_length = 0.1 self.path = None self.smoothed_path = None self.resampled_path = None self.path_list = []
def __init__( self, road: Road, actions: Actions, constraints: Constraints, termination_conditions: TerminationConditions, cost_calculator: CostCalculator, start_state: State, ego, subject, ): self.road = road self.actions = actions self.start_state = start_state self.constraints = constraints self.termination_conditions = termination_conditions self.cost_calculator = cost_calculator self.collision_checker = CollisionChecker(1.75, 1) self.ego_vehicle = ego self.subject_vehicle = subject
[pi, -pi], [-pi, -pi]]) # --- Set start/goal point world.start = np.array([-pi / 2, -pi / 2]) * 1.9 world.goal = np.array([pi / 2, pi / 2]) * 1.9 # --- Generate objects og = ObjectGenerator(world) world.objects = og.example() # og.generate(10, 5) # og.locate([2.0, 3.0]) # world.objects = og.world.objects # --- Generte Collision Checker cc = CollisionChecker(world, 0.1) ''' Functions ''' class Individual(np.ndarray): """Container of a individual.""" fitness = None def __new__(cls, a): return np.asarray(a).view(cls) def create_pop_prm(m, n):
''' World setting ''' # --- world class world = World() # --- Set frame world.frame = np.array( [[-pi, -pi], [-pi, pi], [pi, pi], [pi, -pi], [-pi, -pi]]) # --- Set start/goal point world.start = np.array([-pi / 2, -pi / 2]) * 1.9 world.goal = np.array([pi / 2, pi / 2]) * 1.9 # --- Generate objects og = ObjectGenerator(world) world.objects = og.example() world_margin = world.mcopy(rate=1.1) cc = CollisionChecker(world_margin) # In[]: # --- Visualize gd = GraphDrawer(world_margin) plt.show()
world = World() # --- Set frame world.frame = np.array([[-pi, -pi], [-pi, pi], [pi, pi], [pi, -pi], [-pi, -pi]]) # --- Set start/goal point world.start = np.array([-pi / 2, -pi / 2]) * 1.9 world.goal = np.array([pi / 2, pi / 2]) * 1.9 # --- Generate objects og = ObjectGenerator(world) world.objects = og.example() # --- CollisionChecker cc = CollisionChecker(world) # In[]: m = 6 n = 6 w = 1.0 h = 1.0 x0 = -3.0 y0 = -3.0 V = [[x0 + i * w, y0 + j * h] for j in range(n) for i in range(m)] E = [] for i in range(m * n - 1): if i % m != (m - 1): E.append([(i, i + 1), 1.0])
global sensor_data sensor_data = data if __name__ == '__main__': os.environ["WANDB_SILENT"] = "true" # Avoid small wandb bug # Subscribe to essential pathfinding info sailbot = sbot.Sailbot(nodeName='log_stats') rospy.Subscriber("sensors", msg.Sensors, sensorsCallback) sailbot.waitForFirstSensorDataAndGlobalPath() # Setup subscribers log_closest_obstacle = LogClosestObstacle(create_ros_node=False) path_storer = PathStorer(create_ros_node=False) collision_checker = CollisionChecker(create_ros_node=False) sailbot_gps_data = SailbotGPSData(create_ros_node=False) rate = rospy.Rate(UPDATE_TIME_SECONDS) # Set wandb run name and directory path start_datetime = datetime.now().strftime('%b_%d-%H_%M_%S') run_name = 'stats-' + start_datetime dir_dir = os.path.expanduser('~/.ros/stats/') dir_path = dir_dir + start_datetime os.makedirs(dir_dir + start_datetime) os.symlink(dir_path, dir_dir + 'tmp') os.rename(dir_dir + 'tmp', dir_dir + 'latest-dir') # use replace() in Python 3 wandb.init(entity='sailbot', project='stats', dir=dir_path, name=run_name)