From 8c6b17f24aec66d252dbcbf246b8b526946e096b Mon Sep 17 00:00:00 2001
From: rachelmoan <moanrachel516@gmail.com>
Date: Thu, 9 Jan 2025 15:11:02 -0600
Subject: [PATCH] Fixing visualization, removing magic constants
---
.../controllers/multi_path_tracking.py | 2 +
.../controllers/multi_path_tracking_db.py | 270 ++++++------------
guided_mrmp/main.py | 3 +-
guided_mrmp/simulator.py | 58 ++--
settings_files/settings.yaml | 3 +-
5 files changed, 137 insertions(+), 199 deletions(-)
diff --git a/guided_mrmp/controllers/multi_path_tracking.py b/guided_mrmp/controllers/multi_path_tracking.py
index 2ac036e..775e0b3 100644
--- a/guided_mrmp/controllers/multi_path_tracking.py
+++ b/guided_mrmp/controllers/multi_path_tracking.py
@@ -18,6 +18,8 @@ class MultiPathTracker:
"""
# State of the robot [x,y, heading]
self.env = env
+ self.x_range = env.boundary[0]
+ self.y_range = env.boundary[1]
self.states = initial_positions
self.num_robots = len(initial_positions)
self.dynamics = dynamics
diff --git a/guided_mrmp/controllers/multi_path_tracking_db.py b/guided_mrmp/controllers/multi_path_tracking_db.py
index a425dda..4ab24dc 100644
--- a/guided_mrmp/controllers/multi_path_tracking_db.py
+++ b/guided_mrmp/controllers/multi_path_tracking_db.py
@@ -1,4 +1,4 @@
-from guided_mrmp.planners.singlerobot.RRTStar import RRTStar
+from guided_mrmp.planners.RRTStar import RRTStar
from guided_mrmp.utils import Roomba
from guided_mrmp.utils import Conflict, Robot, Env
@@ -41,7 +41,7 @@ def plot_roomba(x, y, yaw, color, fill, radius):
ax.arrow(x, y, dx, dy, head_width=0.1, head_length=0.1, fc='r', ec='r')
class MultiPathTrackerDB(MultiPathTracker):
- def get_temp_starts_and_goals(self):
+ def get_temp_starts_and_goals(self, grid_origin):
# the temporary starts are the current positions of the robots snapped to the grid
# 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
@@ -49,10 +49,9 @@ class MultiPathTrackerDB(MultiPathTracker):
import math
temp_starts = []
for r in range(self.num_robots):
- print(f"self.states = {self.states}")
x, y, theta = self.states[r]
- cell_x = min(max(math.floor((x - self.top_left_grid[0]) / self.cell_size), 0), self.grid_size - 1)
- cell_y = min(max(math.floor((self.top_left_grid[1] - y) / 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)
temp_starts.append([cell_x, cell_y])
@@ -62,13 +61,10 @@ class MultiPathTrackerDB(MultiPathTracker):
traj = self.ego_to_global_roomba(self.states[r], self.trajs[r])
x = traj[0][-1]
y = traj[1][-1]
- cell_x = min(max(math.floor((x - self.top_left_grid[0]) / self.cell_size), 0), self.grid_size - 1)
- cell_y = min(max(math.floor((self.top_left_grid[1] - y) / 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)
temp_goals.append([cell_x,cell_y])
- # self.starts = temp_starts
- # self.goals = temp_goals
-
return temp_starts, temp_goals
def create_discrete_robots(self, starts, goals):
@@ -79,12 +75,16 @@ class MultiPathTrackerDB(MultiPathTracker):
discrete_robots.append(DiscreteRobot(start, goal, i))
return discrete_robots
- def get_discrete_solution(self, conflict, all_conflicts, grid):
- # create an instance of a discrete solver
+ def get_discrete_solution(self, conflict, all_conflicts, grid, grid_origin):
+ """
+ Inputs:
+ - conflict (list): list of robot idxs involved in the conflict
+ - all_conflicts (list): list of all conflicts
+ - grid (bool array): the obstacle map of grid that we placed
+ - grid_origin (tuple): the top left corner of the grid in continuous space
+ """
- starts, goals = self.get_temp_starts_and_goals()
- # print(f"temp starts = {starts}")
- # print(f"temp goals = {goals}")
+ starts, goals = self.get_temp_starts_and_goals(grid_origin)
disc_robots = self.create_discrete_robots(starts, goals)
@@ -99,27 +99,17 @@ class MultiPathTrackerDB(MultiPathTracker):
this_conflict.append(disc_robots[i])
disc_all_conflicts.append(this_conflict)
-
-
- print(f"this conflict = {disc_conflict}")
- print(f"all conflicts = {all_conflicts}")
-
- # visualize the grid
- self.draw_grid(self.states)
-
grid_solver = DiscreteResolver(disc_conflict, disc_robots, starts, goals, disc_all_conflicts,grid, self.lib_2x3, self.lib_3x3, self.lib_2x5)
subproblem = grid_solver.find_subproblem()
if subproblem is None:
print("Couldn't find a discrete subproblem")
return None
- # print(f"subproblem = {subproblem}")
grid_solution = grid_solver.solve_subproblem(subproblem)
- # print(f"grid_solution = {grid_solution}")
return grid_solution
def get_initial_guess(self, grid_solution, num_robots, N, cp_dist):
- # turn this solution into an initial guess
+
# turn this solution into an initial guess
initial_guess_state = np.zeros((num_robots*3, N+1))
# the initial guess for time is the length of the longest path in the solution
@@ -130,7 +120,6 @@ class MultiPathTrackerDB(MultiPathTracker):
for i in range(num_robots):
- print(f"Robot {i+1} solution:")
rough_points = np.array(grid_solution[i])
points = []
for point in rough_points:
@@ -139,11 +128,6 @@ class MultiPathTrackerDB(MultiPathTracker):
points.append([point[0]*self.cell_size, point[1]*self.cell_size])
points = np.array(points)
- print(f"points = {points}")
-
- # plot the points
- plt.plot(points[:, 0], points[:, 1], 'o-')
-
smoothed_curve, _ = smooth_path(points, N+1, cp_dist)
@@ -153,7 +137,6 @@ class MultiPathTrackerDB(MultiPathTracker):
current_robot_x_pos = self.states[i][0]
current_robot_y_pos = self.states[i][1]
-
# translate the initial guess so that the first point is at (0,0)
initial_guess_state[i*3, :] -= initial_guess_state[i*3, 0]
initial_guess_state[i*3 + 1, :] -= initial_guess_state[i*3+1, 0]
@@ -168,8 +151,6 @@ class MultiPathTrackerDB(MultiPathTracker):
initial_guess_state[i*3 + 2, :] = headings
- plt.show()
-
initial_guess_control = np.zeros((num_robots*2, N))
dt = initial_guess_T / N
@@ -178,7 +159,6 @@ class MultiPathTrackerDB(MultiPathTracker):
x = initial_guess_state[i*3, :] # x
y = initial_guess_state[i*3 + 1, :] # y
-
change_in_position = []
for j in range(len(x)-1):
pos1 = np.array([x[j], y[j]])
@@ -221,62 +201,48 @@ class MultiPathTrackerDB(MultiPathTracker):
self.temp_avg_x = avg_x
self.temp_avg_y = avg_y
- print(f"avg_x = {avg_x}, 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))
- print(f"avg_x = {avg_x} - {int(self.grid_size*self.cell_size/2)}")
- print(f"avg_y = {avg_y} - {int(self.grid_size*self.cell_size/2)}")
- self.top_left_grid = (avg_x - int(self.grid_size*self.cell_size/2), avg_y + int(self.grid_size*self.cell_size/2))
- print(f"self.grid_size = {self.grid_size}")
- print(f"top_left_grid = {self.top_left_grid}")
-
- self.draw_grid(self.states)
# 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.
- all_starts_goals_equal = self.all_starts_goals_equal()
+ start_equal = self.start_equal(grid_origin)
import copy
- original_top_left = copy.deepcopy(self.top_left_grid)
+ 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
- # print(f"x = {x}, y = {y}")
- self.top_left_grid = (original_top_left[0] + x*self.cell_size*.5, original_top_left[1] - y*self.cell_size*.5)
- all_starts_goals_equal = self.all_starts_goals_equal()
- # self.draw_grid()
- if not all_starts_goals_equal: break
+ grid_origin = (original_top_left[0] + x*self.cell_size*.5, original_top_left[1] - y*self.cell_size*.5)
+ start_equal = self.start_equal(grid_origin)
+ if not start_equal: break
- if all_starts_goals_equal:
+ if start_equal:
for y in np.arange(y_shift[0], y_shift[1],.5):
x =0
- # print(f"x = {x}, y = {y}")
- self.top_left_grid = (original_top_left[0] + x*self.cell_size*.5, original_top_left[1] - y*self.cell_size*.5)
- all_starts_goals_equal = self.all_starts_goals_equal()
- # self.draw_grid()
- if not all_starts_goals_equal: break
-
- print(f"updated top_left_grid = {self.top_left_grid}")
- # self.draw_grid()
+ grid_origin = (original_top_left[0] + x*self.cell_size*.5, original_top_left[1] - y*self.cell_size*.5)
+ start_equal = self.start_equal(grid_origin)
+ if not start_equal: break
- if all_starts_goals_equal:
- print("All starts and goals are equal")
+ if start_equal:
+ print("Some robots are in the same cell")
return None
- grid = self.get_obstacle_map()
+ grid = self.get_obstacle_map(grid_origin)
- return grid
+ return grid, grid_origin
- def get_obstacle_map(self):
+ def get_obstacle_map(self, grid_origin):
"""
Create a map of the environment with obstacles
"""
@@ -287,7 +253,7 @@ class MultiPathTrackerDB(MultiPathTracker):
grid = np.zeros((self.grid_size, self.grid_size))
for i in range(self.grid_size):
for j in range(self.grid_size):
- x, y = self.get_grid_cell_location(i, j)
+ 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:
@@ -296,26 +262,27 @@ class MultiPathTrackerDB(MultiPathTracker):
return grid
- def get_grid_cell(self, x, y):
+ 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
# find the closest grid cell that is not an obstacle
- cell_x = min(max(math.floor((x - self.top_left_grid[0]) / self.cell_size), 0), self.grid_size - 1)
- cell_y = min(max(math.floor((self.top_left_grid[1] - y) / 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)
return cell_x, cell_y
- def get_grid_cell_location(self, 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 = self.top_left_grid[0] + (cell_x + 0.5) * self.cell_size
- y = self.top_left_grid[1] - (cell_y + 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
return x, y
+
def plot_trajs(self, state, traj1, traj2, radius):
"""
Plot the trajectories of two robots.
@@ -336,8 +303,7 @@ class MultiPathTrackerDB(MultiPathTracker):
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)
@@ -346,11 +312,9 @@ class MultiPathTrackerDB(MultiPathTracker):
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(0, 10)
- plt.ylim(0, 10)
+ 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')
@@ -358,8 +322,12 @@ class MultiPathTrackerDB(MultiPathTracker):
plt.show()
- def draw_grid(self,state):
- starts, goals = self.get_temp_starts_and_goals()
+ def draw_grid(self, state, grid_origin):
+ """
+ inputs:
+ - state (list): list of robot states
+ - grid_origin (tuple): top left corner of the grid
+ """
# draw the whole environment with the local grid drawn on top
import matplotlib.pyplot as plt
@@ -370,7 +338,7 @@ class MultiPathTrackerDB(MultiPathTracker):
colors = cm.rainbow(np.linspace(0, 1, self.num_robots))
for i in range(self.num_robots):
- plot_roomba(self.states[i][0], self.states[i][1], self.states[i][2], colors[i], False, self.radius)
+ 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):
@@ -382,11 +350,11 @@ class MultiPathTrackerDB(MultiPathTracker):
# draw the horizontal and vertical lines of the grid
for i in range(self.grid_size + 1):
# Draw vertical lines
- plt.plot([self.top_left_grid[0] + i * self.cell_size, self.top_left_grid[0] + i * self.cell_size],
- [self.top_left_grid[1], self.top_left_grid[1] - self.grid_size * self.cell_size], 'k-')
+ 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([self.top_left_grid[0], self.top_left_grid[0] + self.grid_size * self.cell_size],
- [self.top_left_grid[1] - i * self.cell_size, self.top_left_grid[1] - i * self.cell_size], 'k-')
+ 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:
@@ -397,7 +365,7 @@ class MultiPathTrackerDB(MultiPathTracker):
# plot the robots' continuous space subgoals
for idx in range(self.num_robots):
- traj = self.ego_to_global_roomba(self.states[idx], self.trajs[idx])
+ 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])
@@ -405,15 +373,15 @@ class MultiPathTrackerDB(MultiPathTracker):
plt.gca().add_artist(circle1)
# set the size of the plot to be 10x10
- plt.xlim(0, 10)
- plt.ylim(0, 10)
+ 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()
- def draw_grid_solution(self, state, state_of_robots):
+ def draw_grid_solution(self, state, state_of_robots, grid_origin):
# draw the whole environment with the local grid drawn on top
import matplotlib.pyplot as plt
@@ -436,11 +404,11 @@ class MultiPathTrackerDB(MultiPathTracker):
# draw the horizontal and vertical lines of the grid
for i in range(self.grid_size + 1):
# Draw vertical lines
- plt.plot([self.top_left_grid[0] + i * self.cell_size, self.top_left_grid[0] + i * self.cell_size],
- [self.top_left_grid[1], self.top_left_grid[1] - self.grid_size * self.cell_size], 'k-')
+ 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([self.top_left_grid[0], self.top_left_grid[0] + self.grid_size * self.cell_size],
- [self.top_left_grid[1] - i * self.cell_size, self.top_left_grid[1] - i * self.cell_size], 'k-')
+ 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:
@@ -463,8 +431,8 @@ class MultiPathTrackerDB(MultiPathTracker):
plt.gca().add_artist(circle1)
# set the size of the plot to be 10x10
- plt.xlim(0, 10)
- plt.ylim(0, 10)
+ 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')
@@ -474,34 +442,28 @@ class MultiPathTrackerDB(MultiPathTracker):
plt.show()
- def all_starts_goals_equal(self):
+ def start_equal(self, grid_origin):
"""
- Check if, for every robot, the cell value of the start and the cell value
- of the goal are the same.
+ Check if the start cells of any two robots are the same
"""
- all_starts_goals_equal = True
- for r in range(self.num_robots):
- start = self.states[r]
- traj = self.ego_to_global_roomba(start, self.trajs[r])
- goal = [traj[0, -1], traj[1, -1]]
-
- start_cell = self.get_grid_cell(start[0], start[1])
- goal_cell = self.get_grid_cell(goal[0], goal[1])
+ for i in range(self.num_robots):
+ for j in range(i + 1, self.num_robots):
+ start_i = self.states[i]
+ start_j = self.states[j]
- if start_cell != goal_cell:
- all_starts_goals_equal = False
- break
+ 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)
- return all_starts_goals_equal
+ if cell_i == cell_j:
+ return True
+ return False
def advance(self, state, show_plots=False):
# optimization loop
- # start=time.time()
self.states = state
self.update_ref_paths = False
- # Get Reference_traj -> inputs are in worldframe
# 1. Get the reference trajectory for each robot
targets = []
for i in range(self.num_robots):
@@ -512,9 +474,7 @@ class MultiPathTrackerDB(MultiPathTracker):
self.DT,
self.visited_points_on_guide_paths[i])
- print(f"visited_guide_points = {visited_guide_points}")
self.visited_points_on_guide_paths[i] = visited_guide_points
- print(f"visited_points_on_guide_paths[i] = {self.visited_points_on_guide_paths[i]}")
targets.append(ref)
self.trajs = targets
@@ -523,20 +483,15 @@ class MultiPathTrackerDB(MultiPathTracker):
self.all_conflicts = []
for i in range(self.num_robots):
for j in range(i + 1, self.num_robots):
- print(f"targets[i] = {targets[i]}")
traj1 = self.ego_to_global_roomba(state[i], targets[i])
traj2 = self.ego_to_global_roomba(state[j], targets[j])
if self.trajectories_overlap(traj1, traj2, self.radius):
# plot the trajectories
- self.plot_trajs(state, traj1, traj2, self.radius)
-
+ if show_plots: self.plot_trajs(state, traj1, traj2, self.radius)
- print(f"Collision detected between robot {i} and robot {j}")
self.all_conflicts.append((i, j))
-
-
for c in self.all_conflicts:
# 3. If they do collide, then reroute the reference trajectories of these robots
@@ -545,7 +500,8 @@ class MultiPathTrackerDB(MultiPathTracker):
robot_positions = [state[i] for i in robots]
# Put down a local grid
- self.grid = self.place_grid(robot_positions)
+ grid_obstacle_map, grid_origin = self.place_grid(robot_positions)
+ if show_plots: self.draw_grid(state, grid_origin)
# set the starts (robots' current positions)
self.starts = []
@@ -558,13 +514,9 @@ class MultiPathTrackerDB(MultiPathTracker):
y = traj[1][-1]
self.goals.append([x,y])
-
-
# Solve a discrete version of the problem
# Find a subproblem and solve it
- grid_solution = self.get_discrete_solution(c, [c],self.grid)
-
-
+ grid_solution = self.get_discrete_solution(c, [c], grid_obstacle_map, grid_origin)
if grid_solution:
@@ -572,11 +524,8 @@ class MultiPathTrackerDB(MultiPathTracker):
initial_guess = self.get_initial_guess(grid_solution, self.num_robots, 20, 1)
initial_guess_state = initial_guess['X']
- self.draw_grid_solution(initial_guess_state, self.states)
+ if show_plots: self.draw_grid_solution(initial_guess_state, self.states, grid_origin)
- print(f"initial_guess_state shape = {initial_guess_state.shape}")
- print(f"initial_guess_state = {initial_guess_state}")
-
# for each robot in conflict, reroute its reference trajectory to match the grid solution
num_robots_in_conflict = len(c)
import copy
@@ -584,22 +533,15 @@ class MultiPathTrackerDB(MultiPathTracker):
self.paths = []
for i in range(num_robots_in_conflict):
- r = c[i]
new_ref = initial_guess_state[i*3:i*3+3, :]
- print(f"Robot {r} rerouting to {new_ref}")
# 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
- # last_point = new_ref[:, 0]
- # x_start = (last_point[0], last_point[1])
-
x_start = (new_ref[:, -1][0], new_ref[:, -1][1])
x_goal = (old_paths[i][:, -1][0], old_paths[i][:, -1][1])
- print(f"x_start = {x_start}, x_goal = {x_goal}")
-
- rrtstar2 = RRTStar(self.env,x_start, x_goal, 0.5, 0.05, 1000, r=2.0)
+ rrtstar2 = RRTStar(self.env,x_start, x_goal, 0.5, 0.05, 500, r=2.0)
rrtstarpath2,tree = rrtstar2.run()
rrtstarpath2 = list(reversed(rrtstarpath2))
xs = new_ref[0, :].tolist()
@@ -623,12 +565,8 @@ class MultiPathTrackerDB(MultiPathTracker):
self.DT,
self.visited_points_on_guide_paths[i])
- print(f"visited_guide_points = {visited_guide_points}")
self.visited_points_on_guide_paths[i] = visited_guide_points
- print(f"visited_points_on_guide_paths[i] = {self.visited_points_on_guide_paths[i]}")
-
- print(f"Robot {i} reference trajectory = {ref}")
targets.append(ref)
self.trajs = targets
@@ -639,9 +577,6 @@ class MultiPathTrackerDB(MultiPathTracker):
# the conflict later
print("No grid solution found, proceeding with the current paths")
-
-
-
# dynamycs w.r.t robot frame
# curr_state = np.array([0, 0, self.state[2], 0])
curr_states = np.zeros((self.num_robots, 3))
@@ -658,39 +593,7 @@ class MultiPathTrackerDB(MultiPathTracker):
for i in range(self.num_robots):
self.control.append([u_mpc[i*2, 0], u_mpc[i*2+1, 0]])
- # if len(self.all_conflicts) > 0:
- # # update the reference paths for each robot
- # if grid_solution:
- # self.update_reference_paths()
-
return x_mpc, self.control
-
- def update_reference_paths(self):
- """
- Update the reference paths for each robot.
- """
- # create copy of current self.paths
- import copy
- old_paths = copy.deepcopy(self.paths)
-
- self.paths = []
- for i in range(self.num_robots):
- # plan an RRT path from the current state to the goal
- x_start = (self.states[i][0], self.states[i][1])
- x_goal = (old_paths[i][:, -1][0], old_paths[i][:, -1][1])
- rrtstar2 = RRTStar(self.env,x_start, x_goal, 0.5, 0.05, 1000, r=2.0)
- rrtstarpath2,tree = rrtstar2.run()
- rrtstarpath2 = list(reversed(rrtstarpath2))
- xs = []
- ys = []
- for node in rrtstarpath2:
- xs.append(node[0])
- ys.append(node[1])
-
- wp = [xs,ys]
-
- # Path from waypoint interpolation
- self.paths.append(compute_path_from_wp(wp[0], wp[1], 0.05))
def main():
@@ -711,7 +614,7 @@ def main():
random.seed(seed)
- initial_pos_1 = np.array([6.0, 2.0, 2.2])
+ initial_pos_1 = np.array([6.0, 2.0, 5.2])
target_vocity = 3.0 # m/s
T = .5 # Prediction Horizon [s]
DT = 0.1 # discretization step [s]
@@ -720,6 +623,7 @@ def main():
x_start = (6, 2) # Starting node
x_goal = (6.5, 8) # Goal node
+
env = Env([0,10], [0,10], [], [])
dynamics = Roomba(settings)
@@ -737,12 +641,16 @@ def main():
wp_1 = [xs,ys]
+ start_heading = np.arctan2(ys[1] - x_start[1], xs[1] - x_start[0])
+ initial_pos_1 = [initial_pos_1[0], initial_pos_1[1], start_heading]
+
+
print(f"wp_1 = {wp_1}")
# sim = PathTracker(initial_position=initial_pos_1, dynamics=dynamics,target_v=target_vocity, T=T, DT=DT, waypoints=wp_1, settings=settings)
# x1,y1,h1 = sim.run(show_plots=False)
# path1 = sim.path
- initial_pos_2 = np.array([6.0, 8.0, 4.8])
+ initial_pos_2 = np.array([6.0, 8.0, 1.5])
target_vocity = 3.0 # m/s
@@ -759,6 +667,10 @@ def main():
wp_2 = [xs,ys]
+ start_heading = np.arctan2(ys[1] - x_start[1], xs[1] - x_start[0])
+ initial_pos_2 = [initial_pos_2[0], initial_pos_2[1], start_heading]
+
+
lib_2x3, lib_3x3, lib_2x5 = initialize_libraries()
sim = MultiPathTrackerDB(env, [initial_pos_1, initial_pos_2], dynamics, target_vocity, T, DT, [wp_1, wp_2], settings, lib_2x3, lib_3x3, lib_2x5)
diff --git a/guided_mrmp/main.py b/guided_mrmp/main.py
index 778002c..ebe3032 100644
--- a/guided_mrmp/main.py
+++ b/guided_mrmp/main.py
@@ -7,7 +7,6 @@ from guided_mrmp.utils import Env, Roomba, Robot, create_random_starts_and_goals
from guided_mrmp.planners import RRT
from guided_mrmp.planners import RRTStar
-from guided_mrmp.planners.db_guided_mrmp import GuidedMRMP
from guided_mrmp.simulator import Simulator
from guided_mrmp.utils.helpers import initialize_libraries
from guided_mrmp.controllers import MultiPathTracker, MultiPathTrackerDB
@@ -146,7 +145,7 @@ if __name__ == "__main__":
)
# Create the simulator
- show_vis = False
+ show_vis = settings['simulator']['show_plots']
sim = Simulator(robots, dynamics_models, env, policy, settings)
# Run the simulation
diff --git a/guided_mrmp/simulator.py b/guided_mrmp/simulator.py
index 58d0f25..830b72b 100644
--- a/guided_mrmp/simulator.py
+++ b/guided_mrmp/simulator.py
@@ -19,7 +19,8 @@ class Simulator:
self.env = env
self.circ_obstacles = env.circle_obs
self.rect_obstacles = env.rect_obs
- self.policy = policy
+ self.policy = policy
+ self.settings = settings
self.state = [robot.current_position for robot in robots]
@@ -27,8 +28,6 @@ class Simulator:
self.dynamics_models = dynamics_models
self.time = 0
- self.scaling_factor = settings['simulator']['scaling_factor']
-
# Helper variables to keep track of the sim
self.sim_time = 0
self.x_history = [ [] for _ in range(self.num_robots) ]
@@ -49,7 +48,7 @@ class Simulator:
"""
# Get the controls from the policy
- x_mpc, controls = self.policy.advance(screen, self.state)
+ x_mpc, controls = self.policy.advance(self.state, show_plots=self.settings['simulator']['show_collision_resolution'])
# # Update the state of each robot
# for i in range(self.num_robots):
@@ -76,9 +75,10 @@ class Simulator:
self.x_history[i].append(self.state[i, 0])
self.y_history[i].append(self.state[i, 1])
self.h_history[i].append(self.state[i, 2])
+ self.optimized_trajectories_hist[i].append([])
# if show_plots: self.plot_sim()
- self.plot_current_world_state()
+ # self.plot_current_world_state()
while 1:
# check if all robots have reached their goal
@@ -87,7 +87,7 @@ class Simulator:
return np.asarray([self.x_history, self.y_history, self.h_history])
# plot the current state of the robots
- self.plot_current_world_state()
+ if show_plots: self.plot_current_world_state()
# get the next control for all robots
x_mpc, controls = self.advance(self.state, self.policy.DT)
@@ -103,12 +103,7 @@ class Simulator:
self.x_history[i].append(self.state[i, 0])
self.y_history[i].append(self.state[i, 1])
self.h_history[i].append(self.state[i, 2])
-
- # use the optimizer output to preview the predicted state trajectory
- # self.optimized_trajectory = self.ego_to_global(x_mpc.value)
- # if show_plots: self.optimized_trajectory = self.ego_to_global_roomba(x_mpc)
- # if show_plots: self.plot_sim()
-
+ self.optimized_trajectories_hist[i].append(self.policy.trajs[i])
def plot_current_world_state(self):
@@ -133,7 +128,6 @@ class Simulator:
circle1 = plt.Circle((x, y), self.policy.radius, color=colors[i], fill=False)
plt.gca().add_artist(circle1)
-
# plot the ref path of each robot
for i in range(self.num_robots):
plt.plot(self.policy.paths[i][0, :], self.policy.paths[i][1, :], '--', color=colors[i])
@@ -143,16 +137,18 @@ class Simulator:
plt.xlim(x_range[0], x_range[1])
plt.ylim(y_range[0], y_range[1])
+ self.plot_all_traj(self.policy.radius)
+
# force equal aspect ratio
plt.gca().set_aspect('equal', adjustable='box')
plt.tight_layout()
- plt.show()
- # plt.draw()
- # plt.pause(0.1)
- # plt.clf()
+ # plt.show()
+ plt.draw()
+ plt.pause(0.3)
+ plt.clf()
def plot_roomba(self, x, y, yaw, color, fill, radius):
"""
@@ -173,3 +169,31 @@ class Simulator:
dy = 1 * np.sin(yaw)
ax.arrow(x, y, dx, dy, head_width=0.1, head_length=0.1, fc='r', ec='r')
+ def plot_all_traj(self, 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):
+ self.plot_roomba(self.x_history[i][-1], self.y_history[i][-1], self.h_history[i][-1], colors[i], False, radius)
+
+ # plot the goal of each robot with solid circle
+ for i in range(self.num_robots):
+ x, y, theta = self.policy.paths[i][:, -1]
+ plt.plot(x, y, 'o', color=colors[i])
+ circle1 = plt.Circle((x, y), radius, color=colors[i], fill=False)
+ plt.gca().add_artist(circle1)
+
+
+ for i in range(self.num_robots):
+ traj = self.optimized_trajectories_hist[i][-1]
+ if len(traj) == 0:
+ continue
+ traj = self.policy.ego_to_global_roomba([self.x_history[i][-1], self.y_history[i][-1], self.h_history[i][-1]], traj)
+ plt.plot(traj[0, :], traj[1, :], '+-',color=colors[i])
diff --git a/settings_files/settings.yaml b/settings_files/settings.yaml
index 4006410..87f799c 100644
--- a/settings_files/settings.yaml
+++ b/settings_files/settings.yaml
@@ -25,7 +25,8 @@ multi_robot_traj_opt:
simulator:
dt: .1
- scaling_factor: 10.0
+ show_plots: 1
+ show_collision_resolution: 1
environment:
circle_obstacles: []
--
GitLab