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
 

waypointsTest.json

+{"robot_list": {"robot_0": {"waypoint_0": [0, -1, 37.192333745472595, -80.58975613439212, 71.00475380312369], "waypoint_1": [1, -1, 37.19250703908084, -80.58963826692215, 71.96493225721139], "waypoint_2": [2, -1, 37.19267643441753, -80.58953143530742, 73.58737392449981], "waypoint_3": [3, -1, 37.19284694170755, -80.58943186747948, 73.3918474011853], "waypoint_4": [4, -1, 37.19301058445765, -80.58933303881946, 73.51759491180243], "waypoint_5": [5, -1, 37.19316660711881, -80.58923131982827, 73.11674415083539], "waypoint_6": [6, -1, 37.193318341735555, -80.5891340830878, 73.7716950352818], "waypoint_7": [7, -1, 37.193471606664986, -80.58904167262106, 72.5270753505881], "waypoint_8": [8, -1, 37.19361920019565, -80.58895365985101, 72.9948335499069], "waypoint_9": [9, -1, 37.1937619333606, -80.58886797171512, 73.10745210059028], "waypoint_10": [10, -1, 37.19389803801843, -80.58878522448245, 72.06478681997788], "waypoint_11": [11, -1, 37.19403308453666, -80.58870535861678, 71.95598197152387], "waypoint_12": [12, -1, 37.194165866943514, -80.58862726659832, 73.05430245296705], "waypoint_13": [13, -1, 37.19430013940011, -80.58855139854923, 73.93969837909134], "waypoint_14": [14, -1, 37.19443433753184, -80.58847907590489, 72.20272538387624], "waypoint_15": [15, -1, 37.19456685498899, -80.58840637111058, 72.73525172203176], "waypoint_16": [16, -1, 37.19469870132625, -80.58833411401137, 73.22766977129974], "waypoint_17": [17, -1, 37.19483077640347, -80.58826349888538, 72.47288740664874], "waypoint_18": [18, -1, 37.194964570561694, -80.58819633244912, 73.64525910602049], "waypoint_19": [19, -1, 37.195098674878714, -80.5881332439677, 72.749855762512], "waypoint_20": [20, -1, 37.19522925541854, -80.58806975565018, 73.30885015038504], "waypoint_21": [21, -1, 37.195364627528846, -80.58800683107233, 72.84153106803795], "waypoint_22": [22, -1, 37.195497911347296, -80.58794335743794, 73.08554749110448], "waypoint_23": [23, -1, 37.195634977617985, -80.58788081937975, 72.80320203037088], "waypoint_24": [24, -1, 37.19577381302076, -80.58781943349605, 72.00336002963198], "waypoint_25": [25, -1, 37.19591044457003, -80.58775998028105, 62.321712394391604], "waypoint_26": [26, -1, 37.196046800328894, -80.58770310748417, 59.14393291730737], "waypoint_27": [27, -1, 37.196178360445785, -80.58765249253442, 58.60009536298108], "waypoint_28": [28, -1, 37.1963102960665, -80.58760527949126, 57.85711942096236], "waypoint_29": [29, -1, 37.19643952327471, -80.58755552509626, 57.45676117617242], "waypoint_30": [30, -1, 37.196567070539636, -80.58751254598273, 55.78668635289058], "waypoint_31": [31, -1, 37.19669112524877, -80.58747302140128, 55.70035653130564], "waypoint_32": [32, -1, 37.1968126965209, -80.58743416879359, 56.35287086577224], "waypoint_33": [33, -1, 37.196935419648334, -80.5873974888668, 55.9334107777696], "waypoint_34": [34, -1, 37.19705206900757, -80.58737310017852, 55.67040392224192], "waypoint_35": [35, -1, 37.197169676641145, -80.58734408116123, 55.110582444962716], "waypoint_36": [36, -1, 37.19728389990665, -80.58732420385927, 55.72655479637153], "waypoint_37": [37, -1, 37.197395176763564, -80.58730373328748, 54.58336984426041], "waypoint_38": [38, -1, 37.19750414919892, -80.58728619453562, 54.24893949958263], "waypoint_39": [39, -1, 37.197609007280505, -80.58727295997106, 54.31574347198086], "waypoint_40": [40, -1, 37.19771589234359, -80.58726303898612, 55.4186231290578], "waypoint_41": [41, -1, 37.19781536628438, -80.58726556786225, 54.21525277651625], "waypoint_42": [42, -1, 37.19792096130104, -80.58726500028943, 53.92324403986359], "waypoint_43": [43, -1, 37.198022643337275, -80.5872684676246, 53.25850770698442], "waypoint_44": [44, -1, 37.19812562087664, -80.58727463391982, 53.47590738753879], "waypoint_45": [45, -1, 37.198230509481235, -80.58728944292098, 53.43365921906498], "waypoint_46": [46, -1, 37.19833431372803, -80.58730446022854, 53.488053733206414], "waypoint_47": [47, -1, 37.19844145967795, -80.58732837161747, 53.87567886968577], "waypoint_48": [48, -1, 37.198551847546966, -80.58735576102893, 52.83395182203651], "waypoint_49": [49, -1, 37.1986636758098, -80.58738836891219, 52.745033230315855], "waypoint_50": [50, -1, 37.19877807753798, -80.58742167119357, 52.79463955429469], "waypoint_51": [51, -1, 37.198895513719044, -80.58746645314157, 52.89871896046907], "waypoint_52": [52, -1, 37.199016389439656, -80.58751837581131, 52.58691876152328], "waypoint_53": [53, -1, 37.199143843501496, -80.58757137147636, 52.59287499740142], "waypoint_54": [54, -1, 37.19926660792028, -80.5876257062506, 52.34661727587775], "waypoint_55": [55, -1, 37.199397786160056, -80.58768412805556, 52.547604959831446], "waypoint_56": [56, -1, 37.19953360421128, -80.58775075744076, 52.418626366849594], "waypoint_57": [57, -1, 37.19968381209937, -80.58782975427924, 52.07031767419956], "waypoint_58": [58, -1, 37.199829909335534, -80.58791605928151, 51.988903846606476], "waypoint_59": [59, -1, 37.19999006224342, -80.58800696118841, 50.47332928127133], "waypoint_60": [60, -1, 37.20015280112427, -80.58810708059642, 50.079481759801894], "waypoint_61": [61, -1, 37.2003194097042, -80.58819991458878, 50.147764919305956], "waypoint_62": [62, -1, 37.20050011784461, -80.58829441419613, 49.46728963769321], "waypoint_63": [63, -1, 37.20068182497316, -80.58839844375652, 48.88801995121118], "waypoint_64": [64, -1, 37.20087136735218, -80.58850441218398, 48.56995170050429], "waypoint_65": [65, -1, 37.20106697520207, -80.58861856535081, 47.94219023298723], "waypoint_66": [66, -1, 37.201271374614116, -80.58873502108814, 47.328394848589035], "waypoint_67": [67, -1, 37.201484189601565, -80.58885895986953, 47.38138178120904], "waypoint_68": [68, -1, 37.20169976963635, -80.58898207300629, 45.12348099031935], "waypoint_69": [69, -1, 37.20192480764038, -80.58910432541545, 43.06138359719013], "waypoint_70": [70, -1, 37.20215605131146, -80.58922336669224, 41.68325573678043], "waypoint_71": [71, -1, 37.20239255761376, -80.58934428224522, 39.90039722596673], "waypoint_72": [72, -1, 37.202633196700404, -80.5894710561865, 38.25557348404438], "waypoint_73": [73, -1, 37.202880055272765, -80.58960932303431, 36.1868287548114], "waypoint_74": [74, -1, 37.203134026359216, -80.58976078217806, 34.52716136489033]}, "robot_1": {"waypoint_0": [0, -1, 37.19232951950616, -80.58523512105738, 75.52336991797296], "waypoint_1": [1, -1, 37.19252303657682, -80.58526591260265, 76.09214911151517], "waypoint_2": [2, -1, 37.19270647565956, -80.58530382109916, 74.81177346057149], "waypoint_3": [3, -1, 37.19288040358857, -80.58533771691808, 74.98816322917759], "waypoint_4": [4, -1, 37.19305880072417, -80.58536649828275, 75.36441828566143], "waypoint_5": [5, -1, 37.19323394658146, -80.5853926116061, 75.98915719763049], "waypoint_6": [6, -1, 37.193407277946285, -80.58541986588817, 75.86940322780691], "waypoint_7": [7, -1, 37.193574439748666, -80.58544041432502, 75.38952065797073], "waypoint_8": [8, -1, 37.19373781365059, -80.58545898780534, 74.67794067572697], "waypoint_9": [9, -1, 37.19390126072022, -80.58547931994406, 74.75106030041344], "waypoint_10": [10, -1, 37.19406501809131, -80.58549408438364, 74.80149819993939], "waypoint_11": [11, -1, 37.19422770483111, -80.58550422099991, 73.83301857255472], "waypoint_12": [12, -1, 37.19438673395294, -80.58551466386552, 72.77377976134589], "waypoint_13": [13, -1, 37.19454298832509, -80.58552428247529, 71.74334459683845], "waypoint_14": [14, -1, 37.19469735050826, -80.58553398536733, 73.17123436227118], "waypoint_15": [15, -1, 37.19485135020519, -80.58554061587789, 72.57519605092068], "waypoint_16": [16, -1, 37.19500407500967, -80.58554216507054, 72.72611663743629], "waypoint_17": [17, -1, 37.195160025667356, -80.58554970123065, 68.99184596518646], "waypoint_18": [18, -1, 37.19531308874696, -80.58555386610767, 62.580965063820095], "waypoint_19": [19, -1, 37.19546230404865, -80.58555713338535, 60.720078651717856], "waypoint_20": [20, -1, 37.19560883118672, -80.5855600554038, 58.70983725848919], "waypoint_21": [21, -1, 37.19575523606632, -80.58556072709422, 57.50156257928855], "waypoint_22": [22, -1, 37.1959008578708, -80.58556120436477, 57.41059354907138], "waypoint_23": [23, -1, 37.19603946538958, -80.58556452130254, 56.657065103013004], "waypoint_24": [24, -1, 37.19617392029723, -80.58556450937532, 56.05981361236981], "waypoint_25": [25, -1, 37.19630932602565, -80.58556457289158, 55.435279262153486], "waypoint_26": [26, -1, 37.19644065683389, -80.58556699864485, 54.86259036201732], "waypoint_27": [27, -1, 37.196564455271265, -80.58556685898581, 53.830438004716285], "waypoint_28": [28, -1, 37.19668853317402, -80.5855668324978, 54.27788255019041], "waypoint_29": [29, -1, 37.19680355760145, -80.58556894969313, 54.301506590835245], "waypoint_30": [30, -1, 37.196914005820275, -80.58556985794364, 53.256036080783716], "waypoint_31": [31, -1, 37.197027443401254, -80.58557027643003, 54.13870555615182], "waypoint_32": [32, -1, 37.197133084101274, -80.58557056182885, 53.945282407983086], "waypoint_33": [33, -1, 37.19723701019707, -80.58556955364017, 53.40180161196916], "waypoint_34": [34, -1, 37.19733450359542, -80.58557097874906, 53.607961529530485], "waypoint_35": [35, -1, 37.197430634100414, -80.58557601633669, 53.22306134305228], "waypoint_36": [36, -1, 37.197523394510014, -80.58557447184958, 53.33696422540794], "waypoint_37": [37, -1, 37.19761388168367, -80.58558102326792, 53.08961804571231], "waypoint_38": [38, -1, 37.19770820753487, -80.58558289219708, 52.92363700802211], "waypoint_39": [39, -1, 37.19779767085046, -80.5855805559492, 53.1882337361494], "waypoint_40": [40, -1, 37.197888034338526, -80.58558244396711, 52.80908227274322], "waypoint_41": [41, -1, 37.19797625568983, -80.58558428235148, 53.437628375533265], "waypoint_42": [42, -1, 37.19806286022346, -80.58558324841995, 52.57512154311168], "waypoint_43": [43, -1, 37.19815182720584, -80.58558579995159, 52.089412528565], "waypoint_44": [44, -1, 37.19823887869321, -80.5855870728747, 52.2430673981446], "waypoint_45": [45, -1, 37.19832394904062, -80.58558774226923, 53.278806979925605], "waypoint_46": [46, -1, 37.19841522693713, -80.58558692230872, 53.02852620804899], "waypoint_47": [47, -1, 37.19850792556028, -80.58558386254518, 52.92239999420164], "waypoint_48": [48, -1, 37.19860463920241, -80.58557962669721, 52.24695633451741], "waypoint_49": [49, -1, 37.1987014718805, -80.58557689073463, 52.06485632715678], "waypoint_50": [50, -1, 37.19879909130216, -80.5855760309995, 51.98542795208911], "waypoint_51": [51, -1, 37.1989041885878, -80.58557625080518, 51.8495660564538], "waypoint_52": [52, -1, 37.19900633046776, -80.58556312969637, 51.85082802029505], "waypoint_53": [53, -1, 37.19910839450935, -80.58555642338412, 52.52756047920221], "waypoint_54": [54, -1, 37.19922157808933, -80.58554579218848, 52.52833238370596], "waypoint_55": [55, -1, 37.199342027093614, -80.58553619160188, 52.245310799204574], "waypoint_56": [56, -1, 37.199470683072676, -80.585523089618, 52.007430823142236], "waypoint_57": [57, -1, 37.19960639650673, -80.58551239297167, 52.133845221742476], "waypoint_58": [58, -1, 37.19974261598057, -80.58549947280342, 51.67990779997885], "waypoint_59": [59, -1, 37.1998948183596, -80.58548711052943, 51.21236958653179], "waypoint_60": [60, -1, 37.20004064124751, -80.58547229840963, 51.88723005343885], "waypoint_61": [61, -1, 37.20019996167112, -80.5854590035161, 51.277801236810795], "waypoint_62": [62, -1, 37.200373709343715, -80.58544526827748, 51.27317025381086], "waypoint_63": [63, -1, 37.20055896628266, -80.58542555055044, 51.179518308298476], "waypoint_64": [64, -1, 37.20075034689399, -80.58540706117778, 50.401798477644704], "waypoint_65": [65, -1, 37.20094958140671, -80.58538735241251, 50.54612959210617], "waypoint_66": [66, -1, 37.201156967774104, -80.5853705802531, 49.97038243353977], "waypoint_67": [67, -1, 37.20137488472328, -80.58535456534464, 49.34519786924277], "waypoint_68": [68, -1, 37.20160582754508, -80.58533627575677, 49.743166179767556], "waypoint_69": [69, -1, 37.201844233126224, -80.5853161705879, 48.64298807296909], "waypoint_70": [70, -1, 37.20208903196951, -80.58529081854446, 48.27091255455176], "waypoint_71": [71, -1, 37.202336919552174, -80.5852724948885, 47.2657878572014], "waypoint_72": [72, -1, 37.20260082749124, -80.58525193318972, 44.9885018927217], "waypoint_73": [73, -1, 37.202863147858295, -80.58524679645895, 38.73315460920392], "waypoint_74": [74, -1, 37.203127443606064, -80.58524328655545, 32.68831275018165]}, "robot_2": {"waypoint_0": [0, -1, 37.19233334582494, -80.58071642141155, 90.72029521366302], "waypoint_1": [1, -1, 37.19251245350042, -80.5808797168193, 78.49493529486963], "waypoint_2": [2, -1, 37.1926949328841, -80.581044571241, 77.57793109705402], "waypoint_3": [3, -1, 37.19287894509461, -80.5812030430387, 75.01851194333784], "waypoint_4": [4, -1, 37.193059095080486, -80.58135584968946, 73.39595244226433], "waypoint_5": [5, -1, 37.19323406612278, -80.58150433314682, 74.17570102366294], "waypoint_6": [6, -1, 37.193406501562166, -80.58164408537966, 72.77476739084591], "waypoint_7": [7, -1, 37.193577931529454, -80.5817765696946, 72.41783100915957], "waypoint_8": [8, -1, 37.19375301961146, -80.58190098588629, 70.01819835620736], "waypoint_9": [9, -1, 37.1939258707871, -80.58202186034572, 69.68899601991397], "waypoint_10": [10, -1, 37.19409679907616, -80.58214116086164, 68.5075253277275], "waypoint_11": [11, -1, 37.19426957824381, -80.58226410301009, 67.86246333596299], "waypoint_12": [12, -1, 37.19444249799743, -80.58237753813322, 65.7967749591699], "waypoint_13": [13, -1, 37.19461305812782, -80.5824888877958, 63.65771025017526], "waypoint_14": [14, -1, 37.1947760778138, -80.58258772422549, 62.94228093793288], "waypoint_15": [15, -1, 37.19493663836617, -80.58268450290934, 61.833049280756555], "waypoint_16": [16, -1, 37.19508967486191, -80.58278206986314, 59.79804486316767], "waypoint_17": [17, -1, 37.19523139766227, -80.58287159567378, 59.812093545460506], "waypoint_18": [18, -1, 37.195379598389785, -80.58296411533188, 58.994501938391736], "waypoint_19": [19, -1, 37.195515700188146, -80.58305016643725, 59.37722340641557], "waypoint_20": [20, -1, 37.19565271952422, -80.58313101304621, 59.330194627776855], "waypoint_21": [21, -1, 37.19578486570242, -80.58320262870117, 58.31870432189291], "waypoint_22": [22, -1, 37.19591370390049, -80.58327315771396, 57.948175113988285], "waypoint_23": [23, -1, 37.196041413799755, -80.58333587994046, 57.211598680681206], "waypoint_24": [24, -1, 37.19616495376707, -80.58340098752805, 54.91294957383363], "waypoint_25": [25, -1, 37.1962886100697, -80.5834643700218, 55.8820537462355], "waypoint_26": [26, -1, 37.19640751848901, -80.58351872315421, 55.02251058102111], "waypoint_27": [27, -1, 37.19652648895883, -80.58357788905418, 54.854032664651996], "waypoint_28": [28, -1, 37.19663817653614, -80.58362850361247, 54.450212596039655], "waypoint_29": [29, -1, 37.196747634045764, -80.58367231664795, 53.987360293531836], "waypoint_30": [30, -1, 37.196853375483954, -80.5837165667497, 54.575007557093606], "waypoint_31": [31, -1, 37.19696082557791, -80.58375534089572, 54.22998471205574], "waypoint_32": [32, -1, 37.19705843536385, -80.5837899362862, 53.74630976194464], "waypoint_33": [33, -1, 37.197159178342964, -80.58382547987038, 53.26901417086742], "waypoint_34": [34, -1, 37.19725952591939, -80.58385468284222, 53.23480540111319], "waypoint_35": [35, -1, 37.197356378648884, -80.58387896509736, 52.871550511837924], "waypoint_36": [36, -1, 37.19745143647636, -80.58389776428709, 52.91153559110123], "waypoint_37": [37, -1, 37.197544388011686, -80.58391234774301, 52.244841945688364], "waypoint_38": [38, -1, 37.197638648002396, -80.58392584988498, 52.80798921397091], "waypoint_39": [39, -1, 37.1977242110689, -80.58392780662503, 52.66815680035702], "waypoint_40": [40, -1, 37.197816265316426, -80.58393143178395, 51.570560638467946], "waypoint_41": [41, -1, 37.1979084978766, -80.58393099822136, 52.323412159971824], "waypoint_42": [42, -1, 37.19800122194492, -80.58392056082138, 51.431919114463724], "waypoint_43": [43, -1, 37.19809242554675, -80.58390768127614, 52.69517047045166], "waypoint_44": [44, -1, 37.19818528745725, -80.58388878988985, 52.59325705551666], "waypoint_45": [45, -1, 37.19827719159946, -80.58386374701013, 52.39190057469385], "waypoint_46": [46, -1, 37.1983716459742, -80.58382784575424, 52.3879481794051], "waypoint_47": [47, -1, 37.1984699870766, -80.58379000883413, 52.00555041346459], "waypoint_48": [48, -1, 37.19856791579748, -80.58374816274916, 52.418746468999075], "waypoint_49": [49, -1, 37.19867449279365, -80.58369890538656, 52.7658642718552], "waypoint_50": [50, -1, 37.19878527083356, -80.58363826692825, 51.60143441594147], "waypoint_51": [51, -1, 37.1988972760335, -80.58357724410658, 52.4977526325905], "waypoint_52": [52, -1, 37.1990163991275, -80.58350622216004, 51.41902689500941], "waypoint_53": [53, -1, 37.19913957698338, -80.58343089757777, 51.20087237622297], "waypoint_54": [54, -1, 37.19927366703799, -80.58334632538852, 50.65230614941444], "waypoint_55": [55, -1, 37.19940683279771, -80.58325719797513, 49.556950355597564], "waypoint_56": [56, -1, 37.19955013622865, -80.58315936354545, 50.410654594756096], "waypoint_57": [57, -1, 37.19969407379816, -80.58306175410169, 49.829479578807984], "waypoint_58": [58, -1, 37.19984534861585, -80.58295181603877, 49.9547448820115], "waypoint_59": [59, -1, 37.200000146358924, -80.58284270682285, 49.10262450258978], "waypoint_60": [60, -1, 37.20017199381479, -80.58272334855572, 49.15778982123054], "waypoint_61": [61, -1, 37.200347484092156, -80.58259730044057, 48.89344072513812], "waypoint_62": [62, -1, 37.20052844166582, -80.5824676893727, 48.211368661481636], "waypoint_63": [63, -1, 37.200712697705654, -80.58233560145513, 48.081904076164975], "waypoint_64": [64, -1, 37.200897545856776, -80.5822022950622, 48.614318654923395], "waypoint_65": [65, -1, 37.20108991955294, -80.5820622483388, 47.934248008177725], "waypoint_66": [66, -1, 37.20129286717658, -80.581922129359, 47.85790428050338], "waypoint_67": [67, -1, 37.201503178199204, -80.58177643696702, 47.853569775076565], "waypoint_68": [68, -1, 37.201718887784956, -80.58163097485263, 47.94347405294465], "waypoint_69": [69, -1, 37.20193851550196, -80.58148984658452, 47.56541796299244], "waypoint_70": [70, -1, 37.20216390035067, -80.5813402937398, 48.20882763865745], "waypoint_71": [71, -1, 37.20239337351496, -80.58118522998686, 47.42430570893184], "waypoint_72": [72, -1, 37.20263162184667, -80.58103123524154, 47.855439481267176], "waypoint_73": [73, -1, 37.202879947833644, -80.58087683564065, 46.984780782755585], "waypoint_74": [74, -1, 37.2031267007015, -80.58071626715879, 41.614885091994445]}}}
\ No newline at end of file