From 764b2559dfbb40a40653d347d625e12b438ee9f0 Mon Sep 17 00:00:00 2001
From: rachelmoan <moanrachel516@gmail.com>
Date: Thu, 17 Oct 2024 12:09:16 -0500
Subject: [PATCH] Add warm start to MPC
---
guided_mrmp/planners/db_guided_mrmp.py | 21 +++++++++++++++------
guided_mrmp/utils/robot.py | 3 +++
2 files changed, 18 insertions(+), 6 deletions(-)
diff --git a/guided_mrmp/planners/db_guided_mrmp.py b/guided_mrmp/planners/db_guided_mrmp.py
index bd095f2..f16a617 100644
--- a/guided_mrmp/planners/db_guided_mrmp.py
+++ b/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)
diff --git a/guided_mrmp/utils/robot.py b/guided_mrmp/utils/robot.py
index e717bbb..59e76fc 100644
--- a/guided_mrmp/utils/robot.py
+++ b/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
--
GitLab