v = View()
v.update_all(rstate, bstate)
# Main simulation loop
rstate = None
iters = 0
start_time = time.time()
for i in range(MAX_STEPS):
iters += 1
reading = sim.read_sensors()[robot_id]
p.update_belief(reading)
p_act = p.get_action(reading)
#p_act = action_seq[i]
sim.add_action(p_act, robot_id)
sim.update_turn()
p.pred_step(p_act)
# Check if at goal
rstate = sim.get_state()
rid = robot_params['id']
rx, ry, ro = rstate['robots'][rid].state
[goalx, goaly] = sim_params['goal_pos']
goal_dist = np.sqrt((rx - goalx)**2 + (ry - goaly)**2)
if goal_dist <= sim_params['goal_radius']:
goal_dist = 0
break
# Update View at the end of turn
if GUI_ON:
