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))