Got better search path data by reducing the path length cost. More progress on Gym.

gp/robotgp.py

@@ -81,6 +81,7 @@ class RobotGP(torch.nn.Module):
 
         self._init_riskcost_scaling = 1e7  # 1e7
         self._init_lengthcost_scaling = 1e2  # 1e7
+        self._init_lengthcost_scaling = 0.5e1
 
         self.riskcost_scaling = self._init_riskcost_scaling
         self.lengthcost_scaling = self._init_lengthcost_scaling

mrmh_model/montecarlo.py

@@ -227,7 +227,7 @@ class MonteCarlo(params.Default):
             self.p[dist_mat <= self.params['ring_mobi'][2]] += 0.25 # outter ring
             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
+            self.p = ndimage.gaussian_filter(self.p, sigma=32) # to make betta heatmap (sigma needs to scale w/ env size)
         #Makes heatmap favoring a corner
         elif self.params['lp_model'] == 'trust':    #test for trust
             self.p = np.zeros((self._x_shape, self._y_shape), dtype=float)
@@ -253,16 +253,6 @@ class MonteCarlo(params.Default):
         else:
             self.p = np.zeros((self._x_shape, self._y_shape), dtype=float)
 
-
-        #Show LP Heatmap for testing
-        show = False
-        if(show):
-            print('Environment shape = %d x %d' % (self._x_shape,self._y_shape))
-            print(np.shape(self.p))
-            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],
@@ -275,6 +265,16 @@ class MonteCarlo(params.Default):
         self.p = self.p - np.min(self.p)
         self.p = self.p/np.max(self.p)
 
+        #Show LP Heatmap for testing
+        show = True
+        if(show):
+            print('Environment shape = %d x %d' % (self._x_shape,self._y_shape))
+            print(np.shape(self.p))
+            plt.imshow(self.p)
+            plt.title('Lost Person Model Search Probability')
+            plt.show()
+            #np.save('./deeprl_data/test_prob.npy',self.p)
+
         self.searcher_class.go_forth(params=self.params) # sets sweep bounds and starting points
         while True:
             self.searcher_class.step_all(dt=self.params['dt']) # and this makes each step?

results/trust-planner/kentland_n3_s0_basic/01_05_2024-01_38_47_PM_d-basic_l-kentland_i-0.json

+{"min_cost_bcls": 229324787010.07285, "curr_cost_bcls": 229331952345.00833, "curr_delta_bcls": 706270.9504394531, "init_cost_bcls": 229383725097.5324, "delta_cost_bcls": 58938087.45956421, "min_cost_old": 19723784479.009815, "min_risk_old": 19694467039.962288, "min_len_old": 29317439.047525816, "find_percentage_curr_old": 0.4735243055555556, "mean_find_ratio_curr_old": 1.0000026330236846, "find_percentage_total_old": 0.7873263888888888, "mean_find_ratio_total_old": 1.000321066544102, "search_advantage": 0.5028393388564546, "time_to_find": 1.464032943251904, "find_percentage": 0.0122, "comp_time": 58.967463970184326}
\ No newline at end of file

results/trust-planner/kentland_n3_s0_basic/01_05_2024-01_55_54_PM_d-basic_l-kentland_i-0.json

+{"min_cost_bcls": 229358833649.8525, "curr_cost_bcls": 229361724182.93137, "curr_delta_bcls": 156292.30221557617, "init_cost_bcls": 229429554281.75354, "delta_cost_bcls": 70720631.9010315, "min_cost_old": 19132862516.1268, "min_risk_old": 19093197424.21231, "min_len_old": 39665091.91448985, "find_percentage_curr_old": 0.4266493055555556, "mean_find_ratio_curr_old": 1.0000005201494666, "find_percentage_total_old": 0.7278645833333334, "mean_find_ratio_total_old": 1.00039854639231, "search_advantage": 0.5029008485866513, "time_to_find": 1.3174273370176781, "find_percentage": 0.0036, "comp_time": 66.1751618385315}
\ No newline at end of file

results/trust-planner/kentland_n3_s0_basic/01_05_2024-02_06_14_PM_d-basic_l-kentland_i-0.json

