Merge pull request #18 from pavelacamposp/feature/parameterize-mpc-setpoint

Parameterize controller setpoints

Author: pavelacamposp

direct_data_driven_mpc/lti_data_driven_mpc_controller.py

@@ -493,13 +493,27 @@ def define_optimization_parameters(self) -> None:
         Define MPC optimization parameters that are updated at every step
         iteration.
 
-        This method initializes the past inputs (`u_past_param`) and past
-        outputs (`y_past_param`) MPC parameters.
+        This method initializes the input setpoint (`u_s_param`), output
+        setpoint (`y_s_param`), past inputs (`u_past_param`), and past outputs
+        (`y_past_param`) MPC parameters.
 
-        These parameters are updated at each MPC iteration. Using CVXPY
-        `Parameter` objects allows efficient updates without the need of
-        reformulating the MPC problem at every step.
+        The values of `u_s_param` and `y_s_param` are initialized to `u_s` and
+        `y_s`.
+
+        These parameters are updated at each MPC iteration, except for
+        `u_s_param` and `y_s_param`, which must be manually updated when
+        setting new controller setpoint pairs.
+
+        Using CVXPY `Parameter` objects allows efficient updates without the
+        need of reformulating the MPC problem at every step.
         """
+        # Define input-output setpoint parameters and initialize their values
+        self.u_s_param = cp.Parameter(self.u_s.shape, name="u_s")
+        self.u_s_param.value = self.u_s
+
+        self.y_s_param = cp.Parameter(self.y_s.shape, name="y_s")
+        self.y_s_param.value = self.y_s
+
         # u[t-n, t-1]
         self.u_past_param = cp.Parameter((self.n * self.m, 1), name="u_past")
 
@@ -566,7 +580,7 @@ def define_mpc_constraints(self) -> None:
             self.define_internal_state_constraints()
         )
         self.terminal_constraints = (
-            self.define_terminal_state_constraints(u_s=self.u_s, y_s=self.y_s)
+            self.define_terminal_state_constraints()
             if self.use_terminal_constraints
             else []
         )
@@ -673,9 +687,7 @@ def define_internal_state_constraints(self) -> list[cp.Constraint]:
 
         return internal_state_constraints
 
-    def define_terminal_state_constraints(
-        self, u_s: np.ndarray, y_s: np.ndarray
-    ) -> list[cp.Constraint]:
+    def define_terminal_state_constraints(self) -> list[cp.Constraint]:
         """
         Define the terminal state constraints for the Data-Driven MPC
         formulation.
@@ -703,8 +715,8 @@ def define_terminal_state_constraints(
 
         # Replicate steady-state vectors to match minimum realization
         # dimensions for constraint comparison
-        u_sn = np.tile(u_s, (self.n, 1))
-        y_sn = np.tile(y_s, (self.n, 1))
+        u_sn = cp.vstack([self.u_s_param] * self.n)
+        y_sn = cp.vstack([self.y_s_param] * self.n)
 
         # Define terminal state constraints for Nominal and Robust MPC
         # based on Equations (3d) and (6c) of [1], respectively.
@@ -816,8 +828,10 @@ def define_cost_function(self) -> None:
 
         # Define control-related cost
         control_cost = cp.quad_form(
-            ubar_pred - np.tile(self.u_s, (self.L, 1)), self.R
-        ) + cp.quad_form(ybar_pred - np.tile(self.y_s, (self.L, 1)), self.Q)
+            ubar_pred - cp.vstack([self.u_s_param] * self.L), self.R
+        ) + cp.quad_form(
+            ybar_pred - cp.vstack([self.y_s_param] * self.L), self.Q
+        )
 
         # Define noise-related cost if controller type is Robust
         if self.controller_type == LTIDataDrivenMPCType.ROBUST:
