Skip to content
Snippets Groups Projects
Commit 5e4af514 authored by Adam Sitabkhan's avatar Adam Sitabkhan
Browse files

Centering grid added to cost

parent 9640a694
No related branches found
No related tags found
No related merge requests found
......@@ -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,
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment