Use one MPC for both single and multirobot

guided_mrmp/controllers/multi_mpc.py

@@ -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}
 

guided_mrmp/controllers/multi_path_tracking.py

@@ -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
 

guided_mrmp/controllers/multi_path_tracking_db.py

@@ -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

guided_mrmp/controllers/utils.py

@@ -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