Integrating the new place grid method

README.md

+# To run the code
+
+1. Specify problem in a settings file in directory settings_files
+2. Run main.py <path_to_your_settings_file>
\ No newline at end of file

experiments/environments/narrow_passage.yaml

+circle_obstacles: []
+rectangle_obstacles: [[[0,6], [10,10]], [[0,0], [10,4]]]
+x_range: [0, 10]
+y_range: [0, 10]
\ No newline at end of file

guided_mrmp/conflict_resolvers/discrete_resolver.py

@@ -3,7 +3,6 @@ DBResolver solves a discretized version of the conflict resolution problem.
 """
 
 from guided_mrmp.conflict_resolvers.subproblems import SubproblemPlacer
-from .query import QueryDatabase
 from guided_mrmp.conflict_resolvers.subproblems.subproblem_og import Subproblem, find_subproblem, query_library
 
 class DiscreteResolver():

guided_mrmp/controllers/multi_path_tracking.py

@@ -45,9 +45,10 @@ class MultiPathTracker:
 
         self.settings = settings
 
-        self.mpc = MultiMPC(self.num_robots, dynamics, T, DT, settings, settings['environment']['circle_obstacles'])
+        self.mpc = MultiMPC(self.num_robots, dynamics, T, DT, settings, env.circle_obs)
 
-        self.circle_obs = settings['environment']['circle_obstacles']
+        self.circle_obs = env.circle_obs
+        self.rect_obs = env.rect_obs
 
         # Path from waypoint interpolation
         self.paths = []
@@ -83,9 +84,8 @@ class MultiPathTracker:
             bool: True if trajectories overlap, False otherwise.
         """
         for i in range(traj1.shape[1]):
-            for j in range(traj2.shape[1]):
-                if np.linalg.norm(traj1[0:2, i] - traj2[0:2, j]) < 2*threshold:
-                    return True
+            if np.linalg.norm(traj1[0:2, i] - traj2[0:2, i]) < 2*threshold:
+                return True
         return False
     
     def ego_to_global_roomba(self, state, mpc_out):

guided_mrmp/controllers/multi_path_tracking_db.py

@@ -13,6 +13,8 @@ from guided_mrmp.conflict_resolvers.curve_path import smooth_path, calculate_hea
 
 from guided_mrmp.utils.helpers import initialize_libraries
 
+from guided_mrmp.controllers.place_grid import place_grid
+
 class DiscreteRobot:
     def __init__(self, start, goal, label):
         self.start = start
@@ -51,7 +53,7 @@ class MultiPathTrackerDB(MultiPathTracker):
         for r in range(self.num_robots):
             x, y, theta = state[r]
             cell_x = min(max(math.floor((x - grid_origin[0]) / self.cell_size), 0), self.grid_size - 1)
-            cell_y = min(max(math.floor((grid_origin[1] - y) / self.cell_size), 0), self.grid_size - 1)
+            cell_y = min(max(math.floor((y- grid_origin[1]) / self.cell_size), 0), self.grid_size - 1)
             temp_starts.append([cell_x, cell_y])
 
 
@@ -62,7 +64,7 @@ class MultiPathTrackerDB(MultiPathTracker):
             x = traj[0][-1]
             y = traj[1][-1]
             cell_x = min(max(math.floor((x - grid_origin[0]) / self.cell_size), 0), self.grid_size - 1)
-            cell_y = min(max(math.floor((grid_origin[1] - y) / self.cell_size), 0), self.grid_size - 1)
+            cell_y = min(max(math.floor((y- grid_origin[1]) / self.cell_size), 0), self.grid_size - 1)
             temp_goals.append([cell_x,cell_y])
 
         return temp_starts, temp_goals
@@ -115,11 +117,9 @@ class MultiPathTrackerDB(MultiPathTracker):
         # the initial guess for time is the length of the longest path in the solution
         initial_guess_T = 2*max([len(grid_solution[i]) for i in range(num_robots)])
 
-        # reverse the order of grid solution
-        grid_solution = grid_solution[::-1]
-
         for i in range(num_robots):
 
+
             rough_points = np.array(grid_solution[i])
             points = []
             for point in rough_points:
@@ -176,67 +176,67 @@ 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)
+    # 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
+    #     return grid, grid_origin
     
     def get_obstacle_map(self, grid_origin):
         """
@@ -266,7 +266,7 @@ class MultiPathTrackerDB(MultiPathTracker):
 
         # find the closest grid cell that is not an obstacle
         cell_x = min(max(math.floor((x - grid_origin[0]) / self.cell_size), 0), self.grid_size - 1)
-        cell_y = min(max(math.floor((grid_origin[1] - y) / self.cell_size), 0), self.grid_size - 1)
+        cell_y = min(max(math.floor((y - grid_origin[1]) / self.cell_size), 0), self.grid_size - 1)
 
         return cell_x, cell_y
     
@@ -275,7 +275,7 @@ class MultiPathTrackerDB(MultiPathTracker):
         Given a cell in the grid, find the center of that cell in continuous space
         """
         x = grid_origin[0] + (cell_x + 0.5) * self.cell_size
-        y = grid_origin[1] - (cell_y + 0.5) * self.cell_size
+        y = grid_origin[1] + (cell_y + 0.5) * self.cell_size
         return x, y
   
     
@@ -318,7 +318,7 @@ class MultiPathTrackerDB(MultiPathTracker):
 
         plt.show()
 