+{"min_cost_bcls": 229355020368.46344, "curr_cost_bcls": 229364796564.25848, "curr_delta_bcls": 188170.25454711914, "init_cost_bcls": 229477452286.45734, "delta_cost_bcls": 122431917.99389648, "min_cost_old": 18319135484.3424, "min_risk_old": 18291988972.036346, "min_len_old": 27146512.306052096, "find_percentage_curr_old": 0.5815972222222222, "mean_find_ratio_curr_old": 1.0000034116660632, "find_percentage_total_old": 0.81640625, "mean_find_ratio_total_old": 1.0004374318893912, "search_advantage": 0.5028688691555725, "time_to_find": 1.194480360490085, "find_percentage": 0.0088, "comp_time": 63.6818208694458}
\ No newline at end of file

results/trust-planner/kentland_n3_s0_basic/01_05_2024-12_40_53_PM_d-basic_l-kentland_i-0.json

+{"min_cost_bcls": 229308445696.26715, "curr_cost_bcls": 229333075991.13364, "curr_delta_bcls": 2998649.391571045, "init_cost_bcls": 229432252223.6156, "delta_cost_bcls": 123806527.3484497, "min_cost_old": 20135309109.431248, "min_risk_old": 20104750120.32219, "min_len_old": 30558989.10905969, "find_percentage_curr_old": 0.5928819444444444, "mean_find_ratio_curr_old": 1.0000032227729585, "find_percentage_total_old": 0.7491319444444444, "mean_find_ratio_total_old": 1.000332915662093, "search_advantage": 0.5028471262599199, "time_to_find": 1.1829233991299126, "find_percentage": 0.0134, "comp_time": 71.85460257530212}
\ No newline at end of file

results/trust-planner/kentland_n3_s0_basic/02_05_2024-06_56_45_PM_d-basic_l-kentland_i-0.json

+{"min_cost_bcls": 229702480649.12128, "curr_cost_bcls": 229690675835.78653, "curr_delta_bcls": 767685.6897277832, "init_cost_bcls": 229775338443.88544, "delta_cost_bcls": 72857794.76416016, "min_cost_old": 17520243739.85434, "min_risk_old": 17443262334.80884, "min_len_old": 76981405.04549891, "find_percentage_curr_old": 0.5542534722222222, "mean_find_ratio_curr_old": 0.9999996241499337, "find_percentage_total_old": 0.8020833333333334, "mean_find_ratio_total_old": 1.0000698613038908, "search_advantage": 0.5013633208922221, "time_to_find": 0.8105801820808669, "find_percentage": 0.0036, "comp_time": 52.671302318573}
\ No newline at end of file

results/trust-planner/kentland_n3_s0_basic/02_05_2024-06_58_18_PM_d-basic_l-kentland_i-0.json

+{"min_cost_bcls": 229456906801.79526, "curr_cost_bcls": 229436688481.48495, "curr_delta_bcls": 5814080.936431885, "init_cost_bcls": 229631504913.25647, "delta_cost_bcls": 174598111.46121216, "min_cost_old": 17761789529.348713, "min_risk_old": 17634814539.1485, "min_len_old": 126974990.20021392, "find_percentage_curr_old": 0.5290798611111112, "mean_find_ratio_curr_old": 0.9999998813692506, "find_percentage_total_old": 0.8237847222222222, "mean_find_ratio_total_old": 1.0001555570439349, "search_advantage": 0.5016389803784571, "time_to_find": 0.9283722471565711, "find_percentage": 0.0072, "comp_time": 92.8504068851471}
\ No newline at end of file

sargym/sar_gym.py

@@ -7,21 +7,33 @@ import numpy as np
 class GridWorldSAR(gym.Env):
     metadata = {"render_modes": ["human", "rgb_array"], "render_fps": 4}
 
-    def __init__(self, render_mode=None, size=5):
-        self.size = size  # The size of the square grid
+    def __init__(self, render_mode=None, map_size=48):
+        #Gym parameters
+        self.size = map_size  # The size of the square grid
         self.window_size = 512  # The size of the PyGame window
 
+        #Values the agent will need
+        self.pos_x = 0.0    #Current position, update w/ velocity
+        self.pos_y = 0.0
+        self.heatmap = np.load('./deeprl_data/lpm4prob.npy')
+
         # Observations are dictionaries with the agent's and the target's location.
         # Each location is encoded as an element of {0, ..., `size`}^2, i.e. MultiDiscrete([size, size]).
