diff --git a/guided_mrmp/controllers/multi_path_tracking_db.py b/guided_mrmp/controllers/multi_path_tracking_db.py
index 957809ba2ded3cffd89e617614e1cf213b7eb46c..8eb5be1db427e4fdc710719b8bce8973bd3be4c2 100644
--- a/guided_mrmp/controllers/multi_path_tracking_db.py
+++ b/guided_mrmp/controllers/multi_path_tracking_db.py
@@ -1,43 +1,21 @@
import numpy as np
-import matplotlib.pyplot as plt
from guided_mrmp.controllers.multi_path_tracking import MultiPathTracker
-from guided_mrmp.controllers.utils import compute_path_from_wp, get_ref_trajectory
+from guided_mrmp.controllers.utils import compute_path_from_wp, get_ref_trajectory, get_grid_cell_location, get_grid_cell, get_obstacle_map
from guided_mrmp.conflict_resolvers.discrete_resolver import DiscreteResolver
from guided_mrmp.conflict_resolvers.curve_path import smooth_path, calculate_headings
-from shapely.geometry import Point
-from shapely.geometry import Polygon
from guided_mrmp.utils.helpers import plan_decoupled_path
from guided_mrmp.controllers.multi_mpc import MultiMPC
from guided_mrmp.controllers.place_grid import place_grid
class DiscreteRobot:
- def __init__(self, start, goal, label):
+ def __init__(self, start, goal, label, outside_grid=False):
self.start = start
self.goal = goal
self.current_position = start
self.label = label
-
-def plot_roomba(x, y, yaw, color, fill, radius):
- """
-
- Args:
- x ():
- y ():
- yaw ():
- """
- fig = plt.gcf()
- ax = fig.gca()
- if fill: alpha = .3
- else: alpha = 1
- circle = plt.Circle((x, y), radius, color=color, fill=fill, alpha=alpha)
- ax.add_patch(circle)
-
- # Plot direction marker
- dx = 1 * np.cos(yaw)
- dy = 1 * np.sin(yaw)
- ax.arrow(x, y, dx, dy, head_width=0.1, head_length=0.1, fc='r', ec='r')
+ self.outside_grid = False
class MultiPathTrackerDB(MultiPathTracker):
def get_temp_starts_and_goals(self, state, grid_origin):
@@ -45,13 +23,22 @@ class MultiPathTrackerDB(MultiPathTracker):
# based on the continuous space location of the robot, we find the cell in the grid that
# includes that continuous space location using the top left of the grid as a reference point
+ # find the minimum and maximum x and y values of the grid
+ min_x = grid_origin[0]
+ max_x = grid_origin[0] + self.cell_size * self.grid_size
+ min_y = grid_origin[1]
+ max_y = grid_origin[1] + self.cell_size * self.grid_size
+
import math
temp_starts = []
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((y- grid_origin[1]) / self.cell_size), 0), self.grid_size - 1)
- temp_starts.append([cell_x, cell_y])
+ if x < min_x or x > max_x or y < min_y or y > max_y:
+ temp_starts.append([-1, -1])
+ else:
+ cell_x = min(max(math.floor((x - grid_origin[0]) / 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])
# the temmporary goal is the point at the end of the robot's predicted traj
@@ -78,9 +65,14 @@ class MultiPathTrackerDB(MultiPathTracker):
def create_discrete_robots(self, starts, goals):
discrete_robots = []
for i in range(len(starts)):
- start = starts[i]
- goal = goals[i]
- discrete_robots.append(DiscreteRobot(start, goal, i))
+ if starts[i][0] == -1:
+ start = starts[i]
+ goal = goals[i]
+ discrete_robots.append(DiscreteRobot(start, goal, i, outside_grid=True))
+ else:
+ start = starts[i]
+ goal = goals[i]
+ discrete_robots.append(DiscreteRobot(start, goal, i, outside_grid=False))
return discrete_robots
def get_discrete_solution(self, state, conflict, all_conflicts, grid, grid_origin, libs):
@@ -103,16 +95,32 @@ class MultiPathTrackerDB(MultiPathTracker):
disc_robots = self.create_discrete_robots(starts, goals)
disc_conflict = []
- for c in conflict:
- disc_conflict.append(disc_robots[c])
+ for r in disc_robots:
+ if r.label in conflict:
+ disc_conflict.append(r)
disc_all_conflicts = []
for c in all_conflicts:
this_conflict = []
- for i in c:
- this_conflict.append(disc_robots[i])
+ for r in disc_robots:
+ if r.label in c:
+ this_conflict.append(r)
disc_all_conflicts.append(this_conflict)
+ print(f"discrete robots = ")
+ for r in disc_robots:
+ print(f"Robot {r.label}")
+
+ print(f"discrete conflict = ")
+ for r in disc_conflict:
+ print(f"Robot {r.label}")
+
+ print(f"discrete all conflicts = ")
+ for c in disc_all_conflicts:
+ print(f"Conflict = ")
+ for r in c:
+ print(f"Robot {r.label}")
+
grid_solver = DiscreteResolver(disc_conflict, disc_robots, conflict_starts, conflict_goals, disc_all_conflicts,grid, libs[0], libs[1], libs[2])
subproblem = grid_solver.find_subproblem()
@@ -121,6 +129,21 @@ class MultiPathTrackerDB(MultiPathTracker):
return None
grid_solution = grid_solver.solve_subproblem(subproblem)
+ if grid_solution is None:
+
+
+ print("major error")
+ print("subproblem types = ", subproblem.type)
+ print(f"Robots involved in subproblem = ")
+ for r in subproblem.all_robots_involved_in_subproblem:
+ print(r.label)
+ print(state[r.label])
+ print("subproblem starts = ", subproblem.get_starts())
+ print("subproblem goals = ", subproblem.get_goals())
+ print(f"subproblem top left = {subproblem.top_left}")
+ print(f"subproblem bottom right = {subproblem.bottom_right}")
+ self.draw_grid_solution([], state, grid_origin, grid, conflict, 0)
+
# The solution for each robot must be of the same length, so the arrays are padded with [-1,-1]
# so they are of the form [[x, y], [x, y], ..., [-1,1], [-1,1], ...]
# Clean up the grid solution so that it doesn't contain any -1s
@@ -152,133 +175,129 @@ class MultiPathTrackerDB(MultiPathTracker):
- estimated_state (np.ndarray): the estimated state of the robots in continuous space
"""
num_robots = len(robots_in_conflict)
- estimated_state = np.zeros((num_robots*3, N+2))
+ estimated_state = np.zeros((num_robots*3, N+1))
+ final_estimated_state = np.zeros((num_robots*3, 12+N+1))
+
for i in range(num_robots):
points = np.array(grid_solution[i])
# smooth the path using bezier curves and gaussian smoothing
smoothed_curve, _ = smooth_path(points, N+1, cp_dist, smoothing)
-
- current_robot_x_pos = state[robots_in_conflict[i]][0]
- current_robot_y_pos = state[robots_in_conflict[i]][1]
-
- # insert the current robot position as the first point of the path
- estimated_state[i*3, 0] = current_robot_x_pos
- estimated_state[i*3 + 1, 0] = current_robot_y_pos
- estimated_state[i*3 + 2, 0] = state[robots_in_conflict[i]][2]
-
- estimated_state[i*3, 1:] = smoothed_curve[:, 0] # x
- estimated_state[i*3 + 1, 1:] = smoothed_curve[:, 1] # y
-
-
+ estimated_state[i*3, :] = smoothed_curve[:, 0] # x
+ estimated_state[i*3 + 1, :] = smoothed_curve[:, 1] # y
# translate the initial guess so that the first point is at (0,0)
- estimated_state[i*3, 1:] -= estimated_state[i*3, 1]
- estimated_state[i*3 + 1, 1:] -= estimated_state[i*3+1, 1]
+ estimated_state[i*3, :] -= estimated_state[i*3, 1]
+ estimated_state[i*3 + 1, :] -= estimated_state[i*3+1, 1]
- smoothed_curve_first_point = self.get_grid_cell_location(points[0][0], points[0][1], grid_origin)
+ smoothed_curve_first_point = get_grid_cell_location(points[0][0], points[0][1], grid_origin)
# translate the initial guess so that the first point is at the current robot position
- estimated_state[i*3, 1:] += smoothed_curve_first_point[0]
- estimated_state[i*3 + 1, 1:] += smoothed_curve_first_point[1]
-
+ current_robot_position = state[robots_in_conflict[i]][:2]
+ estimated_state[i*3, :] += smoothed_curve_first_point[0]
+ estimated_state[i*3 + 1, :] += smoothed_curve_first_point[1]
+ # estimated_state[i*3, :] += current_robot_position[0]
+ # estimated_state[i*3 + 1, :] += current_robot_position[1]
headings = calculate_headings(smoothed_curve)
headings.append(headings[-1])
- estimated_state[i*3 + 2, 1:] = headings
+ estimated_state[i*3 + 2, :] = headings
- return estimated_state
+ # now that we have the continuous space soln, we need to connect the robot to it
+ x_start, y_start = state[robots_in_conflict[i]][:2]
+ x_end, y_end = estimated_state[i*3, 0], estimated_state[i*3 + 1, 0]
- def estimate_controls_from_state(self, num_robots, state_trajectory, dt, N):
- """
- Estimate the controls for each robot from a given state trajectory
- """
- initial_guess_control = np.zeros((num_robots*2, N))
+ # first, rotate the robot in place to face the first point of the continuous space soln
+ current_heading = state[robots_in_conflict[i]][2]
+ first_heading = np.arctan2(y_end - y_start, x_end - x_start)
- change_in_position = []
- for i in range(num_robots):
- x = state_trajectory[i*3, :] # x
- y = state_trajectory[i*3 + 1, :] # y
+ delta_heading = first_heading - current_heading
- change_in_position = []
- for j in range(len(x)-1):
- pos1 = np.array([x[j], y[j]])
- pos2 = np.array([x[j+1], y[j+1]])
+ headings = [current_heading+(delta_heading/4),
+ current_heading+(delta_heading/2),
+ current_heading+(delta_heading*3/4),
+ first_heading]
+
+
- change_in_position.append(np.linalg.norm(pos2 - pos1))
+
- velocity = np.array(change_in_position) / dt
- initial_guess_control[i*2, :] = velocity
+ xs = [x_start, x_start, x_start, x_start]
+ ys = [y_start, y_start, y_start, y_start]
- # omega is the difference between consecutive headings
- headings = state_trajectory[i*3 + 2, :]
- omega = np.diff(headings)
- initial_guess_control[i*2 + 1, :] = omega
+ # print(f"i = {i}")
+ # for j in range(len(xs)):
+ # import matplotlib.pyplot as plt
+ # plot_roomba(xs[j], ys[j], headings[j], 'black', False, self.radius)
+ # plot_roomba(xs[j], ys[j], first_heading, 'red', False, self.radius)
+ # plt.plot(estimated_state[i*3, 0], estimated_state[i*3 + 1, 0], 'o-')
+ # plt.show()
- return initial_guess_control
+ # interpolate between the current state of the robot and the first point of the continuous space soln
+ num_points = 4
- 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((grid_size, grid_size))
+ # Create evenly spaced x-values between the start and end x-values
+ x_interp = np.linspace(x_start, x_end, num_points + 2)[1:-1]
- 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 self.circle_obs:
- # collision check this square and the obstacle circle using shapely
- circle = Point(obs[0], obs[1]).buffer(obs[2])
- # create a square with the top left corner at (x - cell_size/2, y - cell_size/2)
- tl = (x - self.cell_size/2, y - self.cell_size/2)
- coords = [tl, (tl[0] + self.cell_size, tl[1]), (tl[0] + self.cell_size, tl[1] + self.cell_size), (tl[0], tl[1] + self.cell_size)]
- square = Polygon(coords)
- overlap_area = circle.intersection(square).area
- if overlap_area >= (self.cell_size**2) / 4:
- grid[i][j] = 1
-
-
- return grid
-
- def get_grid_cell(self, x, y, grid_origin):
- """
- Given a continuous space x and y, find the cell in the grid that includes that location
- """
- import math
+ # Calculate corresponding y-values using linear interpolation
+ y_interp = np.interp(x_interp, [x_start, x_end], [y_start, y_end])
- # 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((y - grid_origin[1]) / self.cell_size), 0), self.grid_size - 1)
+ for x,y in zip(x_interp[1:], y_interp[1:]):
+ xs.append(x)
+ ys.append(y)
- return cell_x, cell_y
-
- def get_grid_cell_location(self, cell_x, cell_y, grid_origin):
- """
- 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
- return x, y
-
- def starts_equal(self, state, grid_origin):
- """
- Check if the start cells of any two robots are the same
- """
- for i in range(self.num_robots):
- for j in range(i + 1, self.num_robots):
- start_i = state[i]
- start_j = state[j]
+ xs.append(x_end)
+ ys.append(y_end)
- cell_i = self.get_grid_cell(start_i[0], start_i[1], grid_origin)
- cell_j = self.get_grid_cell(start_j[0], start_j[1], grid_origin)
+ for j in range(num_points-1):
+ dx = x_interp[j+1] - x_interp[j]
+ dy = y_interp[j+1] - y_interp[j]
+ headings.append(np.arctan2(dy, dx))
+
+ headings.append(headings[-1])
+
+ # now get the heading between the first two points in the estimated soln
+ dx = estimated_state[i*3, 1] - estimated_state[i*3, 0]
+ dy = estimated_state[i*3 + 1, 1] - estimated_state[i*3 + 1, 0]
+ next_heading = np.arctan2(dy, dx)
+
+ last_heading = headings[-1]
+
+ delta_heading = next_heading - last_heading
+
+ headings.append(last_heading + (delta_heading/4))
+ headings.append(last_heading + (delta_heading/2))
+ headings.append(last_heading + (delta_heading*3/4))
+ headings.append(next_heading)
+
+ xs.append(xs[-1])
+ xs.append(xs[-1])
+ xs.append(xs[-1])
+ xs.append(xs[-1])
+ ys.append(ys[-1])
+ ys.append(ys[-1])
+ ys.append(ys[-1])
+ ys.append(ys[-1])
+
+
+ # print(f"i = {i}")
+ # for j in range(len(xs)):
+ # import matplotlib.pyplot as plt
+ # plot_roomba(xs[j], ys[j], headings[j], 'black', False, self.radius)
+ # plt.plot(estimated_state[i*3, :], estimated_state[i*3 + 1, :], 'o-')
+ # plt.show()
+
+
+
+ # Finally, combine the interpolated points with the estimated state
+ final_estimated_state[i*3, :] = np.concatenate((xs, estimated_state[i*3, :]))
+ final_estimated_state[i*3 + 1, :] = np.concatenate((ys, estimated_state[i*3 + 1, :]))
+ final_estimated_state[i*3 + 2, :] = np.concatenate((headings, estimated_state[i*3 + 2, :]))
+
+ return final_estimated_state
- if cell_i == cell_j:
- return True
- return False
-
def get_next_state_control(self, state, u_next, x_next):
targets = []
for i in range(self.num_robots):
@@ -383,13 +402,12 @@ class MultiPathTrackerDB(MultiPathTracker):
for j in range(i + 1, self.num_robots):
traj2 = self.ego_to_global_roomba(state[j], self.trajs[j])
if self.trajectories_overlap(traj1, traj2, self.radius):
- # plot the trajectories of the robots that are in conflict
- if show_plots: self.plot_trajs(state, traj1, traj2, self.radius)
this_robot_conflicts.append(j)
if len(this_robot_conflicts) > 1:
all_conflicts.append(this_robot_conflicts)
for c in all_conflicts:
+ print(f"Conflict between robots {c}")
# 3. If they do collide, then reroute the reference trajectories of these robots
# Get the robots involved in the conflict
@@ -415,11 +433,11 @@ class MultiPathTrackerDB(MultiPathTracker):
subgoals = self.get_subgoals(state, c)
grid_origin, centers = place_grid(robot_positions,
- cell_size=self.radius*2,
+ cell_size=self.cell_size,
grid_size=5,
subgoals=subgoals,
obstacles=circle_obs)
- grid_obstacle_map = self.get_obstacle_map(grid_origin, self.grid_size, self.radius)
+ grid_obstacle_map = get_obstacle_map(grid_origin, self.grid_size, self.radius)
# Solve a discrete version of the problem
# Find a subproblem and solve it
@@ -428,6 +446,7 @@ class MultiPathTrackerDB(MultiPathTracker):
print(f"obstacle map = {grid_obstacle_map}")
print(f"grid_solution = {grid_solution}")
+ print(f"show_plots = {show_plots}")
if grid_solution:
# if we found a grid solution, we can use it to reroute the robots
continuous_soln = self.convert_db_sol_to_continuous(state,
@@ -439,9 +458,8 @@ class MultiPathTrackerDB(MultiPathTracker):
self.settings["database"]["smoothing_sigma"]
)
- if show_plots: self.draw_grid_solution(continuous_soln, state, grid_origin, grid_obstacle_map, c, round(time, 2))
+ self.draw_grid_solution(continuous_soln, state, grid_origin, grid_obstacle_map, c, round(time, 2))
- # for each robot in conflict, reroute its reference trajectory to match the grid solution
# for each robot in conflict, reroute its reference trajectory to match the grid solution
import copy
old_paths = copy.deepcopy(self.paths)
@@ -451,12 +469,12 @@ class MultiPathTrackerDB(MultiPathTracker):
# plan from the last point of the ref path to the robot's goal
# plan an RRT path from the current state to the goal
- start = (new_ref[:, -1][0], new_ref[:, -1][1])
- goal = (old_paths[i][:, -1][0], old_paths[i][:, -1][1])
- print(f"planning from {start} to {goal}")
- print(f"new_ref = {new_ref}")
+ start = (new_ref[0][-1], new_ref[1][-1])
+ goal = (old_paths[i][0][-1], old_paths[i][1][-1])
- print(f"other new ref = {continuous_soln[(robot_idx+1)*3:(robot_idx+1)*3+3, :]}")
+ # plot the start and goal, with the obstacles in the environment
+ # if show_plots:
+ # self.plot_start_goal(start, goal, self.env.circle_obs, self.env.rect_obs)
rrtpath, tree = plan_decoupled_path(self.settings["sampling_based_planner"]["name"],
start,
@@ -466,11 +484,11 @@ class MultiPathTrackerDB(MultiPathTracker):
self.env.circle_obs,
self.env.rect_obs,
vis=False,
- iter=self.settings["sampling_based_planner"]["num_samples"])
+ iter=self.settings["sampling_based_planner"]["num_samples"],
+ obstacle_tol=.5)
-
- xs = new_ref[0, :].tolist()
- ys = new_ref[1, :].tolist()
+ xs = []
+ ys = []
for node in rrtpath:
xs.append(node[0])
@@ -479,7 +497,14 @@ class MultiPathTrackerDB(MultiPathTracker):
wp = [xs,ys]
# Path from waypoint interpolation
- path = compute_path_from_wp(wp[0], wp[1], 0.05)
+ path = compute_path_from_wp(wp[0], wp[1], 0.2)
+
+ # plot the path from the RRT and the new ref in different colors
+ # if show_plots:
+ # import matplotlib.pyplot as plt
+ # plt.plot(path[0], path[1], 'r--')
+ # plt.plot(new_ref[0, :], new_ref[1, :], 'b--')
+ # plt.show()
# combine the path with new_ref
new_ref_x = np.concatenate((new_ref[0, :], path[0]))
@@ -488,8 +513,24 @@ class MultiPathTrackerDB(MultiPathTracker):
self.paths[i] = np.array([new_ref_x, new_ref_y, new_ref_theta])
+
+
+ # plot the old path and the new path, side by side
+ # if show_plots:
+ # self.plot_old_and_new_guides(old_paths[i], self.paths[i], new_ref[0,:], new_ref[1,:])
+
+
self.visited_points_on_guide_paths[i] = []
+ if show_plots:
+ self.plot_following_ref_exactly(round(time, 2))
+
+ print(f"guide paths = {self.paths}")
+
+ # create a yaml file and dump the robot
+ # for each robot in conflict, reroute its reference trajectory to match the grid solution
+ import copy
+ old_paths = copy.deepcopy(self.paths)
for i in c:
ref, visited_guide_points = get_ref_trajectory(np.array(state[i]),
np.array(self.paths[i]),
@@ -500,7 +541,9 @@ class MultiPathTrackerDB(MultiPathTracker):
self.visited_points_on_guide_paths[i] = visited_guide_points
self.trajs[i] = ref
+
+
# use MPC to track the new reference trajectories
# include all the robots that were in conflict in the MPC problem
@@ -550,9 +593,71 @@ class MultiPathTrackerDB(MultiPathTracker):
self.control
)
+ # for each robot in conflict, reroute its reference trajectory to match the grid solution
+ import copy
+ old_paths = copy.deepcopy(self.paths)
for i in c:
u_next[i] = [u_mpc[i*2, 0], u_mpc[i*2+1, 0]]
x_next[i] = [x_mpc[i*3, 1], x_mpc[i*3+1, 1], x_mpc[i*3+2, 1]]
+
+ # update the guide paths of the robots to reflect the new state
+ new_state = self.ego_to_global_roomba(state[i], x_mpc)
+ new_theta = x_mpc[i*3+2, :]
+
+ print(f"new state = {new_state}")
+ print(f"new theta = {new_theta}")
+
+ # plan from the last point of the ref path to the robot's goal
+ # plan an RRT path from the current state to the goal
+ start = (new_state[0][-1], new_state[1][-1])
+ goal = (old_paths[i][0][-1], old_paths[i][1][-1])
+
+ # plot the start and goal, with the obstacles in the environment
+ # if show_plots:
+ # self.plot_start_goal(start, goal, self.env.circle_obs, self.env.rect_obs)
+
+ rrtpath, tree = plan_decoupled_path(self.settings["sampling_based_planner"]["name"],
+ start,
+ goal,
+ self.env,
+ self.radius,
+ self.env.circle_obs,
+ self.env.rect_obs,
+ vis=False,
+ iter=self.settings["sampling_based_planner"]["num_samples"],
+ obstacle_tol=.5)
+
+ xs = []
+ ys = []
+
+ for node in rrtpath:
+ xs.append(node[0])
+ ys.append(node[1])
+
+ wp = [xs,ys]
+
+ # Path from waypoint interpolation
+ path = compute_path_from_wp(wp[0], wp[1], 0.2)
+
+ # combine the path with new_ref
+ new_ref_x = np.concatenate((new_state[0, :], path[0]))
+ new_ref_y = np.concatenate((new_state[1, :], path[1]))
+ new_ref_theta = np.concatenate((new_theta, path[2]))
+
+ self.paths[i] = np.array([new_ref_x, new_ref_y, new_ref_theta])
+
+
+
+ # plot the old path and the new path, side by side
+ # if show_plots:
+ # self.plot_old_and_new_guides(old_paths[i], self.paths[i], new_ref[0,:], new_ref[1,:])
+
+
+ self.visited_points_on_guide_paths[i] = []
+
+
+
+
else:
print("The robots are too far apart to place a grid")
print("Proceeding with the current paths")
@@ -593,195 +698,3 @@ class MultiPathTrackerDB(MultiPathTracker):
return waiting
- def draw_grid(self, state, grid_origin, obstacle_map, 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
- - obstacle_map (bool array): the obstacle map 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
- import matplotlib.cm as cm
-
- # Plot the current state of each robot using the current state
- colors = cm.rainbow(np.linspace(0, 1, self.num_robots))
- for i in range(self.num_robots):
- plot_roomba(state[i][0], state[i][1], state[i][2], colors[i], False, self.radius)
-
- # draw the horizontal and vertical lines of the grid
- 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-')
- # 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-')
-
- # draw the obstacles
- for obs in self.circle_obs:
- circle = plt.Circle((obs[0], obs[1]), obs[2], color='red', fill=False)
- plt.gca().add_artist(circle)
-
- for rect_obs in self.rect_obs:
- rect = plt.Rectangle((rect_obs[0], rect_obs[1]), rect_obs[2], rect_obs[3], color='k', fill=True)
- plt.gca().add_artist(rect)
-
- # if a cell is true in the obstacle map, that cell is an obstacle.
- # fill that cell with a translucent red color
- for i in range(self.grid_size):
- for j in range(self.grid_size):
- if obstacle_map[i][j]:
- x, y = self.get_grid_cell_location(i, j, grid_origin)
- # create a square with the top left corner at (x - cell_size/2, y - cell_size/2)
- square = plt.Rectangle((x - self.cell_size/2, y - self.cell_size/2), self.cell_size, self.cell_size, color='r', alpha=0.3)
- plt.gca().add_artist(square)
-
-
- # plot the robots' continuous space subgoals
- for idx in robots_in_conflict:
- traj = self.ego_to_global_roomba(state[idx], self.trajs[idx])
- x = traj[0][-1]
- y = traj[1][-1]
- plt.plot(x, y, '^', color=colors[idx])
- circle1 = plt.Circle((x, y), self.radius, color=colors[idx], fill=False)
- plt.gca().add_artist(circle1)
-
- # set the size of the plot to be 10x10
- plt.xlim(self.x_range[0], self.x_range[1])
- plt.xlim(self.y_range[0], self.y_range[1])
-
- # force equal aspect ratio
- plt.gca().set_aspect('equal', adjustable='box')
-
- # title
- plt.title("Discrete Solution")
-
- plt.show()
-
- def draw_grid_solution(self, initial_guess, state, grid_origin, obstacle_map, robots_in_conflict, time):
- """
- 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
- - obstacle_map (bool array): the obstacle map 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
- import matplotlib.cm as cm
-
- # Plot the current state of each robot using the most recent values from
- # x_history, y_history, and h_history
- colors = cm.rainbow(np.linspace(0, 1, self.num_robots))
-
- for i in range(self.num_robots):
- plot_roomba(state[i][0], state[i][1], state[i][2], colors[i], False, self.radius)
-
- # plot the goal of each robot with solid circle
- # for i in range(self.num_robots):
- # x, y, theta = self.paths[i][:, -1]
- # plt.plot(x, y, 'o', color=colors[i])
- # circle1 = plt.Circle((x, y), self.radius, color=colors[i], fill=False)
- # plt.gca().add_artist(circle1)
-
- # draw the horizontal and vertical lines of the grid
- 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-')
- # 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-')
-
- # draw the obstacles
- for obs in self.circle_obs:
- circle = plt.Circle((obs[0], obs[1]), obs[2], color='red', fill=False)
- plt.gca().add_artist(circle)
-
- for rect_obs in self.rect_obs:
- rect = plt.Rectangle((rect_obs[0], rect_obs[1]), rect_obs[2], rect_obs[3], color='k', fill=True)
- plt.gca().add_artist(rect)
-
- # if a cell is true in the obstacle map, that cell is an obstacle.
- # fill that cell with a translucent red color
- for i in range(self.grid_size):
- for j in range(self.grid_size):
- if obstacle_map[i][j]:
- x, y = self.get_grid_cell_location(i, j, grid_origin)
- # create a square with the top left corner at (x - cell_size/2, y - cell_size/2)
- square = plt.Rectangle((x - self.cell_size/2, y - self.cell_size/2), self.cell_size, self.cell_size, color='r', alpha=0.3)
- plt.gca().add_artist(square)
-
-
- # for i in range(num_robots):
- for robot_idx, i in enumerate(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 robots_in_conflict:
- traj = self.ego_to_global_roomba(state[idx], self.trajs[idx])
- x = traj[0][-1]
- y = traj[1][-1]
- plt.plot(x, y, '^', color=colors[idx])
- circle1 = plt.Circle((x, y), self.radius, color=colors[idx], fill=False)
- plt.gca().add_artist(circle1)
-
- # set the size of the plot to be 10x10
- plt.xlim(self.x_range[0], self.x_range[1])
- plt.xlim(self.y_range[0], self.y_range[1])
-
- # force equal aspect ratio
- plt.gca().set_aspect('equal', adjustable='box')
-
- # title
- plt.title("Discrete Solution")
-
- # plt.savefig(f"figures/sim_00{time}5.png")
- plt.show()
-
- def plot_trajs(self, state, traj1, traj2, radius):
- """
- Plot the trajectories of two robots.
- """
- import matplotlib.pyplot as plt
- import matplotlib.cm as cm
-
- # Plot the current state of each robot using the most recent values from
- # x_history, y_history, and h_history
- colors = cm.rainbow(np.linspace(0, 1, self.num_robots))
-
- for i in range(self.num_robots):
- plot_roomba(state[i][0], state[i][1], state[i][2], colors[i], False, self.radius)
-
- # plot the goal of each robot with solid circle
- for i in range(self.num_robots):
- x, y, theta = self.paths[i][:, -1]
- plt.plot(x, y, 'o', color=colors[i])
- circle1 = plt.Circle((x, y), self.radius, color=colors[i], fill=False)
- plt.gca().add_artist(circle1)
-
- for i in range(traj1.shape[1]):
- circle1 = plt.Circle((traj1[0, i], traj1[1, i]), radius, color='k', fill=False)
- plt.gca().add_artist(circle1)
-
- for i in range(traj2.shape[1]):
- circle2 = plt.Circle((traj2[0, i], traj2[1, i]), radius, color='k', fill=False)
- plt.gca().add_artist(circle2)
-
- # set the size of the plot to be 10x10
- plt.xlim(self.x_range[0], self.x_range[1])
- plt.xlim(self.y_range[0], self.y_range[1])
-
- # force equal aspect ratio
- plt.gca().set_aspect('equal', adjustable='box')
-
-
- plt.show()