Skip to content
Snippets Groups Projects
Commit 7bfb0215 authored by rachelmoan's avatar rachelmoan
Browse files

Integrating the new place grid method

parent c78eda07
No related branches found
No related tags found
1 merge request!1Updated place grid
# To run the code
1. Specify problem in a settings file in directory settings_files
2. Run main.py <path_to_your_settings_file>
\ No newline at end of file
circle_obstacles: []
rectangle_obstacles: [[[0,6], [10,10]], [[0,0], [10,4]]]
x_range: [0, 10]
y_range: [0, 10]
\ No newline at end of file
...@@ -3,7 +3,6 @@ DBResolver solves a discretized version of the conflict resolution problem. ...@@ -3,7 +3,6 @@ DBResolver solves a discretized version of the conflict resolution problem.
""" """
from guided_mrmp.conflict_resolvers.subproblems import SubproblemPlacer from guided_mrmp.conflict_resolvers.subproblems import SubproblemPlacer
from .query import QueryDatabase
from guided_mrmp.conflict_resolvers.subproblems.subproblem_og import Subproblem, find_subproblem, query_library from guided_mrmp.conflict_resolvers.subproblems.subproblem_og import Subproblem, find_subproblem, query_library
class DiscreteResolver(): class DiscreteResolver():
......
...@@ -45,9 +45,10 @@ class MultiPathTracker: ...@@ -45,9 +45,10 @@ class MultiPathTracker:
self.settings = settings self.settings = settings
self.mpc = MultiMPC(self.num_robots, dynamics, T, DT, settings, settings['environment']['circle_obstacles']) self.mpc = MultiMPC(self.num_robots, dynamics, T, DT, settings, env.circle_obs)
self.circle_obs = settings['environment']['circle_obstacles'] self.circle_obs = env.circle_obs
self.rect_obs = env.rect_obs
# Path from waypoint interpolation # Path from waypoint interpolation
self.paths = [] self.paths = []
...@@ -83,9 +84,8 @@ class MultiPathTracker: ...@@ -83,9 +84,8 @@ class MultiPathTracker:
bool: True if trajectories overlap, False otherwise. bool: True if trajectories overlap, False otherwise.
""" """
for i in range(traj1.shape[1]): for i in range(traj1.shape[1]):
for j in range(traj2.shape[1]): if np.linalg.norm(traj1[0:2, i] - traj2[0:2, i]) < 2*threshold:
if np.linalg.norm(traj1[0:2, i] - traj2[0:2, j]) < 2*threshold: return True
return True
return False return False
def ego_to_global_roomba(self, state, mpc_out): def ego_to_global_roomba(self, state, mpc_out):
......
...@@ -13,6 +13,8 @@ from guided_mrmp.conflict_resolvers.curve_path import smooth_path, calculate_hea ...@@ -13,6 +13,8 @@ from guided_mrmp.conflict_resolvers.curve_path import smooth_path, calculate_hea
from guided_mrmp.utils.helpers import initialize_libraries from guided_mrmp.utils.helpers import initialize_libraries
from guided_mrmp.controllers.place_grid import place_grid
class DiscreteRobot: class DiscreteRobot:
def __init__(self, start, goal, label): def __init__(self, start, goal, label):
self.start = start self.start = start
...@@ -51,7 +53,7 @@ class MultiPathTrackerDB(MultiPathTracker): ...@@ -51,7 +53,7 @@ class MultiPathTrackerDB(MultiPathTracker):
for r in range(self.num_robots): for r in range(self.num_robots):
x, y, theta = state[r] x, y, theta = state[r]
cell_x = min(max(math.floor((x - grid_origin[0]) / self.cell_size), 0), self.grid_size - 1) cell_x = min(max(math.floor((x - grid_origin[0]) / self.cell_size), 0), self.grid_size - 1)
cell_y = min(max(math.floor((grid_origin[1] - y) / self.cell_size), 0), self.grid_size - 1) cell_y = min(max(math.floor((y- grid_origin[1]) / self.cell_size), 0), self.grid_size - 1)
temp_starts.append([cell_x, cell_y]) temp_starts.append([cell_x, cell_y])
...@@ -62,7 +64,7 @@ class MultiPathTrackerDB(MultiPathTracker): ...@@ -62,7 +64,7 @@ class MultiPathTrackerDB(MultiPathTracker):
x = traj[0][-1] x = traj[0][-1]
y = traj[1][-1] y = traj[1][-1]
cell_x = min(max(math.floor((x - grid_origin[0]) / self.cell_size), 0), self.grid_size - 1) cell_x = min(max(math.floor((x - grid_origin[0]) / self.cell_size), 0), self.grid_size - 1)
cell_y = min(max(math.floor((grid_origin[1] - y) / self.cell_size), 0), self.grid_size - 1) cell_y = min(max(math.floor((y- grid_origin[1]) / self.cell_size), 0), self.grid_size - 1)
temp_goals.append([cell_x,cell_y]) temp_goals.append([cell_x,cell_y])
return temp_starts, temp_goals return temp_starts, temp_goals
...@@ -115,11 +117,9 @@ class MultiPathTrackerDB(MultiPathTracker): ...@@ -115,11 +117,9 @@ class MultiPathTrackerDB(MultiPathTracker):
# the initial guess for time is the length of the longest path in the solution # the initial guess for time is the length of the longest path in the solution
initial_guess_T = 2*max([len(grid_solution[i]) for i in range(num_robots)]) initial_guess_T = 2*max([len(grid_solution[i]) for i in range(num_robots)])
# reverse the order of grid solution
grid_solution = grid_solution[::-1]
for i in range(num_robots): for i in range(num_robots):
rough_points = np.array(grid_solution[i]) rough_points = np.array(grid_solution[i])
points = [] points = []
for point in rough_points: for point in rough_points:
...@@ -176,67 +176,67 @@ class MultiPathTrackerDB(MultiPathTracker): ...@@ -176,67 +176,67 @@ class MultiPathTrackerDB(MultiPathTracker):
return {'X': initial_guess_state, 'U': initial_guess_control, 'T': initial_guess_T} return {'X': initial_guess_state, 'U': initial_guess_control, 'T': initial_guess_T}
def place_grid(self, state, robot_locations): # def place_grid(self, state, robot_locations):
""" # """
Given the locations of robots that need to be covered in continuous space, find # Given the locations of robots that need to be covered in continuous space, find
and place the grid. We don't need a very large grid to place subproblems, so # and place the grid. We don't need a very large grid to place subproblems, so
we will only place a grid of size self.grid_size x self.grid_size # we will only place a grid of size self.grid_size x self.grid_size
inputs: # inputs:
- robot_locations (list): locations of robots involved in conflict [[x,y], [x,y], ...] # - robot_locations (list): locations of robots involved in conflict [[x,y], [x,y], ...]
outputs: # outputs:
- grid (numpy array): The grid that we placed # - grid (numpy array): The grid that we placed
- top_left (tuple): The top left corner of the grid in continuous space # - top_left (tuple): The top left corner of the grid in continuous space
""" # """
# Find the minimum and maximum x and y coordinates of the robot locations # # Find the minimum and maximum x and y coordinates of the robot locations
self.min_x = min(robot_locations, key=lambda loc: loc[0])[0] # self.min_x = min(robot_locations, key=lambda loc: loc[0])[0]
self.max_x = max(robot_locations, key=lambda loc: loc[0])[0] # self.max_x = max(robot_locations, key=lambda loc: loc[0])[0]
self.min_y = min(robot_locations, key=lambda loc: loc[1])[1] # self.min_y = min(robot_locations, key=lambda loc: loc[1])[1]
self.max_y = max(robot_locations, key=lambda loc: loc[1])[1] # self.max_y = max(robot_locations, key=lambda loc: loc[1])[1]
# find the average x and y coordinates of the robot locations # # find the average x and y coordinates of the robot locations
avg_x = sum([loc[0] for loc in robot_locations]) / len(robot_locations) # avg_x = sum([loc[0] for loc in robot_locations]) / len(robot_locations)
avg_y = sum([loc[1] for loc in robot_locations]) / len(robot_locations) # avg_y = sum([loc[1] for loc in robot_locations]) / len(robot_locations)
self.temp_avg_x = avg_x # self.temp_avg_x = avg_x
self.temp_avg_y = avg_y # self.temp_avg_y = avg_y
# Calculate the top left corner of the grid # # Calculate the top left corner of the grid
# make it so that the grid is centered around the robots # # make it so that the grid is centered around the robots
self.cell_size = self.radius*3 # self.cell_size = self.radius*3
self.grid_size = 5 # self.grid_size = 5
grid_origin = (avg_x - int(self.grid_size*self.cell_size/2), avg_y + int(self.grid_size*self.cell_size/2)) # grid_origin = (avg_x - int(self.grid_size*self.cell_size/2), avg_y + int(self.grid_size*self.cell_size/2))
# Check if, for every robot, the cell value of the start and the cell value # # Check if, for every robot, the cell value of the start and the cell value
# of the goal are the same. If they are, then we can't find a discrete solution that # # of the goal are the same. If they are, then we can't find a discrete solution that
# will make progress. # # will make progress.
starts_equal = self.starts_equal(state, grid_origin) # starts_equal = self.starts_equal(state, grid_origin)
import copy # import copy
original_top_left = copy.deepcopy(grid_origin) # original_top_left = copy.deepcopy(grid_origin)
x_shift = [-5,5] # x_shift = [-5,5]
y_shift = [-5,5] # y_shift = [-5,5]
for x in np.arange(x_shift[0], x_shift[1],.5): # for x in np.arange(x_shift[0], x_shift[1],.5):
y =0 # y =0
grid_origin = (original_top_left[0] + x*self.cell_size*.5, original_top_left[1] - y*self.cell_size*.5) # grid_origin = (original_top_left[0] + x*self.cell_size*.5, original_top_left[1] - y*self.cell_size*.5)
starts_equal = self.starts_equal(state, grid_origin) # starts_equal = self.starts_equal(state, grid_origin)
if not starts_equal: break # if not starts_equal: break
if starts_equal: # if starts_equal:
for y in np.arange(y_shift[0], y_shift[1],.5): # for y in np.arange(y_shift[0], y_shift[1],.5):
x =0 # x =0
grid_origin = (original_top_left[0] + x*self.cell_size*.5, original_top_left[1] - y*self.cell_size*.5) # grid_origin = (original_top_left[0] + x*self.cell_size*.5, original_top_left[1] - y*self.cell_size*.5)
starts_equal = self.starts_equal(state, grid_origin) # starts_equal = self.starts_equal(state, grid_origin)
if not starts_equal: break # if not starts_equal: break
if starts_equal: # if starts_equal:
print("Some robots are in the same cell") # print("Some robots are in the same cell")
return None # return None
grid = self.get_obstacle_map(grid_origin) # grid = self.get_obstacle_map(grid_origin)
return grid, grid_origin # return grid, grid_origin
def get_obstacle_map(self, grid_origin): def get_obstacle_map(self, grid_origin):
""" """
...@@ -266,7 +266,7 @@ class MultiPathTrackerDB(MultiPathTracker): ...@@ -266,7 +266,7 @@ class MultiPathTrackerDB(MultiPathTracker):
# find the closest grid cell that is not an obstacle # find the closest grid cell that is not an obstacle
cell_x = min(max(math.floor((x - grid_origin[0]) / self.cell_size), 0), self.grid_size - 1) cell_x = min(max(math.floor((x - grid_origin[0]) / self.cell_size), 0), self.grid_size - 1)
cell_y = min(max(math.floor((grid_origin[1] - y) / self.cell_size), 0), self.grid_size - 1) cell_y = min(max(math.floor((y - grid_origin[1]) / self.cell_size), 0), self.grid_size - 1)
return cell_x, cell_y return cell_x, cell_y
...@@ -275,7 +275,7 @@ class MultiPathTrackerDB(MultiPathTracker): ...@@ -275,7 +275,7 @@ class MultiPathTrackerDB(MultiPathTracker):
Given a cell in the grid, find the center of that cell in continuous space Given a cell in the grid, find the center of that cell in continuous space
""" """
x = grid_origin[0] + (cell_x + 0.5) * self.cell_size x = grid_origin[0] + (cell_x + 0.5) * self.cell_size
y = grid_origin[1] - (cell_y + 0.5) * self.cell_size y = grid_origin[1] + (cell_y + 0.5) * self.cell_size
return x, y return x, y
...@@ -318,7 +318,7 @@ class MultiPathTrackerDB(MultiPathTracker): ...@@ -318,7 +318,7 @@ class MultiPathTrackerDB(MultiPathTracker):
plt.show() plt.show()
def draw_grid(self, state, grid_origin): def draw_grid(self, state, grid_origin, cell_size, grid_size):
""" """
inputs: inputs:
- state (list): list of robot states - state (list): list of robot states
...@@ -344,13 +344,13 @@ class MultiPathTrackerDB(MultiPathTracker): ...@@ -344,13 +344,13 @@ class MultiPathTrackerDB(MultiPathTracker):
plt.gca().add_artist(circle1) plt.gca().add_artist(circle1)
# draw the horizontal and vertical lines of the grid # draw the horizontal and vertical lines of the grid
for i in range(self.grid_size + 1): for i in range(grid_size + 1):
# Draw vertical lines # Draw vertical lines
plt.plot([grid_origin[0] + i * self.cell_size, grid_origin[0] + i * self.cell_size], plt.plot([grid_origin[0] + i * cell_size, grid_origin[0] + i * cell_size],
[grid_origin[1], grid_origin[1] - self.grid_size * self.cell_size], 'k-') [grid_origin[1], grid_origin[1] + grid_size * cell_size], 'k-')
# Draw horizontal lines # Draw horizontal lines
plt.plot([grid_origin[0], grid_origin[0] + self.grid_size * self.cell_size], plt.plot([grid_origin[0], grid_origin[0] + grid_size * cell_size],
[grid_origin[1] - i * self.cell_size, grid_origin[1] - i * self.cell_size], 'k-') [grid_origin[1] + i * cell_size, grid_origin[1] + i * cell_size], 'k-')
# draw the obstacles # draw the obstacles
for obs in self.circle_obs: for obs in self.circle_obs:
...@@ -401,10 +401,10 @@ class MultiPathTrackerDB(MultiPathTracker): ...@@ -401,10 +401,10 @@ class MultiPathTrackerDB(MultiPathTracker):
for i in range(self.grid_size + 1): for i in range(self.grid_size + 1):
# Draw vertical lines # Draw vertical lines
plt.plot([grid_origin[0] + i * self.cell_size, grid_origin[0] + i * self.cell_size], plt.plot([grid_origin[0] + i * self.cell_size, grid_origin[0] + i * self.cell_size],
[grid_origin[1], grid_origin[1] - self.grid_size * self.cell_size], 'k-') [grid_origin[1], grid_origin[1] + self.grid_size * self.cell_size], 'k-')
# Draw horizontal lines # Draw horizontal lines
plt.plot([grid_origin[0], grid_origin[0] + self.grid_size * self.cell_size], plt.plot([grid_origin[0], grid_origin[0] + self.grid_size * self.cell_size],
[grid_origin[1] - i * self.cell_size, grid_origin[1] - i * self.cell_size], 'k-') [grid_origin[1] + i * self.cell_size, grid_origin[1] + i * self.cell_size], 'k-')
# draw the obstacles # draw the obstacles
for obs in self.circle_obs: for obs in self.circle_obs:
...@@ -489,30 +489,26 @@ class MultiPathTrackerDB(MultiPathTracker): ...@@ -489,30 +489,26 @@ class MultiPathTrackerDB(MultiPathTracker):
# Get the robots involved in the conflict # Get the robots involved in the conflict
robots = c robots = c
robot_positions = [state[i] for i in robots] robot_positions = []
# Put down a local grid
grid_obstacle_map, grid_origin = self.place_grid(state, robot_positions)
if show_plots: self.draw_grid(state, grid_origin)
# set the starts (robots' current positions)
self.starts = []
self.goals = []
for i in range(self.num_robots): for i in range(self.num_robots):
self.starts.append(state[i]) robot_positions.append(state[i][:2])
traj = self.ego_to_global_roomba(state[i], self.trajs[i]) # Put down a local grid
x = traj[0][-1] self.cell_size = self.radius*3
y = traj[1][-1] self.grid_size = 5
self.goals.append([x,y]) grid_origin, centers = place_grid(robot_positions, cell_size=self.radius*3)
grid_obstacle_map = self.get_obstacle_map(grid_origin)
if show_plots: self.draw_grid(state, grid_origin, self.cell_size, 5)
# Solve a discrete version of the problem # Solve a discrete version of the problem
# Find a subproblem and solve it # Find a subproblem and solve it
grid_solution = self.get_discrete_solution(state, c, all_conflicts, grid_obstacle_map, grid_origin) grid_solution = self.get_discrete_solution(state, c, all_conflicts, grid_obstacle_map, grid_origin)
if grid_solution: if grid_solution:
# if we found a grid solution, we can use it to reroute the robots # if we found a grid solution, we can use it to reroute the robots
initial_guess = self.get_initial_guess(state, grid_solution, self.num_robots, 20, 1) initial_guess = self.get_initial_guess(state, grid_solution, self.num_robots, 20, .5)
initial_guess_state = initial_guess['X'] initial_guess_state = initial_guess['X']
if show_plots: self.draw_grid_solution(initial_guess_state, state, grid_origin) if show_plots: self.draw_grid_solution(initial_guess_state, state, grid_origin)
......
...@@ -92,22 +92,26 @@ if __name__ == "__main__": ...@@ -92,22 +92,26 @@ if __name__ == "__main__":
# get the name of the settings file from the command line # get the name of the settings file from the command line
import sys import sys
if len(sys.argv) < 2: if len(sys.argv) < 3:
print("Using default settings file") print("Using default settings file")
settings_file = "settings_files/settings.yaml" settings_file = "settings_files/settings.yaml"
environment_file = "settings_files/env.yaml"
else: settings_file = sys.argv[1] else:
settings_file = sys.argv[1]
environment_file = sys.argv[2]
# Load the settings # Load the settings
settings = load_settings(settings_file) settings = load_settings(settings_file)
environment = load_settings(environment_file)
set_python_seed(1123) set_python_seed(1123)
# Load and create the environment # Load and create the environment
circle_obstacles = settings['environment']['circle_obstacles'] circle_obstacles = environment['circle_obstacles']
rectangle_obstacles = settings['environment']['rectangle_obstacles'] rectangle_obstacles = environment['rectangle_obstacles']
x_range = settings['environment']['x_range'] x_range = environment['x_range']
y_range = settings['environment']['y_range'] y_range = environment['y_range']
env = Env(x_range, y_range, circle_obstacles, rectangle_obstacles) env = Env(x_range, y_range, circle_obstacles, rectangle_obstacles)
# Load the dynamics models # Load the dynamics models
......
circle_obstacles: []
rectangle_obstacles: []
x_range: [0, 10]
y_range: [0, 10]
\ No newline at end of file
...@@ -28,19 +28,13 @@ simulator: ...@@ -28,19 +28,13 @@ simulator:
show_plots: 1 show_plots: 1
show_collision_resolution: 1 show_collision_resolution: 1
environment:
circle_obstacles: []
rectangle_obstacles: []
x_range: [0, 10]
y_range: [0, 10]
robot_starts: robot_starts:
- [6.0, 2.0, 4.8] - [6.0, 2.0, 4.8]
- [7.0, 8.0, 2.0] - [6.0, 8.0, 2.0]
robot_goals: robot_goals:
- [6.5, 8.0, 0] - [6, 8.0, 0]
- [7.5, 2.0, 0] - [6, 2.0, 0]
robot_radii: robot_radii:
- .5 - .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