Enforces unique grid indices

guided_mrmp/controllers/place_grid.py

@@ -42,40 +42,38 @@ def place_grid(robot_locations, cell_size=1, grid_shape=(5, 5)):
         constraints.append(grid_indices[:,1] <= grid_shape[0] - 1)
     
     # No two robots can share a cell
-    # Reformulation of the constraints
-    #   abs(grid_indices[i, 0] - grid_indices[j, 0]) >= 1 and
-    #   abs(grid_indices[i, 1] - grid_indices[j, 1]) >= 1
-    # to be compatible with solver
+    # Use Big M method to ensure separation in at least one direction
+    M = max(grid_shape) * 10
     for i in range(N):
         for j in range(i+1, N):
-            # Auxiliary variable for the distance between cell centers i and j in the x direction
-            xdist = cp.Variable(name=f"xdist_{i}_{j}")  
-            constraints.append(xdist >= grid_indices[i, 0] - grid_indices[j, 0])
-            constraints.append(xdist >= -(grid_indices[i, 0] - grid_indices[j, 0]))
+            # At least one of the two constraints below must be true
+            y1 = cp.Variable(boolean=True)
+            y2 = cp.Variable(boolean=True)
+            constraints.append(y1 + y2 >= 1)
             
-            # Auxiliary variable for the distance between cell centers i and j in the y direction
-            ydist = cp.Variable(name=f"ydist_{i}_{j}")  
-            constraints.append(ydist >= grid_indices[i, 1] - grid_indices[j, 1])
-            constraints.append(ydist >= -(grid_indices[i, 1] - grid_indices[j, 1]))
+            # Enforces separation by at least 1 in the x direction
+            if robot_locations[i, 0] >= robot_locations[j, 0]:
+                constraints.append(grid_indices[i, 0] - grid_indices[j, 0] + M * (1 - y1) >= 1)
+            else:
+                constraints.append(grid_indices[j, 0] - grid_indices[i, 0] + M * (1 - y1) >= 1)
             
-            # Enforce that robots must be at least one cell apart
-            constraints.append(xdist + ydist >= cell_size) 
+            # Enforces separation by at least 1 in the y direction
+            if robot_locations[i, 1] >= robot_locations[j, 1]:
+                constraints.append(grid_indices[i, 1] - grid_indices[j, 1] + M * (1 - y2) >= 1)
+            else:
+                constraints.append(grid_indices[j, 1] - grid_indices[i, 1] + M * (1 - y2) >= 1)
     
     # Solve the optimization problem
     prob = cp.Problem(cp.Minimize(cost), constraints)
-    prob.solve(solver=cp.SCIP, scip_params={
-        "numerics/feastol": 1e-6, 
-        "numerics/dualfeastol": 1e-6,
-    })
+    prob.solve(solver=cp.SCIP)
 
     if prob.status not in ["optimal", "optimal_inaccurate"]:
-        print("problem could not be solved to optimality")
+        print("Problem could not be solved to optimality.")
         return None
-    print(prob.status)
     return origin.value, cell_centers.value
         
 def main():
-    robot_locations = [(1.2, 1.6), (1.6, 1.2)]
+    robot_locations = np.random.uniform(low=0, high=5, size=(3, 2))
     cell_size = 1
     grid_shape = (5, 5)