Add warm start to MPC

guided_mrmp/planners/db_guided_mrmp.py

@@ -118,9 +118,10 @@ class GuidedMRMP:
 
         return conflicts
 
-    def get_next_controls_and_trajectories(self, screen):
+    def get_next_controls_and_trajectories(self,screen):
         """
         Get the next control for each robot.
+
         """
         from joblib import Parallel, delayed
         
@@ -140,14 +141,22 @@ class GuidedMRMP:
             
             mpc = MPC(self.dynamics_models[idx], self.T, self.DT, self.Q, self.Qf, self.R, self.P, self.settings['model_predictive_controller'])
 
-            # dynamics w.r.t robot frame
-            curr_state = np.array([0, 0, 0])
-            x_mpc, u_mpc = mpc.step(curr_state, target_traj, np.array(r.control))
+            if r.last_mpc_trajectory is not None:
+                # dynamics w.r.t robot frame
+                initial_guess = {'X': r.last_mpc_trajectory, 'U': r.last_mpc_controls}
+                curr_state = np.array([0, 0, 0])
+                x_mpc, u_mpc = mpc.step(curr_state, target_traj, np.array(r.control), initial_guess)
+
+            else:
+                # dynamics w.r.t robot frame
+                curr_state = np.array([0, 0, 0])
+                x_mpc, u_mpc = mpc.step(curr_state, target_traj, np.array(r.control))
 
-            # self.add_vis_target_traj(screen, self.scaling_factor, r, x_mpc)
-            
             # only the first one is used to advance the simulation
             control = [u_mpc[0, :], u_mpc[1, :]]
+
+            r.last_mpc_trajectory = x_mpc
+            r.last_mpc_controls = u_mpc
             
             return np.asarray(control), self.ego_to_global(r, x_mpc)
 

guided_mrmp/utils/robot.py

@@ -12,6 +12,9 @@ class Robot:
         self.rrtpath = rrtpath
         self.dynamics_model = dynamics_model
         self.waypoints = waypoints
+
+        self.last_mpc_trajectory = None
+        self.last_mpc_controls = None
         
         self.next_step = None
         self.next_control = None