Add waiting to policy

guided_mrmp/controllers/multi_path_tracking_db.py

@@ -407,6 +407,9 @@ class MultiPathTrackerDB(MultiPathTracker):
     
     def advance(self, state, show_plots=False):
 
+        waiting = False
+        all_waiting_robots = []
+
         u_next = [[] for _ in range(self.num_robots)]
         x_next = [[] for _ in range(self.num_robots)]
 
@@ -544,12 +547,11 @@ class MultiPathTrackerDB(MultiPathTracker):
                         x_next[i] = x_mpc
 
                 if grid_solution is None:
-                    # if there isnt a grid solution, the most likely scenario is that the robots 
-                    # are not close enough together to place down a subproblem
-                    # in this case, we just allow the robts to continue on their paths and resolve 
-                    # the conflict later
-                    print("No grid solution found, proceeding with the current paths")
-                    # TODO: This should call coupled MPC to resolve the conflict
+                    if waiting: 
+                        # Choose all but one robot to "wait" (i.e. controls are 0)
+                        c_wait = c[1:]
+                        all_waiting_robots.extend(c_wait)
+                    # Discrete solver failed, resolve the conflict with coupled MPC
 
                     # dynamycs w.r.t robot frame
                     curr_states = np.zeros((self.num_robots, 3))
@@ -568,10 +570,43 @@ class MultiPathTrackerDB(MultiPathTracker):
                 print("Proceeding with the current paths")
 
 
+        if waiting:
+            all_waiting_robots = self.find_all_waiting_robots(all_waiting_robots, state, x_next)
+            for i in all_waiting_robots:
+                u_next[i] = [0, 0]
+                x_next[i] = state[i]
+
         self.control = u_next
 
         return x_next, u_next
 
+    def find_all_waiting_robots(self, waiting, state, x_mpc):
+        """
+        Given a list of robots that have been marked as waiting, find all other robots
+        whose next state intersects with the waiting robots' current state. Those robots
+        should also be marked as waiting. This will need to be resursive since the robots
+        that are marked as waiting will also need to be checked for intersections with others
+    
+
+        params:
+            - waiting (list): list of robots that have already been marked as waiting
+            - x_mpcs (list): list of the next states of all robots
+        """
+
+        # check if the next state of any of the robots intersects with the current state of the waiting robots
+        for i in range(self.num_robots):
+            if i in waiting: continue
+            for j in waiting:
+                if np.linalg.norm(x_mpc[i][0][:2] - state[j][:2]) < 2*self.radius:
+                    waiting.append(i)
+                    self.find_all_waiting_robots(waiting, x_mpc)
+                    break
+
+        return waiting
+
+
+
+
 
 def main():
     import os

guided_mrmp/simulator.py

@@ -48,7 +48,7 @@ class Simulator:
         """
 
         # Get the controls from the policy
-        x_mpc, controls = self.policy.advance(self.state, show_plots=self.settings['simulator']['show_collision_resolution'])
+        x_mpc, controls = self.policy.advance_with_waiting(self.state, show_plots=self.settings['simulator']['show_collision_resolution'])
 
         # # Update the state of each robot
         # for i in range(self.num_robots):