Check against global x_mpc what finding waiting robots

guided_mrmp/controllers/multi_path_tracking_db.py

@@ -427,8 +427,9 @@ class MultiPathTrackerDB(MultiPathTracker):
                 self.control[i]
             )
 
+            x_mpc_global = self.ego_to_global_roomba(state[i], x_mpc)
             u_next[i] = [u_mpc[0, 0], u_mpc[1, 0]]
-            x_next[i] = x_mpc
+            x_next[i] = x_mpc_global
 
         self.trajs = targets 
         return x_next, u_next
@@ -584,7 +585,9 @@ class MultiPathTrackerDB(MultiPathTracker):
                         )
 
                         u_next[i] = [u_mpc[0, 0], u_mpc[1, 0]]
-                        x_next[i] = x_mpc
+
+                        x_mpc_global = self.ego_to_global_roomba(state[i], x_mpc)
+                        x_next[i] = x_mpc_global
 
                         self.trajs[i] = ref
 
@@ -595,18 +598,19 @@ class MultiPathTrackerDB(MultiPathTracker):
                         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))
-                    x_mpc, u_mpc = self.coupled_mpc.step(
-                        curr_states,
-                        self.trajs,
-                        self.control
-                    )
 
-                    for i in c:
-                        u_next[i] = [u_mpc[i*2, 0], u_mpc[i*2+1, 0]]
-                        x_next[i] = x_mpc[i]
+                    else:
+                        # dynamycs w.r.t robot frame
+                        curr_states = np.zeros((self.num_robots, 3))
+                        x_mpc, u_mpc = self.coupled_mpc.step(
+                            curr_states,
+                            self.trajs,
+                            self.control
+                        )
 
+                        for i in c:
+                            u_next[i] = [u_mpc[i*2, 0], u_mpc[i*2+1, 0]]
+                            x_next[i] = [x_mpc[i*3, 1], x_mpc[i*3+1, 1], x_mpc[i*3+2, 1]]
             else:
                 print("The robots are too far apart to place a grid")
                 print("Proceeding with the current paths")
@@ -639,7 +643,9 @@ class MultiPathTrackerDB(MultiPathTracker):
         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:
+                print(f"x_mpc[i] = {x_mpc[i]}")
+                i_state = [x_mpc[i][0][1], x_mpc[i][1][1]]
+                if np.linalg.norm(i_state - state[j][:2]) < 2*self.radius:
                     waiting.append(i)
                     self.find_all_waiting_robots(waiting, x_mpc)
                     break