Commit 91fe94ef authored by Bryson Howell's avatar Bryson Howell

Running searches without humans. Need to fix path optimizer.

parent 817009a2
...@@ -35,6 +35,7 @@ class RobotGP(torch.nn.Module): ...@@ -35,6 +35,7 @@ class RobotGP(torch.nn.Module):
self.update_mc_hotspots = True self.update_mc_hotspots = True
self.update_searcher_paths = True self.update_searcher_paths = True
#!! turn grad clamp back on...?
self.grad_clamp_value = 1e50 self.grad_clamp_value = 1e50
#self.grad_clamp = lambda grad: torch.clamp(grad, -self.grad_clamp_value, self.grad_clamp_value) #self.grad_clamp = lambda grad: torch.clamp(grad, -self.grad_clamp_value, self.grad_clamp_value)
# self.X_batch.tregister_hook(lambda grad: torch.clamp(grad, -self.grad_clamp, self.grad_clamp)) # self.X_batch.tregister_hook(lambda grad: torch.clamp(grad, -self.grad_clamp, self.grad_clamp))
...@@ -56,6 +57,8 @@ class RobotGP(torch.nn.Module): ...@@ -56,6 +57,8 @@ class RobotGP(torch.nn.Module):
self.scaled_risk_cost = 1 self.scaled_risk_cost = 1
self.scaled_path_length_cost = 1 self.scaled_path_length_cost = 1
self.num_searchers_paths = 0
self._iter = 0 self._iter = 0
self._updates = 0 self._updates = 0
self._iter_since_update = 0 self._iter_since_update = 0
...@@ -115,6 +118,8 @@ class RobotGP(torch.nn.Module): ...@@ -115,6 +118,8 @@ class RobotGP(torch.nn.Module):
def collect_trainData(self): def collect_trainData(self):
# sweep_paths = False in current config # sweep_paths = False in current config
self.collect_mc_trainData() self.collect_mc_trainData()
#Get human data
if(self.params['num_searchers'] != 0):
self.collect_searchers_trainData() self.collect_searchers_trainData()
if self.params['path_style'] == 'sweep': # lawnmower drones if self.params['path_style'] == 'sweep': # lawnmower drones
...@@ -147,7 +152,16 @@ class RobotGP(torch.nn.Module): ...@@ -147,7 +152,16 @@ class RobotGP(torch.nn.Module):
# self.values = np.concatenate( (self.mc_hotspot_values, self.searchers_path_values, self.robot_path_values), axis=0 ) # self.values = np.concatenate( (self.mc_hotspot_values, self.searchers_path_values, self.robot_path_values), axis=0 )
if self.update_mc_hotspots or self.update_searcher_paths: if self.update_mc_hotspots or self.update_searcher_paths:
#Collect search paths. Make sure searchers_paths exists when combining inputs
if(self.num_searchers_paths == 0):
self.fixed_points = np.concatenate((self.mc_hotspots, self.robot_fixed_paths), axis=0)
else:
self.fixed_points = np.concatenate((self.mc_hotspots, self.searchers_paths, self.robot_fixed_paths), axis=0) self.fixed_points = np.concatenate((self.mc_hotspots, self.searchers_paths, self.robot_fixed_paths), axis=0)
#Collect interpolated values
if(self.num_searchers_paths == 0):
self.fixed_values = np.concatenate((self.mc_hotspot_values, self.robot_fixed_path_values), axis=0)
else:
self.fixed_values = np.concatenate( self.fixed_values = np.concatenate(
(self.mc_hotspot_values, self.searchers_path_values, self.robot_fixed_path_values), axis=0) (self.mc_hotspot_values, self.searchers_path_values, self.robot_fixed_path_values), axis=0)
self.num_fixed_train_points = self.fixed_points.shape[0] self.num_fixed_train_points = self.fixed_points.shape[0]
...@@ -180,6 +194,9 @@ class RobotGP(torch.nn.Module): ...@@ -180,6 +194,9 @@ class RobotGP(torch.nn.Module):
self.Xstar_batch.shape[0]).cuda() self.Xstar_batch.shape[0]).cuda()
def update_trainXY(self): def update_trainXY(self):
#Print to check if data is correct
self.X = torch.cat((self.fixed_points, self.robot_points), dim=0).cuda() self.X = torch.cat((self.fixed_points, self.robot_points), dim=0).cuda()
self.ytrain = torch.cat((self.fixed_values, self.robot_values), dim=0).cuda() self.ytrain = torch.cat((self.fixed_values, self.robot_values), dim=0).cuda()
...@@ -623,6 +640,8 @@ class RobotGP(torch.nn.Module): ...@@ -623,6 +640,8 @@ class RobotGP(torch.nn.Module):
self._curr_risk_ratio = self.min_risk_cost / self._init_risk_cost self._curr_risk_ratio = self.min_risk_cost / self._init_risk_cost
self.min_X = self.X
self._iter_since_update += 1 self._iter_since_update += 1
# if self.risk_cost_data < self.min_risk_cost: # if self.risk_cost_data < self.min_risk_cost:
......
...@@ -38,10 +38,12 @@ class SearchSpace(terrain.Terrain): ...@@ -38,10 +38,12 @@ class SearchSpace(terrain.Terrain):
def init_robots(self): def init_robots(self):
robot.Robot.max_nodes = self.params['max_plan_nodes'] robot.Robot.max_nodes = self.params['max_plan_nodes']
for ix in range(self.num_robots): for ix in range(self.num_robots):
#Evenly space robots along edge of terrain
xpos = self.xmin + (2*ix+1)*self._xrange/(2*self.num_robots) xpos = self.xmin + (2*ix+1)*self._xrange/(2*self.num_robots)
ypos = self.ymin ypos = self.ymin
zpos = self.h_smooth.ev(xpos, ypos) + 1 zpos = self.h_smooth.ev(xpos, ypos) + 1
#Sets end goal to opposite edge of terrain
xgoal = xpos xgoal = xpos
ygoal = self.ymax ygoal = self.ymax
zgoal = self.h_smooth.ev(xgoal, ygoal) + 1 zgoal = self.h_smooth.ev(xgoal, ygoal) + 1
......
...@@ -237,6 +237,8 @@ class MonteCarlo(params.Default): ...@@ -237,6 +237,8 @@ class MonteCarlo(params.Default):
self.p = np.zeros((self._x_shape, self._y_shape), dtype=float) self.p = np.zeros((self._x_shape, self._y_shape), dtype=float)
#Show LP Heatmap #Show LP Heatmap
show = False
if(show):
plt.imshow(self.p) plt.imshow(self.p)
plt.title('Selected heatmap') plt.title('Selected heatmap')
plt.show() plt.show()
......
...@@ -107,7 +107,7 @@ class Searcher(params.Default): ...@@ -107,7 +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])
......
This diff is collapsed.
...@@ -184,17 +184,17 @@ if __name__ == "__main__": ...@@ -184,17 +184,17 @@ if __name__ == "__main__":
if True: if True:
n_max = 3 #Number of robots n_max = 3 #Number of robots
s_max = 1 #Number of searchers (humans)??? s_max = 0 #Number of searchers (humans)???
h_max = 0 #Number of humans h_max = 0 #Number of humans
#global_fail_max = 1000 #global_fail_max = 1000
global_fail_max = 5 global_fail_max = 5
global_fails = 0 global_fails = 0
avg_runs = 1 avg_runs = 5
start_time = time.time() start_time = time.time()
params = ({ params = ({
'lp_model': 'trust', 'lp_model': 'trust', #custom, ring, or trust
'opt_iterations': 2, 'opt_iterations': 1000,
'path_style': 'basic', 'path_style': 'basic',
'stats_name': 'kentland', 'stats_name': 'kentland',
'anchor_point': [37.197730, -80.585233], # kentland 'anchor_point': [37.197730, -80.585233], # kentland
...@@ -202,7 +202,7 @@ if __name__ == "__main__": ...@@ -202,7 +202,7 @@ if __name__ == "__main__":
'num_robots': n_max, 'num_robots': n_max,
'num_humans' : h_max, 'num_humans' : h_max,
'lp_filename': kentland_heatmap, 'lp_filename': kentland_heatmap,
'lin_feat_filename': kentland_linfeat 'lin_feat_filename': kentland_linfeat,
}) })
params['save_folder'] = 'trust_{}_n{}_s{}_{}'.format( params['save_folder'] = 'trust_{}_n{}_s{}_{}'.format(
params['stats_name'],n_max,s_max,params['path_style']) params['stats_name'],n_max,s_max,params['path_style'])
......
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