def initializeHMM(numStates=numStates, possibleObservations=numCats): print "Initializing HMM..." hmm = HMM(numStates=numStates, numCats=possibleObservations) hmm.pi = np.random.rand(numStates) hmm.pi /= sum(hmm.pi) hmm.A = np.random.rand(numStates, numStates) A_row_sums = hmm.A.sum(axis=1) hmm.A /= A_row_sums[:, np.newaxis] hmm.B = np.random.rand(numStates, numCats) B_row_sums = hmm.B.sum(axis=1) hmm.B /= B_row_sums[:, np.newaxis] print "Initial HMM stats" print "A: ", print hmm.A print "B: ", print hmm.B print "pi: ", print hmm.pi return hmm
def main():
global leave_trace, list_traces, pomdp
if r_visual_granularity > wall_thickness:
print "PARAMETER ERROR: r_visual_granularity exceeds wall_thickness!"
print "This can cause wall detection errors!"
if r_init_x < wall_thickness or r_init_y < wall_thickness:
print "PARAMETER ERROR: starting position overlaps wall!"
print "Check r_init_x|y_topleft and wall_thickness"
pygame.init() # also calls display.init()
startTime = time.time()
caption = sim_version + " \tmode: teleoperation "
pygame.display.set_caption(caption + str(startTime))
r_sprite = load_image(r_image)
g_sprite = load_image("goal.bmp")
background = load_image(back_image)
# prepare simulation objects
clock = pygame.time.Clock()
screen.blit(background, (0, 0))
goal = Goal(g_sprite)
r = Robot(
r_sprite,
r_init_x,
r_init_y,
r_init_azi,
r_init_fwd_speed,
r_init_spin_speed,
r_visual_range,
r_visual_angle,
goal,
pomdp,
)
robotSprite = pygame.sprite.Group(r)
goalSprite = pygame.sprite.Group(goal)
# display the environment once, right before event loop
count = -1
for ob in list_obstacles:
count = count + 1
s = pygame.display.get_surface()
s.fill(ob.color, list_rect_obstacles[count])
r.draw_rays(screen)
r.showGoalPath(screen)
pygame.display.flip()
# recording time
going = True
time_down = 0.0
time_elapsed = 0.0
T1 = -1
T2 = -1
# HMM initalize
hmm = HMM()
hmm.pi = np.array([0.5, 0.5])
hmm.A = np.array([[0.5, 0.5], [0.5, 0.5]])
hmm.B = np.array([[0.3, 0.7], [0.99, 0.01]])
while going:
clock.tick(fps) # at most that many fps
time_elapsed = 0.0
# Event loop################################
global user_inputF, user_inputR, totalUserInput, o, R, numTrials
for event in pygame.event.get():
if event == QUIT:
going = False
elif event.type == KEYDOWN:
if event.key == K_ESCAPE:
going = False
elif event.key == K_w:
T1 = 1
r.direction = "w"
time_down = pygame.time.get_ticks()
elif event.key == K_d:
T2 = 1
r.direction = "d"
time_down = pygame.time.get_ticks()
elif event.key == K_a:
T2 = 1
r.direction = "a"
time_down = pygame.time.get_ticks()
elif event.key == K_s:
T1 = 1
r.direction = "s"
time_down = pygame.time.get_ticks()
if event.key == K_SPACE:
r.opmode = 0 # teleop mode
caption = sim_version + " \tmode: teleoperation "
if event.key == K_1:
r.opmode = 1 # autonomous navigation mode
caption = sim_version + " \tmode: autonomous "
if event.key == K_2:
r.opmode = 2 # autonomous navigation mode
caption = sim_version + " \tmode: assist "
if event.key == K_3:
r.opmode = 3 # autonomous navigation mode
caption = sim_version + " \tmode: pomdp "
if event.key == K_4:
r.opmode = 4 # autonomous navigation mode
caption = sim_version + " \tmode: hmm "
elif event.type == KEYUP:
if event.key == K_w:
time_elapsed = pygame.time.get_ticks() - time_down
T1 = -1
r.direction = "N"
elif event.key == K_d:
time_elapsed = pygame.time.get_ticks() - time_down
T2 = -1
r.direction = "N"
elif event.key == K_a:
time_elapsed = pygame.time.get_ticks() - time_down
T2 = -1
r.direction = "N"
elif event.key == K_s:
time_elapsed = pygame.time.get_ticks() - time_down
T1 = -1
r.direction = "N"
totalUserInput += time_elapsed / 1000.0
user_inputF += T1 * 0.1
if user_inputF > 1.0:
user_inputF = 1.0
elif user_inputF < 0.0:
user_inputF = 0.0
user_inputR += T2 * 0.1
if user_inputR > 1.0:
user_inputR = 1.0
elif user_inputR < 0.0:
user_inputR = 0.0
pygame.display.set_caption(caption)
# slow down when stopped moving
if r.speed > 0.0:
r.speed -= 0.5
if r.speed < 0.5:
r.speed = 0.0
elif r.speed < 0.0:
r.speed += 0.5
if r.speed > 0.5:
r.speed = 0.0
# Find if goal reached
if pygame.sprite.spritecollide(r, goalSprite, False) != []:
print "You made it to the goal"
print r.numCollision
R = R + 1
if len(list_obstacles) > 30:
mapLevel = 3
else:
mapLevel = 0
if r.numCollision > 5:
observation = 0 + mapLevel
elif r.numCollision > 0:
observation = 1 + mapLevel
else:
observation = 2 + mapLevel
r.pomdp.update_belief(r.action, observation)
r.action = r.pomdp.get_best_action()[0]
print "Most likely state: " + r.pomdp.pomdpenv.states[np.argmax(r.pomdp.belief)]
print "Observation: " + pomdp.pomdpenv.observations[observation]
print "Action: " + pomdp.pomdpenv.actions[r.action]
prevOb = observation
print r.pomdp.belief
r.numCollision = 0
# if R == numTrials:
# hmm.train(o,0.0001,graphics=False)
# print 'probabilities\n',hmm.pi
# print 'state transition probabililities\n',hmm.A
# print 'observation probabililities\n',hmm.B
# R = 0
# o = np.zeros(numTrials)
goal.getNew()
startTime = time.time()
robotSprite.update()
goalSprite.update()
screen.blit(background, (0, 0)) # redraws the entire bkgrnd.
# screen.fill((255,255,255)) # white background
# screen.blit(red_block, (100,100))
count = -1
for ob in list_obstacles:
count = count + 1
s = pygame.display.get_surface()
s.fill(ob.color, list_rect_obstacles[count])
ob.detected = False
ob.dist = 1000.0
ob.xCollide = 0.0
ob.yCollide = 0.0
r.draw_rays(screen)
r.showGoalPath(screen)
robotSprite.draw(screen)
goalSprite.draw(screen)
# pygame.display.update()
pygame.display.flip() # all changes are drawn at once (double buffer)
# pygame.time.delay(100)
pygame.quit() # also calls display.quit()
f = open("results", "w")
f.write(str(Distance) + ",")
f.write(str(totalUserInput) + ",")
f.write(str(minObsDist))
def main():
global leave_trace, list_traces,pomdp
if r_visual_granularity > wall_thickness:
print 'PARAMETER ERROR: r_visual_granularity exceeds wall_thickness!'
print 'This can cause wall detection errors!'
if r_init_x<wall_thickness or r_init_y<wall_thickness:
print 'PARAMETER ERROR: starting position overlaps wall!'
print 'Check r_init_x|y_topleft and wall_thickness'
pygame.init() #also calls display.init()
startTime = time.time()
caption = (sim_version + ' \tmode: teleoperation ' )
pygame.display.set_caption(caption+ str(startTime))
r_sprite = load_image(r_image)
g_sprite = load_image('goal.bmp')
background = load_image(back_image)
#prepare simulation objects
clock = pygame.time.Clock()
screen.blit(background, (0, 0))
goal = Goal(g_sprite)
r = Robot(r_sprite, r_init_x, r_init_y,r_init_azi, r_init_fwd_speed,\
r_init_spin_speed, r_visual_range, r_visual_angle,goal,pomdp)
robotSprite = pygame.sprite.Group(r)
goalSprite = pygame.sprite.Group(goal)
#display the environment once, right before event loop
count = -1
for ob in list_obstacles:
count = count + 1
s = pygame.display.get_surface()
s.fill(ob.color, list_rect_obstacles[count])
r.draw_rays(screen)
r.showGoalPath(screen)
pygame.display.flip()
going = True
time_down = 0.0
time_elapsed = 0.0
T = -1
#HMM initalize
hmm = HMM()
hmm.pi = np.array([0.5, 0.5])
hmm.A = np.array([[0.5, 0.5],[0.5, 0.5]])
hmm.B = np.array([[0.3, 0.7],[0.99, 0.01]])
while going:
clock.tick(fps) #at most that many fps
time_elapsed = 0.0
#Event loop################################
global user_input, totalUserInput, o,R,numTrials
for event in pygame.event.get():
if event == QUIT:
going = False
elif event.type == KEYDOWN:
if event.key == K_ESCAPE:
going = False
elif event.key == K_w:
T = 1
r.direction = 'w'
time_down = pygame.time.get_ticks()
elif event.key == K_d:
T = 1
r.direction = 'd'
time_down = pygame.time.get_ticks()
elif event.key == K_a:
T = 1
r.direction = 'a'
time_down = pygame.time.get_ticks()
elif event.key == K_s:
T = 1
r.direction = 's'
time_down = pygame.time.get_ticks()
if event.key == K_SPACE:
r.opmode = 0 #teleop mode
caption = sim_version + ' \tmode: teleoperation '
if event.key == K_1:
r.opmode = 1 #autonomous navigation mode
caption = (sim_version + ' \tmode: autonomous ')
if event.key == K_2:
r.opmode = 2 #autonomous navigation mode
caption = (sim_version + ' \tmode: assist ')
if event.key == K_3:
r.opmode = 3 #autonomous navigation mode
caption = (sim_version + ' \tmode: pomdp ')
if event.key == K_4:
r.opmode = 4 #autonomous navigation mode
caption = (sim_version + ' \tmode: hmm ')
if event.key == K_t: #toggles the tracing mode
if leave_trace:
leave_trace = 0
list_traces = list()
print 'changing leave_trace from 1 to 0'
else:
leave_trace = 1
print 'changing leave_trace from 0 to 1'
elif event.type == KEYUP:
if event.key == K_w:
time_elapsed = pygame.time.get_ticks() - time_down
T = -1
r.direction = 'N'
elif event.key == K_d:
time_elapsed = pygame.time.get_ticks() - time_down
T = -1
r.direction = 'N'
elif event.key == K_a:
time_elapsed = pygame.time.get_ticks() - time_down
T = -1
r.direction = 'N'
elif event.key == K_s:
time_elapsed = pygame.time.get_ticks() - time_down
T = -1
r.direction = 'N'
totalUserInput += time_elapsed/1000.0
user_input += T*0.1
if user_input > 1.0:
user_input = 1.0
elif user_input < 0.0:
user_input = 0.0
pygame.display.set_caption(caption)
if r.speed > 0.0:
r.speed -= 0.5
elif r.speed < 0.0:
r.speed += 0.5
# Find if goal reached
if pygame.sprite.spritecollide(r, goalSprite, False) != []:
print 'You made it to the goal'
R=R+1
r.pomdp.update_belief(0,5)
print o
if R == numTrials:
hmm.train(o,0.0001,graphics=False)
print 'probabilities\n',hmm.pi
print 'state transition probabililities\n',hmm.A
print 'observation probabililities\n',hmm.B
R = 0
o = np.zeros(numTrials)
goal.getNew()
startTime = time.time()
robotSprite.update()
goalSprite.update()
screen.blit(background, (0, 0)) #redraws the entire bkgrnd.
#screen.fill((255,255,255)) # white background
#screen.blit(red_block, (100,100))
count = -1
for ob in list_obstacles:
count = count + 1
s = pygame.display.get_surface()
s.fill(ob.color, list_rect_obstacles[count])
r.draw_rays(screen)
r.showGoalPath(screen)
#
draw_traces(screen)
robotSprite.draw(screen)
goalSprite.draw(screen)
#pygame.display.update()
pygame.display.flip() #all changes are drawn at once (double buffer)
#pygame.time.delay(100)
pygame.quit() #also calls display.quit()
f = open('results','w')
f.write(str(Distance)+',')
f.write(str(totalUserInput)+',')
f.write(str(minObsDist))
