Integrated 1D time-interpolation into the hj_reachability_planner.py

Author: MariusWiggert

ocean_navigation_simulator/planners/hj_reachability_planners/hj_reachability_planner.py

@@ -202,15 +202,21 @@ def extract_trajectory(self, x_start, traj_rel_times_vector=None):
     def update_current_data(self, x_t):
         print("Reachability Planner: Loading new current data.")
 
+        # get the t, lat, lon bounds for sub-setting the data
         t_interval, lat_bnds, lon_bnds = \
             simulation_utils.convert_to_lat_lon_time_bounds(x_t.flatten(), self.x_T,
                                                             deg_around_x0_xT_box=self.specific_settings['deg_around_xt_xT_box'],
                                                             temp_horizon_in_h=self.specific_settings['T_goal_in_h'])
 
+        # get the data subset from the file
         grids_dict, water_u, water_v = simulation_utils.get_current_data_subset(
             t_interval, lat_bnds, lon_bnds,
             file_dicts=self.cur_forecast_dicts)
 
+        # do spatial interpolation to the desired resolution to run HJ_reachability
+        grids_dict['x_grid'], grids_dict['y_grid'], water_u, water_v = simulation_utils.spatial_interpolation(
+            grids_dict, water_u, water_v, target_shape=self.specific_settings['grid_res'], kind='linear')
+
         # set absolute time in UTC Posix time
         self.current_data_t_0 = grids_dict['t_grid'][0]
 

ocean_navigation_simulator/planners/hj_reachability_planners/platform_2D_for_sim.py

@@ -44,13 +44,10 @@ def update_jax_interpolant(self, x_grid, y_grid, t_grid, water_u, water_v):
         Input Params:
         - water_u, water_v      the array comes in from HYCOM as (T, Y, X)
         """
-        # need to flip array around to fit with grid_axis in X, Y, T and dynamics
-        water_u = jnp.swapaxes(water_u, 0, 2)
-        water_v = jnp.swapaxes(water_v, 0, 2)
-        self.x_current = lambda point: interpolation.lin_interpo_3D_fields(
-            point, water_u, x_grid, y_grid, t_grid)
-        self.y_current = lambda point: interpolation.lin_interpo_3D_fields(
-            point, water_v, x_grid, y_grid, t_grid)
+
+        # create 1D interpolation functions for running in the loop of the dynamics
+        self.x_current = lambda point: interpolation.lin_interpo_1D(point, water_u, x_grid, y_grid, t_grid)
+        self.y_current = lambda point: interpolation.lin_interpo_1D(point, water_v, x_grid, y_grid, t_grid)
 
     def __call__(self, state, control, disturbance, time):
         """Implements the continuous-time dynamics ODE."""

scripts/experiments/reachability_planner.py

@@ -54,7 +54,10 @@
 # planner.plot_ctrl_seq()
 #%% Set stuff up
 sim = OceanNavSimulator(sim_config_dict="simulator.yaml", control_config_dict='reach_controller.yaml', problem=prob)
+start = time.time()
 sim.run(T_in_h=70)
+print("Took : ", time.time() - start)
+# after doing now 1D interpolation in the loop planning time drops from 214, 209s to 46.5s
 #%% Step 5: plot from Simulator
 # # plot Battery levels over time
 sim.plot_trajectory(plotting_type='battery')