Skip to content
Snippets Groups Projects
Commit 90516c23 authored by rachelmoan's avatar rachelmoan
Browse files

Constrain robot grid indices to be outside the range of obstacle grid indices

parent bff423ae
No related branches found
No related tags found
1 merge request!2Updated place grid
......@@ -38,8 +38,8 @@ def place_grid(robot_locations, cell_size, grid_size=5, subgoals=[], obstacles=[
# Reshape origin to (1, 2) for broadcasting
cell_centers = cp.reshape(bottom_left, (1, 2), order='C') + grid_indices * cell_size + cell_size / 2
if obstacles:
obstacle_cells = cp.Variable((num_obst, grid_size, grid_size), boolean=True, name='obstacle_cells')
# if obstacles:
obstacle_cells = cp.Variable((num_obst, grid_size*grid_size), boolean=True, name='obstacle_cells')
# Objective: Minimize the sum of squared distances
if obstacles:
......@@ -92,60 +92,29 @@ def place_grid(robot_locations, cell_size, grid_size=5, subgoals=[], obstacles=[
constraints.append(bottom_left <= sg)
constraints.append(sg <= top_right)
if obstacles:
M = 100
for i, (cx, cy, r) in enumerate(obstacles):
for x_idx in range(grid_size):
for y_idx in range(grid_size):
cell_bot_left = bottom_left + np.array([x_idx, y_idx]) * cell_size # Bottom left point of grid cell at (x_idx, y_idx)
cell_top_right = cell_bot_left + np.ones(2) * cell_size # Top right point
# Is the obstacle center within the x and y ranges of the cell
inside_x = cp.Variable(boolean=True)
constraints.append(cx >= cell_bot_left[0] - M * (1 - inside_x))
constraints.append(cx <= cell_top_right[0] + M * (1 - inside_x))
constraints.append(cx <= cell_bot_left[0] + M * inside_x)
constraints.append(cx >= cell_top_right[0] - M * inside_x)
inside_y = cp.Variable(boolean=True)
constraints.append(cy >= cell_bot_left[1] - M * (1 - inside_y))
constraints.append(cy <= cell_top_right[1] + M * (1 - inside_y))
constraints.append(cy <= cell_bot_left[1] + M * inside_y)
constraints.append(cy >= cell_top_right[1] - M * inside_y)
# Checking if the obstacles lie over the x coordinates of vertical edges, y coords of horizontal edges
intersects_bottom_y = cp.Variable(boolean=True)
intersects_top_y = cp.Variable(boolean=True)
intersects_left_x = cp.Variable(boolean=True)
intersects_right_x = cp.Variable(boolean=True)
constraints.append(cell_bot_left[1] - cy <= r + M * (1 - intersects_bottom_y))
constraints.append(cy - cell_top_right[1] <= r + M * (1 - intersects_top_y))
constraints.append(cell_bot_left[0] - cx <= r + M * (1 - intersects_left_x))
constraints.append(cx - cell_top_right[0] <= r + M * (1 - intersects_right_x))
# Does the obstacle intersect the cell edges
intersects_bottom_edge = cp.Variable(boolean=True)
intersects_top_edge = cp.Variable(boolean=True)
intersects_left_edge = cp.Variable(boolean=True)
intersects_right_edge = cp.Variable(boolean=True)
constraints.append(intersects_bottom_y + inside_x >= 2 - 3 * (1 - intersects_bottom_edge))
constraints.append(intersects_top_y + inside_x >= 2 - 3 * (1 - intersects_top_edge))
constraints.append(intersects_left_x + inside_y >= 2 - 3 * (1 - intersects_left_edge))
constraints.append(intersects_right_x + inside_y >= 2 - 3 * (1 - intersects_right_edge))
inside_x_and_y_ranges = cp.Variable(boolean=True)
constraints.append(inside_x + inside_y >= 2 - 3 * (1 - inside_x_and_y_ranges))
circle_intersects_edge = cp.Variable(boolean=True)
constraints.append(intersects_bottom_edge + intersects_top_edge + intersects_left_edge + intersects_right_edge >= 1 - circle_intersects_edge)
# Obstacles intersects with the cell if
# the center is within the cell OR
# the obstacles intersects a cell edge
constraints.append(obstacle_cells[i,x_idx,y_idx] >= inside_x_and_y_ranges)
constraints.append(obstacle_cells[i,x_idx,y_idx] >= circle_intersects_edge)
constraints.append(obstacle_cells[i,x_idx,y_idx] <= inside_x_and_y_ranges + circle_intersects_edge)
for cx, cy, r in obstacles:
for i in range(num_robots):
# Define temp binary variables for each condition
temp_x_min = cp.Variable(boolean=True)
temp_x_max = cp.Variable(boolean=True)
temp_y_min = cp.Variable(boolean=True)
temp_y_max = cp.Variable(boolean=True)
# Define the obstacle's bounds in grid coordinates
x_min = (cx - r - bottom_left[0]) / cell_size
x_max = (cx + r - bottom_left[0]) / cell_size
y_min = (cy - r - bottom_left[1]) / cell_size
y_max = (cy + r - bottom_left[1]) / cell_size
# Enforce that robots cannot occupy cells overlapping with obstacles
constraints.append(grid_indices[i, 0] <= x_min + M_ind * temp_x_min)
constraints.append(grid_indices[i, 0] >= x_max - M_ind * (1 - temp_x_max))
constraints.append(grid_indices[i, 1] <= y_min + M_ind * temp_y_min)
constraints.append(grid_indices[i, 1] >= y_max - M_ind * (1 - temp_y_max))
# constraints.append(temp_x_min + temp_x_max + temp_y_min + temp_y_max <= 1)
# Solve the optimization problem
prob_init_start_time = time.time()
prob = cp.Problem(cp.Minimize(cost), constraints)
......@@ -346,6 +315,8 @@ def get_roomba_locs(low, high, num_robots, radius=0.5, obstacles=[]):
def main(seed, num_robots, plot, two_corner):
if seed is not None:
np.random.seed(seed)
np.random.seed(11235)
if not two_corner:
roomba_radius = 0.5
......
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