Fixed a bug: ALL robots were being passed into the discrete solver. Now only...

Fixed a bug: ALL robots were being passed into the discrete solver. Now only robots in conflict are passed in

guided_mrmp/controllers/multi_path_tracking_db.py

@@ -88,6 +88,12 @@ class MultiPathTrackerDB(MultiPathTracker):
 
         starts, goals = self.get_temp_starts_and_goals(state, grid_origin)
 
+        # get the starts and goals of ONLY the robots in conflict
+        conflict_starts = [starts[c] for c in conflict]
+        conflict_goals = [goals[c] for c in conflict]
+
+        # create a "discrete" version of the robot, i.e. the 
+        # format that the discrete solver expects to receive 
         disc_robots = self.create_discrete_robots(starts, goals)
 
         disc_conflict = []
@@ -101,7 +107,7 @@ class MultiPathTrackerDB(MultiPathTracker):
                 this_conflict.append(disc_robots[i])
             disc_all_conflicts.append(this_conflict)
 
-        grid_solver = DiscreteResolver(disc_conflict, disc_robots, starts, goals, disc_all_conflicts,grid, self.lib_2x3, self.lib_3x3, self.lib_2x5)
+        grid_solver = DiscreteResolver(disc_conflict, disc_robots, conflict_starts, conflict_goals, disc_all_conflicts,grid, self.lib_2x3, self.lib_3x3, self.lib_2x5)
         subproblem = grid_solver.find_subproblem()
 
         if subproblem is None:
@@ -176,83 +182,21 @@ class MultiPathTrackerDB(MultiPathTracker):
 
         return {'X': initial_guess_state, 'U': initial_guess_control, 'T': initial_guess_T}
 
-    # def place_grid(self, state, robot_locations):
-    #     """
-    #     Given the locations of robots that need to be covered in continuous space, find 
-    #     and place the grid. We don't need a very large grid to place subproblems, so 
-    #     we will only place a grid of size self.grid_size x self.grid_size
-
-    #     inputs:
-    #         - robot_locations (list): locations of robots involved in conflict [[x,y], [x,y], ...]
-    #     outputs:
-    #         - grid (numpy array): The grid that we placed
-    #         - top_left (tuple): The top left corner of the grid in continuous space
-    #     """
-    #     # Find the minimum and maximum x and y coordinates of the robot locations
-    #     self.min_x = min(robot_locations, key=lambda loc: loc[0])[0]
-    #     self.max_x = max(robot_locations, key=lambda loc: loc[0])[0]
-    #     self.min_y = min(robot_locations, key=lambda loc: loc[1])[1]
-    #     self.max_y = max(robot_locations, key=lambda loc: loc[1])[1]
-
-    #     # find the average x and y coordinates of the robot locations
-    #     avg_x = sum([loc[0] for loc in robot_locations]) / len(robot_locations)
-    #     avg_y = sum([loc[1] for loc in robot_locations]) / len(robot_locations)
-
-    #     self.temp_avg_x = avg_x 
-    #     self.temp_avg_y = avg_y
-
-    #     # Calculate the top left corner of the grid
-    #     # make it so that the grid is centered around the robots
-    #     self.cell_size = self.radius*3
-    #     self.grid_size = 5
-    #     grid_origin = (avg_x - int(self.grid_size*self.cell_size/2), avg_y + int(self.grid_size*self.cell_size/2))
-
-    #     # Check if, for every robot, the cell value of the start and the cell value 
-    #     # of the goal are the same. If they are, then we can't find a discrete solution that 
-    #     # will make progress.
-    #     starts_equal = self.starts_equal(state, grid_origin)
-
-    #     import copy
-    #     original_top_left = copy.deepcopy(grid_origin)
-
-    #     x_shift = [-5,5]
-    #     y_shift = [-5,5]
-    #     for x in np.arange(x_shift[0], x_shift[1],.5):
-    #         y =0 
-    #         grid_origin = (original_top_left[0] + x*self.cell_size*.5, original_top_left[1] - y*self.cell_size*.5)
-    #         starts_equal = self.starts_equal(state, grid_origin)
-    #         if not starts_equal: break
-
-    #     if starts_equal:
-    #         for y in np.arange(y_shift[0], y_shift[1],.5):
-    #             x =0 
-    #             grid_origin = (original_top_left[0] + x*self.cell_size*.5, original_top_left[1] - y*self.cell_size*.5)
-    #             starts_equal = self.starts_equal(state, grid_origin)
-    #             if not starts_equal: break
-
-    #     if starts_equal:
-    #         print("Some robots are in the same cell")
-    #         return None
-
-    #     grid = self.get_obstacle_map(grid_origin)
-        
-    #     return grid, grid_origin
     
-    def get_obstacle_map(self, grid_origin):
+    def get_obstacle_map(self, grid_origin, grid_size, radius):
         """
         Create a map of the environment with obstacles
         """
         # create a grid of size self.grid_size x self.grid_size
-        grid = np.zeros((self.grid_size, self.grid_size))
+        grid = np.zeros((grid_size, grid_size))
 
         # check if there are any obstacles in any of the cells
