def update_particle_weights(self, robot_spec, robot_sensor_readings, world):
particle_readings = [Particle.get_expected_sensor_outputs(robot_spec, world, self.particle_pos[:, i,np.newaxis], self.particle_h[i]) for
i in range(self.N_PARTICLES)]
self.particle_weights = np.zeros(self.N_PARTICLES)
for i in range(self.N_PARTICLES):
if Particle.is_position_valid(world, self.particle_pos[:, i]):
self.particle_weights[i] = Particle.get_sensor_reading_probabilities(robot_spec, robot_sensor_readings,
particle_readings[i])
self.particle_weights = self.particle_weights / np.sum(self.particle_weights)
def draw(self, window):
pos_px = window.m_to_px(self.pos)
# Robot
draw_triangle(window.screen,
int(pos_px[0]),
int(pos_px[1]),
self.h,
r=7,
c=Colours.ROBOT_COLOUR,
fill=True)
readings = Particle.get_expected_sensor_outputs(
self.robot_spec, self.world, self.pos, self.h)
for reading, sensor in zip(readings, self.robot_spec.sensors):
sensor.draw(window, self.pos, self.h, reading)
def move_particles(self, d, dh):
self.particle_pos, self.particle_h = Particle.move(self.particle_pos, self.particle_h, d, dh)
return g
window = SimulatorWindow(text='Robot simulation')
world = World()
world.add_to_window(window)
robot = SimulatedRobot(world, pos=[1, 1], h=0)
robot.add_to_window(window)
particles = []
for x in range(8 * 10):
for y in range(4 * 10):
p = Particle(world,
x / (8 * Units.METERS_IN_A_FOOT * 10),
y / (4 * Units.METERS_IN_A_FOOT * 10),
h=robot.h)
p.add_to_window(window)
particles.append(p)
# Update particle weights.
robot_sensor_readings = robot.get_expected_sensor_outputs()
particle_weights = np.zeros(len(particles))
invalid_particle_indices = []
for i, p in enumerate(particles):
if p.is_position_valid():
particle_sensor_readings = p.get_expected_sensor_outputs()
particle_weights[i] = np.prod([
w_gauss(r_s, p_s, s) for r_s, p_s, s in zip(
robot_sensor_readings, particle_sensor_readings,
sensor_reading_sigmas)
# Extract positions and headings
particle_pos = np.array(positions[:]).reshape((2, N_PARTICLES))
particle_h = np.array(headings[:])
# FIND COM
com_pos = np.mean(particle_pos, axis=1)
com_h = np.mean(particle_h)
com_uncertainty = np.std(particle_pos, axis=1)
is_converged = np.all(2 * com_uncertainty < 0.4)
# ---- DRAW OBJECTS
# ------------------------------------------------------------
window.draw()
for i in range(N_PARTICLES):
Particle.draw(window, particle_pos[:, i], particle_h[i])
# Draw the center of mass position and heading
Particle.draw_com(window, com_pos, com_h, com_uncertainty)
# Set the variables on the robot
robot.localized = is_converged
robot.state = state.value
robot.pos = np.reshape(com_pos, (2, 1))
robot.h = com_h
robot.sensor_readings = readings[:]
robot.draw(window)
# Go ahead and update the screen with what we've drawn.
# This MUST happen after all the other drawing commands.
window = SimulatorWindow(text='Robot simulation')
world = World()
world.add_to_window(window)
robot = SimulatedRobot(robot_spec, world, pos=[1.5, 1], h=45)
robot.add_to_window(window)
# Evenly distribute the particles to start
particles = []
for x in range(8 * 10):
for y in range(4 * 10):
p = Particle(robot_spec,
world,
x / (8 * Units.METERS_IN_A_FOOT * 10),
y / (4 * Units.METERS_IN_A_FOOT * 10),
h=random.randint(0, 359))
particles.append(p)
p.add_to_window(window)
# Initial weights.
particle_readings = [p.get_expected_sensor_outputs() for p in particles]
particle_weights = update_particle_weights(robot, particles, particle_readings)
# Draw to screen.
should_quit = False
t = 0
UPDATE_EVERY = 10
update_clock = pygame.time.Clock()
update_clock = pygame.time.Clock()
com_pos, com_h, com_uncertainty, is_converged = np.array([0, 0]), 0, np.array(
[1, 1]), False
while not should_quit:
t += 1
# Limit fps.
update_clock.tick(10)
td = update_clock.get_time()
print("Update took {} ms".format(update_clock.get_time()))
# ---- DRAW OBJECTS
# ------------------------------------------------------------
window.draw()
for i in range(N_PARTICLES):
Particle.draw(window, pf.particle_pos[:, i], pf.particle_h[i])
# Draw the center of mass position and heading
Particle.draw_com(window, com_pos, com_h, com_uncertainty, is_converged)
robot.draw(window)
# Go ahead and update the screen with what we've drawn.
# This MUST happen after all the other drawing commands.
pygame.display.flip()
# ------------------------------------------------------------
# Process events.
mouse_clicked = False
events = pygame.event.get()
for event in events:
if event.type == pygame.QUIT:
def move(self, d_d, d_h):
self.pos, self.h = Particle.move(self.pos, self.h, d_d, d_h)
def get_sensor_outputs(self):
return Particle.get_expected_sensor_outputs(self.robot_spec,
self.world, self.pos,
self.h)