diff --git a/README.md b/README.md
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..e81961e4991958384398de55dbbb586118584309 100644
--- a/README.md
+++ b/README.md
@@ -0,0 +1,4 @@
+# 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
diff --git a/experiments/environments/narrow_passage.yaml b/experiments/environments/narrow_passage.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..785d6804b1512b4842103cfc6598dbfc5f7dca74
--- /dev/null
+++ b/experiments/environments/narrow_passage.yaml
@@ -0,0 +1,4 @@
+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
diff --git a/guided_mrmp/conflict_resolvers/discrete_resolver.py b/guided_mrmp/conflict_resolvers/discrete_resolver.py
index 38fd07cbb79b2c6c68bdd73fe5670bbd5c7f9aa8..fd1ecc8e3539cee5b351c6f2f5c8a342161cfa08 100644
--- a/guided_mrmp/conflict_resolvers/discrete_resolver.py
+++ b/guided_mrmp/conflict_resolvers/discrete_resolver.py
@@ -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():
diff --git a/guided_mrmp/controllers/multi_path_tracking.py b/guided_mrmp/controllers/multi_path_tracking.py
index 775e0b3fb372e3b9fef997383e99ebb5f201c453..bd34177359834391b5653a0dd47c26489f34a294 100644
--- a/guided_mrmp/controllers/multi_path_tracking.py
+++ b/guided_mrmp/controllers/multi_path_tracking.py
@@ -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):
diff --git a/guided_mrmp/controllers/multi_path_tracking_db.py b/guided_mrmp/controllers/multi_path_tracking_db.py
index 274a07fa1191adee7f6b3a6c0639404bb98d3f8a..59ac3f746a846ef5be7a2f7dcccd5282c3715ae1 100644
--- a/guided_mrmp/controllers/multi_path_tracking_db.py
+++ b/guided_mrmp/controllers/multi_path_tracking_db.py
@@ -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)
diff --git a/guided_mrmp/main.py b/guided_mrmp/main.py
index ebe30325a8d5af3d0cfaec805c510b75fb05d8d3..af5f8c4ca90d00a66e3be0900ac7f2501a5daef8 100644
--- a/guided_mrmp/main.py
+++ b/guided_mrmp/main.py
@@ -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
diff --git a/settings_files/env.yaml b/settings_files/env.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..52b7b428084ad9318b6979177bfa2824624c97b4
--- /dev/null
+++ b/settings_files/env.yaml
@@ -0,0 +1,4 @@
+circle_obstacles: []
+rectangle_obstacles: []
+x_range: [0, 10]
+y_range: [0, 10]
\ No newline at end of file
diff --git a/settings_files/settings.yaml b/settings_files/settings.yaml
index 87f799c3113aaefebb71ca5a4b75c98f6c9ec9b9..a99c7050ab8a7eb89947287028f2953291109acf 100644
--- a/settings_files/settings.yaml
+++ b/settings_files/settings.yaml
@@ -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