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)
