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)
control_theta = min(max(control_theta, - MAX_CONTROL), MAX_CONTROL)
self.h += control_theta
# Calculate cartesian distance
dx = math.cos(self.h) * speed
dy = math.sin(self.h) * speed
# Checks if, after advancing, shark is still in the box
if checker is None or checker(self, dx, dy):
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)
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:
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)
while True:
# Read robbie's sensor
r_d, r_d_x, r_d_y = 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):
p_d = world.distance(r_d_x, r_d_y, p.x, p.y)
"""
Move the robot. Note that the movement is stochastic too.
"""
while True:
self.step_count += 1
xx, yy = add_noise(0.02, self.x + self.dx, self.y + self.dy)
if maze.is_free(xx, yy) and self.step_count % 60 != 0:
self.x, self.y = xx, yy
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
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)
start = getStart()
#noisyHeadings = getNoisyHeadings()
#noisyDistances = getNoisyDistances()
#orientations = getOrientations()
distances = getNoisyDistances()
headings = getNoisyHeadings()
maze_data,offset_x,offset_y = getRFID()
ranges = getRanges()
# print(ranges)
world = Maze(maze_data)
world.draw()
# initial distribution assigns each particle an equal probability
particles = Particle.create_random(PARTICLE_COUNT, world)
robbie = Robot(world)
count = 0
maxParticle = Particle(-100, -100,
heading=robbie.h,
noisy=True)
while count < len(headings):
# Read robbie's sensor
"""
Move the robot. Note that the movement is stochastic too.
"""
while True:
self.step_count += 1
xx, yy = add_noise(0.02, self.x + self.dx, self.y + self.dy)
if maze.is_free(xx, yy) and self.step_count % 30 != 0:
self.x, self.y = xx, yy
break
# Bumped into something or too long in same direction,
# chose random new direction
self.chose_random_direction()
# ------------------------------------------------------------------------
world = Maze(maze_data)
world.draw()
particles = Particle.create_random(PARTICLE_COUNT, world)
robbie = Robot(world)
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):
p_d = p.read_sensor(world)
p.w = w_gauss(r_d, p_d)
# 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
import maze_generator as mg
maze_data = mg.generateMazeData()
world = Maze(maze_data)
#world.draw()
ROBOT_HAS_COMPASS = True # Does the robot know where north is? If so, it
# makes orientation a lot easier since it knows which direction it is facing.
# If not -- and that is really fascinating -- the particle filter can work
# out its heading too, it just takes more particles and more time. Try this
# with 3000+ particles, it obviously needs lots more hypotheses as a particle
# now has to correctly match not only the position but also the heading.
# ------------------------------------------------------------------------
# Some utility functions
def rms(list):
sum = 0
for term in list:
def move(self, maze):
"""
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)
sharkie = Shark(world)
robert = Robot(world)
while True:
# Read robbie's sensor
# robot_wall_dist = robbie.read_sensor(world)
shark_dist_robot1 = sharkie.read_distance_sensor(robbie)[0]
shark_angle_robot1 = sharkie.read_angle_sensor(robbie)
shark_dist_robot2 = sharkie.read_distance_sensor(robert)[0]
shark_angle_robot2 = sharkie.read_angle_sensor(robert)
def move(self, maze):
"""
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)
while True:
# Read robbie's sensor
r_d = robbie.read_sensor(world)
if r_d is not None:
# Update particle weight according to how good every particle matches
# robbie's sensor reading
for p in particles:
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(
) #well, i didn't see any sentimental functionality in here, I guess it
#will be triggered when bumped into wall right? as author mention above
# ------------------------------------------------------------------------
world = Maze(maze_data)
world.draw()
# initial distribution assigns each particle an equal probability
particles = Particle.create_random(PARTICLE_COUNT, world)
robbie = Robot(world)
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
keep.append(particles[i])
return keep
def sus(particles):
weight_sum = sum([p.w for p in particles])
n = len(particles)
p = weight_sum/n
start = random.uniform(0, p)
pointers = [start + i*p for i in range(n)]
return rws(particles, pointers)
world = Maze(maze_data)
world.draw()
# initial distribution assigns each particle an equal probability
particles = Particle.create_random(PARTICLE_COUNT, world)
nao = Robot(world)
while True:
# Read Robot's sensor
r_d = nao.read_sensor(world)
weight_sum = 0
for particle in particles:
d = particle.read_sensor(world)
"""
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)
errorList = []
while INSData and len(INSData) > 20:
insBuffer = [INSData.pop(0) for i in range(21)]
beaconBuffer = {}
startTime = insBuffer[0]['ts']
endTime = insBuffer[20]['ts']
groundTruth = None
while gt[0]['ts'] < startTime:
groundTruth = gt.pop(0)
#"here we need to show the ground truth of car"
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 move(self, maze):
"""
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:
( 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 ))
print 'Creating world map'
world_map = DummyGPSMap()
print 'Creating world'
world = Maze(world_map.get_maze_data())
print 'Drawing world'
world.draw()
print 'Showing the GPS path'
print world_map.get_center_line()
world.show_path(world_map.get_center_line())
#print 'Reading sensor information'
#print 'Showing laser bounds'
raw_input("Press ENTER to exit")
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
Move the robot. Note that the movement is stochastic too.
"""
while True:
self.step_count += 2
xx, yy = add_noise(0.02, self.x + self.dx, self.y + self.dy)
if maze.is_free(xx, yy) and self.step_count % 30 != 0:
self.x, self.y = xx, yy
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
robot = Robot(world)
state = Particle.create_random(N, world)
StdDev = 1.4
while True:
# draw the state
world.show_particles(state)
world.show_robot(robot)
# move randomly