-    def draw_grid(self, state, grid_origin):
+    def draw_grid(self, state, grid_origin, cell_size, grid_size):
         """
         inputs:
             - state (list): list of robot states
@@ -344,13 +344,13 @@ class MultiPathTrackerDB(MultiPathTracker):
             plt.gca().add_artist(circle1)
 
         # draw the horizontal and vertical lines of the grid
-        for i in range(self.grid_size + 1):
+        for i in range(grid_size + 1):
             # Draw vertical lines
-            plt.plot([grid_origin[0] + i * self.cell_size, grid_origin[0] + i * self.cell_size], 
-                        [grid_origin[1], grid_origin[1] - self.grid_size * self.cell_size], 'k-')
+            plt.plot([grid_origin[0] + i * cell_size, grid_origin[0] + i * cell_size], 
+                        [grid_origin[1], grid_origin[1] + grid_size * cell_size], 'k-')
             # Draw horizontal lines
-            plt.plot([grid_origin[0], grid_origin[0] + self.grid_size * self.cell_size], 
-                        [grid_origin[1] - i * self.cell_size, grid_origin[1] - i * self.cell_size], 'k-')
+            plt.plot([grid_origin[0], grid_origin[0] + grid_size * cell_size], 
+                        [grid_origin[1] + i * cell_size, grid_origin[1] + i * cell_size], 'k-')
 
         # draw the obstacles
         for obs in self.circle_obs:
@@ -401,10 +401,10 @@ class MultiPathTrackerDB(MultiPathTracker):
         for i in range(self.grid_size + 1):
             # Draw vertical lines
             plt.plot([grid_origin[0] + i * self.cell_size, grid_origin[0] + i * self.cell_size], 
-                        [grid_origin[1], grid_origin[1] - self.grid_size * self.cell_size], 'k-')
+                        [grid_origin[1], grid_origin[1] + self.grid_size * self.cell_size], 'k-')
             # Draw horizontal lines
             plt.plot([grid_origin[0], grid_origin[0] + self.grid_size * self.cell_size], 
-                        [grid_origin[1] - i * self.cell_size, grid_origin[1] - i * self.cell_size], 'k-')
+                        [grid_origin[1] +  i * self.cell_size, grid_origin[1] + i * self.cell_size], 'k-')
 
         # draw the obstacles
         for obs in self.circle_obs:
@@ -489,30 +489,26 @@ class MultiPathTrackerDB(MultiPathTracker):
 
             # Get the robots involved in the conflict
             robots = c
-            robot_positions = [state[i] for i in robots]
-
-            # Put down a local grid
-            grid_obstacle_map, grid_origin = self.place_grid(state, robot_positions)
-            if show_plots: self.draw_grid(state, grid_origin)
-
-            # set the starts (robots' current positions) 
-            self.starts = []
-            self.goals = []
+            robot_positions = []
             for i in range(self.num_robots):
-                self.starts.append(state[i])
+                robot_positions.append(state[i][:2])
 
-                traj = self.ego_to_global_roomba(state[i], self.trajs[i])
-                x = traj[0][-1]
-                y = traj[1][-1]
-                self.goals.append([x,y])
+            # 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)
+            if show_plots: self.draw_grid(state, grid_origin, self.cell_size, 5)
 
             # Solve a discrete version of the problem 
             # Find a subproblem and solve it
             grid_solution = self.get_discrete_solution(state, c, all_conflicts, grid_obstacle_map, grid_origin)
 
+            
+
             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, 1)
+                initial_guess = self.get_initial_guess(state, grid_solution, self.num_robots, 20, .5)
                 initial_guess_state = initial_guess['X']
 
                 if show_plots: self.draw_grid_solution(initial_guess_state, state, grid_origin)

guided_mrmp/main.py

@@ -92,22 +92,26 @@ if __name__ == "__main__":
     
     # get the name of the settings file from the command line
     import sys
-    if len(sys.argv) < 2:
+    if len(sys.argv) < 3:
         print("Using default settings file")
         settings_file = "settings_files/settings.yaml"
+        environment_file = "settings_files/env.yaml"
 
-    else: settings_file = sys.argv[1]
+    else: 
+        settings_file = sys.argv[1]
+        environment_file = sys.argv[2]
 
     # Load the settings
     settings = load_settings(settings_file)
+    environment = load_settings(environment_file)
 
     set_python_seed(1123)
 
     # Load and create the environment
-    circle_obstacles = settings['environment']['circle_obstacles']
-    rectangle_obstacles = settings['environment']['rectangle_obstacles']
-    x_range = settings['environment']['x_range']
-    y_range = settings['environment']['y_range']
+    circle_obstacles = environment['circle_obstacles']
+    rectangle_obstacles = environment['rectangle_obstacles']
+    x_range = environment['x_range']
+    y_range = environment['y_range']
     env = Env(x_range, y_range, circle_obstacles, rectangle_obstacles)
 
     # Load the dynamics models

settings_files/env.yaml

+circle_obstacles: []
+rectangle_obstacles: []
+x_range: [0, 10]
+y_range: [0, 10]
\ No newline at end of file

settings_files/settings.yaml

@@ -28,19 +28,13 @@ simulator:
   show_plots: 1
   show_collision_resolution: 1
 
-environment:
-  circle_obstacles: []
-  rectangle_obstacles: []
-  x_range: [0, 10]
-  y_range: [0, 10]
-
 robot_starts: 
   - [6.0, 2.0, 4.8]
-  - [7.0, 8.0, 2.0]
+  - [6.0, 8.0, 2.0]
 
 robot_goals: 
-  - [6.5, 8.0, 0]
-  - [7.5, 2.0, 0]
+  - [6, 8.0, 0]
+  - [6, 2.0, 0]
 
 robot_radii:
   - .5