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

Fixed error with zero obstacles, main.py works

parent 73b4e600
No related branches found
No related tags found
1 merge request!2Updated place grid
......@@ -16,8 +16,6 @@ def place_grid(robot_locations, cell_size, grid_size=5, subgoals=[], obstacles=[
- origin (tuple): bottom-left corner of the grid in continuous space
- cell_centers (list): centers of grid cells for each robot (same order as robot_locations)
"""
start_time = time.time()
robot_locations = np.array(robot_locations)
subgoals = np.array(subgoals)
num_robots = len(robot_locations)
......@@ -37,11 +35,15 @@ def place_grid(robot_locations, cell_size, grid_size=5, subgoals=[], obstacles=[
cell_centers = cp.reshape(bottom_left, (1, 2), order='C') + grid_indices * cell_size + cell_size / 2
# Existence of overlap between each obstacle and each robot
overlaps = cp.Variable((num_obst, num_robots), boolean=True)
if num_obst > 0:
overlaps = cp.Variable((num_obst, num_robots), boolean=True)
# Objective: Minimize the sum of squared distances and number of robot cell / obstacle overlaps
alpha = 1
cost = cp.sum_squares(robot_locations - cell_centers) + alpha * cp.sum(overlaps)
if num_obst > 0:
alpha = 1
cost = cp.sum_squares(robot_locations - cell_centers) + alpha * cp.sum(overlaps)
else:
cost = cp.sum_squares(robot_locations - cell_centers)
# Constraints
constraints = []
......@@ -131,23 +133,18 @@ def place_grid(robot_locations, cell_size, grid_size=5, subgoals=[], obstacles=[
constraints.append(overlaps[obst_idx, i] >= temp_x_sep + temp_y_sep - 1)
# Solve the optimization problem
prob_init_start_time = time.time()
prob = cp.Problem(cp.Minimize(cost), constraints)
solve_start_time = time.time()
prob.solve(solver=cp.SCIP)
solve_end_time = time.time()
print("Time to add vars/constraints:", prob_init_start_time - start_time)
print("Time to parse:", solve_start_time - prob_init_start_time)
print("Time to solve:", solve_end_time - solve_start_time)
print("Solve time:", solve_end_time - solve_start_time)
if prob.status != "optimal":
print("Problem could not be solved to optimality.")
return None
print(f"Number of obstacle/robot-cell overlaps: {int(np.sum(overlaps.value))}")
print(f"Cost: {cost.value}")
return bottom_left.value, cell_centers.value
......
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