def main(): world = Maze(maze_data, HALF_WIDTH, HALF_HEIGHT) world.draw() for shark_count in range(151)[10::10]: # Initialize Items sharks = Shark.create_random(shark_count, world, 0) robert = Robot(world, 0,0) robots = [robert] no_particles = [] # Write to File my_file = open("%sSharksDistFromLine.txt" %(shark_count), "w") # Header describing model my_file.write("Line Start: %s, Line End: %s" %(LINE_START, LINE_END)) my_file.write("\n") my_file.write("NumSharks: %s, K_att: %s, K_rep: %s, Sigma_Rand: %s, Speed/ts: %s" %(shark_count, K_ATT, K_REP, SIGMA_RAND, Shark.speed)) my_file.write("\n") att_line = LineString([LINE_START, LINE_END]) # Let sharks move towards attraction line first for time_step in range(300): move(world, robots, sharks, att_line, no_particles, SIGMA_RAND, K_ATT, K_REP) # while True: for _ in range(3): for time_step in range(TIME_STEPS): # # ---------- Show current state ---------- if SHOW_VISUALIZATION: world.show_sharks(sharks) world.show_robot(robert) world.show_att_line(LINE_START, LINE_END) move(world, robots, sharks, att_line, no_particles, SIGMA_RAND, K_ATT, K_REP) for shark in sharks: # my_file.write("%s, %s," % (shark.x, shark.y)) my_file.write("%s, " %(distance_from_line(shark, att_line))) my_file.write("\n") print time_step my_file.close()
def main(): world = Maze(MAZE_DATA) if SHOW_VISUALIZATION: world.draw() # Initialize particles, robots, means and sharks particles_list = [] for _ in range(TRACK_COUNT): particles_list.append(Particle.create_random(PARTICLE_COUNT, world)) robots = Robot.create_random(ROBOT_COUNT, world) sharks = Shark.create_random(SHARK_COUNT, world, TRACK_COUNT) # Initialize error lists error_x1 = [] error_y1 = [] # Filter for time step for time_step in range(TIME_STEPS): means_list = [] for i, particles in enumerate(particles_list): particles_list[i] = estimate(robots, sharks[i], particles, world, error_x1, error_y1) means_list.append(compute_particle_mean(particles, world)) # Move robots, sharks and particles move(world, robots, sharks, particles_list, sp.SIGMA_RAND, sp.K_ATT, sp.K_REP) # Show current state if SHOW_VISUALIZATION: show(world, robots, sharks, particles_list, means_list) print(time_step) # Plot actual vs. estimated into graph errorPlot(error_x1, error_y1)
""" Move the robot. Note that the movement is stochastic too. """ while True: self.step_count += 1 if self.advance_by(self.speed, noisy=True, checker=lambda r, dx, dy: maze.is_free(r.x+dx, r.y+dy)): break # Bumped into something or too long in same direction, # chose random new direction self.chose_random_direction() # ------------------------------------------------------------------------ world = Maze(maze_data) world.draw() # initial distribution assigns each particle an equal probability particles = Particle.create_random(PARTICLE_COUNT, world) robbie = Robot(world) raw_input() while True: # Read robbie's sensor r_d = robbie.read_sensor(world) # Update particle weight according to how good every particle matches # robbie's sensor reading for p in particles: if world.is_free(*p.xy):
from draw import Maze import time """ # Smaller maze """ maze_data = ((2, 0, 1, 0, 0), (0, 0, 0, 0, 1), (1, 1, 1, 0, 0), (1, 0, 0, 0, 0), (0, 0, 2, 0, 1)) # 0 - empty square # 1 - occupied square # 2 - occupied square with a beacon at each corner, detectable by the robot # maze_data = ( ( 1, 1, 0, 0, 2, 0, 0, 0, 0, 1 ), # ( 1, 2, 0, 0, 1, 1, 0, 0, 0, 0 ), # ( 0, 1, 1, 0, 0, 0, 0, 1, 0, 1 ), # ( 0, 0, 0, 0, 1, 0, 0, 1, 1, 2 ), # ( 1, 1, 0, 1, 1, 2, 0, 0, 1, 0 ), # ( 1, 1, 1, 0, 1, 1, 1, 0, 2, 0 ), # ( 2, 0, 0, 0, 0, 0, 0, 0, 0, 0 ), # ( 1, 2, 0, 1, 1, 1, 1, 0, 0, 0 ), # ( 0, 0, 0, 0, 1, 0, 0, 0, 1, 0 ), # ( 0, 0, 1, 0, 0, 2, 1, 1, 1, 0 )) world = Maze(maze_data) world.draw() while True: time.sleep(1000)
def main(): mat_contents = sio.loadmat('tracks.mat') t_LoL = mat_contents['t'] x_LoL = mat_contents['x'] y_LoL = mat_contents['y'] num_sharks = len(x_LoL) time_steps = len(x_LoL[0]) # Initialize Objects world = Maze(maze_data, HALF_WIDTH, HALF_HEIGHT) if SHOW_VISUALIZATION: world.draw() sharks = Shark.create_random(num_sharks, world, x_LoL, y_LoL, t_LoL, num_sharks) robots = sp.Robot.create_random(0, world) particles_list = [sp.Particle.create_random(PARTICLE_COUNT, world)] # Actual Line (from Kim) # y = -0.2168x + 0.113 act_line_start = (-HALF_WIDTH, -0.2168*-HALF_WIDTH + 0.113) act_line_end = (HALF_WIDTH, -0.2168* HALF_WIDTH + 0.113) act_att_line = att_pf.LineString([act_line_start, act_line_end]) # Initialize error lists and file my_file = open("catalina_error_squared.txt", "w") # Header describing model my_file.write("x, y for all sharks, line break represents next time step") my_file.write("\n") my_file.write("z_t = sum of (distance of shark_i)") my_file.write("\n") error_list = [] for time_step in range(TIME_STEPS): # TODO: Consolidate shark_sim.py and att_line_pf.py # Calculate mean position p_means_list = [] for i, particles in enumerate(particles_list): particles_list[i] = att_pf.estimate(particles, world, sharks) m1, m2, m_num_sharks = att_pf.compute_particle_means(particles, world) # TODO p_means_list.append(m1) move(world, sharks, particles_list, time_step) # Find total error (performance metric) and add to list est_line = att_pf.LineString([m1, m2]) est_error_sum = 0 act_error_sum = 0 raw_error_sum = 0 for shark in sharks: est_error_sum += ((att_pf.distance_from_line(shark, est_line))) ** 2 act_error_sum += ((att_pf.distance_from_line(shark, act_att_line))) ** 2 # raw_error_sum += (distance_from_line(shark, est_line) - distance_from_line(shark, attraction_line))**2 # error = math.sqrt(raw_error_sum/track_count) est_error = att_pf.math.sqrt(est_error_sum / num_sharks) act_error = att_pf.math.sqrt(act_error_sum / num_sharks) error_list.append(est_error) error_list.append(act_error) # # ---------- Show current state ---------- if SHOW_VISUALIZATION: sp.show(world, robots, sharks, particles_list, p_means_list, m1, m2, act_att_line) print time_step for item in error_list: my_file.write(str(item) + ",") my_file.write("\n") my_file.close()
def move(self, maze): """ Move the robot. Note that the movement is stochastic too. """ while True: xx, yy = add_noise(0.02, self.x + self.dx, self.y + self.dy) if maze.is_free(xx, yy): self.x, self.y = xx, yy break # Bumped into something, chose random new direction self.chose_random_direction() # ------------------------------------------------------------------------ m = Maze(maze_data) m.draw() particles = Particle.create_random(PARTICLE_COUNT, m) robbie = Robot(m) while True: # Read robbie's sensor r_d = robbie.read_sensor(m) # Update particle weight according to how good every particle matches # robbie's sensor reading for p in particles: p_d = p.read_sensor(m) # This is just a gaussian I pulled out of my hat, near to # robbie's measurement => 1, further away => 0 g = math.e ** -((r_d - p_d)**2 * 19)