add library initialization to main

guided_mrmp/main.py

@@ -10,9 +10,10 @@ 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
 
-class_names_dist = {
-    'Roomba': Roomba,
+class_function_names_dict = {
+    'Roomba': Roomba
     # 'RRT': RRT(),
     # 'RRTStar': RRTStar(),
     }
@@ -91,9 +92,30 @@ 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_mpc.yaml")
+    settings = load_settings("settings_files/settings.yaml")
 
     set_python_seed(1123)
     # set_python_seed(11235813)
@@ -112,7 +134,7 @@ if __name__ == "__main__":
 
     dynamics_models = []
     for model in dynamics_models_st:
-        dynamics_models.append(class_names_dist[model]())
+        dynamics_models.append(class_function_names_dict[model]())
 
     # Load and create the robots
     robot_starts = settings['robot_starts']
@@ -125,10 +147,14 @@ if __name__ == "__main__":
 
     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']
-    policy = GuidedMRMP(env, robots, dynamics_models, T, DT, settings)
+    policy = GuidedMRMP(env, robots, dynamics_models, T, DT, libs, settings)
 
     # Create the simulator
     scaling_factor = settings['simulator']['scaling_factor']