Centering grid added to cost

guided_mrmp/controllers/place_grid.py

@@ -44,6 +44,11 @@ def place_grid(robot_locations, cell_size, grid_size=5, subgoals=[], obstacles=[
         cost = cp.sum_squares(robot_locations - cell_centers) + alpha * cp.sum(overlaps)
     else:
         cost = cp.sum_squares(robot_locations - cell_centers)
+        
+    # Add cost for robots center distance from grid center
+    robots_center = robot_locations.sum(axis=0) / len(robot_locations)
+    grid_center = bottom_left + (grid_size / 2) * np.array([cell_size, cell_size])
+    cost += 10 * cp.sum_squares(robots_center - grid_center)
     
     # Constraints
     constraints = []
@@ -196,6 +201,11 @@ def two_corner_place_grid(robot_locations, grid_size=5, subgoals=[], obstacles=[
     # Objective: Minimize the sum of squared distances
     cost = cp.sum_squares(robot_locations - cell_centers)
     
+    # Add c
+    grid_center = bottom_left + (grid_size / 2) * (grid_x_hat + grid_y_hat)
+    robots_center = robot_locations.sum(axis=1) / len(robot_locations)
+    cost += 100 * cp.sum_squares(robots_center - grid_center)
+    
     # Constraints
     constraints = []
     
@@ -369,16 +379,19 @@ def main(seed, num_robots):
     
     roomba_radius = 0.5
     cell_size = 2.5 * roomba_radius
-    grid_size = 5
+    grid_size = 6
     circ_obstacle_radii = [0.8]
     rect_obstacle_wh = [(2, 1.5), (1.1, 1.1)]
     
-    robot_locations, subgoals, obstacles = get_locations(low=0, 
-                                                            high=6, 
-                                                            num_robots=num_robots, 
-                                                            robot_radius=roomba_radius,
-                                                            circ_obstacle_radii=circ_obstacle_radii,
-                                                            rect_obstacle_wh=rect_obstacle_wh)
+    # robot_locations, subgoals, obstacles = get_locations(low=0, 
+    #                                                         high=6, 
+    #                                                         num_robots=num_robots, 
+    #                                                         robot_radius=roomba_radius,
+    #                                                         circ_obstacle_radii=circ_obstacle_radii,
+    #                                                         rect_obstacle_wh=rect_obstacle_wh)
+    robot_locations = [[4.60865128, 6.99409625], [4.16768821, 3.66348758]]
+    obstacles = [('c', 9, 4.5, 0.4), ('c', 8, 4.5, 0.4), ('c', 7, 4.5, 0.4), ('c', 6.5, 4.5, 0.4), ('c', 1, 4.5, 0.4), ('c', 2, 4.5, 0.4), ('c', 3, 4.5, 0.4), ('c', 4, 4.5, 0.4)]
+    subgoals = [[4.60865128, 6.99409625]] * 2
 
     bottom_left, cell_centers = place_grid(robot_locations=robot_locations, 
                                             cell_size=cell_size,