Running searches without humans. Need to fix path optimizer.

gp/robotgp.py

@@ -35,6 +35,7 @@ class RobotGP(torch.nn.Module):
         self.update_mc_hotspots = True
         self.update_searcher_paths = True
 
+        #!! turn grad clamp back on...?
         self.grad_clamp_value = 1e50
         #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))
@@ -56,6 +57,8 @@ class RobotGP(torch.nn.Module):
         self.scaled_risk_cost = 1
         self.scaled_path_length_cost = 1
 
+        self.num_searchers_paths = 0
+
         self._iter = 0
         self._updates = 0
         self._iter_since_update = 0
@@ -115,6 +118,8 @@ class RobotGP(torch.nn.Module):
     def collect_trainData(self):
         # sweep_paths = False in current config
         self.collect_mc_trainData()
+        #Get human data
+        if(self.params['num_searchers'] != 0):
             self.collect_searchers_trainData()
 
         if self.params['path_style'] == 'sweep': # lawnmower drones
@@ -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 )
 
         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)
+
+            #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.mc_hotspot_values, self.searchers_path_values, self.robot_fixed_path_values), axis=0)
             self.num_fixed_train_points = self.fixed_points.shape[0]
@@ -180,6 +194,9 @@ class RobotGP(torch.nn.Module):
             self.Xstar_batch.shape[0]).cuda()
 
     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.ytrain = torch.cat((self.fixed_values, self.robot_values), dim=0).cuda()
 
@@ -623,6 +640,8 @@ class RobotGP(torch.nn.Module):
 
                 self._curr_risk_ratio = self.min_risk_cost / self._init_risk_cost
 
+                self.min_X = self.X
+
             self._iter_since_update += 1
 
             # if self.risk_cost_data < self.min_risk_cost:

larrt/searchspace.py

@@ -38,10 +38,12 @@ class SearchSpace(terrain.Terrain):
     def init_robots(self):
         robot.Robot.max_nodes = self.params['max_plan_nodes']
         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)
             ypos = self.ymin
             zpos = self.h_smooth.ev(xpos, ypos) + 1
 
+            #Sets end goal to opposite edge of terrain
             xgoal = xpos
             ygoal = self.ymax
             zgoal = self.h_smooth.ev(xgoal, ygoal) + 1

mrmh_model/montecarlo.py

@@ -237,6 +237,8 @@ class MonteCarlo(params.Default):
             self.p = np.zeros((self._x_shape, self._y_shape), dtype=float)
 
         #Show LP Heatmap
+        show = False
+        if(show):
             plt.imshow(self.p)
             plt.title('Selected heatmap')
             plt.show()

mrmh_model/searcher.py

@@ -107,7 +107,7 @@ class Searcher(params.Default):
         self.index = Searcher.num_searchers
         Searcher.num_searchers += 1
         Searcher.searchers_list.append(self)
-        print("!!Made a searcher")
+        
 
     def get_pos(self):
         return (self.x[0], self.x[1])

test_trustplanner.py

@@ -184,17 +184,17 @@ if __name__ == "__main__":
 
     if True:
         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
         #global_fail_max = 1000
         global_fail_max = 5
         global_fails = 0
-        avg_runs = 1
+        avg_runs = 5
         start_time = time.time()
 
         params = ({
-            'lp_model': 'trust',
-            'opt_iterations': 2,
+            'lp_model': 'trust',    #custom, ring, or trust
+            'opt_iterations': 1000,
             'path_style': 'basic',
             'stats_name': 'kentland',
             'anchor_point': [37.197730, -80.585233], # kentland
@@ -202,7 +202,7 @@ if __name__ == "__main__":
             'num_robots': n_max,
             'num_humans' : h_max,
             'lp_filename': kentland_heatmap,
-            'lin_feat_filename': kentland_linfeat
+            'lin_feat_filename': kentland_linfeat,
         })
         params['save_folder'] = 'trust_{}_n{}_s{}_{}'.format(
             params['stats_name'],n_max,s_max,params['path_style'])