@@ -1072,10 +1086,6 @@ def set_input_output_setpoints(
         """
         Set the control and system setpoints of the Data-Driven MPC controller.
 
-        This method updates the control and system setpoints, `u_s` and `y_s`
-        to their provided values. Then, it reinitializes the controller to
-        redefine the Data-Driven MPC formulation.
-
         Args:
             u_s (np.ndarray): The setpoint for control inputs.
             y_s (np.ndarray): The setpoint for system outputs.
@@ -1086,7 +1096,8 @@ def set_input_output_setpoints(
 
         Note:
             This method sets the values of the `u_s` and `y_s` attributes with
-            the provided new setpoints.
+            the provided new setpoints and updates the values of `u_s_param`
+            and `y_r_param` to update the data-driven MPC controller setpoint.
         """
         # Validate input types and dimensions
         if u_s.shape != self.u_s.shape:
@@ -1100,9 +1111,8 @@ def set_input_output_setpoints(
                 f"{self.y_s.shape}, but got {y_s.shape} instead."
             )
 
-        # Update Input-Output setpoint pairs
+        # Update input-output setpoints and their parameter values
         self.u_s = u_s
         self.y_s = y_s
-
-        # Reinitialize Data-Driven MPC controller
-        self.initialize_data_driven_mpc()
+        self.u_s_param.value = u_s
+        self.y_s_param.value = y_s

direct_data_driven_mpc/nonlinear_data_driven_mpc_controller.py

@@ -709,16 +709,26 @@ def define_optimization_parameters(self) -> None:
 
         This method initializes the following MPC parameters:
 
+        - Output setpoint: `y_r_param`.
         - Hankel matrices: `HLn1_u_param` and `HLn1_y_param`.
         - Past inputs and outputs: `u_past_param` and `y_past_param`.
         - Past input increments: `du_past_param` (if applicable).
         - Computed value of `alpha_Lin^sr(D_t)`: `alpha_sr_Lin_D_param` (if
           `alpha` is not regularized with respect to zero).
 
-        These parameters are updated at each MPC iteration. Using CVXPY
-        `Parameter` objects allows efficient updates without the need of
-        reformulating the MPC problem at every step.
+        The value of `y_r_param` is initialized to `y_r`.
+
+        These parameters are updated at each MPC iteration, except for
+        `y_r_param`, which must be manually updated when setting a new
+        controller setpoint.
+
+        Using CVXPY `Parameter` objects allows efficient updates without the
+        need of reformulating the MPC problem at every step.
         """
+        # Define the parameter for y_r and initialize its value
+        self.y_r_param = cp.Parameter(self.y_r.shape, name="y_r")
+        self.y_r_param.value = self.y_r
+
         # H_{L+n+1}(u) - H_{L+n+1}(du)
         self.HLn1_u_param = cp.Parameter(self.HLn1_u.shape, name="HLn1_u")
 
@@ -1014,7 +1024,7 @@ def define_cost_function(self) -> None:
 
         # Define control-related cost
         control_cost = self.tracking_cost + cp.quad_form(
-            self.y_s[: self.p] - self.y_r, self.S
+            self.y_s[: self.p] - self.y_r_param, self.S
         )
 
         # Define alpha-related cost
@@ -1360,10 +1370,6 @@ def set_output_setpoint(self, y_r: np.ndarray) -> None:
         """
         Set the system output setpoint of the Data-Driven MPC controller.
 
-        This method updates the system setpoint `y_r` to the provided values.
-        Then, it reinitializes the controller to redefine the Data-Driven MPC
-        formulation.
-
         Args:
             y_r (np.ndarray): The setpoint for system outputs.
 
@@ -1372,7 +1378,8 @@ def set_output_setpoint(self, y_r: np.ndarray) -> None:
 
         Note:
             This method sets the values of the `y_r` attribute with the
-            provided new setpoint.
+            provided new setpoint and updates the value of `y_r_param`
+            to update the data-driven MPC controller setpoint.
         """
         # Validate input types and dimensions
         if y_r.shape != self.y_r.shape:
@@ -1381,11 +1388,9 @@ def set_output_setpoint(self, y_r: np.ndarray) -> None:
                 f"{self.y_s.shape}, but got {y_r.shape} instead."
             )
 
-        # Update Output setpoint
+        # Update output setpoint and its parameter value
         self.y_r = y_r
-
-        # Reinitialize Data-Driven MPC controller
-        self.initialize_data_driven_mpc()
+        self.y_r_param.value = y_r
 
     def define_alpha_sr_Lin_Dt_prob(self) -> None:
         """
@@ -1442,7 +1447,7 @@ def define_alpha_sr_Lin_Dt_prob(self) -> None:
 
         # Define objective
         objective = cp.Minimize(
-            cp.quad_form(self.y_s[: self.p] - self.y_r, self.S)
+            cp.quad_form(self.y_s[: self.p] - self.y_r_param, self.S)
             + self.lamb_alpha_s * cp.norm(self.alpha_s, 2) ** 2
             + self.lamb_sigma_s * cp.norm(self.sigma_s, 2) ** 2
         )

requirements-dev.txt

@@ -1,7 +1,7 @@
 numpy
 matplotlib>=3.9.0
 clarabel==0.10.0
-cvxpy
+cvxpy==1.6.5
 tqdm
 PyYAML
 PyQt6

tests/config/controllers/test_nonlinear_dd_mpc_params.yaml

@@ -3,9 +3,9 @@ test_nonlinear_dd_mpc_params:
   n: 4
   N: 50
   L: 10
-  Q_weights: 1
+  Q_weights: 10
   R_weights: 1
-  S_weights: 10
+  S_weights: 100
   lambda_alpha: 10.0
   lambda_sigma: 1.0e7
   U:
@@ -15,7 +15,7 @@ test_nonlinear_dd_mpc_params:
   u_range:
     - [-1.0, 1.0]
   alpha_reg_type: 0
-  lambda_alpha_s: 1e-3
+  lambda_alpha_s: 1e-5
   lambda_sigma_s: 1.0e7
   y_r: [3.0]
   ext_out_incr_in: true

tests/test_controllers/conftest.py

@@ -16,9 +16,11 @@
 
 
 @pytest.fixture
-@patch.object(LTIDataDrivenMPCController, "initialize_data_driven_mpc")
+@patch.object(LTIDataDrivenMPCController, "get_optimal_control_input")
+@patch.object(LTIDataDrivenMPCController, "solve_mpc_problem")
 def dummy_lti_controller(
-    _: Mock,
+    mock_solve_mpc_problem: Mock,
+    mock_get_optimal_control_input: Mock,
     dummy_lti_controller_data: tuple[
         LTIDataDrivenMPCParams, np.ndarray, np.ndarray
     ],
@@ -27,6 +29,9 @@ def dummy_lti_controller(
     m = u_d.shape[1]
     p = y_d.shape[1]
 
+    # Patch controller methods to bypass solver status checks
+    mock_get_optimal_control_input.return_value = np.ones((1,))
+
     dummy_lti_controller = LTIDataDrivenMPCController(
         n=controller_params["n"],
         m=m,
@@ -55,9 +60,13 @@ def dummy_lti_controller(
 
 
 @pytest.fixture
-@patch.object(LTIDataDrivenMPCController, "initialize_data_driven_mpc")
+@patch.object(NonlinearDataDrivenMPCController, "get_optimal_control_input")
+@patch.object(NonlinearDataDrivenMPCController, "solve_mpc_problem")
+@patch.object(NonlinearDataDrivenMPCController, "solve_alpha_sr_Lin_Dt")
 def dummy_nonlinear_controller(
-    _: Mock,
+    mock_solve_alpha_problem: Mock,
+    mock_solve_mpc_problem: Mock,
+    mock_get_optimal_control_input: Mock,
     dummy_nonlinear_controller_data: tuple[
         NonlinearDataDrivenMPCParams, np.ndarray, np.ndarray
     ],
@@ -66,6 +75,13 @@ def dummy_nonlinear_controller(
     m = u.shape[1]
     p = y.shape[1]
 
+    # Patch controller methods to bypass solver status checks
+    N = controller_params["N"]
+    L = controller_params["L"]
+    n = controller_params["n"]
+    mock_solve_alpha_problem.return_value = np.zeros((N - L - n, 1))
+    mock_get_optimal_control_input.return_value = np.ones((1,))
+
     dummy_nonlinear_controller = NonlinearDataDrivenMPCController(
         n=controller_params["n"],
         m=m,

tests/test_controllers/test_lti_dd_mpc_controller_unit.py

@@ -187,15 +187,10 @@ def test_lti_dd_mpc_controller_invalid_params(
 
 
 @pytest.mark.parametrize("valid_dimensions", [True, False])
-@patch.object(LTIDataDrivenMPCController, "get_optimal_control_input")
 def test_lti_store_input_output_measurement(
-    mock_controller_get_optimal_input: Mock,
     valid_dimensions: bool,
     dummy_lti_controller: LTIDataDrivenMPCController,
 ) -> None:
-    # Patch optimal control input retrieval to bypass solver status checks
-    mock_controller_get_optimal_input.return_value = np.ones((1,))
-
     # Get dummy LTI data-driven MPC controller
     controller = dummy_lti_controller
 
@@ -217,15 +212,10 @@ def test_lti_store_input_output_measurement(
 
 
 @pytest.mark.parametrize("valid_dimensions", [True, False])
-@patch.object(LTIDataDrivenMPCController, "get_optimal_control_input")
 def test_lti_set_past_input_output_data(
-    mock_controller_get_optimal_input: Mock,
     valid_dimensions: bool,
     dummy_lti_controller: LTIDataDrivenMPCController,
 ) -> None:
-    # Patch optimal control input retrieval to bypass solver status checks
-    mock_controller_get_optimal_input.return_value = np.ones((1,))
-
     # Get dummy LTI data-driven MPC controller
     controller = dummy_lti_controller
 
@@ -247,9 +237,7 @@ def test_lti_set_past_input_output_data(
 
 
 @pytest.mark.parametrize("valid_dimensions", [True, False])
-@patch.object(LTIDataDrivenMPCController, "initialize_data_driven_mpc")
 def test_lti_set_input_output_setpoints(
-    mock_controller_init: Mock,
     valid_dimensions: bool,
     dummy_lti_controller: LTIDataDrivenMPCController,
 ) -> None:
@@ -266,7 +254,6 @@ def test_lti_set_input_output_setpoints(
         assert np.allclose(controller.u_s, u_s)
         assert np.allclose(controller.y_s, y_s)
 
-        mock_controller_init.assert_called()
     else:
         u_s = np.ones((controller.u_s.shape[0] + 1, 1))
         y_s = np.ones((controller.y_s.shape[0] + 1, 1))

tests/test_controllers/test_nonlinear_dd_mpc_controller_unit.py

@@ -149,15 +149,10 @@ def test_nonlinear_dd_mpc_controller_invalid_params(
 
 
 @pytest.mark.parametrize("valid_dimensions", [True, False])
-@patch.object(NonlinearDataDrivenMPCController, "get_optimal_control_input")
 def test_nonlinear_store_input_output_measurement(
-    mock_controller_get_optimal_input: Mock,
     valid_dimensions: bool,
     dummy_nonlinear_controller: NonlinearDataDrivenMPCController,
 ) -> None:
-    # Patch optimal control input retrieval to bypass solver status checks
-    mock_controller_get_optimal_input.return_value = np.ones((1,))
-
     # Get dummy nonlinear data-driven MPC controller
     controller = dummy_nonlinear_controller
 
@@ -179,15 +174,10 @@ def test_nonlinear_store_input_output_measurement(
 
 
 @pytest.mark.parametrize("valid_dimensions", [True, False])
-@patch.object(NonlinearDataDrivenMPCController, "get_optimal_control_input")
 def test_nonlinear_set_input_output_data(
-    mock_controller_get_optimal_input: Mock,
     valid_dimensions: bool,
     dummy_nonlinear_controller: NonlinearDataDrivenMPCController,
 ) -> None:
-    # Patch optimal control input retrieval to bypass solver status checks
-    mock_controller_get_optimal_input.return_value = np.ones((1,))
-
     # Get dummy nonlinear data-driven MPC controller
     controller = dummy_nonlinear_controller
 
@@ -209,9 +199,7 @@ def test_nonlinear_set_input_output_data(
 
 
 @pytest.mark.parametrize("valid_dimensions", [True, False])
-@patch.object(NonlinearDataDrivenMPCController, "initialize_data_driven_mpc")
 def test_nonlinear_set_output_setpoint(
-    mock_controller_init: Mock,
     valid_dimensions: bool,
     dummy_nonlinear_controller: NonlinearDataDrivenMPCController,
 ) -> None:
@@ -226,7 +214,6 @@ def test_nonlinear_set_output_setpoint(
 
         assert np.allclose(controller.y_r, y_r)
 
-        mock_controller_init.assert_called()
     else:
         y_r = np.ones((controller.y_r.shape[0] + 1, 1))