Make sure the correct guide path is getting rerouted with the database solution

guided_mrmp/conflict_resolvers/discrete_resolver.py

@@ -32,15 +32,19 @@ class DiscreteResolver():
     def solve_subproblem(self,s):
         sol = query_library(self.grid,s, self.lib_2x3, self.lib_3x3, self.lib_2x5)
 
-        final_sol = []
+        if sol:
 
-        for r in range(len(self.starts)):
-            this_sol = sol[r]
-            final_this_sol = []
-            for i in range(len(this_sol)):
-                if this_sol[i] != [-1,-1]:
-                    final_this_sol.append(s.get_world_coordinates(this_sol[i][0], this_sol[i][1]))
-            final_sol.append(final_this_sol)
+            final_sol = []
 
-        return final_sol
+            for r in range(len(self.starts)):
+                this_sol = sol[r]
+                final_this_sol = []
+                for i in range(len(this_sol)):
+                    if this_sol[i] != [-1,-1]:
+                        final_this_sol.append(s.get_world_coordinates(this_sol[i][0], this_sol[i][1]))
+                final_sol.append(final_this_sol)
+
+            return final_sol
+        
+        return None
 

guided_mrmp/controllers/multi_path_tracking_db.py

@@ -116,7 +116,8 @@ class MultiPathTrackerDB(MultiPathTracker):
         grid_solution = grid_solver.solve_subproblem(subproblem)
         return grid_solution
     
-    def get_initial_guess(self, state, grid_solution, num_robots, N, cp_dist):
+    def get_initial_guess(self, state, grid_solution, robots_in_conflict, N, cp_dist):
+        num_robots = len(robots_in_conflict)
 
         # turn this solution into an initial guess 
         initial_guess_state = np.zeros((num_robots*3, N+1))
@@ -140,8 +141,8 @@ class MultiPathTrackerDB(MultiPathTracker):
             initial_guess_state[i*3, :] = smoothed_curve[:, 0]     # x
             initial_guess_state[i*3 + 1, :] = smoothed_curve[:, 1]    # y
 
-            current_robot_x_pos = state[i][0]
-            current_robot_y_pos = state[i][1]
+            current_robot_x_pos = state[robots_in_conflict[i]][0]
+            current_robot_y_pos = state[robots_in_conflict[i]][1]
 
             # translate the initial guess so that the first point is at (0,0)
             initial_guess_state[i*3, :] -= initial_guess_state[i*3, 0]
@@ -362,7 +363,7 @@ class MultiPathTrackerDB(MultiPathTracker):
             plt.gca().add_artist(circle)
 
         # for i in range(num_robots):
-        for i, robot_idx in enumerate(range(robots_in_conflict)):
+        for robot_idx, i  in enumerate(robots_in_conflict):
             x = initial_guess[robot_idx*3, :]
             y = initial_guess[robot_idx*3 + 1, :]
             plt.plot(x, y, 'x', color=colors[i])
@@ -459,8 +460,9 @@ class MultiPathTrackerDB(MultiPathTracker):
                 robot_positions.append(state[i][:2])
 
             # get the largest x and y difference between the robots in conflict
-            x_diff = max([abs(robot_positions[i][0] - robot_positions[j][0]) for i in c for j in c])
-            y_diff = max([abs(robot_positions[i][1] - robot_positions[j][1]) for i in c for j in c])
+            print(f"Robots in conflict: {c}")
+            x_diff = max([abs(robot_positions[i][0] - robot_positions[j][0]) for i in range(len(c)) for j in range(len(c))])
+            y_diff = max([abs(robot_positions[i][1] - robot_positions[j][1]) for i in range(len(c)) for j in range(len(c))])
             diff = max(x_diff, y_diff)
 
             
@@ -477,15 +479,13 @@ class MultiPathTrackerDB(MultiPathTracker):
                 # Find a subproblem and solve it
                 grid_solution = self.get_discrete_solution(state, c, all_conflicts, grid_obstacle_map, grid_origin)
 
-            
-
                 if grid_solution:
                     print(f"Grid Solution: {grid_solution}")
                     # if we found a grid solution, we can use it to reroute the robots
-                    initial_guess = self.get_initial_guess(state, grid_solution, len(c), 20, 0.0)
+                    initial_guess = self.get_initial_guess(state, grid_solution, c, 20, 0.0)
                     initial_guess_state = initial_guess['X']
 
-                    if show_plots: self.draw_grid_solution(initial_guess_state, state, grid_origin, len(c))
+                    if show_plots: self.draw_grid_solution(initial_guess_state, state, grid_origin, c)
                     
                     # for each robot in conflict, reroute its reference trajectory to match the grid solution
                     import copy
@@ -493,7 +493,7 @@ class MultiPathTrackerDB(MultiPathTracker):
 
                     # self.paths = []
                     # for i in range(num_robots_in_conflict):
-                    for i, robot_idx in enumerate(c):
+                    for robot_idx, i in enumerate(c):
                         new_ref = initial_guess_state[robot_idx*3:robot_idx*3+3, :]
 
                         # plan from the last point of the ref path to the robot's goal
@@ -568,13 +568,6 @@ class MultiPathTrackerDB(MultiPathTracker):
                 print("Proceeding with the current paths")
 
 
-
-        
-        # only the first one is used to advance the simulation
-        # self.control = []
-        # for i in range(self.num_robots):
-        #     self.control.append([u_mpc[i*2, 0], u_mpc[i*2+1, 0]])
-
         self.control = u_next
 
         return x_next, u_next