Put initialize_libraries function into general utils folder

guided_mrmp/main.py

@@ -11,6 +11,7 @@ from guided_mrmp.planners import RRTStar
 from guided_mrmp.planners.db_guided_mrmp import GuidedMRMP
 from guided_mrmp.simulator import Simulator
 from guided_mrmp.utils.library import Library
+from guided_mrmp.utils.helpers import initialize_libraries
 
 class_function_names_dict = {
     'Roomba': Roomba
@@ -92,27 +93,6 @@ def initialize_robots(starts, goals, dynamics_models, radii, target_v, env):
     return robots
 
 
-def initialize_libraries(library_fnames=["guided_mrmp/database/2x3_library","guided_mrmp/database/3x3_library","guided_mrmp/database/5x2_library"]):
-    """
-    Load the 2x3, 3x3, and 2x5 libraries. Store them in self.lib-x- 
-    Inputs: 
-        library_fnames - the folder containing the library files
-    """
-    # Create each of the libraries
-    print(f"Loading libraries. This usually takes about 10 seconds...")
-    lib_2x3 = Library(library_fnames[0])
-    lib_2x3.read_library_from_file()
-    
-    lib_3x3 = Library(library_fnames[1])
-    lib_3x3.read_library_from_file()
-
-    
-    lib_2x5 = Library(library_fnames[2])
-    lib_2x5.read_library_from_file()
-
-    return [lib_2x3, lib_3x3, lib_2x5]
-
-
 if __name__ == "__main__":
     # Load the settings
     settings = load_settings("settings_files/settings.yaml")
@@ -125,8 +105,6 @@ if __name__ == "__main__":
     rectangle_obstacles = settings['environment']['rectangle_obstacles']
     x_range = settings['environment']['x_range']
     y_range = settings['environment']['y_range']
-    # sim_x_range = settings['simulator']['sim_x_range']
-    # sim_y_range = settings['simulator']['sim_y_range']
     env = Env(x_range, y_range, circle_obstacles, rectangle_obstacles)
 
     # Load the dynamics models
@@ -143,14 +121,11 @@ if __name__ == "__main__":
     target_v = settings['target_v']
     if robot_starts == []:
         robot_starts, robot_goals = create_random_starts_and_goals(env, len(robot_radii))
-    # print(f"starts = {len(robot_starts)}")
-
     robots = initialize_robots(robot_starts, robot_goals, dynamics_models, robot_radii, target_v, env)
 
     # Load the libraries
     libs = initialize_libraries()
-    # libs = [None, None, None]
-
+    
     # Create the Guided MRMP policy
     T = settings['prediction_horizon']
     DT = settings['discretization_step']
@@ -164,6 +139,7 @@ if __name__ == "__main__":
         screen = pygame.display.set_mode((x_range[1]*scaling_factor, y_range[1]*scaling_factor)) 
 
     else: screen = None
+
     sim = Simulator(robots, dynamics_models, env, policy, settings)
 
     # Run the simulation

guided_mrmp/utils/helpers.py

@@ -40,6 +40,28 @@ def create_random_starts_and_goals(env, num_agents):
 
     return starts,goals
 
+def initialize_libraries(library_fnames=["guided_mrmp/database/2x3_library","guided_mrmp/database/3x3_library","guided_mrmp/database/5x2_library"]):
+    """
+    Load the 2x3, 3x3, and 2x5 libraries. Store them in self.lib-x- 
+    Inputs: 
+        library_fnames - the folder containing the library files
+    """
+    from guided_mrmp.utils.library import Library
+    # Create each of the libraries
+    print(f"Loading libraries. This usually takes about 10 seconds...")
+    lib_2x3 = Library(library_fnames[0])
+    lib_2x3.read_library_from_file()
+    
+    lib_3x3 = Library(library_fnames[1])
+    lib_3x3.read_library_from_file()
+
+    
+    lib_2x5 = Library(library_fnames[2])
+    lib_2x5.read_library_from_file()
+
+    return lib_2x3, lib_3x3, lib_2x5
+
+
 def read_agents_from_file(fname):
     pass