Skip to content
Snippets Groups Projects
Commit fce350a6 authored by rachelmoan's avatar rachelmoan
Browse files

Use one MPC for both single and multirobot

parent 1024ee5b
No related branches found
No related tags found
No related merge requests found
......@@ -214,8 +214,6 @@ class MultiMPC:
"""
As, Bs, Cs = [], [], []
for i in range(self.num_robots):
# print(f"initial_state[i] = {initial_state[i]}")
# print(f"prev_cmd[i] = {prev_cmd[i]}")
A, B, C = self.robot_model.linearize(initial_state[i], prev_cmd[i], self.dt)
As.append(A)
Bs.append(B)
......@@ -223,7 +221,6 @@ class MultiMPC:
solver_options = {'ipopt.print_level': self.print_level,
'print_time': self.print_time,
# 'ipopt.tol': 1e-3,
'ipopt.acceptable_tol': self.acceptable_tol,
'ipopt.acceptable_iter': self.acceptable_iter}
......
......@@ -44,8 +44,6 @@ class MultiPathTracker:
self.coupled_mpc = MultiMPC(self.num_robots, dynamics, T, DT, settings, env.circle_obs, env.rect_obs)
self.mpc = MPC(dynamics, T, DT, settings, env.circle_obs, env.rect_obs)
self.circle_obs = env.circle_obs
self.rect_obs = env.rect_obs
......
......@@ -12,7 +12,7 @@ from shapely.geometry import Point
from shapely.geometry import Polygon
from guided_mrmp.utils.helpers import plan_decoupled_path
from guided_mrmp.controllers.mpc import MPC
from guided_mrmp.controllers.multi_mpc import MultiMPC
from guided_mrmp.controllers.place_grid import place_grid
class DiscreteRobot:
......@@ -69,6 +69,15 @@ class MultiPathTrackerDB(MultiPathTracker):
return temp_starts, temp_goals
def get_subgoals(self, state, robots_in_conflict):
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]
subgoals.append((x, y))
return subgoals
def create_discrete_robots(self, starts, goals):
discrete_robots = []
for i in range(len(starts)):
......@@ -274,19 +283,25 @@ class MultiPathTrackerDB(MultiPathTracker):
self.target_v,
self.T,
self.DT,
[])
self.visited_points_on_guide_paths[i])
self.visited_points_on_guide_paths[i] = visited_guide_points
targets.append(ref)
mpc = MultiMPC(1, # num robots
self.dynamics,
self.T,
self.DT,
self.settings,
self.env.circle_obs,
self.env.rect_obs)
curr_state = np.array([0,0,0])
x_mpc, u_mpc = self.mpc.step(
curr_state = np.zeros((1, 3))
x_mpc, u_mpc = mpc.step(
curr_state,
ref,
self.control[i]
[ref],
[self.control[i]]
)
x_mpc_global = self.ego_to_global_roomba(state[i], x_mpc)
......@@ -314,15 +329,21 @@ class MultiPathTrackerDB(MultiPathTracker):
[])
mpc = MPC(self.dynamics, self.T, self.DT, self.settings)
mpc = MultiMPC(1, # num robots
self.dynamics,
self.T,
self.DT,
self.settings,
self.env.circle_obs,
self.env.rect_obs)
curr_state = np.array([0,0,0])
curr_state = np.zeros((1, 3))
x_mpc, u_mpc = mpc.step(
curr_state,
ref,
current_control
)
curr_state,
[ref],
[self.control[idx]]
)
# only the first one is used to advance the simulation
control = [u_mpc[0, 0], u_mpc[1, 0]]
......@@ -379,15 +400,21 @@ class MultiPathTrackerDB(MultiPathTracker):
# Put down a local grid
self.cell_size = self.radius*3
self.cell_size = self.radius*2
self.grid_size = 5
if diff < 4*self.cell_size:
if diff < 5*self.cell_size:
circle_obs = []
for obs in self.circle_obs:
circle_obs.append(('c',obs[0], obs[1], obs[2]))
subgoals = self.get_subgoals(state, c)
grid_origin, centers = place_grid(robot_positions,
cell_size=self.radius*3,
cell_size=self.radius*2,
grid_size=5,
subgoals=[],
obstacles=self.circle_obs)
subgoals=subgoals,
obstacles=circle_obs)
grid_obstacle_map = self.get_obstacle_map(grid_origin, self.grid_size, self.radius)
# Solve a discrete version of the problem
......@@ -409,6 +436,7 @@ class MultiPathTrackerDB(MultiPathTracker):
if show_plots: 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)
......@@ -418,7 +446,6 @@ 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])
......@@ -441,11 +468,17 @@ class MultiPathTrackerDB(MultiPathTracker):
wp = [xs,ys]
# Path from waypoint interpolation
self.paths[i] = compute_path_from_wp(wp[0], wp[1], 0.05)
else:
print("RRT* failed to find a path")
self.paths[i] = old_paths[i]
# Path from waypoint interpolation
path = compute_path_from_wp(wp[0], wp[1], 0.05)
# combine the path with new_ref
new_ref_x = np.concatenate((new_ref[0, :], path[0]))
new_ref_y = np.concatenate((new_ref[1, :], path[1]))
new_ref_theta = np.concatenate((new_ref[2, :], path[2]))
self.paths[i] = np.array([new_ref_x, new_ref_y, new_ref_theta])
self.visited_points_on_guide_paths[i] = []
for i in c:
ref, visited_guide_points = get_ref_trajectory(np.array(state[i]),
......@@ -456,21 +489,31 @@ class MultiPathTrackerDB(MultiPathTracker):
self.visited_points_on_guide_paths[i])
self.visited_points_on_guide_paths[i] = visited_guide_points
curr_state = np.array([0,0,0])
x_mpc, u_mpc = self.mpc.step(
curr_state,
ref,
self.control[i]
)
u_next[i] = [u_mpc[0, 0], u_mpc[1, 0]]
x_mpc_global = self.ego_to_global_roomba(state[i], x_mpc)
x_next[i] = x_mpc_global
self.trajs[i] = ref
# use MPC to track the new reference trajectories
# include all the robots that were in conflict in the MPC problem
mpc = MultiMPC(len(c), # num robots
self.dynamics,
self.T,
self.DT,
self.settings,
self.env.circle_obs,
self.env.rect_obs)
curr_states = np.zeros((len(c), 3))
these_trajs = [self.trajs[i] for i in c]
these_controls = [self.control[i] for i in c]
x_mpc, u_mpc = mpc.step(
curr_states,
these_trajs,
these_controls
)
for i, r in enumerate(c):
u_next[r] = [u_mpc[i*2, 0], u_mpc[i*2+1, 0]]
x_next[r] = [x_mpc[i*3, 1], x_mpc[i*3+1, 1], x_mpc[i*3+2, 1]]
else:
if waiting:
......@@ -482,9 +525,16 @@ class MultiPathTrackerDB(MultiPathTracker):
else:
print("Using coupled solver to resolve conflict")
# dynamycs w.r.t robot frame
mpc = MultiMPC(self.num_robots, # num robots
self.dynamics,
self.T,
self.DT,
self.settings,
self.env.circle_obs,
self.env.rect_obs)
curr_states = np.zeros((self.num_robots, 3))
x_mpc, u_mpc = self.coupled_mpc.step(
x_mpc, u_mpc = mpc.step(
curr_states,
self.trajs,
self.control
......
......@@ -58,12 +58,12 @@ def get_nn_idx(state, path, visited=[]):
distances = np.linalg.norm(path[:2] - state[:2].reshape(2, 1), axis=0)
# Set the distance to infinity for visited points
for point in visited:
point = np.array(point)
# print(f"point = {point}")
# print(f"path[:2] = {path[:2]}")
# Set the distance to infinity for visited points
distances = np.where(np.linalg.norm(path[:2] - point.reshape(2, 1), axis=0) < 1e-3, np.inf, distances)
# for point in visited:
# point = np.array(point)
# # print(f"point = {point}")
# # print(f"path[:2] = {path[:2]}")
# # Set the distance to infinity for visited points
# distances = np.where(np.linalg.norm(path[:2] - point.reshape(2, 1), axis=0) < 1e-3, np.inf, distances)
return np.argmin(distances)
......@@ -101,10 +101,27 @@ def get_ref_trajectory(state, path, target_v, T, DT, path_visited_points=[]):
xref = np.zeros((3, K)) # Reference trajectory for [x, y, theta]
# find the nearest path point to the current state
ind = get_nn_idx(state, path, path_visited_points)
path_distances = [0]
for i in range(1, len(path)):
dist = np.linalg.norm(np.array(path[i]) - np.array(path[i-1]))
path_distances.append(path_distances[-1] + dist)
# Find the last visited point
last_visited_idx = 0 if path_visited_points == [] else path_visited_points[-1]
# Find the spatially closest point after the last visited point
next_ind = last_visited_idx + 2
path_visited_points.append([path[0, ind], path[1, ind]])
ind = next_ind
# ind = get_nn_idx(state, path, path_visited_points)
# min_dist = float('inf')
# for i in range(last_visited_idx+2, len(path)):
# dist = np.linalg.norm(np.array(path[i][:2]) - np.array(state[:2]))
# if dist < min_dist:
# min_dist = dist
# ind = i
path_visited_points.append(ind)
# calculate the cumulative distance along the path
cdist = np.append([0.0], np.cumsum(np.hypot(np.diff(path[0, :]), np.diff(path[1, :]))))
......@@ -130,4 +147,55 @@ def get_ref_trajectory(state, path, target_v, T, DT, path_visited_points=[]):
xref[2, :] = (xref[2, :] + np.pi) % (2.0 * np.pi) - np.pi
xref[2, :] = fix_angle_reference(xref[2, :], xref[2, 0])
return xref, path_visited_points
\ No newline at end of file
return xref, path_visited_points
# def get_ref_trajectory(state, path, target_v, T, DT, path_visited_points=[]):
# """
# Generates a reference trajectory for the Roomba.
# Args:
# state (array-like): Current state [x, y, theta]
# path (ndarray): Path points [x, y, theta] in the global frame
# path_visited_points (array-like): Visited path points [[x, y], [x, y], ...]
# target_v (float): Desired speed
# T (float): Control horizon duration
# DT (float): Control horizon time-step
# Returns:
# ndarray: Reference trajectory [x_k, y_k, theta_k] in the ego frame
# """
# K = int(T / DT)
# xref = np.zeros((3, K)) # Reference trajectory for [x, y, theta]
# # find the nearest path point to the current state
# ind = get_nn_idx(state, path, path_visited_points)
# path_visited_points.append([path[0, ind], path[1, ind]])
# # calculate the cumulative distance along the path
# cdist = np.append([0.0], np.cumsum(np.hypot(np.diff(path[0, :]), np.diff(path[1, :]))))
# cdist = np.clip(cdist, cdist[0], cdist[-1])
# # determine where we want the robot to be at each time step
# start_dist = cdist[ind]
# interp_points = [d * DT * target_v + start_dist for d in range(1, K + 1)]
# # interpolate between these points to get the reference trajectory
# xref[0, :] = np.interp(interp_points, cdist, path[0, :])
# xref[1, :] = np.interp(interp_points, cdist, path[1, :])
# xref[2, :] = np.interp(interp_points, cdist, path[2, :])
# # Transform to ego frame
# dx = xref[0, :] - state[0]
# dy = xref[1, :] - state[1]
# xref[0, :] = dx * np.cos(-state[2]) - dy * np.sin(-state[2]) # X
# xref[1, :] = dy * np.cos(-state[2]) + dx * np.sin(-state[2]) # Y
# xref[2, :] = path[2, ind] - state[2] # Theta
# # Normalize the angles
# xref[2, :] = (xref[2, :] + np.pi) % (2.0 * np.pi) - np.pi
# xref[2, :] = fix_angle_reference(xref[2, :], xref[2, 0])
# return xref, path_visited_points
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