working on getting distinct trajectories

.gitignore

@@ -10,4 +10,4 @@ datachop_hiker.mat
 BW_LFandInac_Zelev.mat
 e_x_y_data.pkl
 .pytest_cache
-z__test__
\ No newline at end of file
+z__test__*.pyc

gp/robotgp.py

@@ -103,6 +103,7 @@ class RobotGP(torch.nn.Module):
         y = torch.as_tensor(self.mc_handle.terrain.y, dtype=torch.float64).view(-1, 1)
         z = 1 + torch.as_tensor(self.mc_handle.terrain.h, dtype=torch.float64).reshape(-1,1)
         self.Xstar = torch.cat([x, y, z], 1).cuda()  # [nTest, 3]
+        self.min_Xstar = self.Xstar
         self.num_test = self.Xstar.shape[0]
         # print('Initializing Xstar took {} secs.'.format(time.time() - stime))
 

mrmh_model/montecarlo.py

@@ -228,10 +228,19 @@ class MonteCarlo(params.Default):
             self.p[dist_mat <= self.params['ring_mobi'][3]] += 0.20 # containment ring
             # print(self.p)
             self.p = ndimage.gaussian_filter(self.p, sigma=8) # to make betta heatmap
-
+        elif self.params['lp_model'] == 'trust':    #test for trust
+            self.p = np.zeros((self._x_shape, self._y_shape), dtype=float)
+            for r in range(0, self._x_shape):
+                for c in range(0, self._y_shape):
+                    self.p[r,c] = np.float(r+c) / np.float(self._x_shape+self._y_shape)
         else:
             self.p = np.zeros((self._x_shape, self._y_shape), dtype=float)
 
+        #Show LP Heatmap
+        plt.imshow(self.p)
+        plt.title('Selected heatmap')
+        plt.show()
+
         # generate lp from real model for comparison
         fn = self.params['lp_filename']
         self.comp_map, trash = load_heatmap(filename=fn, map_size=[self._x_shape, self._y_shape],

mrmh_model/searcher.py

@@ -336,6 +336,7 @@ class Searcher(params.Default):
 
     @staticmethod
     def go_forth( params={} ):
+        print("Searchers going forth")
         if(params):
             Searcher.params = params
 
@@ -354,6 +355,9 @@ class Searcher(params.Default):
 
         dt = params.get('dt', 1)
 
+        if(num_searchers == 0):
+            Searcher.x_sweep_width = ( (_xrange) / (2) ) * 1.2
+        else:
             Searcher.x_sweep_width = ( (_xrange) / (2*num_searchers) ) * 1.2 # whats the deal with these constants?
         Searcher.y_sweep_lambda = float(dt) *  (_yrange) * 6.7e-2
 
@@ -365,6 +369,9 @@ class Searcher(params.Default):
         - grouped search method
         '''
         sweep_num = params['sweep_num'] # number of lengths of area to perform (for 'grouped' case)
+        if(num_searchers == 0):
+            s_x = _xrange/(sweep_num*2)
+        else:
             s_x = _xrange/(sweep_num*2*num_searchers) # adjusted sweep length (2*s per searcher)
         sw = 2*num_searchers*s_x
         s_y = np.int(_yrange/20) # y axis bound need not be calculated

test_trustplanner.py

@@ -125,6 +125,10 @@ def main(iteration = 0, parameters = -1):
     mc = MC.MonteCarlo(params=params) # calls terrain builder
     mc.run_experiment()
 
+    #Show terrain for fun (want to see heatmap too)
+    #mc.terrain.plot()
+    #plt.show()
+
     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
@@ -179,22 +183,24 @@ if __name__ == "__main__":
     # KENTLAND case
 
     if True:
-        n_max = 4   #Number of robots
-        s_max = 0   #Number of human searchers
+        n_max = 3   #Number of robots
+        s_max = 1   #Number of searchers (humans)???
+        h_max = 0   #Number of humans
         #global_fail_max = 1000
         global_fail_max = 5
         global_fails = 0
-        avg_runs = 5
+        avg_runs = 1
         start_time = time.time()
 
         params = ({
-            'lp_model': 'custom',
+            'lp_model': 'trust',
             'opt_iterations': 2,
             'path_style': 'basic',
             'stats_name': 'kentland',
             'anchor_point': [37.197730, -80.585233], # kentland
             'num_searchers': s_max,
             'num_robots': n_max,
+            'num_humans' : h_max,
             'lp_filename': kentland_heatmap,
             'lin_feat_filename': kentland_linfeat
         })