Constrain robot grid indices to be outside the range of obstacle grid indices

guided_mrmp/controllers/place_grid.py

@@ -38,8 +38,8 @@ def place_grid(robot_locations, cell_size, grid_size=5, subgoals=[], obstacles=[
     # Reshape origin to (1, 2) for broadcasting
     cell_centers = cp.reshape(bottom_left, (1, 2), order='C') + grid_indices * cell_size + cell_size / 2
     
-    if obstacles:
-        obstacle_cells = cp.Variable((num_obst, grid_size, grid_size), boolean=True, name='obstacle_cells')
+    # if obstacles:
+    obstacle_cells = cp.Variable((num_obst, grid_size*grid_size), boolean=True, name='obstacle_cells')
     
     # Objective: Minimize the sum of squared distances
     if obstacles:
@@ -92,60 +92,29 @@ def place_grid(robot_locations, cell_size, grid_size=5, subgoals=[], obstacles=[
         constraints.append(bottom_left <= sg)
         constraints.append(sg <= top_right)
 
-    if obstacles:
-        M = 100
-        for i, (cx, cy, r) in enumerate(obstacles):
-            for x_idx in range(grid_size):
-                for y_idx in range(grid_size):
-                    cell_bot_left = bottom_left + np.array([x_idx, y_idx]) * cell_size # Bottom left point of grid cell at (x_idx, y_idx)
-                    cell_top_right = cell_bot_left + np.ones(2) * cell_size  # Top right point
-                    
-                    # Is the obstacle center within the x and y ranges of the cell
-                    inside_x = cp.Variable(boolean=True)
-                    constraints.append(cx >= cell_bot_left[0] - M * (1 - inside_x))
-                    constraints.append(cx <= cell_top_right[0] + M * (1 - inside_x))
-                    constraints.append(cx <= cell_bot_left[0] + M * inside_x)
-                    constraints.append(cx >= cell_top_right[0] - M * inside_x)
-    
-                    inside_y = cp.Variable(boolean=True)
-                    constraints.append(cy >= cell_bot_left[1] - M * (1 - inside_y))
-                    constraints.append(cy <= cell_top_right[1] + M * (1 - inside_y))
-                    constraints.append(cy <= cell_bot_left[1] + M * inside_y)
-                    constraints.append(cy >= cell_top_right[1] - M * inside_y)
-                    
-                    # Checking if the obstacles lie over the x coordinates of vertical edges, y coords of horizontal edges
-                    intersects_bottom_y = cp.Variable(boolean=True)
-                    intersects_top_y = cp.Variable(boolean=True)
-                    intersects_left_x = cp.Variable(boolean=True)
-                    intersects_right_x = cp.Variable(boolean=True)
-                    constraints.append(cell_bot_left[1] - cy <= r + M * (1 - intersects_bottom_y))
-                    constraints.append(cy - cell_top_right[1] <= r + M * (1 - intersects_top_y))
-                    constraints.append(cell_bot_left[0] - cx <= r + M * (1 - intersects_left_x))
-                    constraints.append(cx - cell_top_right[0] <= r + M * (1 - intersects_right_x))
-                    
-                    # Does the obstacle intersect the cell edges
-                    intersects_bottom_edge = cp.Variable(boolean=True)
-                    intersects_top_edge = cp.Variable(boolean=True)
-                    intersects_left_edge = cp.Variable(boolean=True)
-                    intersects_right_edge = cp.Variable(boolean=True)
-                    constraints.append(intersects_bottom_y + inside_x >= 2 - 3 * (1 - intersects_bottom_edge))
-                    constraints.append(intersects_top_y + inside_x >= 2 - 3 * (1 - intersects_top_edge))
-                    constraints.append(intersects_left_x + inside_y >= 2 - 3 * (1 - intersects_left_edge))
-                    constraints.append(intersects_right_x + inside_y >= 2 - 3 * (1 - intersects_right_edge))
-                    
-                    inside_x_and_y_ranges = cp.Variable(boolean=True)
-                    constraints.append(inside_x + inside_y >= 2 - 3 * (1 - inside_x_and_y_ranges))
-                    
-                    circle_intersects_edge = cp.Variable(boolean=True)
-                    constraints.append(intersects_bottom_edge + intersects_top_edge + intersects_left_edge + intersects_right_edge >= 1 - circle_intersects_edge)
-                    
-                    # Obstacles intersects with the cell if 
-                    #   the center is within the cell OR
-                    #   the obstacles intersects a cell edge
-                    constraints.append(obstacle_cells[i,x_idx,y_idx] >= inside_x_and_y_ranges)
-                    constraints.append(obstacle_cells[i,x_idx,y_idx] >= circle_intersects_edge)
-                    constraints.append(obstacle_cells[i,x_idx,y_idx] <= inside_x_and_y_ranges + circle_intersects_edge)
-    
+    for cx, cy, r in obstacles:
+        for i in range(num_robots):
+            # Define temp binary variables for each condition
+            temp_x_min = cp.Variable(boolean=True)
+            temp_x_max = cp.Variable(boolean=True)
+            temp_y_min = cp.Variable(boolean=True)
+            temp_y_max = cp.Variable(boolean=True)
+
+            # Define the obstacle's bounds in grid coordinates
+            x_min = (cx - r - bottom_left[0]) / cell_size
+            x_max = (cx + r - bottom_left[0]) / cell_size
+            y_min = (cy - r - bottom_left[1]) / cell_size
+            y_max = (cy + r - bottom_left[1]) / cell_size
+
+            # Enforce that robots cannot occupy cells overlapping with obstacles
+            constraints.append(grid_indices[i, 0] <= x_min + M_ind * temp_x_min)
+            constraints.append(grid_indices[i, 0] >= x_max - M_ind * (1 - temp_x_max))
+            constraints.append(grid_indices[i, 1] <= y_min + M_ind * temp_y_min)
+            constraints.append(grid_indices[i, 1] >= y_max - M_ind * (1 - temp_y_max))
+
+            # constraints.append(temp_x_min + temp_x_max + temp_y_min + temp_y_max <= 1)
+                      
+
     # Solve the optimization problem
     prob_init_start_time = time.time()
     prob = cp.Problem(cp.Minimize(cost), constraints)
@@ -346,6 +315,8 @@ def get_roomba_locs(low, high, num_robots, radius=0.5, obstacles=[]):
 def main(seed, num_robots, plot, two_corner):
     if seed is not None:
         np.random.seed(seed)
+
+    np.random.seed(11235)
     
     if not two_corner:
         roomba_radius = 0.5