Simulator now uses path tracker as its policy

guided_mrmp/controllers/multi_path_tracking.py

@@ -134,7 +134,7 @@ class MultiPathTracker:
 
         return trajectory
     
-    def get_next_control(self, state, show_plots=False):
+    def advance(self, state, show_plots=False):
         # optimization loop
         # start=time.time()
 
@@ -237,7 +237,7 @@ class MultiPathTracker:
             self.plot_current_world_state()
             
             # get the next control for all robots
-            x_mpc, controls = self.get_next_control(self.states)
+            x_mpc, controls = self.advance(self.states)
 
             next_states = []
             for i in range(self.num_robots):

guided_mrmp/controllers/multi_path_tracking_db.py

@@ -494,7 +494,7 @@ class MultiPathTrackerDB(MultiPathTracker):
 
         return all_starts_goals_equal
     
-    def get_next_control(self, state, show_plots=False):
+    def advance(self, state, show_plots=False):
         # optimization loop
         # start=time.time()
 

guided_mrmp/main.py

@@ -10,8 +10,8 @@ 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
+from guided_mrmp.controllers import MultiPathTracker, MultiPathTrackerDB
 
 class_function_names_dict = {
     'Roomba': Roomba
@@ -136,7 +136,18 @@ if __name__ == "__main__":
     # Create the Guided MRMP policy
     T = settings['prediction_horizon']
     DT = settings['discretization_step']
-    policy = GuidedMRMP(env, robots, dynamics_models, T, DT, libs, settings)
+    policy = MultiPathTrackerDB(env=env, 
+                                 initial_positions=robot_starts, 
+                                 dynamics=dynamics_models[0], # NOTE: Using the same dynamics model for all robots for now
+                                 target_v=target_v, 
+                                 T=T, 
+                                 DT=DT, 
+                                 waypoints=[robot.waypoints for robot in robots], 
+                                 settings=settings, 
+                                 lib_2x3=libs[0], 
+                                 lib_3x3=libs[1], 
+                                 lib_2x5=libs[2]
+                                )
 
     # Create the simulator
     scaling_factor = settings['simulator']['scaling_factor']

guided_mrmp/simulator.py

 import pygame
 import numpy as np
 
-
+# Simulator class:
+# Responsible for managing the simulation of the robots in the environment
 class Simulator:
     def __init__(self, robots, dynamics_models, env, policy, settings):
         """
@@ -23,6 +24,8 @@ class Simulator:
         self.num_robots = len(robots)
         self.dynamics_models = dynamics_models
         self.time = 0
+
+        # scaling factor used to adjust the size of the pygame window
         self.scaling_factor = settings['simulator']['scaling_factor']
 
     def all_robots_at_goal(self):
@@ -35,37 +38,29 @@ class Simulator:
         """
         Advance the simulation by dt seconds
         """
-        conflict_detected, controls = self.policy.advance(screen,self.state, self.time, dt)
 
-        # if not conflict_detected:
+        # Get the controls from the policy
+        x_mpc, controls = self.policy.advance(self.state)
+
+        # Update the state of each robot
         for i in range(self.num_robots):
             new_state = self.dynamics_models[i].next_state(self.state[i], controls[i], dt)
             self.robots[i].current_position = new_state
             self.state[i] = new_state
-        self.time += dt
 
-        # if conflict_detected:
-        #     print("Executing full trajectory")
-        #     for i in range(self.num_robots):
-        #         controls_list = controls[i]
-        #         for j in range(len(controls_list)):
-        #             next_control = [controls_list[0][j], controls_list[1][j]]
-        #             print(f"next control = {next_control}")
-        #             new_state = self.dynamics_models[i].next_state(self.state[i], next_control, dt)
-        #             self.robots[i].current_position = new_state
-        #             self.state[i] = new_state
-        #     self.time += dt
+        # Update the time
+        self.time += dt
 
     def draw_environment(self, screen):
+        """ Draw the environment on the screen """
         screen.fill((255,255,255))
         for obs in self.circ_obstacles:
             pygame.draw.circle(screen, (0,0,0), (obs[0]*self.scaling_factor,obs[1]*self.scaling_factor), obs[2]*self.scaling_factor)
         for obs in self.rect_obstacles:
             pygame.draw.rect(screen, (0,0,0), obs)
 
-        
-
     def draw_robots(self, screen):
+        """ Draw the robots on the screen """
         for robot in self.robots:
             x,y,yaw = robot.current_position
             pygame.draw.circle(screen, robot.color, (x*self.scaling_factor,y*self.scaling_factor), robot.radius*self.scaling_factor)
@@ -96,12 +91,11 @@ class Simulator:
             if not pause:
                 if show_vis: 
                     self.draw_environment(screen)
-                    self.policy.add_vis_guide_paths(screen, self.scaling_factor)
+                    # self.policy.add_vis_guide_paths(screen, self.scaling_factor)
                     self.draw_robots(screen)
                 
                 self.advance(screen,self.policy.DT)
 
             if show_vis: 
-                
                 pygame.display.flip()