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()
# Update particle weight according to how good every particle matches
# robbie's sensor reading
for p in particles:
if world.is_free(*p.xy):
p_d = p.read_sensor(world)
p.w = w_gauss(r_d, p_d)
else:
p.w = 0
# ---------- Try to find current best estimate for display ----------
m_x, m_y, m_confident = compute_mean_point(particles)
# ---------- Show current state ----------
world.show_particles(particles)
world.show_mean(m_x, m_y, m_confident)
world.show_robot(robbie)
# ---------- Shuffle particles ----------
new_particles = []
# Normalise weights
nu = sum(p.w for p in particles)
if nu:
for p in particles:
p.w = p.w / nu
# create a weighted distribution, for fast picking
dist = WeightedDistribution(particles)
for _ in particles:
p = dist.pick()
self.move_by(dx, dy)
return True
return False
# ------------------------------------------------------------------------
world = Maze(maze_data)
world.draw()
# Initialize Sharks
sharks = Shark.create_random(SHARK_COUNT, world)
print "length of shark", len(sharks)
print sharks
robert = Robot(world)
while True:
# ---------- Show current state ----------
world.show_sharks(sharks)
world.show_robot(robert)
# # ---------- Move things ----------
# Move sharks with shark's speed
for s in sharks:
s.advance(s.speed)
time.sleep(0.1)
# robbie's sensor reading
for p in particles:
if world.is_free(*p.xy):
p_d = world.distance(r_d_x, r_d_y, p.x, p.y)
#p_d = p.read_sensor(world)
p.w = w_gauss(r_d, p_d)
else:
p.w = 0
# ---------- Try to find current best estimate for display ----------
m_x, m_y, m_confident = compute_mean_point(particles)
# ---------- Show current state ----------
world.show_particles(particles)
world.show_mean(m_x, m_y, m_confident)
world.show_robot(robbie)
# ---------- Shuffle particles ----------
new_particles = []
# Normalise weights
nu = sum(p.w for p in particles)
if nu:
for p in particles:
p.w = p.w / nu
# create a weighted distribution, for fast picking
dist = WeightedDistribution(particles)
for _ in particles:
p = dist.pick()
world = Maze(maze_data)
world.draw()
# initial distribution assigns each particle an equal probability
robot = Robot(world)
state = Particle.create_random(N, world)
StdDev = 1.4
estimate = False
while True:
# draw the state
world.clear()
world.show_particles(state)
world.show_robot(robot)
if estimate:
world.show_estimate(estimate)
world.update()
# move randomly
robot.move(world)
# take measurement
z = robot.read_sensor(world)
# This is the weighted average of the particle estimates, and is used to determine if the model
# converged to the correct location. This extension was not presented in class.
z_estimate = 0
# create a weighted distribution, for fast picking
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)
p.w = g
# Time for some action!
m.show_particles(particles)
m.show_robot(robbie)
# I'm doing the movement first, it makes life easier
robbie.move(m)
# Move particles according to my belief of movement (this may
# be different than the real movement, but it's all I got)
for p in particles:
p.x += robbie.dx
p.y += robbie.dy
# ---------- Shuffle particles ----------
# Remove all particles that cannot be right (out of arena, inside
# obstacle). Remember how many were removed so we can add that
dev = max(abs(d - r_d), 0.00001)
w = 1 / dev
particle.w = w
#particle.w = w_gauss(d, r_d)
weight_sum += particle.w
for particle in particles:
particle.w /= weight_sum
# ---------- Show current state ----------
world.show_particles(particles)
# world.show_mean(m_x, m_y, m_confident)
world.show_robot(nao)
# ---------- Shuffle particles ----------
new_particles = sus(particles)
for p in new_particles:
x, y = p.x, p.y
p.x, p.y = add_some_noise(p.x, p.y)
particles = new_particles
# ---------- Move things ----------
old_heading = nao.h
nao.move(world)
d_h = nao.h - old_heading
