def generate_uniform_prior(grid: UE4Grid,
initial_belief_sum=0.5,
fixed_value=None):
'''Generates a uniform prior which equates to delta/|grid|. The sum of each grid location prior is equal to initial_belief_sum. If fixed_value provided,
each location in the prior will have fixed_value as prior'''
if fixed_value and 0 <= fixed_value <= 1:
prior_val = fixed_value
else:
prior_val = initial_belief_sum / len(grid.get_grid_points())
return np.array([prior_val for grid_loc in grid.get_grid_points()] +
[1 - prior_val * grid.get_no_grid_points()])
def get_grid_from_config():
env_parser = get_env_config()
grid_params = env_parser['GridParams']
return UE4Grid(int(grid_params['SpacingX']),
int(grid_params['SpacingY']),
parse_Vector3r_from_string(grid_params["Origin"])[0],
x_lim=int(grid_params['XLim']),
y_lim=int(grid_params['YLim']))
def _generate_gaussian_prior(
grid: UE4Grid,
means: "list of 2 means",
covariance_matrix: "2x2 covariance matrix",
initial_belief_sum=0.5
) -> "Tuple(np.array normed prior probs, prior_dict":
'''A method that returns the normed z vector as well as the prior dict. Given a 2d vector of means, 2X2 covariance matrix, returns a 2D gaussian prior'''
#maybe should check all dimensions of data here
prior = {}
x, y = np.mgrid[0:grid.get_no_points_x() *
grid.get_lng_spacing():grid.get_lng_spacing(),
0:grid.get_no_points_y() *
grid.get_lat_spacing():grid.get_lat_spacing()]
pos = np.empty(x.shape + (2, ))
pos[:, :, 0] = x
pos[:, :, 1] = y
rv = multivariate_normal(means, covariance_matrix)
z = rv.pdf(pos)
normed_z = z * (initial_belief_sum / z.sum())
#normed_z = normed_z.astype(float)
#return likelihoods in dict with grid points
for x_value in [_[0] for _ in x]:
for y_value in y[0]:
prior[Vector3r(x_value,
y_value)] = float(normed_z[x_value][y_value])
#place prior into ordered numpy array
list_of_grid_points = []
for grid_loc in grid.get_grid_points():
list_of_grid_points.append(prior[grid_loc])
list_of_grid_points = np.array(list_of_grid_points)
numpy_prior = np.append(list_of_grid_points, 1 - list_of_grid_points.sum())
return numpy_prior
def plot_prior(grid: UE4Grid, prior):
fig = plt.figure()
x, y = np.mgrid[0:grid.get_no_points_x() *
grid.get_lng_spacing():grid.get_lng_spacing(),
0:grid.get_no_points_y() *
grid.get_lat_spacing():grid.get_lat_spacing()]
ax = fig.gca(projection='3d')
prior = prior[:-1]
z_lim = prior.max() * 1.02
ax.set_zlim3d(0, z_lim)
if prior.shape != (len(x), len(y)):
prior = prior.reshape(len(x), len(y))
ax.plot_wireframe(x, y, prior)
ax.set_xlabel("physical x-axis of grid")
ax.set_ylabel("physical y-axis of grid")
ax.set_zlabel("Initial prob. evidence present at grid location")
return fig
def __init__(self, grid: UE4Grid):
super().__init__(grid.get_diameter())
return return_move
def move_utility(self, belief_map, expected_battery_value, current_grid_loc, next_grid_loc):
'''
Returns the utilitiy function associated with moving from current_grid_loc to next_grid_loc
with the current expected_battery_value and belief_map
'''
pass
def get_move(self, belief_map, current_grid_loc: Vector3r, explored_grid_locs: 'list of Vector3r') -> (bool, Vector3r):
return self.get_move_from_belief_map_epsilon_greedy(belief_map, current_grid_loc, self.epsilon, self.eff_radius)
#%%
if __name__ == "__main__":
test_grid = UE4Grid(1, 1, Vector3r(0,0), 6, 5)
tsp = TSPActionSelection(test_grid, Vector3r(0,3))
print(tsp.get_moves())
obs1 = AgentObservation(Vector3r(0,0),0.5, 1, 1234, 'agent2')
obs2 = AgentObservation(Vector3r(0,0),0.7, 2, 1235, 'agent2')
obs3 = AgentObservation(Vector3r(0,1),0.95, 3, 1237, 'agent2')
#(grid, agent_name, prior = {})
obs_man = ObservationSetManager("agent1")
obs_man.update_rav_obs_set('agent2', [obs1, obs2, obs3])
belief_map = obs_man.get_discrete_belief_map_from_observations(test_grid)
epsilon_greedy = EpsilonGreedyActionSelection(0.0, 1.8)
#ToDo:
#Currently observations must be made at grid locations - instead compute which observations are made
#in each grid location and then compute the belief map
return_belief_map = create_belief_map(grid, self.agent_name)
return_belief_map.update_from_observations(self.get_all_observations())
return return_belief_map
def get_continuous_belief_map_from_observations(self, grid_bounds):
'''Given grid bounds, returns a function which returns the likelihood given the
continuous position of the RAV. I.E. transform the discrete PDF as above to a
continuous one.'''
pass
if __name__ == "__main__":
test_grid = UE4Grid(1, 1, UE4Coord(0,0), 6, 5)
test_ObservationSetManager = ObservationSetManager('agent1')
test_ObservationSetManager.observation_sets
obs1 = AgentObservation(UE4Coord(0,0),0.5, 1, 1234, 'agent2')
obs2 = AgentObservation(UE4Coord(0,0),0.7, 2, 1235, 'agent2')
obs3 = AgentObservation(UE4Coord(0,1),0.95, 3, 1237, 'agent2')
obs4 = AgentObservation(UE4Coord(0,1),0.9, 3, 1238, 'agent1')
test_ObservationSetManager.init_rav_observation_set('agent2', set([obs1, obs2]))
test_ObservationSetManager.observation_sets
test_ObservationSetManager.update_rav_obs_set('agent2', set([obs3]))
test_ObservationSetManager.get_all_observations()
return_CI_map.update_from_observations(
self.get_observation_set(agent_name))
def get_continuous_belief_map_from_observations(self, grid_bounds):
'''Given grid bounds, returns a function which returns the likelihood given the
continuous position of the RAV. I.E. transform the discrete PDF as above to a
continuous one.'''
pass
#%%
#%%
if __name__ == "__main__":
test_grid = UE4Grid(1, 1, Vector3r(0, 0), 6, 5)
test_ObservationSetManager = ObservationSetManager('agent1')
test_ObservationSetManager.observation_sets
obs1 = AgentObservation(Vector3r(0, 0), 0.5, 1, 1234, 'agent2')
obs2 = AgentObservation(Vector3r(0, 0), 0.7, 2, 1235, 'agent2')
obs3 = AgentObservation(Vector3r(0, 1), 0.95, 3, 1237, 'agent2')
obs4 = AgentObservation(Vector3r(0, 1), 0.9, 3, 1238, 'agent1')
test_ObservationSetManager.init_rav_observation_set(
'agent2', set([obs1, obs2]))
assert test_ObservationSetManager.get_observation_set('agent2') == set(
[obs1, obs2])
test_ObservationSetManager.observation_sets
'''Try and replicate results of Coordinated Search with a Swarm of UAVs'''
#grid = UE4Grid(10, 10, Vector3r(0,0), 90, 90)
pass
if __name__ == "__main__":
#args = parse_args(sys.argv[1:])
#agent_name = args.agent_name
#print('args: ',args)
t1 = time.time()
agent1_name = 'agent1'
agent2_name = 'agent2'
agent3_name = 'agent3'
#x then y
grid = UE4Grid(1, 1, Vector3r(0, 0), 10, 10)
means = [1, 3]
covariance_matrix = [[7.0, 0], [0, 15]]
prior = generate_gaussian_prior(grid,
means,
covariance_matrix,
initial_belief_sum=0.5)
source_location = Vector3r(4, 3)
agent_start_pos = Vector3r(5, 8)
saccadic_selection_method = SaccadicActionSelection(grid)
alpha = 0.2
beta = 0.1
cb_single_source_sensor = SingleSourceSensor(alpha, beta, source_location)
#agent_name: str, grid: UE4Grid, belief_map_components: typing.List[BeliefMapComponent], prior: typing.Dict[Vector3r, float], alpha: 'prob of false pos', beta: 'prob of false neg', apply_blur = False):
def can_coord_with_other(self, other_rav_name, range_m):
'''
Defines the probability with which the current agent can successfully
coordinate with other agent as a function of distance
'''
#check if any other ravs in comm radius. if so, return which ravs can be communicated with
#assume communcications randomly drop with probability in proportion to range_m
#for now 10% communication. This should probably go in a config file
return random.random() < 0.1
#%%
if __name__ == "__main__":
from Utils.ClientMock import KinematicsState, MockRavForTesting, ImageType, Vector3r
grid = UE4Grid(15, 20, Vector3r(0, 0), 60, 45)
#grid, move_from_bel_map_callable, height, epsilon, multirotor_client, agent_name, performance_csv_path: "file path that agent can write performance to", prior = []
#grid, initial_pos, move_from_bel_map_callable, height, epsilon, multirotor_client, agent_name, prior = {}
occupancy_grid_agent = BaseGridAgent(
grid, Vector3r(0, 0), get_move_from_belief_map_epsilon_greedy, -12,
0.2, MockRavForTesting(), 'agent1')
#write some tests for agent here
occupancy_grid_agent.current_pos_intended = Vector3r(0, 0)
occupancy_grid_agent.current_pos_measured = None
occupancy_grid_agent.current_reading = 0.1
occupancy_grid_agent.get_agent_state_for_analysis()
occupancy_grid_agent.explore_timestep()
#belief_map: BeliefMap, current_grid_loc: Vector3r, epsilon: float, eff_radius = None) -> Vector3r:
dont_move = lambda belief_map, current_grid_loc, epsilon: Vector3r(15, 20)
print(grid.get_grid_points())
#plt.xlim(0,2200)
plt.figure()
plt.plot([i**2 for i in range(100, 2200, 100)], sizes)
plt.title("Average size in bytes taken to update vs. grid size")
#plt.xlim(0, 2200)
#%%
if __name__ == '__main__':
#%%
from Utils.UE4Grid import UE4Grid
from Utils.Vector3r import Vector3r
fpr = 0.1
fnr = 0.2
test_grid = UE4Grid(1, 1, Vector3r(0.0), 20, 20)
initial_state = [0.008 for i in range(len(test_grid.get_grid_points()))]
initial_state.append(1 - sum(initial_state))
initial_state = initial_state * np.identity(len(initial_state))
fpr_matrix = fpr * np.identity(len(initial_state))
fnr_matrix = fnr * np.identity(len(initial_state))
identity = np.identity(len(initial_state))
b0 = fnr_matrix
a0 = identity - fpr_matrix
b1 = identity - fnr_matrix
a1 = fpr_matrix
#%%
update_fns = gen_next_estimated_state_functions(initial_state.shape[0], a1,
b1, a0, b0)
#%%
#%%
def calc_likelihood(observations: typing.List[float]):
return functools.reduce(lambda x, y: x * y, observations)
assert calc_likelihood([0.1, 0.1, 0.2, 0.4]) == 0.1 * 0.1 * 0.2 * 0.4
#grid, agent_name, prior = {}
#update_bel_map(update_bel_map(test_map, 0.5, 3), 0.5,3)
if __name__ != '__main__':
from ClientMock import Vector3r
grid = UE4Grid(20, 15, UE4Coord(0, 0), 120, 150)
#grid, move_from_bel_map_callable, height, epsilon, multirotor_client, agent_name, performance_csv_path: "file path that agent can write performance to", prior = []
occupancy_grid_agent = OccupancyGridAgent(
grid, get_move_from_belief_map_epsilon_greedy, -12, 0.2,
MockRavForTesting(), 'agent1')
#write some tests for agent here
occupancy_grid_agent.current_pos_intended = UE4Coord(0, 0)
occupancy_grid_agent.current_pos_measured = None
occupancy_grid_agent.current_reading = 0.1
occupancy_grid_agent.get_agent_state_for_analysis()
occupancy_grid_agent.explore_timestep()
##################### Functions that can deal with the initialization of RAVs ####################
#%%
return_bel_map = create_belief_map(grid, agent_name, agent_belief_map_prior)
#update belief map based on all observations...
return_bel_map.update_from_observations(agent_observations)
return return_bel_map
if __name__ == "__main__":
#tests for calc_posterior
assert abs(calc_posterior(0.5, 0.2) - 0.2) <= 0.001
assert abs(calc_posterior(0.8, 0.2) - 0.5) <= 0.001
#tests for get_posterior_given_obs
assert abs(get_posterior_given_obs([0.5,0.2,0.8], 0.5) - 0.5) <= 0.001
#tests for belief map
test_grid = UE4Grid(1, 1, UE4Coord(0,0), 10, 6)
test_map = create_belief_map(test_grid, "agent1")
assert test_map.get_belief_map_component(UE4Coord(0,0)) == BeliefMapComponent(UE4Coord(0,0), 1/len(test_grid.get_grid_points()))
assert test_map._get_observation_grid_index(UE4Coord(0,0)) == 5
test_map.update_from_prob(UE4Coord(0,0), 0.9)
assert 0.132<test_map.get_belief_map_component(UE4Coord(0,0)).likelihood < 0.133
#prove order in which observations come in doesn't matter
obs1 = AgentObservation(UE4Coord(0,0),0.4, 1, 1234, 'agent1')
obs2 = AgentObservation(UE4Coord(0,0),0.7, 2, 1235, 'agent1')
obs3 = AgentObservation(UE4Coord(0,0),0.93, 3, 1237, 'agent1')
test_map = create_belief_map(test_grid, "agent1")
test_map.update_from_observation(obs1)
assert 0.0111 < test_map.get_belief_map_component(UE4Coord(0,0)).likelihood < 0.0112
test_map.update_from_observation(obs2)
def run_coordinate_search_with_swarm_UAVs():
'''Try and replicate results of Coordinated Search with a Swarm of UAVs'''
#grid = UE4Grid(10, 10, Vector3r(0,0), 90, 90)
pass
if __name__ == "__main__":
#args = parse_args(sys.argv[1:])
#agent_name = args.agent_name
#print('args: ',args)
t1 = time.time()
agent1_name = 'agent1'
#x then y
grid = UE4Grid(1, 1, Vector3r(0,0), 3, 3)
#means = [1,3]
#covariance_matrix = [[7.0, 0], [0, 15]]
#prior = generate_gaussian_prior(grid, means, covariance_matrix, initial_belief_sum = 0.5)
prior = generate_uniform_prior(grid)
source_location = Vector3r(2,2)
agent_start_pos = Vector3r(5,8)
saccadic_selection_method = SaccadicActionSelection(grid)
nearest_neighbor_selection = GreedyActionSelection(eff_radius = min([grid.lat_spacing, grid.lng_spacing]))
#alpha is the "false alarm" rate (of false positive)
alpha = 0.2
#beta is the "missed detection" rate (or false negative)
beta = 0.25
self.update_observations_file(self.observations_file_loc,
newest_observation)
logger(str(newest_observation), "info", "observations")
#if agent is in range, communicate
def explore_t_timesteps(self, t: int):
for i in range(t):
self.explore_timestep()
self.timestep += 1
#print("current belief map: {}".format(self.current_belief_map))
return self.current_belief_map
if __name__ == "__main__":
from Utils.ClientMock import KinematicsState, MockRavForTesting, ImageType, Vector3r
grid = UE4Grid(15, 20, UE4Coord(0, 0), 60, 45)
#grid, move_from_bel_map_callable, height, epsilon, multirotor_client, agent_name, performance_csv_path: "file path that agent can write performance to", prior = []
#grid, initial_pos, move_from_bel_map_callable, height, epsilon, multirotor_client, agent_name, prior = {}
occupancy_grid_agent = OccupancyGridAgent(
grid, UE4Coord(0, 0), get_move_from_belief_map_epsilon_greedy, -12,
0.2, MockRavForTesting(), 'agent1')
#write some tests for agent here
occupancy_grid_agent.current_pos_intended = UE4Coord(0, 0)
occupancy_grid_agent.current_pos_measured = None
occupancy_grid_agent.current_reading = 0.1
occupancy_grid_agent.get_agent_state_for_analysis()
occupancy_grid_agent.explore_timestep()
#belief_map: BeliefMap, current_grid_loc: UE4Coord, epsilon: float, eff_radius = None) -> UE4Coord:
dont_move = lambda belief_map, current_grid_loc, epsilon: UE4Coord(15, 20)
print(grid.get_grid_points())
'''Try and replicate results of Coordinated Search with a Swarm of UAVs'''
pass
if __name__ == "__main__":
#args = parse_args(sys.argv[1:])
#agent_name = args.agent_name
#print('args: ',args)
t1 = time.time()
agent1_name = 'agent1'
agent2_name = 'agent2'
agent3_name = 'agent3'
#hard code grid for now
#x then y
grid = UE4Grid(10, 15, Vector3r(0, 0), 180, 150)
sources_locations = [Vector3r(140,
150)] #[Vector3r(25,120),Vector3r(140,15)]
rad_model = RadModel(sources_locations, grid, 50000)
#within 8m gives 100% reading
rad_sensor = RadSensor(rad_model, 10)
#rad_model.plot_falloff("D:\\ReinforcementLearning\\DetectSourceAgent\\Visualisations\\RadFalloff.png")
#rad_sensor.plot_falloff("D:\\ReinforcementLearning\\DetectSourceAgent\\Visualisations\\RadSensorFalloff.png")
# agent1 = SimpleGridAgentWithSources(grid, Vector3r(20,0), get_move_from_belief_map_epsilon_greedy, -10, 0.1, agent1_name, [Vector3r(80, 120)], other_active_agents = ['agent2'], comms_radius = 2, prior = {grid_loc:0.3 for grid_loc in grid.get_grid_points()})
# agent2 = SimpleGridAgentWithSources(grid, Vector3r(40,135), get_move_from_belief_map_epsilon_greedy, -10, 0.1, agent2_name, [Vector3r(80,120)], other_active_agents = ['agent1'], comms_radius = 2, prior = {grid_loc:0.3 for grid_loc in grid.get_grid_points()})
epsilon = 0.05
agent_start_pos = Vector3r(30, 60)
with open(file_path) as f:
reader = csv.reader(f)
header = next(reader)
return [
AgentObservation(Vector3r(row[1], row[0], row[2]), *row[3:])
for row in reader
]
except Exception as e:
return []
#%%
if __name__ == "__main__":
#%%
test_grid = UE4Grid(1, 1, Vector3r(0, 0), 10, 6)
test_agent_observations = AgentObservations(test_grid)
obs1 = AgentObservation(Vector3r(0, 0), 0.5, 1, 1234, 'agent1')
obs2 = AgentObservation(Vector3r(0, 0), 0.7, 2, 1235, 'agent1')
obs3 = AgentObservation(Vector3r(0, 1), 0.9, 3, 1237, 'agent1')
obs4 = AgentObservation(Vector3r(0, 0), 0.7, 2, 1235, 'agent1')
obs5 = AgentObservation(Vector3r(1.0, 2, 0), 0.4, 1, 1234, 'agent1')
#%%
assert obs5.grid_loc == Vector3r(1, 2, 0)
#check eq method of agent observation
assert not obs1.__eq__(obs2)
assert obs2.__eq__(obs4)