Commit 997026ba authored by Bryson Howell's avatar Bryson Howell

started modifying game to work with only robot searchers

parent 9463e157
...@@ -84,12 +84,13 @@ class RobotGP(torch.nn.Module): ...@@ -84,12 +84,13 @@ class RobotGP(torch.nn.Module):
self._stime = _stime if _stime is not None else time.time() self._stime = _stime if _stime is not None else time.time()
#!!Memory problem is here @_@. This registers backwards hooks #Registers backwards hooks
#What is this hook doing though. Why do we need it #Changed hook function to use PyTorch clamp
def _hook(self, tensor, _min=-1e10, _max=1e10): def _hook(self, tensor, _min=-1e10, _max=1e10):
curframe = inspect.currentframe() #Debug - show who is registering a hook
calframe = inspect.getouterframes(curframe,2) #curframe = inspect.currentframe()
print(calframe[1][3]) #calframe = inspect.getouterframes(curframe,2)
#print(calframe[1][3])
tensor.requires_grad_(True) tensor.requires_grad_(True)
#tensor.retain_grad() #tensor.retain_grad()
tensor.register_hook(lambda grad: grad.clamp(min=_min, max=_max)) tensor.register_hook(lambda grad: grad.clamp(min=_min, max=_max))
...@@ -366,8 +367,6 @@ class RobotGP(torch.nn.Module): ...@@ -366,8 +367,6 @@ class RobotGP(torch.nn.Module):
#self.risk_cost = self.scaled_risk_cost + self.scaled_path_length_cost #self.risk_cost = self.scaled_risk_cost + self.scaled_path_length_cost
self.risk_cost = torch.add(self.scaled_risk_cost, self.scaled_path_length_cost) self.risk_cost = torch.add(self.scaled_risk_cost, self.scaled_path_length_cost)
self.risk_cost = self._hook(self.risk_cost) self.risk_cost = self._hook(self.risk_cost)
print("!!Ran hook on Risk cost, why are we doing it again...\n\n")
print(self.risk_cost)
def compute_risk_cost_batchless(self, yst_mu, yst_cov): def compute_risk_cost_batchless(self, yst_mu, yst_cov):
# kind of assuming that risk cost would be different if evaluated without batches included? # kind of assuming that risk cost would be different if evaluated without batches included?
...@@ -500,7 +499,7 @@ class RobotGP(torch.nn.Module): ...@@ -500,7 +499,7 @@ class RobotGP(torch.nn.Module):
print("oh no") print("oh no")
while self._iter_since_update < self._max_iter: while self._iter_since_update < self._max_iter:
print('iter %d' % self._iter) #Run into problems at iter 0 #print('iter %d' % self._iter) #Run into problems at iter 0
stime = time.time() stime = time.time()
self.optimizer.zero_grad() self.optimizer.zero_grad()
...@@ -511,7 +510,7 @@ class RobotGP(torch.nn.Module): ...@@ -511,7 +510,7 @@ class RobotGP(torch.nn.Module):
self.compute_risk_cost() self.compute_risk_cost()
#self.risk_cost.backward(retain_graph=True) #self.risk_cost.backward(retain_graph=True)
print('\n\n\n\nRunning backward on risk cost...') #print('\n\n\n\nRunning backward on risk cost...')
self.risk_cost.backward(retain_graph=False) # testing if this is required self.risk_cost.backward(retain_graph=False) # testing if this is required
# no_nans_p_grad = _find_nans(self.robot_points.grad).shape[0] == 0 # no_nans_p_grad = _find_nans(self.robot_points.grad).shape[0] == 0
......
...@@ -35,7 +35,7 @@ class MonteCarlo(params.Default): ...@@ -35,7 +35,7 @@ class MonteCarlo(params.Default):
super().__init__(params) super().__init__(params)
# self.params = params # self.params = params
self.set_defaults() #self.set_defaults() #think this is called twice
self.count = 0 self.count = 0
self.store_logs = params['store_logs'] self.store_logs = params['store_logs']
......
...@@ -107,6 +107,7 @@ class Searcher(params.Default): ...@@ -107,6 +107,7 @@ class Searcher(params.Default):
self.index = Searcher.num_searchers self.index = Searcher.num_searchers
Searcher.num_searchers += 1 Searcher.num_searchers += 1
Searcher.searchers_list.append(self) Searcher.searchers_list.append(self)
print("!!Made a searcher")
def get_pos(self): def get_pos(self):
return (self.x[0], self.x[1]) return (self.x[0], self.x[1])
...@@ -337,7 +338,7 @@ class Searcher(params.Default): ...@@ -337,7 +338,7 @@ class Searcher(params.Default):
def go_forth( params={} ): def go_forth( params={} ):
if(params): if(params):
Searcher.params = params Searcher.params = params
num_searchers = params.get('num_searchers', 1) num_searchers = params.get('num_searchers', 1)
if Searcher.terrain is None: if Searcher.terrain is None:
...@@ -426,4 +427,4 @@ def vertical_dist( a, b ): ...@@ -426,4 +427,4 @@ def vertical_dist( a, b ):
return (a[1] - b[1] )**2 return (a[1] - b[1] )**2
def vertical_abs_dist( a, b ): def vertical_abs_dist( a, b ):
return abs(a[1] - b[1] ) return abs(a[1] - b[1] )
\ No newline at end of file
from larrt import planning
import pickle as pkl
import json
from mrmh_model import terrain, human, montecarlo, searcher
import plotting_utils as plotter
import waypoint_maker
import matplotlib.pyplot as plt
from scouter.terrain_viewer import plot_all
from matplotlib.backends.backend_pdf import PdfPages
from mrmh_model import montecarlo as MC
import numpy as np
from mrmh_model import searcher
import torch
from gp import beziergp
from gp import robotgp
import sys
from mrmh_model.params import Default
import datetime as dt
import time, os
import pdb
def multipage(filename, figs=None, dpi=200):
pp = PdfPages(filename)
if figs is None:
figs = [plt.figure(n) for n in plt.get_fignums()]
for fig in figs:
try:
fig.savefig(pp, format='pdf')
except:
print('At least one of the figures could not be saved to PDF')
pp.close()
def objective_printer(rgp_object = None, comp_time = 0.0, iteration = 0, folder = 'blah',
time_to_find = 0, find_percentage = 0, save_data = False):
# prints and/or saves the optimization objective values
rgp = rgp_object
min_Xstar = rgp.min_Xstar[:,2].reshape(np.sqrt(rgp.min_Xstar.shape[0]).astype(np.int),-1).cpu()
# find_x = min_Xstar[rgp.mc_handle.find_pt[0], rgp.mc_handle.find_pt[1]]
ystar_mu_min, ystar_cov_min = rgp.eval_batchless(rgp.min_X,rgp.min_Xstar)
ystar_mu_first, ystar_cov_first = rgp.eval_batchless(rgp.first_X,rgp.first_Xstar)
ystar_mu_prev, ystar_cov_prev = rgp.eval_batchless(rgp.prev_X,rgp.prev_Xstar)
ystar_mu_curr, ystar_cov_curr = rgp.eval_batchless(rgp.X,rgp.Xstar)
# undo log odds into just prb
find_mat_min = 1 - (1/(1 + np.exp(ystar_mu_min)))
find_mat_first = 1 - (1/(1 + np.exp(ystar_mu_first)))
find_mat_prev = 1 - (1/(1 + np.exp(ystar_mu_prev)))
find_mat_curr = 1 - (1/(1 + np.exp(ystar_mu_curr)))
risk_cost_min = rgp.compute_risk_cost_batchless(ystar_mu_min, ystar_cov_min).detach().cpu().numpy()
risk_cost_first = rgp.compute_risk_cost_batchless(ystar_mu_first, ystar_cov_first).detach().cpu().numpy()
risk_cost_prev = rgp.compute_risk_cost_batchless(ystar_mu_prev, ystar_cov_prev).detach().cpu().numpy()
risk_cost_curr = rgp.compute_risk_cost_batchless(ystar_mu_curr, ystar_cov_curr).detach().cpu().numpy()
# this only makes sense if paths were optimized
find_ratio_curr = (find_mat_curr/find_mat_prev)/((1 - find_mat_curr)/(1 - find_mat_prev)) # ratio of successfully finding lp over false positives
find_ratio_total = (find_mat_min/find_mat_first)/((1 - find_mat_min)/(1 - find_mat_first)) # same thing but compare first and best
# take sample from real lp map
lp_dist = rgp.mc_handle.comp_map/np.sum(rgp.mc_handle.comp_map)
# compare chance person is in location and chance we *think* person is in location
search_prb_mat = lp_dist*find_mat_min
# rgp.min_risk_cost = rgp.risk_cost_data.detach().cpu.numpy().item()
# rgp.min_risk_paths = rgp.robot_points_data
# rgp._min_scaled_risk_cost = rgp.scaled_risk_cost_data
# rgp._min_scaled_path_length_cost = rgp.scaled_path_length_cost_data
# wherever find_ratio is > 1 we have a higher chance of actually finding the person vs getting a false positive (or something like that)
save_dict = {'min_cost_bcls' : risk_cost_min.item(),
'curr_cost_bcls' : risk_cost_curr.item(),
'curr_delta_bcls' : np.abs(risk_cost_prev - risk_cost_curr),
'init_cost_bcls' : risk_cost_first.item(),
'delta_cost_bcls' : np.abs(risk_cost_first - risk_cost_min),
'min_cost_old' : rgp.min_risk_cost.item(),
'min_risk_old' : rgp._min_scaled_risk_cost.item(),
'min_len_old' : rgp._min_scaled_path_length_cost.item(),
'find_percentage_curr_old' : np.count_nonzero(find_ratio_curr>=1)/np.size(find_ratio_curr),
'mean_find_ratio_curr_old' : np.mean(find_ratio_curr),
'find_percentage_total_old' : np.count_nonzero(find_ratio_total>=1)/np.size(find_ratio_total),
'mean_find_ratio_total_old' : np.mean(find_ratio_total),
'search_advantage' : np.sum(search_prb_mat),
'time_to_find' : time_to_find,
'find_percentage' : find_percentage,
'comp_time' : comp_time
}
if save_data:
filename = dt.datetime.now().strftime("%d_%m_%Y-%I_%M_%S_%p_")
filename += "d-" + rgp.params['path_style'] + \
"_l-" + rgp.params['stats_name'] + \
"_i-" + str(iteration) + ".json"
# force create directory
os.makedirs('results/' + folder, exist_ok=True)
with open('results/' + folder + '/' + filename, 'w') as f:
json.dump(save_dict, f)
return save_dict
def main(iteration = 0, parameters = -1):
# iteration = 1
stime = time.time()
params = parameters
# params = ({
# 'exp_id': 0,
# 'exp_name': 'test',
# 'exp_description': 'Generate a heatmap',
# })
# params = Default(params).params
mc = MC.MonteCarlo(params=params) # calls terrain builder
mc.run_experiment()
planner = planning.Planning(params, on_terrain=mc.terrain, mode='TOTALDIST') # also calls terrain builder...
rgp = robotgp.RobotGP(mc, planner, _stime = stime, parameters = params)
rgp.collect_trainData() # sets out paths for each robot
rgp.optimize_risk_cost_grad(_show_detail=True)
robot_paths_local = waypoint_maker.write_file(rgp_object = rgp, terrain_class = mc.terrain, filename = 'waypoints.json') # write waypoints to file
# write serialized files for plotter to run
# with open("scouter/bozoo_save_{}.pkl".format(params['anchor_point']),'wb') as f:
# pkl.dump({'params' : params,
# 'mc_object' : mc,
# 'robot_paths' : robot_paths_local,
# 'searcher_paths' : mc.searcher_class.searchers_list}, f)
etime = time.time()
comp_time = etime - stime
ttf, srpc = rgp.time_to_find(robot_paths = robot_paths_local, searcher_paths = mc.searcher_class.searchers_list)
print("Total time required: {}".format(comp_time))
objective_printer(rgp_object = rgp, comp_time = comp_time,
iteration = iteration,
folder = rgp.params['save_folder'],
save_data = rgp.params['save_data'],
time_to_find = ttf,
find_percentage = srpc)
if rgp.params['plot_data']:
plot_all(parameters = params,
mc_object = mc,
robot_paths = robot_paths_local,
searcher_paths = mc.searcher_class.searchers_list,
smooth_paths = True,
show_heatmap = True,
show_contours = True,
cs_name = 'thermal'
)
rgp.garbage_cleanup()
del rgp, planner, mc
if __name__ == "__main__":
#Relative filepaths (Assumes this and ags_grabber projects are in same parent directory)
kentland_heatmap = './LP model/analysis/outputs/ic_1_con_hiker_t12_kentland.csv'
hmpark_heatmap = './LP model/analysis/outputs/ic_1_con_hiker_t12_hmpark.csv'
kentland_linfeat = '../ags_grabber/matlab_data/BW_LFandInac_Zelev_kentland.mat'
hmpark_linfeat = '../ags_grabber/matlab_data/BW_LFandInac_Zelev_hmpark.mat'
# KENTLAND case
if True:
n_max = 4 #Number of robots
s_max = 0 #Number of human searchers
#global_fail_max = 1000
global_fail_max = 5
global_fails = 0
avg_runs = 5
start_time = time.time()
params = ({
'lp_model': 'custom',
'opt_iterations': 2,
'path_style': 'basic',
'stats_name': 'kentland',
'anchor_point': [37.197730, -80.585233], # kentland
'num_searchers': s_max,
'num_robots': n_max,
'lp_filename': kentland_heatmap,
'lin_feat_filename': kentland_linfeat
})
params['save_folder'] = 'trust_{}_n{}_s{}_{}'.format(
params['stats_name'],n_max,s_max,params['path_style'])
params = Default(params).params
counter = 0
while counter < avg_runs and global_fails <= global_fail_max: # number of averaging runs
print('Running on ' + torch.cuda.get_device_name())
torch.cuda.empty_cache()
torch.cuda.ipc_collect()
try:
#try:
# main(iteration = counter, parameters=params)
# print("done run")
#except RuntimeError as e:
# print(e)
# print("\n\n ------ bad memory, re trying ------\n")
# global_fails += 1
# continue
main(iteration = counter, parameters=params)
print("done run")
counter += 1
except AttributeError as e:
print(e)
print("\n\n ------- bad optimization, re trying ---------- \n")
global_fails += 1
#-----------------------------------------------------------------------------------------------
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment