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.
"""
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
class DiscreteResolver():
......
......@@ -45,9 +45,10 @@ class MultiPathTracker:
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
self.paths = []
......@@ -83,9 +84,8 @@ class MultiPathTracker:
bool: True if trajectories overlap, False otherwise.
"""
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, j]) < 2*threshold:
return True
if np.linalg.norm(traj1[0:2, i] - traj2[0:2, i]) < 2*threshold:
return True
return False
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
from guided_mrmp.utils.helpers import initialize_libraries
from guided_mrmp.controllers.place_grid import place_grid
class DiscreteRobot:
def __init__(self, start, goal, label):
self.start = start
......@@ -51,7 +53,7 @@ class MultiPathTrackerDB(MultiPathTracker):
for r in range(self.num_robots):
x, y, theta = state[r]
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])
......@@ -62,7 +64,7 @@ class MultiPathTrackerDB(MultiPathTracker):
x = traj[0][-1]
y = traj[1][-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])
return temp_starts, temp_goals
......@@ -115,11 +117,9 @@ class MultiPathTrackerDB(MultiPathTracker):
# 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)])
# reverse the order of grid solution
grid_solution = grid_solution[::-1]
for i in range(num_robots):
rough_points = np.array(grid_solution[i])
points = []
for point in rough_points:
......@@ -176,67 +176,67 @@ class MultiPathTrackerDB(MultiPathTracker):
return {'X': initial_guess_state, 'U': initial_guess_control, 'T': initial_guess_T}
def place_grid(self, state, robot_locations):
"""
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
we will only place a grid of size self.grid_size x self.grid_size
inputs:
- robot_locations (list): locations of robots involved in conflict [[x,y], [x,y], ...]
outputs:
- grid (numpy array): The grid that we placed
- 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
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.min_y = min(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
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)
self.temp_avg_x = avg_x
self.temp_avg_y = avg_y
# Calculate the top left corner of the grid
# make it so that the grid is centered around the robots
self.cell_size = self.radius*3
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))
# 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
# will make progress.
starts_equal = self.starts_equal(state, grid_origin)
import copy
original_top_left = copy.deepcopy(grid_origin)
x_shift = [-5,5]
y_shift = [-5,5]
for x in np.arange(x_shift[0], x_shift[1],.5):
y =0
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)
if not starts_equal: break
if starts_equal:
for y in np.arange(y_shift[0], y_shift[1],.5):
x =0
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)
if not starts_equal: break
if starts_equal:
print("Some robots are in the same cell")
return None
grid = self.get_obstacle_map(grid_origin)
# def place_grid(self, state, robot_locations):
# """
# 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
# we will only place a grid of size self.grid_size x self.grid_size
# inputs:
# - robot_locations (list): locations of robots involved in conflict [[x,y], [x,y], ...]
# outputs:
# - grid (numpy array): The grid that we placed
# - 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
# 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.min_y = min(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
# 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)
# self.temp_avg_x = avg_x
# self.temp_avg_y = avg_y
# # Calculate the top left corner of the grid
# # make it so that the grid is centered around the robots
# self.cell_size = self.radius*3
# 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))
# # 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
# # will make progress.
# starts_equal = self.starts_equal(state, grid_origin)
# import copy
# original_top_left = copy.deepcopy(grid_origin)
# x_shift = [-5,5]
# y_shift = [-5,5]
# for x in np.arange(x_shift[0], x_shift[1],.5):
# y =0
# 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)
# if not starts_equal: break
# if starts_equal:
# for y in np.arange(y_shift[0], y_shift[1],.5):
# x =0
# 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)
# if not starts_equal: break
# if starts_equal:
# print("Some robots are in the same cell")
# return None
# grid = self.get_obstacle_map(grid_origin)
return grid, grid_origin
# return grid, grid_origin
def get_obstacle_map(self, grid_origin):
"""
......@@ -266,7 +266,7 @@ class MultiPathTrackerDB(MultiPathTracker):
# 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_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
......@@ -275,7 +275,7 @@ class MultiPathTrackerDB(MultiPathTracker):
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
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
......@@ -318,7 +318,7 @@ class MultiPathTrackerDB(MultiPathTracker):
plt.show()
def draw_grid(self, state, grid_origin):
def draw_grid(self, state, grid_origin, cell_size, grid_size):
"""
inputs:
- state (list): list of robot states
......@@ -344,13 +344,13 @@ class MultiPathTrackerDB(MultiPathTracker):
plt.gca().add_artist(circle1)
# 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
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-')
plt.plot([grid_origin[0] + i * cell_size, grid_origin[0] + i * cell_size],
[grid_origin[1], grid_origin[1] + grid_size * cell_size], 'k-')
# Draw horizontal lines
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-')
plt.plot([grid_origin[0], grid_origin[0] + grid_size * cell_size],
[grid_origin[1] + i * cell_size, grid_origin[1] + i * cell_size], 'k-')
# draw the obstacles
for obs in self.circle_obs:
......@@ -401,10 +401,10 @@ class MultiPathTrackerDB(MultiPathTracker):
for i in range(self.grid_size + 1):
# Draw vertical lines
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
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
for obs in self.circle_obs:
......@@ -489,30 +489,26 @@ class MultiPathTrackerDB(MultiPathTracker):
# Get the robots involved in the conflict
robots = c
robot_positions = [state[i] for i in robots]
# 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 = []
robot_positions = []
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])
x = traj[0][-1]
y = traj[1][-1]
self.goals.append([x,y])
# Put down a local grid
self.cell_size = self.radius*3
self.grid_size = 5
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
# Find a subproblem and solve it
grid_solution = self.get_discrete_solution(state, c, all_conflicts, grid_obstacle_map, grid_origin)
if grid_solution:
# 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']
if show_plots: self.draw_grid_solution(initial_guess_state, state, grid_origin)
......
......@@ -92,22 +92,26 @@ if __name__ == "__main__":
# get the name of the settings file from the command line
import sys
if len(sys.argv) < 2:
if len(sys.argv) < 3:
print("Using default settings file")
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
settings = load_settings(settings_file)
environment = load_settings(environment_file)
set_python_seed(1123)
# Load and create the environment
circle_obstacles = settings['environment']['circle_obstacles']
rectangle_obstacles = settings['environment']['rectangle_obstacles']
x_range = settings['environment']['x_range']
y_range = settings['environment']['y_range']
circle_obstacles = environment['circle_obstacles']
rectangle_obstacles = environment['rectangle_obstacles']
x_range = environment['x_range']
y_range = environment['y_range']
env = Env(x_range, y_range, circle_obstacles, rectangle_obstacles)
# 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:
show_plots: 1
show_collision_resolution: 1
environment:
circle_obstacles: []
rectangle_obstacles: []
x_range: [0, 10]
y_range: [0, 10]
robot_starts:
- [6.0, 2.0, 4.8]
- [7.0, 8.0, 2.0]
- [6.0, 8.0, 2.0]
robot_goals:
- [6.5, 8.0, 0]
- [7.5, 2.0, 0]
- [6, 8.0, 0]
- [6, 2.0, 0]
robot_radii:
- .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