From 9316ffd6c8e2bcccd8c1c20d93a2b87a0123f5d8 Mon Sep 17 00:00:00 2001
From: Adam Sitabkhan <adam.sitabkhan@gmail.com>
Date: Fri, 10 Jan 2025 20:07:21 -0600
Subject: [PATCH] Enforces unique grid indices
---
guided_mrmp/controllers/place_grid.py | 40 +++++++++++++--------------
1 file changed, 19 insertions(+), 21 deletions(-)
diff --git a/guided_mrmp/controllers/place_grid.py b/guided_mrmp/controllers/place_grid.py
index e66f048..dae6c5b 100644
--- a/guided_mrmp/controllers/place_grid.py
+++ b/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)
--
GitLab