diff --git a/guided_mrmp/controllers/multi_path_tracking_db.py b/guided_mrmp/controllers/multi_path_tracking_db.py
index 6c71670020ef172483c1fe4d5c6552d56f6d9bd8..1230b0fe4862eb949d9fb36d38274c4b46670967 100644
--- a/guided_mrmp/controllers/multi_path_tracking_db.py
+++ b/guided_mrmp/controllers/multi_path_tracking_db.py
@@ -88,6 +88,12 @@ class MultiPathTrackerDB(MultiPathTracker):
starts, goals = self.get_temp_starts_and_goals(state, grid_origin)
+ # get the starts and goals of ONLY the robots in conflict
+ conflict_starts = [starts[c] for c in conflict]
+ conflict_goals = [goals[c] for c in conflict]
+
+ # create a "discrete" version of the robot, i.e. the
+ # format that the discrete solver expects to receive
disc_robots = self.create_discrete_robots(starts, goals)
disc_conflict = []
@@ -101,7 +107,7 @@ class MultiPathTrackerDB(MultiPathTracker):
this_conflict.append(disc_robots[i])
disc_all_conflicts.append(this_conflict)
- grid_solver = DiscreteResolver(disc_conflict, disc_robots, starts, goals, disc_all_conflicts,grid, self.lib_2x3, self.lib_3x3, self.lib_2x5)
+ grid_solver = DiscreteResolver(disc_conflict, disc_robots, conflict_starts, conflict_goals, disc_all_conflicts,grid, self.lib_2x3, self.lib_3x3, self.lib_2x5)
subproblem = grid_solver.find_subproblem()
if subproblem is None:
@@ -176,83 +182,21 @@ 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)
-
- # return grid, grid_origin
- def get_obstacle_map(self, grid_origin):
+ def get_obstacle_map(self, grid_origin, grid_size, radius):
"""
Create a map of the environment with obstacles
"""
# create a grid of size self.grid_size x self.grid_size
- grid = np.zeros((self.grid_size, self.grid_size))
+ grid = np.zeros((grid_size, grid_size))
# check if there are any obstacles in any of the cells
- grid = np.zeros((self.grid_size, self.grid_size))
- for i in range(self.grid_size):
- for j in range(self.grid_size):
+ grid = np.zeros((grid_size, grid_size))
+ for i in range(grid_size):
+ for j in range(grid_size):
x, y = self.get_grid_cell_location(i, j, grid_origin)
- for obs in []:
- # for obs in self.circle_obs:
- if np.linalg.norm(np.array([x, y]) - np.array(obs[:2])) < obs[2] + self.radius:
+ for obs in self.circle_obs:
+ if np.linalg.norm(np.array([x, y]) - np.array(obs[:2])) < obs[2]:
grid[j, i] = 1
break
@@ -377,7 +321,14 @@ class MultiPathTrackerDB(MultiPathTracker):
plt.show()
- def draw_grid_solution(self, initial_guess, state, grid_origin):
+ def draw_grid_solution(self, initial_guess, state, grid_origin, robots_in_conflict):
+ """
+ params:
+ - initial_guess (dict): the initial guess for the optimization problem
+ - state (list): list of robot states
+ - grid_origin (tuple): top left corner of the grid
+ - num_robots (int): number of robots that are in conflict
+ """
# draw the whole environment with the local grid drawn on top
import matplotlib.pyplot as plt
@@ -411,14 +362,14 @@ class MultiPathTrackerDB(MultiPathTracker):
circle = plt.Circle((obs[0], obs[1]), obs[2], color='red', fill=False)
plt.gca().add_artist(circle)
- for i in range(self.num_robots):
- x = initial_guess[i*3, :]
- y = initial_guess[i*3 + 1, :]
+ # for i in range(num_robots):
+ for i, robot_idx in enumerate(range(robots_in_conflict)):
+ x = initial_guess[robot_idx*3, :]
+ y = initial_guess[robot_idx*3 + 1, :]
plt.plot(x, y, 'x', color=colors[i])
# plot the robots' continuous space subgoals
for idx in range(self.num_robots):
-
traj = self.ego_to_global_roomba(state[idx], self.trajs[idx])
x = traj[0][-1]
y = traj[1][-1]
@@ -488,16 +439,15 @@ class MultiPathTrackerDB(MultiPathTracker):
# 3. If they do collide, then reroute the reference trajectories of these robots
# Get the robots involved in the conflict
- robots = c
robot_positions = []
- for i in range(self.num_robots):
+ for i in (c):
robot_positions.append(state[i][:2])
# 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)
+ grid_obstacle_map = self.get_obstacle_map(grid_origin, self.grid_size, self.radius)
if show_plots: self.draw_grid(state, grid_origin, self.cell_size, 5)
# Solve a discrete version of the problem
@@ -508,18 +458,19 @@ class MultiPathTrackerDB(MultiPathTracker):
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, .5)
+ initial_guess = self.get_initial_guess(state, grid_solution, len(c), 20, .5)
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, len(c))
# for each robot in conflict, reroute its reference trajectory to match the grid solution
num_robots_in_conflict = len(c)
import copy
old_paths = copy.deepcopy(self.paths)
- self.paths = []
- for i in range(num_robots_in_conflict):
+ # self.paths = []
+ # for i in range(num_robots_in_conflict):
+ for i, robot_idx in enumerate(c):
new_ref = initial_guess_state[i*3:i*3+3, :]
# plan from the last point of the ref path to the robot's goal
@@ -541,7 +492,7 @@ class MultiPathTrackerDB(MultiPathTracker):
wp = [xs,ys]
# Path from waypoint interpolation
- self.paths.append(compute_path_from_wp(wp[0], wp[1], 0.05))
+ self.paths[i] = compute_path_from_wp(wp[0], wp[1], 0.05)
targets = []
for i in range(self.num_robots):
diff --git a/settings_files/settings.yaml b/settings_files/settings.yaml
index a99c7050ab8a7eb89947287028f2953291109acf..408337da2dc3d5cb9e4921771865f6e352d09758 100644
--- a/settings_files/settings.yaml
+++ b/settings_files/settings.yaml
@@ -31,16 +31,17 @@ simulator:
robot_starts:
- [6.0, 2.0, 4.8]
- [6.0, 8.0, 2.0]
+ - [1.0, 1.0, 0.0]
robot_goals:
- [6, 8.0, 0]
- [6, 2.0, 0]
+ - [1.0, 8.0, 0]
robot_radii:
- .5
- .5
- .5
- - .5
Roomba:
max_speed: 50.0
@@ -56,7 +57,6 @@ dynamics_models:
- Roomba
- Roomba
- Roomba
- - Roomba