Minor changes to plotting, random seeding

guided_mrmp/controllers/place_grid.py

@@ -42,7 +42,7 @@ 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
-    # Use Big M method to ensure separation in at least one direction
+    # Use Big M method to ensure unique grid indices
     M = max(grid_shape) * 10
     for i in range(N):
         for j in range(i+1, N):
@@ -73,7 +73,8 @@ def place_grid(robot_locations, cell_size=1, grid_shape=(5, 5)):
     return origin.value, cell_centers.value
         
 def main():
-    robot_locations = np.random.uniform(low=0, high=5, size=(3, 2))
+    np.random.seed(54)
+    robot_locations = np.random.uniform(low=0, high=5, size=(5, 2))
     cell_size = 1
     grid_shape = (5, 5)
     
@@ -101,7 +102,12 @@ def main():
     # Plot cell centers
     cell_centers = np.array(cell_centers)
     plt.scatter(cell_centers[:, 0], cell_centers[:, 1], c='b', label='Cell Centers')
-    plt.legend()
+    for (cx, cy) in cell_centers:
+        x = [cx - cell_size/2, cx + cell_size/2, cx + cell_size/2, cx - cell_size/2, cx - cell_size/2]
+        y = [cy - cell_size/2, cy - cell_size/2, cy + cell_size/2, cy + cell_size/2, cy - cell_size/2]
+        plt.plot(x, y, c='r')
+    
+    plt.legend(loc='upper left')
 
     plt.show()