Added comment to prevent weird behavior when initial_set is smaller t…

…han grid_resolution

configs/reach_controller.yaml

@@ -8,10 +8,11 @@ planner:
       'direction': 'forward'
       'fwd_back_buffer_in_h': 1 # this is the time added to the earliest_to_reach as buffer for forward-backward
       'T_goal_in_h': 65
-      'initial_set_radii': [0.03, 0.03]
+      'initial_set_radii': [0.03, 0.03] # this is in deg lat, lon
       'n_time_vector': 60 # Note that this is the number of time-intervals, the vector is +1 longer because of init_time
-      'grid_res': !!python/tuple [100, 100]
-      'deg_around_xt_xT_box': 0.8
+      # Note: grid_res should alwas be bigger than initial_set_radii, otherwise reachability behaves weirdly.
+      'grid_res': !!python/tuple [0.02, 0.02] # Note: this is in deg lat, lon (HYCOM Global is 0.083 and Mexico 0.04)
+      'deg_around_xt_xT_box': 0.8 # area over which to run HJ_reachability
       'accuracy': 'high'
       'artificial_dissipation_scheme': 'local_local'
     dt_replanning: 864000000.     # right now very high as only tested open-loop control once

ocean_navigation_simulator/planners/hj_reachability_planners/hj_reachability_planner.py

@@ -3,8 +3,6 @@
 from ocean_navigation_simulator.utils import simulation_utils
 from ocean_navigation_simulator.planners.hj_reachability_planners.platform_2D_for_sim import Platform2D_for_sim
 import os
-from scipy.interpolate import interp1d
-import bisect
 import sys
 # Note: if you develop on hj_reachability and this library simultaneously uncomment this line
 # sys.path.extend([os.path.dirname(os.path.dirname(os.path.dirname(os.path.dirname(os.path.realpath(__file__))))) + 'hj_reachability_c3'])
@@ -49,7 +47,7 @@ def __init__(self, problem, gen_settings, specific_settings):
         # this is just a variable that persists after planning for plotting/debugging
         self.x_t = None
         # initializes variables needed for planning, they will be filled in the plan method
-        self.reach_times, self.all_values, self.times_abs, self.grid, self.diss_scheme = [None]*5
+        self.reach_times, self.all_values, self.grid, self.diss_scheme = [None]*4
         self.x_traj, self.contr_seq, self.distr_seq = [None]*3
         # initialize variables needed for solving the PDE in non_dimensional terms
         self.characteristic_vec, self.offset_vec, self.nonDimGrid, self.nondim_dynamics = [None] * 4
@@ -213,9 +211,13 @@ def update_current_data(self, x_t):
             t_interval, lat_bnds, lon_bnds,
             file_dicts=self.cur_forecast_dicts)
 
+        # calculate target shape of the grid
+        x_n_res = int((grids_dict['x_grid'][-1] - grids_dict['x_grid'][0])/self.specific_settings['grid_res'][0])
+        y_n_res = int((grids_dict['y_grid'][-1] - grids_dict['y_grid'][0])/self.specific_settings['grid_res'][1])
+
         # 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')
+            grids_dict, water_u, water_v, target_shape=(x_n_res, y_n_res), kind='linear')
 
         # set absolute time in UTC Posix time
         self.current_data_t_0 = grids_dict['t_grid'][0]
@@ -327,11 +329,12 @@ def get_dim_dynamical_system(self):
 
     def initialize_hj_grid(self, grids_dict):
         """Initialize the dimensional grid in degrees lat, lon"""
+        # initialize grid using the grids_dict x-y shape as shape
         self.grid = hj.Grid.from_grid_definition_and_initial_values(
             domain=hj.sets.Box(
                 lo=np.array([grids_dict['x_grid'][0], grids_dict['y_grid'][0]]),
                 hi=np.array([grids_dict['x_grid'][-1], grids_dict['y_grid'][-1]])),
-            shape=self.specific_settings['grid_res'])
+            shape=(len(grids_dict['x_grid']), len(grids_dict['y_grid'])))
 
     def get_initial_values(self, center):
         return hj.shapes.shape_ellipse(grid=self.nonDimGrid,

ocean_navigation_simulator/utils/plotting_utils.py

@@ -1,4 +1,3 @@
-import numpy as np
 import matplotlib.pyplot as plt
 import cartopy.crs as ccrs
 import cartopy.feature as cfeature