-        grid = np.zeros((self.grid_size, self.grid_size)) 
-        for i in range(self.grid_size):
-            for j in range(self.grid_size):
+        grid = np.zeros((grid_size, grid_size)) 
+        for i in range(grid_size):
+            for j in range(grid_size):
                 x, y = self.get_grid_cell_location(i, j, grid_origin)
-                for obs in []:
-                # for obs in self.circle_obs:
-                    if np.linalg.norm(np.array([x, y]) - np.array(obs[:2])) < obs[2] + self.radius:
+                for obs in self.circle_obs:
+                    if np.linalg.norm(np.array([x, y]) - np.array(obs[:2])) < obs[2]:
                         grid[j, i] = 1
                         break
 
@@ -377,7 +321,14 @@ class MultiPathTrackerDB(MultiPathTracker):
 
         plt.show()
 
-    def draw_grid_solution(self, initial_guess, state, grid_origin):
+    def draw_grid_solution(self, initial_guess, state, grid_origin, robots_in_conflict):
+        """
+        params:
+            - initial_guess (dict): the initial guess for the optimization problem
+            - state (list): list of robot states
+            - grid_origin (tuple): top left corner of the grid
+            - num_robots (int): number of robots that are in conflict
+        """
         
         # draw the whole environment with the local grid drawn on top
         import matplotlib.pyplot as plt
@@ -411,14 +362,14 @@ class MultiPathTrackerDB(MultiPathTracker):
             circle = plt.Circle((obs[0], obs[1]), obs[2], color='red', fill=False)
             plt.gca().add_artist(circle)
 
-        for i in range(self.num_robots):
-            x = initial_guess[i*3, :]
-            y = initial_guess[i*3 + 1, :]
+        # for i in range(num_robots):
+        for i, robot_idx in enumerate(range(robots_in_conflict)):
+            x = initial_guess[robot_idx*3, :]
+            y = initial_guess[robot_idx*3 + 1, :]
             plt.plot(x, y, 'x', color=colors[i])
 
         # plot the robots' continuous space subgoals
         for idx in range(self.num_robots):
-        
             traj = self.ego_to_global_roomba(state[idx], self.trajs[idx])
             x = traj[0][-1]
             y = traj[1][-1]
@@ -488,16 +439,15 @@ class MultiPathTrackerDB(MultiPathTracker):
             # 3. If they do collide, then reroute the reference trajectories of these robots
 
             # Get the robots involved in the conflict
-            robots = c
             robot_positions = []
-            for i in range(self.num_robots):
+            for i in (c):
                 robot_positions.append(state[i][:2])
 
             # Put down a local grid
             self.cell_size = self.radius*3
             self.grid_size = 5
             grid_origin, centers = place_grid(robot_positions, cell_size=self.radius*3)
-            grid_obstacle_map = self.get_obstacle_map(grid_origin)
+            grid_obstacle_map = self.get_obstacle_map(grid_origin, self.grid_size, self.radius) 
             if show_plots: self.draw_grid(state, grid_origin, self.cell_size, 5)
 
             # Solve a discrete version of the problem 
@@ -508,18 +458,19 @@ class MultiPathTrackerDB(MultiPathTracker):
 
             if 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, self.num_robots, 20, .5)
+                initial_guess = self.get_initial_guess(state, grid_solution, len(c), 20, .5)
                 initial_guess_state = initial_guess['X']
 
-                if show_plots: self.draw_grid_solution(initial_guess_state, state, grid_origin)
+                if show_plots: self.draw_grid_solution(initial_guess_state, state, grid_origin, len(c))
                 
                 # for each robot in conflict, reroute its reference trajectory to match the grid solution
                 num_robots_in_conflict = len(c)
                 import copy
                 old_paths = copy.deepcopy(self.paths)
 
-                self.paths = []
-                for i in range(num_robots_in_conflict):
+                # self.paths = []
+                # for i in range(num_robots_in_conflict):
+                for i, robot_idx in enumerate(c):
                     new_ref = initial_guess_state[i*3:i*3+3, :]
 
                     # plan from the last point of the ref path to the robot's goal
@@ -541,7 +492,7 @@ class MultiPathTrackerDB(MultiPathTracker):
                     wp = [xs,ys]
 
                     # Path from waypoint interpolation
-                    self.paths.append(compute_path_from_wp(wp[0], wp[1], 0.05))
+                    self.paths[i] = compute_path_from_wp(wp[0], wp[1], 0.05)
 
                 targets = []
                 for i in range(self.num_robots):

settings_files/settings.yaml

@@ -31,16 +31,17 @@ simulator:
 robot_starts: 
   - [6.0, 2.0, 4.8]
   - [6.0, 8.0, 2.0]
+  - [1.0, 1.0, 0.0]
 
 robot_goals: 
   - [6, 8.0, 0]
   - [6, 2.0, 0]
+  - [1.0, 8.0, 0]
 
 robot_radii:
   - .5
   - .5
   - .5
-  - .5
 
 Roomba:
   max_speed: 50.0
@@ -56,7 +57,6 @@ dynamics_models:
   - Roomba
   - Roomba
   - Roomba
-  - Roomba