+        #Agent will observe their location, and the
+        #Pos = agent's current coordinates
+        #risk = search risk. Agent can see in a square area around themselves
+        #visited = which spaces have been surveyed. Will be needed to get paths for multiple drones
         self.observation_space = spaces.Dict(
             {
-                "agent": spaces.Box(0, size - 1, shape=(2,), dtype=int),
-                "target": spaces.Box(0, size - 1, shape=(2,), dtype=int),
+                "pos": spaces.Box(0, size -1, shape=(2,),dtype=int),
+                "risk": spaces.Box(0, 1, shape=(map_size*map_size,), dtype=float),
+                "visited": spaces.Box(0, 1, shape=(map_size*map_size,), dtype=bool),
             }
         )
+        #Might need to take the risk/visited obs out. Let's see if it runs
 
-        #Need a continuous action space for path planning...?
-        self.action_space = spaces.Discrete(4)
+        #Continuous velocity. Keep it at 1 so agent can't skip over cells to end episode early
+        self.action_space = spaces.Box(low=-1.0, high=1.0, shape=(2,), dtype=float)
 
 
 
@@ -37,3 +49,7 @@ class GridWorldSAR(gym.Env):
         """
         self.window = None
         self.clock = None
+
+
+    def _get_obs(self):
+        return {'pos': np.array([np.int32(self.pos_x),np.int32(self.pos_y)])}

test_trustplanner.py

@@ -108,16 +108,6 @@ def objective_printer(rgp_object = None, comp_time = 0.0, iteration = 0, folder
 
     return save_dict
 
-#Save paths from rgp as numpy array
-def gym_paths(rgp):
-
-    #New format: x, y, z, robot,
-
-    #np.save(rgp.)
-
-    return
-
-
 def main(iteration = 0, parameters = -1):
 
     # iteration = 1
@@ -155,9 +145,6 @@ def main(iteration = 0, parameters = -1):
     print(np.shape(rgp.min_risk_paths))
     print(rgp.min_risk_paths[0])
 
-    #Save path info for RL environment
-    gym_paths(rgp)
-
     #Save the waypoints from the optimized paths
     robot_paths_local = waypoint_maker.write_file(rgp_object = rgp, terrain_class = mc.terrain, filename = 'waypointsTest.json') # write waypoints to file
 
@@ -199,6 +186,7 @@ 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'
+    kentland_heatmap = './LP model/analysis/outputs/ic_1_con_hiker_t4.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'
@@ -218,7 +206,7 @@ if __name__ == "__main__":
 
         params = ({
             'lp_model': 'custom',    #custom, ring, trust, stripes
-            'opt_iterations': 100,
+            'opt_iterations': 1000,
             'path_style': 'basic',
             'stats_name': 'kentland',
             'anchor_point': [37.197730, -80.585233], # kentland
@@ -227,6 +215,7 @@ if __name__ == "__main__":
             'num_humans' : h_max,
             'lp_filename': kentland_heatmap,
             'lin_feat_filename': kentland_linfeat,
+            'terrainType': 'loadNumPy',
             'save_data': True
         })
         params['save_folder'] = 'trust-planner/{}_n{}_s{}_{}'.format(

train_cpo.py

@@ -24,7 +24,8 @@ import pdb
 
 #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_heatmap = './LP model/analysis/outputs/ic_1_con_hiker_t4.csv' #I like 4?
+#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'
@@ -40,7 +41,7 @@ def create_data():
 
     #Dictionary of parameters
     params = ({
-        'lp_model': 'ring',    #custom, ring, trust, stripes
+        'lp_model': 'custom',    #custom, ring, trust, stripes
         'opt_iterations': 1000,
         'path_style': 'basic',
         'stats_name': 'kentland',
@@ -51,24 +52,24 @@ def create_data():
         'lp_filename': kentland_heatmap,
         'lin_feat_filename': kentland_linfeat,
         'res': 25,
-        'save_data': True
+        'terrainType': 'real',
+        #'ring_mobi': [120.708, 363.315, 605.921, 848.528],
+        'save_terrain': False,
+        'save_data': False
     })
     params['save_folder'] = 'deeprl/{}_n{}_s{}_{}'.format(
         params['stats_name'],n_max,s_max,params['path_style'])
     params = Default(params).params
-
-
     #MonteCarlo loads LPM, creates searchers (humans on ground)
     mc = MC.MonteCarlo(params=params) # calls terrain builder
     mc.run_experiment()
 
-    #Put Ring search values and paths into one matrix for the gym Environment (have to fix for full res)
+
+    #Make ring search model at 1/1 resolution
     gridsize_x = mc._x_shape*params['res']
     gridsize_y = mc._y_shape*params['res']
-    sar_fullgrid = np.zeros((gridsize_x,gridsize_y),dtype=float)
     [x_crds,y_crds] = np.meshgrid(np.linspace(params['xlims'][0], params['xlims'][1], gridsize_x),
                       np.linspace(params['ylims'][0], params['ylims'][1], gridsize_y))
-
     dist_mat = np.sqrt(x_crds**2 + y_crds**2)
     #Find cutoff points for ring model
     maxdist = np.max(dist_mat)
@@ -77,10 +78,9 @@ def create_data():
     cutoff = np.linspace(mindist+adjust, maxdist, 4)
     print("New Ring boundaries = ")
     print(cutoff)
-
-
-    #params['ring_mobi'] = 25*[0.6e3, 1.8e3, 3.2e3, 9.9e3]
     params['ring_mobi'] = cutoff
+    #Fill in the heatmap
+    sar_fullgrid = np.zeros((gridsize_x,gridsize_y),dtype=float)
     sar_fullgrid[dist_mat <= params['ring_mobi'][0]] += 0.25 # center ring (95%)
     sar_fullgrid[dist_mat <= params['ring_mobi'][1]] += 0.25 # middle ring (70%)
     sar_fullgrid[dist_mat <= params['ring_mobi'][2]] += 0.25 # outter ring (45%)
@@ -95,24 +95,22 @@ def create_data():
     saveprobs = False
     if(saveprobs):
         plt.imshow(sar_fullgrid)
-        plt.title('Selected heatmap')
+        plt.title('Ring Model Search Probability')
         plt.show()
-        np.save('./deeprl_data/ring_prob.npy',sar_fullgrid)
+        np.save('./deeprl_data/ring_prob3.npy',sar_fullgrid)
 
+    #Run path planner (Set rgp to use the full resolution heatmap)
     stime = time.time()
     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)
+    rgp.optimize_risk_cost_grad(_show_detail=True)  #runs ADAM optimizer
 
 
-    #Show optimized waypoints on matrix
-    sar_paths = np.zeros((gridsize_x,gridsize_y),dtype=float)
-
-    robot_paths = []
+    #Save x/y waypoints from robot path
     start_idx = 0
     robot_idx = 0
+    save_way = True
     for path_len in rgp.robot_path_len:
         end_idx = start_idx + path_len
         rpath = rgp.min_risk_paths[start_idx:end_idx]
@@ -124,30 +122,53 @@ def create_data():
         for i in range(0,np.shape(rpath)[0]):
             x = rpath[i][0]
             y = rpath[i][1]
-            sar_paths[x+599][y+599] += 1
             waypoints[i][0] = x + params['xlims'][1]
             waypoints[i][1] = y + params['ylims'][1]
         #Plot path to make sure we're right
-        plt.scatter(waypoints[:,0], waypoints[:,1])
+        lbl = 'robot ' + str(robot_idx)
+        plt.scatter(waypoints[:,0], waypoints[:,1], label=lbl)
+        plt.xlim((0,1200))
+        plt.ylim((0,1200))
         #save numpy array
-        title = './deeprl_data/robot' + str(robot_idx) + '_waypoints.npy'
-        np.save(title,waypoints)
+        if save_way:
+            title = './deeprl_data/robot' + str(robot_idx) + '_waypoints_test3.npy'
+            np.save(title,waypoints)
         #Indices
         start_idx = end_idx
         robot_idx += 1
     #Show waypoints (should be overhead view)
+    plt.title('Test Model Optimized Waypoints')
+    plt.legend()
     plt.show()
 
 
-    plt.imshow(sar_paths)
-    plt.title('Optimized Path Waypoints')
-    plt.show()
+    #Make 3D plot
+    #Save the waypoints from the optimized paths
+    robot_paths_local = waypoint_maker.write_file(rgp_object = rgp, terrain_class = mc.terrain, filename = 'waypointsTest.json') # write waypoints to file
+    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
+
 
 def main():
 
+    #Run this function to create data
     create_data()
-    #Load robot paths from file
 
+    #Load robot paths from file
+    ring_prob = np.load('./deeprl_data/ring_prob.npy')
+    robot0_waypoints = np.load('./deeprl_data/robot0_waypoints.npy')
+    robot1_waypoints = np.load('./deeprl_data/robot1_waypoints.npy')
+    robot2_waypoints = np.load('./deeprl_data/robot2_waypoints.npy')
 
     #Make Gym Environment