Place a grid to cover robot locations with alignment to centers.
inputs:
- robot_locations (list): locations of robots involved in conflict [[x,y], [x,y], ...]
- cell_size (float): the width of each grid cell in continuous space
- grid_shape (tuple): (# of rows, # of columns) of the grid
- grid_size (tuple): width of the grid in cells
- subgoals (list): locations of subgoals for each robot
- obstacles (list): locations of circular ('c') and rectangular ('r') obstacles [['c',x,y,r], ['c',x,y,r], ['r',x,y,w,h], ['r',x,y,w,h], ...]
outputs:
- 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)
- loss: when return_loss=True, sum of squared differences loss
"""
robot_locations=np.array(robot_locations)
N=len(robot_locations)
subgoals=np.array(subgoals)
num_robots=len(robot_locations)
num_obst=len(obstacles)
M_ind=10*grid_size# Big M relative to grid indices
M_cts=10*max(max(robot_locations[:,0])-min(robot_locations[:,0]),max(robot_locations[:,1])-min(robot_locations[:,1]))# Big M relative to robot locations
# Decision variable: Bottom-left corner of the grid in continuous space
origin=cp.Variable(2,name='origin')
bottom_left=cp.Variable(2,name='origin')
# Decision variable: Integer grid indices for each robot
M_ind=10*grid_size# Big M relative to grid indices
M_cts=10*max(max(robot_locations[:,0])-min(robot_locations[:,0]),max(robot_locations[:,1])-min(robot_locations[:,1]))# Big M relative to robot locations
foriinrange(N):
forjinrange(i+1,N):
# At least one of the two constraints below must be true
xsep=cp.Variable(boolean=True)
ysep=cp.Variable(boolean=True)
constraints.append(xsep+ysep>=1)
angle_info[angle]=grid_center,cell_centers,loss
# Enforces separation by at least 1 in the x direction
b0=cp.Variable(boolean=True)# b0 = 0 if robot i's x >= robot j's x, 1 otherwise