From 6a75b1ce12b9ee44193e420465d53d964e476a04 Mon Sep 17 00:00:00 2001 From: Claude Date: Sat, 14 Jun 2025 16:22:28 +0900 Subject: [PATCH 1/9] =?UTF-8?q?=E4=BD=9C=E6=88=90=E4=B8=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- external_libraries/base_utility_cpp | 2 +- external_libraries/python_control_to_cpp | 2 +- external_libraries/python_math_to_cpp | 2 +- external_libraries/python_numpy_to_cpp | 2 +- external_libraries/python_optimization_to_cpp | 2 +- sample/simulation_manager | 2 +- test_vs/MCAP_tester | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/external_libraries/base_utility_cpp b/external_libraries/base_utility_cpp index c4d8663..3508c9f 160000 --- a/external_libraries/base_utility_cpp +++ b/external_libraries/base_utility_cpp @@ -1 +1 @@ -Subproject commit c4d8663116cbfcb197a986c14cc323702d3ca3b4 +Subproject commit 3508c9feed073712bfba88978f35f95e5433550b diff --git a/external_libraries/python_control_to_cpp b/external_libraries/python_control_to_cpp index dadf6a7..3d75dac 160000 --- a/external_libraries/python_control_to_cpp +++ b/external_libraries/python_control_to_cpp @@ -1 +1 @@ -Subproject commit dadf6a7116d95f7795f270c8c81d0181155a831b +Subproject commit 3d75dac323ba9702c73eac8a8a262aa99b4cf09f diff --git a/external_libraries/python_math_to_cpp b/external_libraries/python_math_to_cpp index 903f03f..9d878f0 160000 --- a/external_libraries/python_math_to_cpp +++ b/external_libraries/python_math_to_cpp @@ -1 +1 @@ -Subproject commit 903f03f899896000524adf385545124cf387380f +Subproject commit 9d878f0b9e0980cca9901ea148609d10b5fbf7e5 diff --git a/external_libraries/python_numpy_to_cpp b/external_libraries/python_numpy_to_cpp index 16777a2..ee141a3 160000 --- a/external_libraries/python_numpy_to_cpp +++ b/external_libraries/python_numpy_to_cpp @@ -1 +1 @@ -Subproject commit 16777a2796cb43865100561b43ad6b4f8d53aa96 +Subproject commit ee141a349e2e0d8b3fad2b7a1b8aafd793b6ceca diff --git a/external_libraries/python_optimization_to_cpp b/external_libraries/python_optimization_to_cpp index c9dfb61..deabf4a 160000 --- a/external_libraries/python_optimization_to_cpp +++ b/external_libraries/python_optimization_to_cpp @@ -1 +1 @@ -Subproject commit c9dfb611dca8ae89c6216740d8e41e69e8cf253b +Subproject commit deabf4ae1fa489303e6649a3799ee0be959f2ba9 diff --git a/sample/simulation_manager b/sample/simulation_manager index cf9f0bf..4d3b889 160000 --- a/sample/simulation_manager +++ b/sample/simulation_manager @@ -1 +1 @@ -Subproject commit cf9f0bf7f07c525f7cbff5c8fb13291659263733 +Subproject commit 4d3b889d2632a9e0a62514f3761d5a3a4fcb998b diff --git a/test_vs/MCAP_tester b/test_vs/MCAP_tester index 145a2e3..1fbd7b9 160000 --- a/test_vs/MCAP_tester +++ b/test_vs/MCAP_tester @@ -1 +1 @@ -Subproject commit 145a2e38f72dca331bb9cb99fc2aff51c41a17ec +Subproject commit 1fbd7b9c6ad6c803128a843831a0c3db8746e8b8 From 5ec912de054273d1596841bbef52c52f3f80b2f2 Mon Sep 17 00:00:00 2001 From: Claude Date: Sat, 14 Jun 2025 16:47:05 +0900 Subject: [PATCH 2/9] =?UTF-8?q?=E4=BD=9C=E6=88=90=E4=B8=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mpc_utility/linear_solver_utility.py | 278 +++++- mpc_utility/mpc_linear_solver_utility.hpp | 1001 +++++++++++++++++++++ mpc_utility/state_space_utility.py | 146 ++- 3 files changed, 1414 insertions(+), 11 deletions(-) diff --git a/mpc_utility/linear_solver_utility.py b/mpc_utility/linear_solver_utility.py index 1ec1f2b..bce5f06 100644 --- a/mpc_utility/linear_solver_utility.py +++ b/mpc_utility/linear_solver_utility.py @@ -1,3 +1,8 @@ +""" +File: linear_solver_utility.py + +This module provides the DU_U_Y_Limits class, which is a utility for managing and validating constraints on control increments (delta_U), control variables (U), and output variables (Y) in the context of Model Predictive Control (MPC) or similar optimization problems. The class supports initialization, validation, and dynamic management of lower and upper bounds for these variables, as well as tracking which constraints are active or inactive. +""" import numpy as np import sympy as sp @@ -9,6 +14,18 @@ class DU_U_Y_Limits: + """ + DU_U_Y_Limits class manages the constraints for control increments (delta_U), control variables (U), and output variables (Y) in a Model Predictive Control (MPC) context. + It allows for initialization with specific bounds, checks for compatibility, counts constraints, and generates active sets for each type of constraint. + Attributes: + delta_U_min (np.ndarray): Lower bounds for control increments. + delta_U_max (np.ndarray): Upper bounds for control increments. + U_min (np.ndarray): Lower bounds for control variables. + U_max (np.ndarray): Upper bounds for control variables. + Y_min (np.ndarray): Lower bounds for output variables. + Y_max (np.ndarray): Upper bounds for output variables. + """ + def __init__(self, delta_U_min: np.ndarray = None, delta_U_max: np.ndarray = None, U_min: np.ndarray = None, U_max: np.ndarray = None, @@ -50,7 +67,20 @@ def __init__(self, def check_U_Y_size(self, delta_U_min: np.ndarray = None, delta_U_max: np.ndarray = None, U_min: np.ndarray = None, U_max: np.ndarray = None, Y_min: np.ndarray = None, Y_max: np.ndarray = None): - + """ + Checks the sizes of the provided constraints and ensures they are compatible. + Args: + delta_U_min (np.ndarray): Lower bounds for control increments. + delta_U_max (np.ndarray): Upper bounds for control increments. + U_min (np.ndarray): Lower bounds for control variables. + U_max (np.ndarray): Upper bounds for control variables. + Y_min (np.ndarray): Lower bounds for output variables. + Y_max (np.ndarray): Upper bounds for output variables. + Returns: + tuple: Sizes of the constraints in the order of delta_U_min, delta_U_max, U_min, U_max, Y_min, Y_max. + Raises: + ValueError: If the sizes of the constraints are not compatible. + """ delta_U_min_size = 0 delta_U_max_size = 0 U_min_size = 0 @@ -98,7 +128,11 @@ def check_U_Y_size(self, delta_U_min: np.ndarray = None, delta_U_max: np.ndarray U_min_size, U_max_size, Y_min_size, Y_max_size def check_min_max_compatibility(self): - + """ + Checks the compatibility of the provided constraints. + Raises: + ValueError: If the constraints are not compatible. + """ if self.delta_U_min is not None and self.delta_U_min.shape[1] > 1: raise ValueError("delta_U_min must be a (n, 1) vector.") if self.delta_U_max is not None and self.delta_U_max.shape[1] > 1: @@ -142,6 +176,13 @@ def check_min_max_compatibility(self): "size of Y_max doesn't match the size of initialized ones.") def count_check_constraints(self): + """ + Counts the number of active constraints for delta_U, U, and Y. + Returns: + tuple: Number of active constraints for delta_U, U, and Y. + Raises: + ValueError: If the number of constraints does not match the expected count. + """ self.check_min_max_compatibility() number_of_delta_U_constraints = 0 @@ -197,6 +238,11 @@ def count_check_constraints(self): number_of_Y_constraints) def generate_DU_U_Y_active_set(self): + """ + Generates active sets for delta_U, U, and Y constraints based on their finite values. + Returns: + tuple: Active sets for delta_U_min, delta_U_max, U_min, U_max, Y_min, Y_max. + """ delta_U_min_active_set = np.zeros(self._delta_U_min_size, dtype=bool) delta_U_max_active_set = np.zeros(self._delta_U_max_size, dtype=bool) U_min_active_set = np.zeros(self._U_min_size, dtype=bool) @@ -232,7 +278,16 @@ def generate_DU_U_Y_active_set(self): def update_min_max(self, delta_U_min: np.ndarray = None, delta_U_max: np.ndarray = None, U_min: np.ndarray = None, U_max: np.ndarray = None, Y_min: np.ndarray = None, Y_max: np.ndarray = None): - + """ + Updates the minimum and maximum constraints for delta_U, U, and Y. + Args: + delta_U_min (np.ndarray): New lower bounds for control increments. + delta_U_max (np.ndarray): New upper bounds for control increments. + U_min (np.ndarray): New lower bounds for control variables. + U_max (np.ndarray): New upper bounds for control variables. + Y_min (np.ndarray): New lower bounds for output variables. + Y_max (np.ndarray): New upper bounds for output variables. + """ if delta_U_min is not None and delta_U_min.shape[0] != self._delta_U_min_size: for i in range(self._delta_U_min_size): if self._delta_U_min_active_set[i] and \ @@ -271,6 +326,13 @@ def update_min_max(self, delta_U_min: np.ndarray = None, delta_U_max: np.ndarray # Check if the constraints are active def is_delta_U_min_active(self, index: int) -> bool: + """ + Checks if the delta_U_min constraint at the given index is active. + Args: + index (int): Index of the delta_U_min constraint. + Returns: + bool: True if the constraint is active, False otherwise. + """ if index < 0: index = 0 if index >= self._delta_U_min_size: @@ -279,6 +341,13 @@ def is_delta_U_min_active(self, index: int) -> bool: return self._delta_U_min_active_set[index] def is_delta_U_max_active(self, index: int) -> bool: + """ + Checks if the delta_U_max constraint at the given index is active. + Args: + index (int): Index of the delta_U_max constraint. + Returns: + bool: True if the constraint is active, False otherwise. + """ if index < 0: index = 0 if index >= self._delta_U_max_size: @@ -287,6 +356,13 @@ def is_delta_U_max_active(self, index: int) -> bool: return self._delta_U_max_active_set[index] def is_U_min_active(self, index: int) -> bool: + """ + Checks if the U_min constraint at the given index is active. + Args: + index (int): Index of the U_min constraint. + Returns: + bool: True if the constraint is active, False otherwise. + """ if index < 0: index = 0 if index >= self._U_min_size: @@ -295,6 +371,13 @@ def is_U_min_active(self, index: int) -> bool: return self._U_min_active_set[index] def is_U_max_active(self, index: int) -> bool: + """ + Checks if the U_max constraint at the given index is active. + Args: + index (int): Index of the U_max constraint. + Returns: + bool: True if the constraint is active, False otherwise. + """ if index < 0: index = 0 if index >= self._U_max_size: @@ -303,6 +386,13 @@ def is_U_max_active(self, index: int) -> bool: return self._U_max_active_set[index] def is_Y_min_active(self, index: int) -> bool: + """ + Checks if the Y_min constraint at the given index is active. + Args: + index (int): Index of the Y_min constraint. + Returns: + bool: True if the constraint is active, False otherwise. + """ if index < 0: index = 0 if index >= self._Y_min_size: @@ -311,6 +401,13 @@ def is_Y_min_active(self, index: int) -> bool: return self._Y_min_active_set[index] def is_Y_max_active(self, index: int) -> bool: + """ + Checks if the Y_max constraint at the given index is active. + Args: + index (int): Index of the Y_max constraint. + Returns: + bool: True if the constraint is active, False otherwise. + """ if index < 0: index = 0 if index >= self._Y_max_size: @@ -320,6 +417,11 @@ def is_Y_max_active(self, index: int) -> bool: # set constraints inactive def set_delta_U_min_inactive(self, index: int): + """ + Sets the delta_U_min constraint at the given index to inactive. + Args: + index (int): Index of the delta_U_min constraint. + """ if index < 0: index = 0 if index >= self._delta_U_min_size: @@ -328,6 +430,11 @@ def set_delta_U_min_inactive(self, index: int): self._delta_U_min_active_set[index] = False def set_delta_U_max_inactive(self, index: int): + """ + Sets the delta_U_max constraint at the given index to inactive. + Args: + index (int): Index of the delta_U_max constraint. + """ if index < 0: index = 0 if index >= self._delta_U_max_size: @@ -336,6 +443,11 @@ def set_delta_U_max_inactive(self, index: int): self._delta_U_max_active_set[index] = False def set_U_min_inactive(self, index: int): + """ + Sets the U_min constraint at the given index to inactive. + Args: + index (int): Index of the U_min constraint. + """ if index < 0: index = 0 if index >= self._U_min_size: @@ -344,6 +456,11 @@ def set_U_min_inactive(self, index: int): self._U_min_active_set[index] = False def set_U_max_inactive(self, index: int): + """ + Sets the U_max constraint at the given index to inactive. + Args: + index (int): Index of the U_max constraint. + """ if index < 0: index = 0 if index >= self._U_max_size: @@ -352,6 +469,11 @@ def set_U_max_inactive(self, index: int): self._U_max_active_set[index] = False def set_Y_min_inactive(self, index: int): + """ + Sets the Y_min constraint at the given index to inactive. + Args: + index (int): Index of the Y_min constraint. + """ if index < 0: index = 0 if index >= self._Y_min_size: @@ -363,6 +485,11 @@ def set_Y_min_inactive(self, index: int): self._Y_min_active_set[index] = False def set_Y_max_inactive(self, index: int): + """ + Sets the Y_max constraint at the given index to inactive. + Args: + index (int): Index of the Y_max constraint. + """ if index < 0: index = 0 if index >= self._Y_max_size: @@ -375,39 +502,109 @@ def set_Y_max_inactive(self, index: int): # Getters for the sizes and counts of constraints def get_number_of_all_constraints(self): + """ + Returns the total number of constraints across delta_U, U, and Y. + Returns: + int: Total number of constraints. + """ return self._number_of_delta_U_constraints + \ self._number_of_U_constraints + \ self._number_of_Y_constraints def get_number_of_delta_U_constraints(self): + """ + Returns the number of delta_U constraints. + Returns: + int: Number of delta_U constraints. + """ return self._number_of_delta_U_constraints def get_number_of_U_constraints(self): + """ + Returns the number of U constraints. + Returns: + int: Number of U constraints. + """ return self._number_of_U_constraints def get_number_of_Y_constraints(self): + """ + Returns the number of Y constraints. + Returns: + int: Number of Y constraints. + """ return self._number_of_Y_constraints def get_delta_U_min_size(self): + """ + Returns the size of the delta_U_min constraints. + Returns: + int: Size of the delta_U_min constraints. + """ return self._delta_U_min_size def get_delta_U_max_size(self): + """ + Returns the size of the delta_U_max constraints. + Returns: + int: Size of the delta_U_max constraints. + """ return self._delta_U_max_size def get_U_min_size(self): + """ + Returns the size of the U_min constraints. + Returns: + int: Size of the U_min constraints. + """ return self._U_min_size def get_U_max_size(self): + """ + Returns the size of the U_max constraints. + Returns: + int: Size of the U_max constraints. + """ return self._U_max_size def get_Y_min_size(self): + """ + Returns the size of the Y_min constraints. + Returns: + int: Size of the Y_min constraints. + """ return self._Y_min_size def get_Y_max_size(self): + """ + Returns the size of the Y_max constraints. + Returns: + int: Size of the Y_max constraints. + """ return self._Y_max_size class LTI_MPC_QP_Solver: + """ + LTI_MPC_QP_Solver class implements a solver for Linear Time-Invariant (LTI) Model Predictive Control (MPC) problems using Quadratic Programming (QP). + It utilizes the DU_U_Y_Limits class to manage constraints on control increments (delta_U), control variables (U), and output variables (Y). + Attributes: + number_of_variables (int): Number of control variables. + output_size (int): Size of the output variables. + U (np.ndarray): Control input matrix. + X_augmented (np.ndarray): Augmented state matrix. + Phi (np.ndarray): Prediction matrix. + F (np.ndarray): State transition matrix. + Weight_U_Nc (np.ndarray): Weighting matrix for control increments. + delta_U_Nc (np.ndarray): Control increment matrix. + delta_U_min (np.ndarray): Lower bounds for control increments. + delta_U_max (np.ndarray): Upper bounds for control increments. + U_min (np.ndarray): Lower bounds for control variables. + U_max (np.ndarray): Upper bounds for control variables. + Y_min (np.ndarray): Lower bounds for output variables. + Y_max (np.ndarray): Upper bounds for output variables. + """ + def __init__(self, number_of_variables: int, output_size: int, U: np.ndarray, X_augmented: np.ndarray, Phi: np.ndarray, F: np.ndarray, @@ -467,6 +664,13 @@ def __init__(self, number_of_variables: int, output_size: int, def check_Y_constraints_feasibility(self, Phi: np.ndarray): + """ + Checks the feasibility of Y constraints based on the prediction matrix Phi. + Args: + Phi (np.ndarray): Prediction matrix. + Raises: + ValueError: If the Phi matrix does not match the expected shape. + """ for i in range(self.DU_U_Y_Limits.get_Y_min_size()): Phi_factor_norm = np.linalg.norm( @@ -494,6 +698,16 @@ def check_Y_constraints_feasibility(self, def update_min_max(self, delta_U_min: np.ndarray = None, delta_U_max: np.ndarray = None, U_min: np.ndarray = None, U_max: np.ndarray = None, Y_min: np.ndarray = None, Y_max: np.ndarray = None): + """ + Updates the minimum and maximum constraints for delta_U, U, and Y. + Args: + delta_U_min (np.ndarray): New lower bounds for control increments. + delta_U_max (np.ndarray): New upper bounds for control increments. + U_min (np.ndarray): New lower bounds for control variables. + U_max (np.ndarray): New upper bounds for control variables. + Y_min (np.ndarray): New lower bounds for output variables. + Y_max (np.ndarray): New upper bounds for output variables. + """ self.DU_U_Y_Limits.update_min_max( delta_U_min=delta_U_min, @@ -505,7 +719,13 @@ def update_min_max(self, delta_U_min: np.ndarray = None, delta_U_max: np.ndarray ) def _calculate_M_gamma_delta_U(self, total_index: int): - + """ + Calculates the M and gamma matrices for delta_U constraints. + Args: + total_index (int): The starting index for the M and gamma matrices. + Returns: + int: The updated total index after processing delta_U constraints. + """ initial_position = total_index for i in range(self.DU_U_Y_Limits.get_delta_U_min_size()): @@ -536,7 +756,14 @@ def _calculate_M_gamma_delta_U(self, total_index: int): return total_index def _calculate_M_gamma_U(self, total_index: int, U: np.ndarray): - + """ + Calculates the M and gamma matrices for U constraints. + Args: + total_index (int): The starting index for the M and gamma matrices. + U (np.ndarray): Control input matrix. + Returns: + int: The updated total index after processing U constraints. + """ initial_position = total_index for i in range(self.DU_U_Y_Limits.get_U_min_size()): @@ -568,7 +795,14 @@ def _calculate_M_gamma_U(self, total_index: int, U: np.ndarray): def _calculate_M_gamma_Y(self, total_index: int, X_augmented: np.ndarray, Phi: np.ndarray, F: np.ndarray): - + """ + Calculates the M and gamma matrices for Y constraints. + Args: + total_index (int): The starting index for the M and gamma matrices. + X_augmented (np.ndarray): Augmented state matrix. + Phi (np.ndarray): Prediction matrix. + F (np.ndarray): State transition matrix. + """ initial_position = total_index F_X = F @ X_augmented @@ -601,7 +835,16 @@ def _calculate_M_gamma_Y(self, total_index: int, X_augmented: np.ndarray, def update_constraints(self, U: np.ndarray, X_augmented: np.ndarray, Phi: np.ndarray, F: np.ndarray): - + """ + Updates the M and gamma matrices based on the current constraints. + Args: + U (np.ndarray): Control input matrix. + X_augmented (np.ndarray): Augmented state matrix. + Phi (np.ndarray): Prediction matrix. + F (np.ndarray): State transition matrix. + Raises: + ValueError: If the shapes of U, X_augmented, Phi, or F do not match the initialized shapes. + """ if not (U.shape[0] == self.U_shape[0]) or \ not (U.shape[1] == self.U_shape[1]): raise ValueError("U shape does not match the initialized shape.") @@ -628,13 +871,30 @@ def update_constraints(self, self._calculate_M_gamma_Y(total_index, X_augmented, Phi, F) def update_E(self, Phi: np.ndarray, Weight_U_Nc: np.ndarray): - + """ + Updates the E matrix used in the QP solver. + Args: + Phi (np.ndarray): Prediction matrix. + Weight_U_Nc (np.ndarray): Weighting matrix for control increments. + """ self.solver.update_E(Phi.T @ Phi + Weight_U_Nc) def solve(self, Phi: np.ndarray, F: np.ndarray, reference_trajectory: MPC_ReferenceTrajectory, X_augmented: np.ndarray, Weight_U_Nc: np.ndarray = None) -> np.ndarray: - + """ + Solves the MPC problem using the QP solver. + Args: + Phi (np.ndarray): Prediction matrix. + F (np.ndarray): State transition matrix. + reference_trajectory (MPC_ReferenceTrajectory): Reference trajectory for the MPC problem. + X_augmented (np.ndarray): Augmented state matrix. + Weight_U_Nc (np.ndarray, optional): Weighting matrix for control increments. + Returns: + np.ndarray: Optimal control input. + Raises: + ValueError: If the shapes of Phi, F, or X_augmented do not match the initialized shapes. + """ L = Phi.T @ reference_trajectory.calculate_dif(F @ X_augmented) if Weight_U_Nc is not None: diff --git a/mpc_utility/mpc_linear_solver_utility.hpp b/mpc_utility/mpc_linear_solver_utility.hpp index 74a7746..f7f5c7f 100644 --- a/mpc_utility/mpc_linear_solver_utility.hpp +++ b/mpc_utility/mpc_linear_solver_utility.hpp @@ -1,3 +1,14 @@ +/** + * @file mpc_linear_solver_utility.hpp + * @brief Utility classes and functions for Model Predictive Control (MPC) + * linear solver in C++. + * + * This header provides a set of template classes and meta-programming utilities + * to support the construction and solution of quadratic programming (QP) + * problems for linear MPC. It includes constraint management, matrix + * operations, and QP solver integration, designed for use with Python/C++ + * hybrid MPC frameworks. + */ #ifndef __MPC_LINEAR_SOLVER_UTILITY__ #define __MPC_LINEAR_SOLVER_UTILITY__ @@ -18,16 +29,40 @@ static constexpr double TOL_DEFAULT = 1e-8; namespace SolverUtilityOperation { +/** + * @brief A utility structure to count the number of true conditions in a + * boolean flag. + * + * This structure is used to count how many times a condition evaluates to + * true in a compile-time context. + */ template struct CountTrueCondition {}; +/** + * @brief Specialization of CountTrueCondition for true. + * + * This specialization sets the value to 1 when the condition is true. + */ template <> struct CountTrueCondition { static constexpr std::size_t value = static_cast(1); }; +/** + * @brief Specialization of CountTrueCondition for false. + * + * This specialization sets the value to 0 when the condition is false. + */ template <> struct CountTrueCondition { static constexpr std::size_t value = static_cast(0); }; +/** + * @brief A utility structure to count the number of true conditions in a 2D + * boolean flag array. + * + * This structure is used to count how many times a condition evaluates to + * true in a 2D compile-time context. + */ template struct CountTrue2D_Row { static constexpr std::size_t value = @@ -35,11 +70,24 @@ struct CountTrue2D_Row { CountTrue2D_Row::value; }; +/** + * @brief Specialization of CountTrue2D_Row for the last row. + * + * This specialization sets the value to 0 when the row index is -1, indicating + * no more rows to count. + */ template struct CountTrue2D_Row(-1)> { static constexpr std::size_t value = static_cast(0); }; +/** + * @brief A utility structure to count the number of true conditions in a 2D + * boolean flag array by columns. + * + * This structure is used to count how many times a condition evaluates to + * true in a 2D compile-time context, iterating over columns. + */ template struct CountTrue2D_Col { static constexpr std::size_t value = @@ -47,17 +95,46 @@ struct CountTrue2D_Col { CountTrue2D_Col::value; }; +/** + * @brief Specialization of CountTrue2D_Col for the last column. + * + * This specialization sets the value to 0 when the column index is -1, + * indicating no more columns to count. + */ template struct CountTrue2D_Col(-1), Row> { static constexpr std::size_t value = static_cast(0); }; +/** + * @brief Alias template to count the number of true values in a 2D flag array + * up to a specified column and row. + * + * This alias uses the CountTrue2D_Col metafunction to recursively count the + * number of true values in a 2D compile-time flag array (Flags), considering + * columns up to (Col - 1) and a specified row (Row). + * + * @tparam Flags The 2D array of boolean flags (typically a template parameter + * pack or array). + * @tparam Col The number of columns to consider (exclusive upper bound). + * @tparam Row The row index to consider. + */ template using CountTrue2D = CountTrue2D_Col; } // namespace SolverUtilityOperation /* Define delta U, U, Y limits */ + +/** + * @brief A class to represent limits for delta U, U, and Y in a Model + * Predictive Control (MPC) context. + * + * This class encapsulates the limits for delta U (change in control input), + * U (control input), and Y (output) with their respective minimum and maximum + * values. It provides methods to check the activity of each limit and to + * retrieve the number of constraints. + */ template @@ -181,6 +258,16 @@ class DU_U_Y_Limits { public: /* Function */ + + /** + * @brief Check if a delta U min constraint is active at a given index. + * + * This function checks if the delta U min constraint at the specified index + * is active. It wraps the index to ensure it is within the valid range. + * + * @param index The index of the delta U min constraint to check. + * @return True if the constraint is active, false otherwise. + */ inline auto is_delta_U_min_active(const std::size_t &index) const -> bool { std::size_t index_wrapped; @@ -196,6 +283,15 @@ class DU_U_Y_Limits { return Delta_U_Min_Flags::lists[index_wrapped][0]; } + /** + * @brief Check if a delta U max constraint is active at a given index. + * + * This function checks if the delta U max constraint at the specified index + * is active. It wraps the index to ensure it is within the valid range. + * + * @param index The index of the delta U max constraint to check. + * @return True if the constraint is active, false otherwise. + */ inline auto is_delta_U_max_active(const std::size_t &index) const -> bool { std::size_t index_wrapped; @@ -211,6 +307,15 @@ class DU_U_Y_Limits { return Delta_U_Max_Flags::lists[index_wrapped][0]; } + /** + * @brief Check if a U min constraint is active at a given index. + * + * This function checks if the U min constraint at the specified index is + * active. It wraps the index to ensure it is within the valid range. + * + * @param index The index of the U min constraint to check. + * @return True if the constraint is active, false otherwise. + */ inline auto is_U_min_active(const std::size_t &index) const -> bool { std::size_t index_wrapped; @@ -226,6 +331,15 @@ class DU_U_Y_Limits { return U_Min_Flags::lists[index_wrapped][0]; } + /** + * @brief Check if a U max constraint is active at a given index. + * + * This function checks if the U max constraint at the specified index is + * active. It wraps the index to ensure it is within the valid range. + * + * @param index The index of the U max constraint to check. + * @return True if the constraint is active, false otherwise. + */ inline auto is_U_max_active(const std::size_t &index) const -> bool { std::size_t index_wrapped; @@ -241,6 +355,15 @@ class DU_U_Y_Limits { return U_Max_Flags::lists[index_wrapped][0]; } + /** + * @brief Check if a Y min constraint is active at a given index. + * + * This function checks if the Y min constraint at the specified index is + * active. It wraps the index to ensure it is within the valid range. + * + * @param index The index of the Y min constraint to check. + * @return True if the constraint is active, false otherwise. + */ inline auto is_Y_min_active(const std::size_t &index) const -> bool { std::size_t index_wrapped; @@ -256,6 +379,15 @@ class DU_U_Y_Limits { return Y_Min_Flags::lists[index_wrapped][0]; } + /** + * @brief Check if a Y max constraint is active at a given index. + * + * This function checks if the Y max constraint at the specified index is + * active. It wraps the index to ensure it is within the valid range. + * + * @param index The index of the Y max constraint to check. + * @return True if the constraint is active, false otherwise. + */ inline auto is_Y_max_active(const std::size_t &index) const -> bool { std::size_t index_wrapped; @@ -271,20 +403,49 @@ class DU_U_Y_Limits { return Y_Max_Flags::lists[index_wrapped][0]; } + /** + * @brief Get the total number of all constraints. + * + * This function returns the total number of constraints across delta U, + * U, and Y limits. + * + * @return The total number of constraints. + */ inline auto get_number_of_all_constraints(void) const -> std::size_t { return this->_number_of_delta_U_constraints + this->_number_of_U_constraints + this->_number_of_Y_constraints; } + /** + * @brief Get the number of delta U constraints. + * + * This function returns the total number of delta U constraints. + * + * @return The number of delta U constraints. + */ inline auto get_number_of_delta_U_constraints(void) const -> std::size_t { return this->_number_of_delta_U_constraints; } + /** + * @brief Get the number of U constraints. + * + * This function returns the total number of U constraints. + * + * @return The number of U constraints. + */ inline auto get_number_of_U_constraints(void) const -> std::size_t { return this->_number_of_U_constraints; } + /** + * @brief Get the number of Y constraints. + * + * This function returns the total number of Y constraints. + * + * @return The number of Y constraints. + */ inline auto get_number_of_Y_constraints(void) const -> std::size_t { return this->_number_of_Y_constraints; } @@ -338,6 +499,27 @@ class DU_U_Y_Limits { }; /* make DU_U_Y_Limits */ + +/** + * @brief Factory function to create a DU_U_Y_Limits object. + * + * This function constructs a DU_U_Y_Limits object with the provided limits for + * delta U, U, and Y. + * + * @tparam Delta_U_Min_Type Type of the delta U minimum limits. + * @tparam Delta_U_Max_Type Type of the delta U maximum limits. + * @tparam U_Min_Type Type of the U minimum limits. + * @tparam U_Max_Type Type of the U maximum limits. + * @tparam Y_Min_Type Type of the Y minimum limits. + * @tparam Y_Max_Type Type of the Y maximum limits. + * @param delta_U_min_in Input for delta U minimum limits. + * @param delta_U_max_in Input for delta U maximum limits. + * @param U_min_in Input for U minimum limits. + * @param U_max_in Input for U maximum limits. + * @param Y_min_in Input for Y minimum limits. + * @param Y_max_in Input for Y maximum limits. + * @return A DU_U_Y_Limits object initialized with the provided limits. + */ template @@ -373,6 +555,27 @@ template struct Calculate_M_Gamma_Delta_U_Min_Condition< M_Type, Gamma_Type, Delta_U_min_Matrix_Type, I, true> { + /** + * @brief Applies constraints to the given matrices for a specific index. + * + * This static function modifies the matrix M and vector gamma by applying a + * constraint at the position specified by initial_position and the template + * parameter I. It sets the corresponding entry in M to -1.0 and updates gamma + * using the value from delta_U_matrix at position . The set_count is + * incremented to reflect the addition of a new constraint. + * + * @tparam I Index at which the constraint is applied (template parameter). + * @tparam M_Type Type of the matrix M. + * @tparam Gamma_Type Type of the vector gamma. + * @tparam Delta_U_min_Matrix_Type Type of the delta_U_matrix. + * @param M Reference to the matrix to be modified. + * @param gamma Reference to the vector to be modified. + * @param delta_U_matrix Matrix providing the constraint value. + * @param set_count Reference to the counter tracking the number of + * constraints set. + * @param initial_position The starting position in M and gamma for applying + * the constraint. + */ static void apply(M_Type &M, Gamma_Type &gamma, const Delta_U_min_Matrix_Type &delta_U_matrix, std::size_t &set_count, @@ -390,6 +593,18 @@ template struct Calculate_M_Gamma_Delta_U_Min_Condition< M_Type, Gamma_Type, Delta_U_min_Matrix_Type, I, false> { + /** + * @brief A no-op function for the case when the condition is false. + * This function does nothing and is used to maintain the structure of the + * template specialization. + * @param M Reference to the matrix (unused). + * @param gamma Reference to the vector (unused). + * @param delta_U_matrix Matrix providing the constraint value (unused). + * @param set_count Reference to the counter tracking the number of + * constraints set (unused). + * @param initial_position The starting position in M and gamma for applying + * the constraint (unused). + */ static void apply(M_Type &M, Gamma_Type &gamma, const Delta_U_min_Matrix_Type &delta_U_matrix, std::size_t &set_count, @@ -407,6 +622,27 @@ struct Calculate_M_Gamma_Delta_U_Min_Condition< template struct Calculate_M_Gamma_Delta_U_Min_Loop { + /** + * @brief Applies constraints to the given matrices for a range of indices. + * + * This static function iterates over the indices from I to I_Dif, applying + * constraints to the matrix M and vector gamma based on the delta_U_matrix. + * It updates the total_index with the number of constraints set and calls + * itself recursively for the next index. + * + * @tparam Delta_U_min_Matrix_Type Type of the delta_U_min_matrix. + * @tparam M_Type Type of the matrix M. + * @tparam Gamma_Type Type of the vector gamma. + * @tparam I Current index in the iteration (template parameter). + * @tparam I_Dif The difference in indices for recursion (template parameter). + * @param M Reference to the matrix to be modified. + * @param gamma Reference to the vector to be modified. + * @param delta_U_matrix Matrix providing the constraint value. + * @param total_index Reference to the counter tracking the total number of + * constraints set. + * @param initial_position The starting position in M and gamma for applying + * the constraints. + */ static void apply(M_Type &M, Gamma_Type &gamma, const Delta_U_min_Matrix_Type &delta_U_matrix, std::size_t &total_index, @@ -433,6 +669,24 @@ template struct Calculate_M_Gamma_Delta_U_Min_Loop { + /** + * @brief A no-op function for the case when the loop has no more indices to + * process. + * + * This static function does nothing and is used to terminate the recursion + * in the template specialization. + * + * @tparam Delta_U_min_Matrix_Type Type of the delta_U_min_matrix (unused). + * @tparam M_Type Type of the matrix M (unused). + * @tparam Gamma_Type Type of the vector gamma (unused). + * @param M Reference to the matrix (unused). + * @param gamma Reference to the vector (unused). + * @param delta_U_matrix Matrix providing the constraint value (unused). + * @param total_index Reference to the counter tracking the total number of + * constraints set (unused). + * @param initial_position The starting position in M and gamma for applying + * the constraints (unused). + */ static void apply(M_Type &M, Gamma_Type &gamma, const Delta_U_min_Matrix_Type &delta_U_matrix, std::size_t &total_index, @@ -447,6 +701,18 @@ struct Calculate_M_Gamma_Delta_U_Min_Loop using Calculate_M_Gamma_Delta_U_Min = @@ -454,6 +720,14 @@ using Calculate_M_Gamma_Delta_U_Min = Gamma_Type, 0, Delta_U_Size>; /* calculate M gamma for delta U max */ + +/** + * @brief A utility structure to calculate M gamma for delta U max constraints. + * + * This structure defines a loop to apply constraints for delta U max, iterating + * through the indices and applying conditions based on the flags defined in the + * Delta_U_max_Matrix_Type. + */ template struct Calculate_M_Gamma_Delta_U_Max_Condition {}; @@ -462,6 +736,27 @@ template struct Calculate_M_Gamma_Delta_U_Max_Condition< M_Type, Gamma_Type, Delta_U_max_Matrix_Type, I, true> { + /** + * @brief Applies constraints to the given matrices for a specific index. + * + * This static function modifies the matrix M and vector gamma by applying a + * constraint at the position specified by initial_position and the template + * parameter I. It sets the corresponding entry in M to 1.0 and updates gamma + * using the value from delta_U_matrix at position . The set_count is + * incremented to reflect the addition of a new constraint. + * + * @tparam I Index at which the constraint is applied (template parameter). + * @tparam M_Type Type of the matrix M. + * @tparam Gamma_Type Type of the vector gamma. + * @tparam Delta_U_max_Matrix_Type Type of the delta_U_matrix. + * @param M Reference to the matrix to be modified. + * @param gamma Reference to the vector to be modified. + * @param delta_U_matrix Matrix providing the constraint value. + * @param set_count Reference to the counter tracking the number of + * constraints set. + * @param initial_position The starting position in M and gamma for applying + * the constraint. + */ static void apply(M_Type &M, Gamma_Type &gamma, const Delta_U_max_Matrix_Type &delta_U_matrix, std::size_t &set_count, @@ -478,6 +773,18 @@ template struct Calculate_M_Gamma_Delta_U_Max_Condition< M_Type, Gamma_Type, Delta_U_max_Matrix_Type, I, false> { + /** + * @brief A no-op function for the case when the condition is false. + * This function does nothing and is used to maintain the structure of the + * template specialization. + * @param M Reference to the matrix (unused). + * @param gamma Reference to the vector (unused). + * @param delta_U_matrix Matrix providing the constraint value (unused). + * @param set_count Reference to the counter tracking the number of + * constraints set (unused). + * @param initial_position The starting position in M and gamma for applying + * the constraint (unused). + */ static void apply(M_Type &M, Gamma_Type &gamma, const Delta_U_max_Matrix_Type &delta_U_matrix, std::size_t &set_count, @@ -495,6 +802,27 @@ struct Calculate_M_Gamma_Delta_U_Max_Condition< template struct Calculate_M_Gamma_Delta_U_Max_Loop { + /** + * @brief Applies constraints to the given matrices for a range of indices. + * + * This static function iterates over the indices from I to I_Dif, applying + * constraints to the matrix M and vector gamma based on the delta_U_matrix. + * It updates the total_index with the number of constraints set and calls + * itself recursively for the next index. + * + * @tparam M_Type Type of the matrix M. + * @tparam Gamma_Type Type of the vector gamma. + * @tparam Delta_U_max_Matrix_Type Type of the delta U maximum matrix. + * @tparam I Current index in the iteration (template parameter). + * @tparam I_Dif The difference in indices for recursion (template parameter). + * @param M Reference to the matrix to be modified. + * @param gamma Reference to the vector to be modified. + * @param delta_U_matrix Matrix providing the constraint value. + * @param total_index Reference to the counter tracking the total number of + * constraints set. + * @param initial_position The starting position in M and gamma for applying + * the constraints. + */ static void apply(M_Type &M, Gamma_Type &gamma, const Delta_U_max_Matrix_Type &delta_U_matrix, std::size_t &total_index, @@ -521,6 +849,25 @@ template struct Calculate_M_Gamma_Delta_U_Max_Loop { + /** + * @brief A no-op function for the case when the loop has no more indices to + * process. + * + * This static function does nothing and is used to terminate the recursion + * in the template specialization. + * + * @tparam M_Type Type of the matrix M (unused). + * @tparam Gamma_Type Type of the vector gamma (unused). + * @tparam Delta_U_max_Matrix_Type Type of the delta U maximum matrix + * (unused). + * @param M Reference to the matrix (unused). + * @param gamma Reference to the vector (unused). + * @param delta_U_matrix Matrix providing the constraint value (unused). + * @param total_index Reference to the counter tracking the total number of + * constraints set (unused). + * @param initial_position The starting position in M and gamma for applying + * the constraints (unused). + */ static void apply(M_Type &M, Gamma_Type &gamma, const Delta_U_max_Matrix_Type &delta_U_matrix, std::size_t &total_index, @@ -535,12 +882,32 @@ struct Calculate_M_Gamma_Delta_U_Max_Loop using Calculate_M_Gamma_Delta_U_Max = Calculate_M_Gamma_Delta_U_Max_Loop< M_Type, Gamma_Type, Delta_U_max_Matrix_Type, 0, Delta_U_Size>; /* calculate M gamma for U min */ + +/** + * @brief A utility structure to calculate M gamma for U min constraints. + * + * This structure defines a loop to apply constraints for U min, iterating + * through the indices and applying conditions based on the flags defined in the + * U_min_Matrix_Type. + */ template struct Calculate_M_Gamma_U_Min_Condition {}; @@ -549,6 +916,29 @@ template struct Calculate_M_Gamma_U_Min_Condition { + /** + * @brief Applies constraints to the given matrices for a specific index. + * + * This static function modifies the matrix M and vector gamma by applying a + * constraint at the position specified by initial_position and the template + * parameter I. It sets the corresponding entry in M to -1.0 and updates gamma + * using the value from U_matrix at position <0, I>. The set_count is + * incremented to reflect the addition of a new constraint. + * + * @tparam I Index at which the constraint is applied (template parameter). + * @tparam M_Type Type of the matrix M. + * @tparam Gamma_Type Type of the vector gamma. + * @tparam U_min_Matrix_Type Type of the U_min_matrix. + * @tparam U_Type Type of the U matrix. + * @param M Reference to the matrix to be modified. + * @param gamma Reference to the vector to be modified. + * @param U_matrix Matrix providing the constraint value. + * @param U Matrix providing additional values for gamma. + * @param set_count Reference to the counter tracking the number of + * constraints set. + * @param initial_position The starting position in M and gamma for applying + * the constraint. + */ static void apply(M_Type &M, Gamma_Type &gamma, const U_min_Matrix_Type &U_matrix, const U_Type &U, std::size_t &set_count, @@ -566,6 +956,19 @@ template struct Calculate_M_Gamma_U_Min_Condition { + /** + * @brief A no-op function for the case when the condition is false. + * This function does nothing and is used to maintain the structure of the + * template specialization. + * @param M Reference to the matrix (unused). + * @param gamma Reference to the vector (unused). + * @param U_matrix Matrix providing the constraint value (unused). + * @param U Matrix providing additional values for gamma (unused). + * @param set_count Reference to the counter tracking the number of + * constraints set (unused). + * @param initial_position The starting position in M and gamma for applying + * the constraint (unused). + */ static void apply(M_Type &M, Gamma_Type &gamma, const U_min_Matrix_Type &U_matrix, const U_Type &U, std::size_t &set_count, @@ -583,6 +986,29 @@ struct Calculate_M_Gamma_U_Min_Condition struct Calculate_M_Gamma_U_Min_Loop { + /** + * @brief Applies constraints to the given matrices for a range of indices. + * + * This static function iterates over the indices from I to I_Dif, applying + * constraints to the matrix M and vector gamma based on the U_matrix. It + * updates the total_index with the number of constraints set and calls itself + * recursively for the next index. + * + * @tparam M_Type Type of the matrix M. + * @tparam Gamma_Type Type of the vector gamma. + * @tparam U_min_Matrix_Type Type of the U minimum matrix. + * @tparam U_Type Type of the U matrix. + * @tparam I Current index in the iteration (template parameter). + * @tparam I_Dif The difference in indices for recursion (template parameter). + * @param M Reference to the matrix to be modified. + * @param gamma Reference to the vector to be modified. + * @param U_matrix Matrix providing the constraint value. + * @param U Matrix providing additional values for gamma. + * @param total_index Reference to the counter tracking the total number of + * constraints set. + * @param initial_position The starting position in M and gamma for applying + * the constraints. + */ static void apply(M_Type &M, Gamma_Type &gamma, const U_min_Matrix_Type &U_matrix, const U_Type &U, std::size_t &total_index, @@ -608,6 +1034,26 @@ template struct Calculate_M_Gamma_U_Min_Loop { + /** + * @brief A no-op function for the case when the loop has no more indices to + * process. + * + * This static function does nothing and is used to terminate the recursion + * in the template specialization. + * + * @tparam M_Type Type of the matrix M (unused). + * @tparam Gamma_Type Type of the vector gamma (unused). + * @tparam U_min_Matrix_Type Type of the U minimum matrix (unused). + * @tparam U_Type Type of the U matrix (unused). + * @param M Reference to the matrix (unused). + * @param gamma Reference to the vector (unused). + * @param U_matrix Matrix providing the constraint value (unused). + * @param U Matrix providing additional values for gamma (unused). + * @param total_index Reference to the counter tracking the total number of + * constraints set (unused). + * @param initial_position The starting position in M and gamma for applying + * the constraints (unused). + */ static void apply(M_Type &M, Gamma_Type &gamma, const U_min_Matrix_Type &U_matrix, const U_Type &U, const std::size_t &total_index, @@ -622,6 +1068,19 @@ struct Calculate_M_Gamma_U_Min_Loop using Calculate_M_Gamma_U_Min = @@ -629,6 +1088,14 @@ using Calculate_M_Gamma_U_Min = 0, U_Min_Size>; /* calculate M gamma for U max */ + +/** + * @brief A utility structure to calculate M gamma for U max constraints. + * + * This structure defines a loop to apply constraints for U max, iterating + * through the indices and applying conditions based on the flags defined in the + * U_max_Matrix_Type. + */ template struct Calculate_M_Gamma_U_Max_Condition {}; @@ -637,6 +1104,29 @@ template struct Calculate_M_Gamma_U_Max_Condition { + /** + * @brief Applies constraints to the given matrices for a specific index. + * + * This static function modifies the matrix M and vector gamma by applying a + * constraint at the position specified by initial_position and the template + * parameter I. It sets the corresponding entry in M to 1.0 and updates gamma + * using the value from U_matrix at position . The set_count is + * incremented to reflect the addition of a new constraint. + * + * @tparam I Index at which the constraint is applied (template parameter). + * @tparam M_Type Type of the matrix M. + * @tparam Gamma_Type Type of the vector gamma. + * @tparam U_max_Matrix_Type Type of the U_max_matrix. + * @tparam U_Type Type of the U matrix. + * @param M Reference to the matrix to be modified. + * @param gamma Reference to the vector to be modified. + * @param U_matrix Matrix providing the constraint value. + * @param U Matrix providing additional values for gamma. + * @param set_count Reference to the counter tracking the number of + * constraints set. + * @param initial_position The starting position in M and gamma for applying + * the constraint. + */ static void apply(M_Type &M, Gamma_Type &gamma, const U_max_Matrix_Type &U_matrix, const U_Type &U, std::size_t &set_count, @@ -654,6 +1144,19 @@ template struct Calculate_M_Gamma_U_Max_Condition { + /** + * @brief A no-op function for the case when the condition is false. + * This function does nothing and is used to maintain the structure of the + * template specialization. + * @param M Reference to the matrix (unused). + * @param gamma Reference to the vector (unused). + * @param U_matrix Matrix providing the constraint value (unused). + * @param U Matrix providing additional values for gamma (unused). + * @param set_count Reference to the counter tracking the number of + * constraints set (unused). + * @param initial_position The starting position in M and gamma for applying + * the constraint (unused). + */ static void apply(M_Type &M, Gamma_Type &gamma, const U_max_Matrix_Type &U_matrix, const U_Type &U, std::size_t &set_count, @@ -671,6 +1174,29 @@ struct Calculate_M_Gamma_U_Max_Condition struct Calculate_M_Gamma_U_Max_Loop { + /** + * @brief Applies constraints to the given matrices for a range of indices. + * + * This static function iterates over the indices from I to I_Dif, applying + * constraints to the matrix M and vector gamma based on the U_matrix. It + * updates the total_index with the number of constraints set and calls itself + * recursively for the next index. + * + * @tparam M_Type Type of the matrix M. + * @tparam Gamma_Type Type of the vector gamma. + * @tparam U_max_Matrix_Type Type of the U maximum matrix. + * @tparam U_Type Type of the U matrix. + * @tparam I Current index in the iteration (template parameter). + * @tparam I_Dif The difference in indices for recursion (template parameter). + * @param M Reference to the matrix to be modified. + * @param gamma Reference to the vector to be modified. + * @param U_matrix Matrix providing the constraint value. + * @param U Matrix providing additional values for gamma. + * @param total_index Reference to the counter tracking the total number of + * constraints set. + * @param initial_position The starting position in M and gamma for applying + * the constraints. + */ static void apply(M_Type &M, Gamma_Type &gamma, const U_max_Matrix_Type &U_matrix, const U_Type &U, std::size_t &total_index, @@ -696,6 +1222,26 @@ template struct Calculate_M_Gamma_U_Max_Loop { + /** + * @brief A no-op function for the case when the loop has no more indices to + * process. + * + * This static function does nothing and is used to terminate the recursion + * in the template specialization. + * + * @tparam M_Type Type of the matrix M (unused). + * @tparam Gamma_Type Type of the vector gamma (unused). + * @tparam U_max_Matrix_Type Type of the U maximum matrix (unused). + * @tparam U_Type Type of the U matrix (unused). + * @param M Reference to the matrix (unused). + * @param gamma Reference to the vector (unused). + * @param U_matrix Matrix providing the constraint value (unused). + * @param U Matrix providing additional values for gamma (unused). + * @param total_index Reference to the counter tracking the total number of + * constraints set (unused). + * @param initial_position The starting position in M and gamma for applying + * the constraints (unused). + */ static void apply(M_Type &M, Gamma_Type &gamma, const U_max_Matrix_Type &U_matrix, const U_Type &U, std::size_t &total_index, @@ -710,6 +1256,19 @@ struct Calculate_M_Gamma_U_Max_Loop using Calculate_M_Gamma_U_Max = @@ -721,6 +1280,25 @@ template struct Set_M_Y_Min_Cols { + /** + * @brief Applies constraints to the matrix M based on the Phi matrix. + * + * This static function modifies the matrix M by setting specific entries + * based on the values from the Phi matrix. It iterates through the columns + * of the Phi matrix, applying constraints for each column until J_Dif is + * reached. + * + * @tparam M_Type Type of the matrix M. + * @tparam Phi_Type Type of the Phi matrix. + * @tparam Y_Constraints_Prediction_Offset Offset for Y constraints in Phi. + * @tparam I Current row index in M and Phi. + * @tparam J Current column index in M and Phi. + * @tparam J_Dif The difference in columns for recursion (template parameter). + * @param M Reference to the matrix to be modified. + * @param Phi Reference to the Phi matrix providing constraint values. + * @param initial_position The starting position in M for applying the + * constraints. + */ static void apply(M_Type &M, const Phi_Type &Phi, std::size_t initial_position) { @@ -736,6 +1314,24 @@ template struct Set_M_Y_Min_Cols { + /** + * @brief A no-op function for the case when there are no more columns to + * process. + * + * This static function does nothing and is used to terminate the recursion + * in the template specialization. + * + * @tparam M_Type Type of the matrix M (unused). + * @tparam Phi_Type Type of the Phi matrix (unused). + * @tparam Y_Constraints_Prediction_Offset Offset for Y constraints in Phi + * (unused). + * @tparam I Current row index in M and Phi (unused). + * @tparam J Current column index in M and Phi (unused). + * @param M Reference to the matrix (unused). + * @param Phi Reference to the Phi matrix (unused). + * @param initial_position The starting position in M for applying the + * constraints (unused). + */ static void apply(M_Type &M, const Phi_Type &Phi, std::size_t initial_position) { @@ -757,6 +1353,32 @@ template { + /** + * @brief Applies constraints to the given matrices for a specific index. + * + * This static function modifies the matrix M and vector gamma by applying a + * constraint at the position specified by initial_position and the template + * parameter I. It sets the corresponding entry in M based on the Phi matrix + * and updates gamma using the value from Y_min_matrix at position . + * The set_count is incremented to reflect the addition of a new constraint. + * + * @tparam I Index at which the constraint is applied (template parameter). + * @tparam M_Type Type of the matrix M. + * @tparam Gamma_Type Type of the vector gamma. + * @tparam Y_min_Type Type of the Y minimum matrix. + * @tparam Phi_Type Type of the Phi matrix. + * @tparam F_X_Type Type of the F_X matrix. + * @param M Reference to the matrix to be modified. + * @param gamma Reference to the vector to be modified. + * @param Y_min_matrix Matrix providing the minimum Y values. + * @param Phi Reference to the Phi matrix providing constraint values. + * @param F_X Reference to the F_X matrix providing additional values for + * gamma. + * @param set_count Reference to the counter tracking the number of + * constraints set. + * @param initial_position The starting position in M and gamma for applying + * the constraint. + */ static void apply(M_Type &M, Gamma_Type &gamma, const Y_min_Type &Y_min_matrix, const Phi_Type &Phi, const F_X_Type &F_X, std::size_t &set_count, @@ -778,6 +1400,22 @@ template { + /** + * @brief A no-op function for the case when the condition is false. + * This function does nothing and is used to maintain the structure of the + * template specialization. + * @param M Reference to the matrix (unused). + * @param gamma Reference to the vector (unused). + * @param Y_min_matrix Matrix providing the minimum Y values (unused). + * @param Phi Reference to the Phi matrix providing constraint values + * (unused). + * @param F_X Reference to the F_X matrix providing additional values for + * gamma (unused). + * @param set_count Reference to the counter tracking the number of + * constraints set (unused). + * @param initial_position The starting position in M and gamma for applying + * the constraint (unused). + */ static void apply(M_Type &M, Gamma_Type &gamma, const Y_min_Type Y_min_matrix, const Phi_Type &Phi, const F_X_Type &F_X, std::size_t &set_count, std::size_t initial_position) { @@ -798,6 +1436,33 @@ template struct Calculate_M_Gamma_Y_Min_Loop { + /** + * @brief Applies constraints to the given matrices for a range of indices. + * + * This static function iterates over the indices from I to I_Dif, applying + * constraints to the matrix M and vector gamma based on the Y_min_matrix and + * Phi. It updates the total_index with the number of constraints set and + * calls itself recursively for the next index. + * + * @tparam M_Type Type of the matrix M. + * @tparam Gamma_Type Type of the vector gamma. + * @tparam Y_min_Type Type of the Y minimum matrix. + * @tparam Phi_Type Type of the Phi matrix. + * @tparam F_X_Type Type of the F_X matrix. + * @tparam Y_Constraints_Prediction_Offset Offset for Y constraints in Phi. + * @tparam I Current index in the iteration (template parameter). + * @tparam I_Dif The difference in indices for recursion (template parameter). + * @param M Reference to the matrix to be modified. + * @param gamma Reference to the vector to be modified. + * @param Y_min_matrix Matrix providing the minimum Y values. + * @param Phi Reference to the Phi matrix providing constraint values. + * @param F_X Reference to the F_X matrix providing additional values for + * gamma. + * @param total_index Reference to the counter tracking the total number of + * constraints set. + * @param initial_position The starting position in M and gamma for applying + * the constraints. + */ static void apply(M_Type &M, Gamma_Type &gamma, const Y_min_Type Y_min_matrix, const Phi_Type &Phi, const F_X_Type &F_X, std::size_t &total_index, std::size_t initial_position) { @@ -826,6 +1491,30 @@ template { + /** + * @brief A no-op function for the case when the loop has no more indices to + * process. + * + * This static function does nothing and is used to terminate the recursion + * in the template specialization. + * + * @tparam M_Type Type of the matrix M (unused). + * @tparam Gamma_Type Type of the vector gamma (unused). + * @tparam Y_min_Type Type of the Y minimum matrix (unused). + * @tparam Phi_Type Type of the Phi matrix (unused). + * @tparam F_X_Type Type of the F_X matrix (unused). + * @param M Reference to the matrix (unused). + * @param gamma Reference to the vector (unused). + * @param Y_min_matrix Matrix providing the minimum Y values (unused). + * @param Phi Reference to the Phi matrix providing constraint values + * (unused). + * @param F_X Reference to the F_X matrix providing additional values for + * gamma (unused). + * @param total_index Reference to the counter tracking the total number of + * constraints set (unused). + * @param initial_position The starting position in M and gamma for applying + * the constraints (unused). + */ static void apply(M_Type &M, Gamma_Type &gamma, const Y_min_Type Y_min_matrix, const Phi_Type &Phi, const F_X_Type &F_X, std::size_t &total_index, std::size_t initial_position) { @@ -841,6 +1530,21 @@ struct Calculate_M_Gamma_Y_Min_Loop @@ -854,6 +1558,25 @@ template struct Set_M_Y_Max_Cols { + /** + * @brief Applies constraints to the matrix M based on the Phi matrix. + * + * This static function modifies the matrix M by setting specific entries + * based on the values from the Phi matrix. It iterates through the columns + * of the Phi matrix, applying constraints for each column until J_Dif is + * reached. + * + * @tparam M_Type Type of the matrix M. + * @tparam Phi_Type Type of the Phi matrix. + * @tparam Y_Constraints_Prediction_Offset Offset for Y constraints in Phi. + * @tparam I Current row index in M and Phi. + * @tparam J Current column index in M and Phi. + * @tparam J_Dif The difference in columns for recursion (template parameter). + * @param M Reference to the matrix to be modified. + * @param Phi Reference to the Phi matrix providing constraint values. + * @param initial_position The starting position in M for applying the + * constraints. + */ static void apply(M_Type &M, const Phi_Type &Phi, std::size_t initial_position) { @@ -869,6 +1592,24 @@ template struct Set_M_Y_Max_Cols { + /** + * @brief A no-op function for the case when there are no more columns to + * process. + * + * This static function does nothing and is used to terminate the recursion + * in the template specialization. + * + * @tparam M_Type Type of the matrix M (unused). + * @tparam Phi_Type Type of the Phi matrix (unused). + * @tparam Y_Constraints_Prediction_Offset Offset for Y constraints in Phi + * (unused). + * @tparam I Current row index in M and Phi (unused). + * @tparam J Current column index in M and Phi (unused). + * @param M Reference to the matrix (unused). + * @param Phi Reference to the Phi matrix (unused). + * @param initial_position The starting position in M for applying the + * constraints (unused). + */ static void apply(M_Type &M, const Phi_Type &Phi, std::size_t initial_position) { @@ -890,6 +1631,32 @@ template { + /** + * @brief Applies constraints to the given matrices for a specific index. + * + * This static function modifies the matrix M and vector gamma by applying a + * constraint at the position specified by initial_position and the template + * parameter I. It sets the corresponding entry in M based on the Phi matrix + * and updates gamma using the value from Y_max_matrix at position . + * The set_count is incremented to reflect the addition of a new constraint. + * + * @tparam I Index at which the constraint is applied (template parameter). + * @tparam M_Type Type of the matrix M. + * @tparam Gamma_Type Type of the vector gamma. + * @tparam Y_max_Type Type of the Y maximum matrix. + * @tparam Phi_Type Type of the Phi matrix. + * @tparam F_X_Type Type of the F_X matrix. + * @param M Reference to the matrix to be modified. + * @param gamma Reference to the vector to be modified. + * @param Y_max_matrix Matrix providing the maximum Y values. + * @param Phi Reference to the Phi matrix providing constraint values. + * @param F_X Reference to the F_X matrix providing additional values for + * gamma. + * @param set_count Reference to the counter tracking the number of + * constraints set. + * @param initial_position The starting position in M and gamma for applying + * the constraint. + */ static void apply(M_Type &M, Gamma_Type &gamma, const Y_max_Type &Y_max_matrix, const Phi_Type &Phi, const F_X_Type &F_X, std::size_t &set_count, @@ -911,6 +1678,22 @@ template { + /** + * @brief A no-op function for the case when the condition is false. + * This function does nothing and is used to maintain the structure of the + * template specialization. + * @param M Reference to the matrix (unused). + * @param gamma Reference to the vector (unused). + * @param Y_max_matrix Matrix providing the maximum Y values (unused). + * @param Phi Reference to the Phi matrix providing constraint values + * (unused). + * @param F_X Reference to the F_X matrix providing additional values for + * gamma (unused). + * @param set_count Reference to the counter tracking the number of + * constraints set (unused). + * @param initial_position The starting position in M and gamma for applying + * the constraint (unused). + */ static void apply(M_Type &M, Gamma_Type &gamma, const Y_max_Type Y_max_matrix, const Phi_Type &Phi, const F_X_Type &F_X, std::size_t &set_count, std::size_t initial_position) { @@ -931,6 +1714,33 @@ template struct Calculate_M_Gamma_Y_Max_Loop { + /** + * @brief Applies constraints to the given matrices for a range of indices. + * + * This static function iterates over the indices from I to I_Dif, applying + * constraints to the matrix M and vector gamma based on the Y_max_matrix and + * Phi. It updates the total_index with the number of constraints set and + * calls itself recursively for the next index. + * + * @tparam M_Type Type of the matrix M. + * @tparam Gamma_Type Type of the vector gamma. + * @tparam Y_max_Type Type of the Y maximum matrix. + * @tparam Phi_Type Type of the Phi matrix. + * @tparam F_X_Type Type of the F_X matrix. + * @tparam Y_Constraints_Prediction_Offset Offset for Y constraints in Phi. + * @tparam I Current index in the iteration (template parameter). + * @tparam I_Dif The difference in indices for recursion (template parameter). + * @param M Reference to the matrix to be modified. + * @param gamma Reference to the vector to be modified. + * @param Y_max_matrix Matrix providing the maximum Y values. + * @param Phi Reference to the Phi matrix providing constraint values. + * @param F_X Reference to the F_X matrix providing additional values for + * gamma. + * @param total_index Reference to the counter tracking the total number of + * constraints set. + * @param initial_position The starting position in M and gamma for applying + * the constraints. + */ static void apply(M_Type &M, Gamma_Type &gamma, const Y_max_Type Y_max_matrix, const Phi_Type &Phi, const F_X_Type &F_X, std::size_t &total_index, std::size_t initial_position) { @@ -959,6 +1769,30 @@ template { + /** + * @brief A no-op function for the case when the loop has no more indices to + * process. + * + * This static function does nothing and is used to terminate the recursion + * in the template specialization. + * + * @tparam M_Type Type of the matrix M (unused). + * @tparam Gamma_Type Type of the vector gamma (unused). + * @tparam Y_max_Type Type of the Y maximum matrix (unused). + * @tparam Phi_Type Type of the Phi matrix (unused). + * @tparam F_X_Type Type of the F_X matrix (unused). + * @param M Reference to the matrix (unused). + * @param gamma Reference to the vector (unused). + * @param Y_max_matrix Matrix providing the maximum Y values (unused). + * @param Phi Reference to the Phi matrix providing constraint values + * (unused). + * @param F_X Reference to the F_X matrix providing additional values for + * gamma (unused). + * @param total_index Reference to the counter tracking the total number of + * constraints set (unused). + * @param initial_position The starting position in M and gamma for applying + * the constraints (unused). + */ static void apply(M_Type &M, Gamma_Type &gamma, const Y_max_Type Y_max_matrix, const Phi_Type &Phi, const F_X_Type &F_X, std::size_t &total_index, std::size_t initial_position) { @@ -974,6 +1808,21 @@ struct Calculate_M_Gamma_Y_Max_Loop @@ -985,6 +1834,34 @@ using Calculate_M_Gamma_Y_Max = } // namespace LTI_MPC_QP_SolverOperation /* LTI MPC QP solver */ + +/** + * @brief Class template for LTI MPC QP Solver. + * + * This class template implements a solver for Linear Time-Invariant Model + * Predictive Control (LTI MPC) using Quadratic Programming (QP). It handles + * constraints on control inputs and outputs, and provides methods to update + * constraints and solve the QP problem. + * + * @tparam Number_Of_Variables Number of control variables. + * @tparam Output_Size Size of the output vector. + * @tparam U_Type Type representing the control input matrix. + * @tparam X_augmented_Type Type representing the augmented state vector. + * @tparam Phi_Type Type representing the prediction matrix. + * @tparam F_Type Type representing the system dynamics matrix. + * @tparam Weight_U_Nc_Type Type representing the weight for control input + * changes. + * @tparam Delta_U_Min_Type Type representing the minimum change in control + * input. + * @tparam Delta_U_Max_Type Type representing the maximum change in control + * input. + * @tparam U_Min_Type Type representing the minimum control input limits. + * @tparam U_Max_Type Type representing the maximum control input limits. + * @tparam Y_Min_Type Type representing the minimum output limits. + * @tparam Y_Max_Type Type representing the maximum output limits. + * @tparam Y_Constraints_Prediction_Offset Offset for Y constraints in + * prediction matrix (default is 0). + */ template inline typename std::enable_if<(N > 0), void>::type update_constraints(const U_Type &U_in, const X_augmented_Type &X_augmented_in, @@ -1146,6 +2036,18 @@ class LTI_MPC_QP_Solver { this->_calculate_M_gamma_Y(total_index, X_augmented_in, Phi_in, F_in); } + /** + * @brief A no-op function for the case when there are no constraints to + * update. + * + * This function does nothing and is used to maintain the structure of the + * template specialization when there are no constraints to update. + * + * @param U_in Input control vector (unused). + * @param X_augmented_in Augmented state vector (unused). + * @param Phi_in Prediction matrix (unused). + * @param F_in System dynamics matrix (unused). + */ template inline typename std::enable_if<(N == 0), void>::type update_constraints(const U_Type &U_in, const X_augmented_Type &X_augmented_in, @@ -1158,6 +2060,16 @@ class LTI_MPC_QP_Solver { static_cast(F_in); } + /** + * @brief Updates the E matrix used in the QP solver. + * + * This function updates the E matrix by calculating the product of the + * transpose of Phi and Phi, and adding the provided weight matrix for control + * input changes. + * + * @param Phi The prediction matrix. + * @param Weight_U_Nc The weight matrix for control input changes. + */ inline void update_E(const Phi_Type &Phi, const Weight_U_Nc_Type &Weight_U_Nc) { @@ -1165,11 +2077,27 @@ class LTI_MPC_QP_Solver { Weight_U_Nc); } + /** + * @brief Returns the number of constraints for delta U. + * + * This function returns the number of constraints related to the change in + * control input (delta U). + * + * @return The number of delta U constraints. + */ inline auto get_number_of_Y_constraints_prediction_offset(void) const -> std::size_t { return this->_Y_constraints_prediction_offset; } + /** + * @brief Returns the number of constraints for delta U. + * + * This function returns the number of constraints related to the change in + * control input (delta U). + * + * @return The number of delta U constraints. + */ template inline auto solve(const Phi_Type &Phi, const F_Type &F, ReferenceTrajectoryType &reference_trajectory, @@ -1183,6 +2111,20 @@ class LTI_MPC_QP_Solver { return x_opt; } + /** + * @brief Solves the QP problem with the given parameters. + * + * This function solves the QP problem using the provided prediction matrix + * Phi, system dynamics matrix F, reference trajectory, augmented state + * vector, and weight for control input changes. + * + * @param Phi The prediction matrix. + * @param F The system dynamics matrix. + * @param reference_trajectory The reference trajectory object. + * @param X_augmented The augmented state vector. + * @param Weight_U_Nc The weight matrix for control input changes. + * @return The optimal control input vector U_Nc. + */ template inline auto solve(const Phi_Type &Phi, const F_Type &F, const ReferenceTrajectoryType &reference_trajectory, @@ -1201,6 +2143,16 @@ class LTI_MPC_QP_Solver { protected: /* Function */ + + /** + * @brief Calculates the M and gamma matrices for delta U and U constraints. + * + * This function calculates the M and gamma matrices based on the provided + * limits for delta U and U, updating the total index for constraints. + * + * @param total_index Reference to the counter tracking the total number of + * constraints set. + */ inline void _calculate_M_gamma_delta_U(std::size_t &total_index) { std::size_t initial_position = total_index; @@ -1221,6 +2173,16 @@ class LTI_MPC_QP_Solver { total_index, initial_position); } + /** + * @brief Calculates the M and gamma matrices for U constraints. + * + * This function calculates the M and gamma matrices based on the provided + * limits for U, updating the total index for constraints. + * + * @param total_index Reference to the counter tracking the total number of + * constraints set. + * @param U The control input vector. + */ inline void _calculate_M_gamma_U(std::size_t &total_index, const U_Type &U) { std::size_t initial_position = total_index; @@ -1241,6 +2203,19 @@ class LTI_MPC_QP_Solver { initial_position); } + /** + * @brief Calculates the M and gamma matrices for Y constraints. + * + * This function calculates the M and gamma matrices based on the provided + * augmented state vector, prediction matrix, and system dynamics matrix, + * updating the total index for constraints. + * + * @param total_index Reference to the counter tracking the total number of + * constraints set. + * @param X_augmented The augmented state vector. + * @param Phi The prediction matrix. + * @param F The system dynamics matrix. + */ inline void _calculate_M_gamma_Y(std::size_t &total_index, const X_augmented_Type &X_augmented, const Phi_Type &Phi, const F_Type &F) { @@ -1286,6 +2261,32 @@ class LTI_MPC_QP_Solver { }; /* make LTI_MPC_QP_Solver */ + +/** + * @brief Factory function to create an instance of LTI_MPC_QP_Solver. + * + * This function initializes and returns an instance of LTI_MPC_QP_Solver with + * the provided parameters. + * + * @tparam Number_Of_Variables Number of control variables. + * @tparam Output_Size Size of the output vector. + * @tparam U_Type Type representing the control input matrix. + * @tparam X_augmented_Type Type representing the augmented state vector. + * @tparam Phi_Type Type representing the prediction matrix. + * @tparam F_Type Type representing the system dynamics matrix. + * @tparam Weight_U_Nc_Type Type representing the weight for control input + * changes. + * @tparam Delta_U_Min_Type Type representing the minimum change in control + * input. + * @tparam Delta_U_Max_Type Type representing the maximum change in control + * input. + * @tparam U_Min_Type Type representing the minimum control input limits. + * @tparam U_Max_Type Type representing the maximum control input limits. + * @tparam Y_Min_Type Type representing the minimum output limits. + * @tparam Y_Max_Type Type representing the maximum output limits. + * @return An instance of LTI_MPC_QP_Solver initialized with the provided + * parameters. + */ template np.ndarray: + """ + Convert a symbolic sympy matrix to a numeric numpy matrix. + Args: + symbolic_matrix (sp.Matrix): A sympy matrix containing symbolic expressions. + Returns: + np.ndarray: A numpy array with numeric values converted from the symbolic matrix. + """ numeric_matrix = np.zeros( (symbolic_matrix.shape[0], symbolic_matrix.shape[1]), dtype=float) @@ -14,6 +26,17 @@ def symbolic_to_numeric_matrix(symbolic_matrix: sp.Matrix) -> np.ndarray: class SymbolicStateSpace: + """ + A class representing a symbolic state-space model. + Attributes: + A (sp.Matrix): State matrix. + B (sp.Matrix): Input matrix. + C (sp.Matrix): Output matrix. + D (sp.Matrix, optional): Feedthrough matrix. + delta_time (float): Time step for discrete systems. + Number_of_Delay (int): Number of delays in the system. + """ + def __init__(self, A: sp.Matrix, B: sp.Matrix, C: sp.Matrix, D: sp.Matrix = None, delta_time=0.0, Number_of_Delay=0): self.delta_time = delta_time @@ -46,6 +69,21 @@ def __init__(self, A: sp.Matrix, B: sp.Matrix, C: sp.Matrix, class StateSpaceEmbeddedIntegrator: + """ + A class that augments a state-space model with an embedded integrator. + This class takes a symbolic state-space model and constructs an augmented model + that includes the state, input, and output matrices, along with the necessary + transformations to handle the output as an integral of the state. + Attributes: + delta_time (float): Time step for discrete systems. + INPUT_SIZE (int): Number of inputs in the system. + STATE_SIZE (int): Number of states in the system. + OUTPUT_SIZE (int): Number of outputs in the system. + A (sp.Matrix): Augmented state matrix. + B (sp.Matrix): Augmented input matrix. + C (sp.Matrix): Augmented output matrix. + """ + def __init__(self, state_space: SymbolicStateSpace): if not isinstance(state_space.A, sp.Matrix): raise ValueError( @@ -78,6 +116,13 @@ def __init__(self, state_space: SymbolicStateSpace): def construct_augmented_model(self, A_original: sp.Matrix, B_original: sp.Matrix, C_original: sp.Matrix): + """ + Constructs the augmented state-space model with an embedded integrator. + Args: + A_original (sp.Matrix): Original state matrix. + B_original (sp.Matrix): Original input matrix. + C_original (sp.Matrix): Original output matrix. + """ o_xy_T = sp.Matrix(self.STATE_SIZE, self.OUTPUT_SIZE, lambda i, j: 0.0) @@ -97,6 +142,19 @@ def construct_augmented_model(self, A_original: sp.Matrix, class MPC_PredictionMatrices: + """ + A class to generate prediction matrices for Model Predictive Control (MPC). + This class constructs the F and Phi matrices based on the state-space model + and the specified prediction horizon (Np) and control horizon (Nc). + + Attributes: + Np (int): Prediction horizon. + Nc (int): Control horizon. + INPUT_SIZE (int): Number of inputs in the system. + STATE_SIZE (int): Number of states in the system. + OUTPUT_SIZE (int): Number of outputs in the system. + """ + def __init__(self, Np, Nc, INPUT_SIZE, STATE_SIZE, OUTPUT_SIZE): self.INPUT_SIZE = INPUT_SIZE self.STATE_SIZE = STATE_SIZE @@ -124,6 +182,11 @@ def __init__(self, Np, Nc, INPUT_SIZE, STATE_SIZE, OUTPUT_SIZE): self.ABC_values = {} def initialize_ABC(self): + """ + Initializes symbolic matrices A, B, and C with symbolic variables. + This method creates symbolic matrices for the state, input, and output + matrices, which will be used for symbolic manipulation and substitution. + """ self.A_symbolic = sp.Matrix(self.STATE_SIZE, self.STATE_SIZE, lambda i, j: sp.symbols(f'a{i+1}{j+1}')) self.B_symbolic = sp.Matrix(self.STATE_SIZE, self.INPUT_SIZE, @@ -139,6 +202,14 @@ def initialize_ABC(self): lambda i, j: 0.0) def generate_symbolic_substitution(self, A: np.ndarray, B: np.ndarray, C: np.ndarray): + """ + Generates symbolic variables for the elements of matrices A, B, and C, + and maps them to their corresponding numeric values. + Args: + A (np.ndarray): State matrix. + B (np.ndarray): Input matrix. + C (np.ndarray): Output matrix. + """ # Generate symbolic variables and map them to A's elements for i in range(A.shape[0]): for j in range(A.shape[1]): @@ -158,6 +229,14 @@ def generate_symbolic_substitution(self, A: np.ndarray, B: np.ndarray, C: np.nda self.ABC_values[symbol] = C[i, j] def substitute_ABC_symbolic(self, A: sp.Matrix, B: sp.Matrix, C: sp.Matrix): + """ + Substitutes symbolic variables in the state-space matrices A, B, and C + with their corresponding numeric values. + Args: + A (sp.Matrix): State matrix. + B (sp.Matrix): Input matrix. + C (sp.Matrix): Output matrix. + """ self.A_symbolic = sp.Matrix(self.STATE_SIZE, self.STATE_SIZE, lambda i, j: sp.symbols(f'a{i+1}{j+1}')) for i in range(A.shape[0]): @@ -179,6 +258,13 @@ def substitute_ABC_symbolic(self, A: sp.Matrix, B: sp.Matrix, C: sp.Matrix): self._generate_exponential_A_list(self.A_symbolic) def substitute_ABC_numeric(self, A: np.ndarray, B: np.ndarray, C: np.ndarray): + """ + Substitutes numeric values into the symbolic matrices A, B, and C. + Args: + A (np.ndarray): State matrix. + B (np.ndarray): Input matrix. + C (np.ndarray): Output matrix. + """ self.A_symbolic = sp.Matrix(self.STATE_SIZE, self.STATE_SIZE, lambda i, j: sp.symbols(f'a{i+1}{j+1}')) for i in range(A.shape[0]): @@ -204,6 +290,14 @@ def substitute_ABC_numeric(self, A: np.ndarray, B: np.ndarray, C: np.ndarray): self.A_numeric) def substitute_numeric(self, A: np.ndarray, B: np.ndarray, C: np.ndarray) -> tuple: + """ + Substitutes numeric values into the symbolic matrices A, B, and C, + and builds the F and Phi matrices. + Args: + A (np.ndarray): State matrix. + B (np.ndarray): Input matrix. + C (np.ndarray): Output matrix. + """ if not isinstance(A, np.ndarray): A = symbolic_to_numeric_matrix(A) if not isinstance(B, np.ndarray): @@ -222,10 +316,24 @@ def substitute_numeric(self, A: np.ndarray, B: np.ndarray, C: np.ndarray) -> tup self.Phi_symbolic) def build_matrices(self, B: sp.Matrix, C: sp.Matrix) -> tuple: + """ + Builds the F and Phi matrices based on the symbolic state-space model. + Args: + B (sp.Matrix): Input matrix. + C (sp.Matrix): Output matrix. + """ self.F_symbolic = self._build_F(C) self.Phi_symbolic = self._build_Phi(B, C) def _generate_exponential_A_list(self, A: sp.Matrix): + """ + Generates a list of matrices representing the exponential of the state matrix A + for each step in the prediction horizon. + Args: + A (sp.Matrix): State matrix. + Returns: + list: A list of matrices representing the exponential of A for each step. + """ exponential_A_list = [] for i in range(self.Np): @@ -238,7 +346,14 @@ def _generate_exponential_A_list(self, A: sp.Matrix): return exponential_A_list def _build_F(self, C: sp.Matrix) -> sp.Matrix: - + """ + Builds the F matrix, which is used in the MPC prediction step. + Args: + C (sp.Matrix): Output matrix. + Returns: + sp.Matrix: The F matrix, which is a block matrix containing the outputs + of the system at each step in the prediction horizon. + """ F = sp.zeros(self.OUTPUT_SIZE * self.Np, self.STATE_SIZE) for i in range(self.Np): # C A^{i+1} @@ -247,7 +362,15 @@ def _build_F(self, C: sp.Matrix) -> sp.Matrix: return F def _build_Phi(self, B: sp.Matrix, C: sp.Matrix) -> sp.Matrix: - + """ + Builds the Phi matrix, which is used in the MPC control step. + Args: + B (sp.Matrix): Input matrix. + C (sp.Matrix): Output matrix. + Returns: + sp.Matrix: The Phi matrix, which is a block matrix containing the + contributions of the inputs to the outputs at each step in the prediction horizon. + """ Phi = sp.zeros(self.OUTPUT_SIZE * self.Np, self.INPUT_SIZE * self.Nc) @@ -266,6 +389,18 @@ def _build_Phi(self, B: sp.Matrix, C: sp.Matrix) -> sp.Matrix: class MPC_ReferenceTrajectory: + """ + A class to handle the reference trajectory for Model Predictive Control (MPC). + This class manages the reference vector, which can either be a single row vector + or multiple row vectors, and provides a method to calculate the difference + between the reference vector and the predicted state. + Attributes: + reference_vector (np.ndarray): The reference trajectory vector. + Np (int): Prediction horizon. + OUTPUT_SIZE (int): Number of outputs in the system. + follow_flag (bool): Indicates whether the reference vector has multiple rows. + """ + def __init__(self, reference_vector: np.ndarray, Np: int): if reference_vector.shape[1] == Np: self.follow_flag = True @@ -281,6 +416,13 @@ def __init__(self, reference_vector: np.ndarray, Np: int): self.OUTPUT_SIZE = reference_vector.shape[0] def calculate_dif(self, Fx: np.ndarray) -> np.ndarray: + """ + Calculates the difference between the reference vector and the predicted state. + Args: + Fx (np.ndarray): The predicted state vector. + Returns: + np.ndarray: The difference vector, which is the reference vector minus the predicted state. + """ dif = np.zeros((self.Np * self.OUTPUT_SIZE, 1)) if self.follow_flag: From 49542b6bbcdb67209cf3d4b40f808be1e128a06e Mon Sep 17 00:00:00 2001 From: Claude Date: Sat, 14 Jun 2025 16:50:06 +0900 Subject: [PATCH 3/9] =?UTF-8?q?=E4=BD=9C=E6=88=90=E4=B8=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- mpc_utility/mpc_state_space_utility.hpp | 201 ++++++++++++++++++++++++ 1 file changed, 201 insertions(+) diff --git a/mpc_utility/mpc_state_space_utility.hpp b/mpc_utility/mpc_state_space_utility.hpp index 9212142..2c013e8 100644 --- a/mpc_utility/mpc_state_space_utility.hpp +++ b/mpc_utility/mpc_state_space_utility.hpp @@ -1,3 +1,14 @@ +/** + * @file mpc_state_space_utility.hpp + * @brief Utility classes and functions for Model Predictive Control (MPC) + * state-space prediction and reference trajectory operations. + * + * This header provides a set of template classes and utility functions to + * support the construction and manipulation of prediction matrices and + * reference trajectories for Model Predictive Control (MPC) applications. The + * utilities are designed to be generic and type-safe, supporting compile-time + * checks for matrix dimensions and value types. + */ #ifndef __MPC_STATE_SPACE_UTILITY_HPP__ #define __MPC_STATE_SPACE_UTILITY_HPP__ @@ -10,6 +21,23 @@ namespace PythonMPC { /* MPC Prediction Matrices */ + +/** + * @brief Class template for MPC Prediction Matrices. + * + * This class template encapsulates the prediction matrices used in Model + * Predictive Control (MPC) for state-space systems. It includes the system + * dynamics matrix F and the prediction matrix Phi, which are essential for + * predicting future states and outputs based on current inputs and states. + * + * @tparam F_Type_In Type of the system dynamics matrix F. + * @tparam Phi_Type_In Type of the prediction matrix Phi. + * @tparam Np Number of prediction steps. + * @tparam Nc Number of control steps. + * @tparam Number_Of_Input Number of inputs to the system. + * @tparam Number_Of_State Number of states in the system. + * @tparam Number_Of_Output Number of outputs from the system. + */ template @@ -98,6 +126,23 @@ class MPC_PredictionMatrices { }; /* make MPC Prediction Matrices */ + +/** + * @brief Factory function to create an instance of MPC_PredictionMatrices. + * + * This function initializes and returns an instance of MPC_PredictionMatrices + * with the provided template parameters. + * + * @tparam F_Type Type of the system dynamics matrix F. + * @tparam Phi_Type Type of the prediction matrix Phi. + * @tparam Np Number of prediction steps. + * @tparam Nc Number of control steps. + * @tparam Number_Of_Input Number of inputs to the system. + * @tparam Number_Of_State Number of states in the system. + * @tparam Number_Of_Output Number of outputs from the system. + * @return An instance of MPC_PredictionMatrices initialized with default + * values. + */ template @@ -119,6 +164,28 @@ using MPC_PredictionMatrices_Type = namespace MPC_ReferenceTrajectoryOperation { +/** + * @brief Calculates the difference between a reference value and a predicted + * value for a specific index, and stores the result in the provided difference + * container. + * + * This function is enabled only when ROWS > 1, and asserts at compile time that + * ROWS == Np. It computes the difference between the reference value at + * position (J, I) and the predicted value at the corresponding flattened index, + * then sets this value in the 'dif' container. + * + * @tparam ROWS Number of rows in the reference and prediction matrices (must be + * > 1). + * @tparam Np Prediction horizon length (must be equal to ROWS). + * @tparam I Row index for the operation. + * @tparam J Column index for the operation. + * @tparam Ref_Type Type of the reference matrix. + * @tparam Fx_Type Type of the predicted matrix. + * @tparam Dif_Type Type of the difference container. + * @param ref Reference matrix. + * @param Fx Predicted matrix. + * @param dif Container to store the computed difference. + */ template inline typename std::enable_if<(ROWS > 1), void>::type @@ -130,6 +197,26 @@ calculate_each_dif(const Ref_Type &ref, const Fx_Type &Fx, Dif_Type &dif) { Fx.template get<(I * Ref_Type::COLS) + J, 0>()); } +/** + * @brief Calculates the difference between a reference value and a predicted + * value for a specific index, and stores the result in the provided difference + * container. + * + * This function is enabled only when ROWS == 1. It computes the difference + * between the reference value at position (J, 0) and the predicted value at the + * corresponding flattened index, then sets this value in the 'dif' container. + * + * @tparam ROWS Number of rows in the reference matrix (must be equal to 1). + * @tparam Np Prediction horizon length (must be equal to 1). + * @tparam I Row index for the operation. + * @tparam J Column index for the operation. + * @tparam Ref_Type Type of the reference matrix. + * @tparam Fx_Type Type of the predicted matrix. + * @tparam Dif_Type Type of the difference container. + * @param ref Reference matrix. + * @param Fx Predicted matrix. + * @param dif Container to store the computed difference. + */ template inline typename std::enable_if<(ROWS == 1), void>::type @@ -145,6 +232,24 @@ calculate_each_dif(const Ref_Type &ref, const Fx_Type &Fx, Dif_Type &dif) { template struct DifColumn { + /** + * @brief Calculates the difference for a specific column index and calls the + * next column recursively. + * + * This function calculates the difference for the specified column index + * J_idx and then recursively calls itself to calculate differences for the + * next column index (J_idx - 1). + * + * @tparam Ref_Type Type of the reference matrix. + * @tparam Fx_Type Type of the predicted matrix. + * @tparam Dif_Type Type of the difference container. + * @tparam Np Prediction horizon length. + * @tparam I Row index for the operation. + * @tparam J_idx Current column index for the operation. + * @param ref Reference matrix. + * @param Fx Predicted matrix. + * @param dif Container to store the computed differences. + */ static void calculate(const Ref_Type &ref, const Fx_Type &Fx, Dif_Type &dif) { calculate_each_dif(ref, Fx, dif); @@ -158,6 +263,20 @@ struct DifColumn { template struct DifColumn { + /** + * @brief Calculates the difference for the first column index (0). + * + * This function calculates the difference for the first column index (0) and + * does not call itself recursively. + * + * @tparam Ref_Type Type of the reference matrix. + * @tparam Fx_Type Type of the predicted matrix. + * @tparam Dif_Type Type of the difference container. + * @tparam Np Prediction horizon length. + * @param ref Reference matrix. + * @param Fx Predicted matrix. + * @param dif Container to store the computed differences. + */ static void calculate(const Ref_Type &ref, const Fx_Type &Fx, Dif_Type &dif) { calculate_each_dif(ref, Fx, dif); @@ -168,6 +287,25 @@ struct DifColumn { template struct DifRow { + /** + * @brief Calculates the difference for a specific row index and calls the + * next row recursively. + * + * This function calculates the difference for the specified row index I_idx + * and then recursively calls itself to calculate differences for the next row + * index (I_idx - 1). + * + * @tparam Ref_Type Type of the reference matrix. + * @tparam Fx_Type Type of the predicted matrix. + * @tparam Dif_Type Type of the difference container. + * @tparam Np Prediction horizon length. + * @tparam M Number of rows in the reference matrix. + * @tparam N Number of columns in the reference matrix. + * @tparam I_idx Current row index for the operation. + * @param ref Reference matrix. + * @param Fx Predicted matrix. + * @param dif Container to store the computed differences. + */ static void calculate(const Ref_Type &ref, const Fx_Type &Fx, Dif_Type &dif) { DifColumn::calculate(ref, Fx, dif); @@ -180,12 +318,44 @@ struct DifRow { template struct DifRow { + /** + * @brief Calculates the difference for the first row index (0). + * + * This function calculates the difference for the first row index (0) and + * does not call itself recursively. + * + * @tparam Ref_Type Type of the reference matrix. + * @tparam Fx_Type Type of the predicted matrix. + * @tparam Dif_Type Type of the difference container. + * @tparam Np Prediction horizon length. + * @param ref Reference matrix. + * @param Fx Predicted matrix. + * @param dif Container to store the computed differences. + */ static void calculate(const Ref_Type &ref, const Fx_Type &Fx, Dif_Type &dif) { DifColumn::calculate(ref, Fx, dif); } }; +/** + * @brief Calculates the difference between a reference trajectory and a + * predicted trajectory. + * + * This function computes the difference for each element in the reference + * trajectory and the predicted trajectory, storing the results in the provided + * difference container. It uses the DifRow class to handle the row-wise + * calculations. + * + * @tparam Np Number of prediction steps. + * @tparam Number_Of_Output Number of outputs in the reference trajectory. + * @tparam Ref_Type Type of the reference matrix. + * @tparam Fx_Type Type of the predicted matrix. + * @tparam Dif_Type Type of the difference container. + * @param ref Reference matrix. + * @param Fx Predicted matrix. + * @param dif Container to store the computed differences. + */ template inline void calculate_dif(const Ref_Type &ref, const Fx_Type &Fx, @@ -246,6 +416,25 @@ template class MPC_ReferenceTrajectory { public: /* Function */ + + /** + * @brief Calculates the difference (dif) between the reference trajectory and + * the provided function Fx. + * + * This function computes the difference using the static method + * MPC_ReferenceTrajectoryOperation::calculate_dif, which compares the current + * reference trajectory with the given Fx object and stores the result in a + * Dif_Type object. + * + * @tparam Fx_Type The type of the function or object to compare against the + * reference trajectory. Must have a nested type Value_Type equal to _T. + * @param Fx The function or object to compare with the reference trajectory. + * @return Dif_Type The computed difference between the reference trajectory + * and Fx. + * + * @note A static assertion ensures that Fx_Type::Value_Type matches the + * expected Value_Type (_T). + */ template inline auto calculate_dif(const Fx_Type &Fx) -> Dif_Type { @@ -271,6 +460,18 @@ template class MPC_ReferenceTrajectory { }; /* make MPC Reference Trajectory */ + +/** + * @brief Factory function to create an instance of MPC_ReferenceTrajectory. + * + * This function initializes and returns an instance of MPC_ReferenceTrajectory + * with the provided template parameters. + * + * @tparam Ref_Type Type of the reference matrix. + * @tparam Np Number of prediction steps. + * @return An instance of MPC_ReferenceTrajectory initialized with default + * values. + */ template inline auto make_MPC_ReferenceTrajectory(void) -> MPC_ReferenceTrajectory { From b8a3fbf124c3305c943924884c24a4f1486c8806 Mon Sep 17 00:00:00 2001 From: Claude Date: Sat, 14 Jun 2025 16:55:06 +0900 Subject: [PATCH 4/9] =?UTF-8?q?=E4=BD=9C=E6=88=90=E4=B8=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- python_mpc/linear_mpc.py | 98 ++++++++++++++++++++++++++++++++- python_mpc/linear_mpc_deploy.py | 48 ++++++++++++++++ 2 files changed, 143 insertions(+), 3 deletions(-) diff --git a/python_mpc/linear_mpc.py b/python_mpc/linear_mpc.py index 11e140f..6fe7b02 100644 --- a/python_mpc/linear_mpc.py +++ b/python_mpc/linear_mpc.py @@ -1,3 +1,8 @@ +""" +File: linear_mpc.py + +This module implements Linear Model Predictive Control (MPC) algorithms for discrete-time linear time-invariant (LTI) systems, with and without constraints. It provides classes for unconstrained MPC (LTI_MPC_NoConstraints) and constrained MPC (LTI_MPC), supporting state estimation via a Kalman filter, reference trajectory tracking, and quadratic programming-based constraint handling. +""" import numpy as np from mpc_utility.state_space_utility import * @@ -7,6 +12,12 @@ class LTI_MPC_NoConstraints: + """ + Linear Model Predictive Control (MPC) without constraints. + This class implements a linear MPC controller for discrete-time LTI systems. + It uses a Kalman filter for state estimation and allows for reference trajectory tracking. + """ + def __init__(self, state_space: SymbolicStateSpace, Np: int, Nc: int, Weight_U: np.ndarray, Weight_Y: np.ndarray, Q_kf: np.ndarray = None, R_kf: np.ndarray = None, @@ -67,6 +78,15 @@ def __init__(self, state_space: SymbolicStateSpace, Np: int, Nc: int, def initialize_kalman_filter(self, state_space: SymbolicStateSpace, Q_kf: np.ndarray, R_kf: np.ndarray) -> LinearKalmanFilter: + """ + Initializes the Kalman filter for state estimation. + Args: + state_space (SymbolicStateSpace): The symbolic state space model. + Q_kf (np.ndarray): Process noise covariance matrix. + R_kf (np.ndarray): Measurement noise covariance matrix. + Returns: + LinearKalmanFilter: An instance of the Kalman filter. + """ if Q_kf is None: Q_kf = np.eye(state_space.A.shape[0]) if R_kf is None: @@ -84,10 +104,23 @@ def initialize_kalman_filter(self, state_space: SymbolicStateSpace, return lkf def update_weight(self, Weight: np.ndarray): + """ + Updates the weight matrix for the control input. + Args: + Weight (np.ndarray): The weight matrix for the control input. + Returns: + np.ndarray: A diagonal matrix with the weight values repeated for each control input. + """ return np.diag(np.tile(Weight, (self.Nc, 1)).flatten()) def create_prediction_matrices(self, Weight_Y: np.ndarray) -> MPC_PredictionMatrices: - + """ + Creates the prediction matrices for the MPC controller. + Args: + Weight_Y (np.ndarray): The weight matrix for the output. + Returns: + MPC_PredictionMatrices: An instance containing the prediction matrices. + """ prediction_matrices = MPC_PredictionMatrices( Np=self.Np, Nc=self.Nc, @@ -106,6 +139,13 @@ def create_prediction_matrices(self, Weight_Y: np.ndarray) -> MPC_PredictionMatr return prediction_matrices def create_reference_trajectory(self, reference_trajectory: np.ndarray): + """ + Creates a reference trajectory for the MPC controller. + Args: + reference_trajectory (np.ndarray): The reference trajectory, which can be a single row vector or multiple row vectors. + Returns: + MPC_ReferenceTrajectory: An instance containing the reference trajectory. + """ if self.is_ref_trajectory: if not ((reference_trajectory.shape[1] == self.Np) or (reference_trajectory.shape[1] == 1)): @@ -117,6 +157,14 @@ def create_reference_trajectory(self, reference_trajectory: np.ndarray): return trajectory def update_solver_factor(self, Phi: np.ndarray, Weight_U_Nc: np.ndarray): + """ + Updates the solver factor for the MPC controller. + Args: + Phi (np.ndarray): The prediction matrix Phi. + Weight_U_Nc (np.ndarray): The weight matrix for the control input. + Returns: + None + """ if (Phi.shape[1] != Weight_U_Nc.shape[0]) or (Phi.shape[1] != Weight_U_Nc.shape[1]): raise ValueError("Weight must have compatible dimensions.") @@ -124,6 +172,14 @@ def update_solver_factor(self, Phi: np.ndarray, Weight_U_Nc: np.ndarray): def solve(self, reference_trajectory: MPC_ReferenceTrajectory, X_augmented: np.ndarray): + """ + Solves the MPC optimization problem to compute the control input. + Args: + reference_trajectory (MPC_ReferenceTrajectory): The reference trajectory for the MPC controller. + X_augmented (np.ndarray): The augmented state vector, which includes the state and output. + Returns: + np.ndarray: The computed control input delta_U. + """ # (Phi^T * Phi + Weight)^-1 * Phi^T * (Trajectory - Fx) delta_U = self.solver_factor @ reference_trajectory.calculate_dif( self.prediction_matrices.F_numeric @ X_augmented) @@ -131,12 +187,28 @@ def solve(self, reference_trajectory: MPC_ReferenceTrajectory, return delta_U def calculate_this_U(self, U, delta_U): + """ + Calculates the new control input U based on the previous input and the computed delta_U. + Args: + U (np.ndarray): The previous control input. + delta_U (np.ndarray): The computed change in control input. + Returns: + np.ndarray: The updated control input U. + """ U = U + \ delta_U[:self.AUGMENTED_INPUT_SIZE, :] return U def compensate_X_Y_delay(self, X: np.ndarray, Y: np.ndarray): + """ + Compensates for delays in the state and output vectors. + Args: + X (np.ndarray): The state vector. + Y (np.ndarray): The output vector. + Returns: + tuple: A tuple containing the compensated state vector and output vector. + """ if self.Number_of_Delay > 0: Y_measured = Y @@ -151,7 +223,14 @@ def compensate_X_Y_delay(self, X: np.ndarray, Y: np.ndarray): return X, Y def update(self, reference: np.ndarray, Y: np.ndarray): - + """ + Updates the MPC controller with the latest reference and output measurements. + Args: + reference (np.ndarray): The reference trajectory, which can be a single row vector or multiple row vectors. + Y (np.ndarray): The output measurement vector. + Returns: + np.ndarray: The updated control input U. + """ self.kalman_filter.predict_and_update_with_fixed_G( self.U_latest, Y) X = self.kalman_filter.x_hat @@ -172,6 +251,12 @@ def update(self, reference: np.ndarray, Y: np.ndarray): class LTI_MPC(LTI_MPC_NoConstraints): + """ + Linear Model Predictive Control (MPC) with constraints. + This class extends the LTI_MPC_NoConstraints class to include constraints on the control input and output. + It uses a quadratic programming solver to handle the constraints during the optimization process. + """ + def __init__(self, state_space: SymbolicStateSpace, Np: int, Nc: int, Weight_U: np.ndarray, Weight_Y: np.ndarray, Q_kf: np.ndarray = None, R_kf: np.ndarray = None, @@ -201,7 +286,14 @@ def __init__(self, state_space: SymbolicStateSpace, Np: int, Nc: int, def solve(self, reference_trajectory: MPC_ReferenceTrajectory, X_augmented: np.ndarray): - + """ + Solves the MPC optimization problem with constraints to compute the control input. + Args: + reference_trajectory (MPC_ReferenceTrajectory): The reference trajectory for the MPC controller. + X_augmented (np.ndarray): The augmented state vector, which includes the state and output. + Returns: + np.ndarray: The computed control input delta_U. + """ self.qp_solver.update_constraints( U=self.U_latest, X_augmented=X_augmented, diff --git a/python_mpc/linear_mpc_deploy.py b/python_mpc/linear_mpc_deploy.py index 1e548ec..6102a12 100644 --- a/python_mpc/linear_mpc_deploy.py +++ b/python_mpc/linear_mpc_deploy.py @@ -1,3 +1,11 @@ +""" +File: /c:/work/ModelingCodingAutomationProject/python_mpc_to_cpp/python_mpc/linear_mpc_deploy.py + +This module provides functionality for deploying Linear Model Predictive Control (MPC) objects to C++ code. +It contains utilities to generate C++ header files from Python-based MPC models, including both constrained +and unconstrained LTI MPCs. The generated code includes all necessary matrices, parameters, and type definitions +to instantiate and use the MPC controller in a C++ environment. +""" import os import sys sys.path.append(os.getcwd()) @@ -20,12 +28,36 @@ class LinearMPC_Deploy: + """ + A class for deploying Linear Model Predictive Control (MPC) objects to C++ code. + This class provides static methods to generate C++ header files from Python-based MPC models, + including both constrained and unconstrained LTI MPCs. + The generated code includes all necessary matrices, parameters, and type definitions + to instantiate and use the MPC controller in a C++ environment. + Attributes: + None + Methods: + generate_LTI_MPC_NC_cpp_code(lti_mpc_nc, file_name=None, number_of_delay=0): + Generates C++ code for an LTI MPC without constraints. + generate_LTI_MPC_cpp_code(lti_mpc, file_name=None, number_of_delay=0): + Generates C++ code for an LTI MPC with constraints. + """ + def __init__(self): pass @staticmethod def generate_LTI_MPC_NC_cpp_code( lti_mpc_nc: LTI_MPC_NoConstraints, file_name=None, number_of_delay=0): + """ + Generates C++ code for an LTI MPC without constraints. + Args: + lti_mpc_nc (LTI_MPC_NoConstraints): The LTI MPC without constraints object to deploy. + file_name (str, optional): The name of the file to save the generated C++ code. If None, uses the caller's file name. + number_of_delay (int, optional): The number of delays in the MPC. Defaults to 0. + Returns: + list: A list of file names of the generated C++ code files. + """ deployed_file_names = [] ControlDeploy.restrict_data_type(lti_mpc_nc.kalman_filter.A.dtype.name) @@ -185,6 +217,13 @@ def generate_LTI_MPC_NC_cpp_code( @staticmethod def get_cpp_bool_text(flag: bool) -> str: + """ + Converts a Python boolean to a C++ boolean string representation. + Args: + flag (bool): The boolean value to convert. + Returns: + str: "true" if flag is True, "false" if flag is False. + """ if flag: return "true" else: @@ -193,6 +232,15 @@ def get_cpp_bool_text(flag: bool) -> str: @staticmethod def generate_LTI_MPC_cpp_code( lti_mpc: LTI_MPC, file_name=None, number_of_delay=0): + """ + Generates C++ code for an LTI MPC with constraints. + Args: + lti_mpc (LTI_MPC): The LTI MPC object to deploy. + file_name (str, optional): The name of the file to save the generated C++ code. If None, uses the caller's file name. + number_of_delay (int, optional): The number of delays in the MPC. Defaults to 0. + Returns: + list: A list of file names of the generated C++ code files. + """ deployed_file_names = [] ControlDeploy.restrict_data_type(lti_mpc.kalman_filter.A.dtype.name) From 1fd9b3aeee26e63edd80601a15ab9f2d1630cd71 Mon Sep 17 00:00:00 2001 From: Claude Date: Sat, 14 Jun 2025 16:59:04 +0900 Subject: [PATCH 5/9] =?UTF-8?q?=E4=BD=9C=E6=88=90=E4=B8=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- python_mpc/python_linear_mpc.hpp | 210 ++++++++++++++++++++++++++++++- 1 file changed, 209 insertions(+), 1 deletion(-) diff --git a/python_mpc/python_linear_mpc.hpp b/python_mpc/python_linear_mpc.hpp index 0a294d9..8e56f76 100644 --- a/python_mpc/python_linear_mpc.hpp +++ b/python_mpc/python_linear_mpc.hpp @@ -1,3 +1,15 @@ +/** + * @file python_linear_mpc.hpp + * @brief Linear Model Predictive Control (MPC) implementation for Python/C++ + * integration. + * + * This header provides a set of template classes and utilities for implementing + * Linear Model Predictive Control (MPC) algorithms, with and without + * constraints, in C++. The code is designed to be flexible and extensible, + * supporting various types of Kalman filters, prediction matrices, and solver + * factors, and is intended for use in projects that bridge Python and C++ for + * control system applications. + */ #ifndef __PYTHON_LINEAR_MPC_HPP__ #define __PYTHON_LINEAR_MPC_HPP__ @@ -15,6 +27,20 @@ class SolverFactor_Empty {}; namespace LMPC_Operation { +/** + * @brief Compensates the delay in the state and output vectors for the + * Linear Model Predictive Control (LMPC) operation. + * + * This function adjusts the state and output vectors to account for delays + * in the system, ensuring that the control inputs are correctly aligned with + * the measured outputs. + * + * @tparam Number_Of_Delay The number of delays to compensate for. + * @tparam X_Type Type of the state vector. + * @tparam Y_Type Type of the output vector. + * @tparam Y_Store_Type Type of the delayed output storage. + * @tparam LKF_Type Type of the Kalman filter used in LMPC. + */ template inline typename std::enable_if<(Number_Of_Delay > 0), void>::type @@ -35,6 +61,20 @@ compensate_X_Y_delay(const X_Type &X_in, const Y_Type &Y_in, X_Type &X_out, Y_out = Y + Y_diff; } +/** + * @brief Specialization of the compensate_X_Y_delay function for the case + * where there are no delays. + * + * This specialization simply copies the input state and output vectors to + * the output vectors without any delay compensation. + * + * @tparam Number_Of_Delay The number of delays (should be 0 for this + * specialization). + * @tparam X_Type Type of the state vector. + * @tparam Y_Type Type of the output vector. + * @tparam Y_Store_Type Type of the delayed output storage (not used here). + * @tparam LKF_Type Type of the Kalman filter (not used here). + */ template inline typename std::enable_if<(Number_Of_Delay == 0), void>::type @@ -51,6 +91,15 @@ compensate_X_Y_delay(const X_Type &X_in, const Y_Type &Y_in, X_Type &X_out, template struct Integrate_U { + /** + * @brief Recursively integrates the control input over the horizon. + * + * This function updates the control input U by adding the corresponding + * delta_U_Horizon value for the given index. + * + * @param U The current control input to be updated. + * @param delta_U_Horizon The delta control input for the horizon. + */ static void calculate(U_Type &U, const U_Horizon_Type &delta_U_Horizon) { U.template set(U.template get() + @@ -62,6 +111,15 @@ struct Integrate_U { template struct Integrate_U { + /** + * @brief Base case for the recursive integration of control input. + * + * This function updates the first element of the control input U by adding + * the corresponding delta_U_Horizon value. + * + * @param U The current control input to be updated. + * @param delta_U_Horizon The delta control input for the horizon. + */ static void calculate(U_Type &U, const U_Horizon_Type &delta_U_Horizon) { U.template set<0, 0>(U.template get<0, 0>() + @@ -71,6 +129,21 @@ struct Integrate_U { } // namespace LMPC_Operation +/** + * @brief Linear Model Predictive Control (MPC) class without constraints. + * + * This class implements a basic linear MPC algorithm that does not enforce + * constraints on the control inputs or outputs. It uses a Kalman filter for + * state estimation and prediction matrices for system dynamics. + * + * @tparam LKF_Type_In Type of the Kalman filter used in the MPC. + * @tparam PredictionMatrices_Type_In Type of the prediction matrices used in + * the MPC. + * @tparam ReferenceTrajectory_Type_In Type of the reference trajectory used in + * the MPC. + * @tparam SolverFactor_Type_In Type of the solver factor used in the MPC (can + * be empty). + */ template @@ -210,6 +283,16 @@ class LTI_MPC_NoConstraints { public: /* Function */ + + /** + * @brief Sets the reference trajectory for the MPC. + * + * This function updates the reference trajectory used by the MPC to + * calculate control inputs based on the provided reference vector. + * + * @tparam Ref_Type Type of the reference vector. + * @param ref The reference vector to be set. + */ template inline void set_reference_trajectory(const Ref_Type &ref) { @@ -219,6 +302,19 @@ class LTI_MPC_NoConstraints { this->_reference_trajectory.reference_vector = ref; } + /** + * @brief Updates the control input based on the current state and reference. + * + * This function performs a prediction step using the Kalman filter, + * compensates for delays in the state and output vectors, and calculates the + * new control input based on the reference trajectory and the current state. + * + * @tparam Ref_Type Type of the reference vector. + * @param reference The reference vector to be used for updating control + * input. + * @param Y The measured output vector. + * @return The updated control input vector. + */ template inline auto update(const Ref_Type &reference, const Y_Type &Y) -> U_Type { @@ -249,6 +345,19 @@ class LTI_MPC_NoConstraints { protected: /* Function */ + + /** + * @brief Compensates for delays in the state and output vectors. + * + * This function adjusts the state and output vectors to account for delays + * in the system, ensuring that the control inputs are correctly aligned with + * the measured outputs. + * + * @param X_in The input state vector. + * @param Y_in The input output vector. + * @param X_out The compensated output state vector. + * @param Y_out The compensated output vector. + */ inline void _compensate_X_Y_delay(const X_Type &X_in, const Y_Type &Y_in, X_Type &X_out, Y_Type &Y_out) { @@ -256,6 +365,16 @@ class LTI_MPC_NoConstraints { X_in, Y_in, X_out, Y_out, this->_Y_store, this->_kalman_filter); } + /** + * @brief Solves the MPC optimization problem to calculate the control input. + * + * This function computes the change in control input (delta_U) based on the + * augmented state vector (X_augmented) and the reference trajectory. + * + * @param X_augmented The augmented state vector containing the current state + * and output. + * @return The calculated change in control input (delta_U). + */ virtual inline auto _solve(const X_Augmented_Type &X_augmented) -> U_Horizon_Type { @@ -266,6 +385,16 @@ class LTI_MPC_NoConstraints { return delta_U; } + /** + * @brief Calculates the new control input based on the latest control input + * and the change in control input (delta_U). + * + * This function updates the control input by adding the change in control + * input to the latest control input. + * + * @param delta_U The change in control input to be applied. + * @return The updated control input. + */ inline auto _calculate_this_U(const U_Type &delta_U) -> U_Type { auto U = this->_U_latest + delta_U; @@ -287,6 +416,24 @@ class LTI_MPC_NoConstraints { }; /* make LTI MPC No Constraints */ + +/** + * @brief Factory function to create an instance of LTI_MPC_NoConstraints. + * + * This function initializes the LTI_MPC_NoConstraints class with the provided + * Kalman filter, prediction matrices, reference trajectory, and solver factor. + * + * @tparam LKF_Type Type of the Kalman filter. + * @tparam PredictionMatrices_Type Type of the prediction matrices. + * @tparam ReferenceTrajectory_Type Type of the reference trajectory. + * @tparam SolverFactor_Type Type of the solver factor (optional). + * @param kalman_filter The Kalman filter to be used in the MPC. + * @param prediction_matrices The prediction matrices for the MPC. + * @param reference_trajectory The reference trajectory for the MPC. + * @param solver_factor The solver factor for the MPC (optional). + * @return An instance of LTI_MPC_NoConstraints initialized with the provided + * parameters. + */ template inline auto @@ -311,6 +458,30 @@ using LTI_MPC_NoConstraints_Type = ReferenceTrajectory_Type, SolverFactor_Type>; /* LTI MPC */ + +/** + * @brief Linear Model Predictive Control (MPC) class with constraints. + * + * This class extends the LTI_MPC_NoConstraints class to include constraints on + * the control inputs and outputs, using a quadratic programming solver to + * compute the optimal control inputs while respecting these constraints. + * + * @tparam LKF_Type Type of the Kalman filter used in the MPC. + * @tparam PredictionMatrices_Type Type of the prediction matrices used in the + * MPC. + * @tparam ReferenceTrajectory_Type Type of the reference trajectory used in + * the MPC. + * @tparam Weight_U_Nc_Type Type for the weight matrix for control input + * changes. + * @tparam Delta_U_Min_Type Type for the minimum change in control input. + * @tparam Delta_U_Max_Type Type for the maximum change in control input. + * @tparam U_Min_Type Type for the minimum control input. + * @tparam U_Max_Type Type for the maximum control input. + * @tparam Y_Min_Type Type for the minimum output constraint. + * @tparam Y_Max_Type Type for the maximum output constraint. + * @tparam SolverFactor_Type_In Type of the solver factor used in the MPC (can + * be empty). + */ template ( std::move(other)), @@ -406,6 +578,19 @@ class LTI_MPC : public LTI_MPC_NoConstraints _U_Horizon_Type override { @@ -426,6 +611,29 @@ class LTI_MPC : public LTI_MPC_NoConstraints Date: Sat, 14 Jun 2025 17:01:25 +0900 Subject: [PATCH 6/9] =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sample/lti/servo_motor_constraints.cpp | 11 +++++++++++ sample/lti/servo_motor_constraints.py | 4 +++- 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/sample/lti/servo_motor_constraints.cpp b/sample/lti/servo_motor_constraints.cpp index e6ddea2..f37fd98 100644 --- a/sample/lti/servo_motor_constraints.cpp +++ b/sample/lti/servo_motor_constraints.cpp @@ -1,3 +1,14 @@ +/** + * @file servo_motor_constraints.cpp + * @brief Example of discrete-time state-space simulation and MPC control for a + * servo motor system with constraints. + * + * This file demonstrates how to set up and simulate a discrete-time state-space + * model of a servo motor, and apply a Model Predictive Controller (MPC) with + * constraints. The code initializes the system matrices, constructs the + * state-space system, and runs a closed-loop simulation where the MPC computes + * the control input at each time step to track a reference signal. + */ #include /* CAUTION */ diff --git a/sample/lti/servo_motor_constraints.py b/sample/lti/servo_motor_constraints.py index 85e1778..bc9cb06 100644 --- a/sample/lti/servo_motor_constraints.py +++ b/sample/lti/servo_motor_constraints.py @@ -1,5 +1,7 @@ """ -DC Servo Motor control with MPC +File: servo_motor_constraints.py + +This script demonstrates the setup, simulation, and deployment of a constrained Linear Time-Invariant Model Predictive Controller (LTI-MPC) for a servo motor system. The code constructs a discrete-time state-space model of the plant, defines MPC weights and constraints, generates deployable C++ code for the controller, and simulates the closed-loop response of the system under the designed MPC. The simulation results, including reference tracking, control input, and state trajectories, are visualized using a plotting utility. References: A. Bemporad and E. Mosca, "Fulfilling hard constraints in uncertain linear systems From 9d65d93643325e76bb480d51cbdef87d3fac5096 Mon Sep 17 00:00:00 2001 From: Claude Date: Sat, 14 Jun 2025 17:04:21 +0900 Subject: [PATCH 7/9] =?UTF-8?q?=E4=BD=9C=E6=88=90=E4=B8=AD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sample/lti_no_constraints/servo_motor.cpp | 12 ++++++++++++ sample/lti_no_constraints/servo_motor.py | 6 +++++- sample/lti_no_constraints/state_space_SISO.cpp | 12 ++++++++++++ sample/lti_no_constraints/state_space_SISO.py | 5 +++++ 4 files changed, 34 insertions(+), 1 deletion(-) diff --git a/sample/lti_no_constraints/servo_motor.cpp b/sample/lti_no_constraints/servo_motor.cpp index d6fe792..3271fff 100644 --- a/sample/lti_no_constraints/servo_motor.cpp +++ b/sample/lti_no_constraints/servo_motor.cpp @@ -1,3 +1,15 @@ +/** + * @file servo_motor.cpp + * @brief Example of discrete-time state-space simulation and LTI MPC control + * for a servo motor system. + * + * This file demonstrates how to set up and simulate a discrete-time state-space + * model of a servo motor, and how to apply a Linear Time-Invariant Model + * Predictive Controller (LTI MPC) to control the system. The code initializes + * the state-space matrices (A, B, C, D), constructs the system, and runs a + * closed-loop simulation where the controller computes the control input at + * each time step to track a reference signal. + */ #include /* CAUTION */ diff --git a/sample/lti_no_constraints/servo_motor.py b/sample/lti_no_constraints/servo_motor.py index a33b063..e287777 100644 --- a/sample/lti_no_constraints/servo_motor.py +++ b/sample/lti_no_constraints/servo_motor.py @@ -1,5 +1,9 @@ """ -DC Servo Motor control with MPC +File: servo_motor.py + +This script demonstrates Model Predictive Control (MPC) for a DC servo motor system without constraints. +It models the servo motor as a linear time-invariant (LTI) system, discretizes the plant, sets up an MPC controller, +and simulates the closed-loop response to a pulse reference input. The simulation results are visualized using a plotting utility. References: A. Bemporad and E. Mosca, "Fulfilling hard constraints in uncertain linear systems diff --git a/sample/lti_no_constraints/state_space_SISO.cpp b/sample/lti_no_constraints/state_space_SISO.cpp index eac8904..91b9e04 100644 --- a/sample/lti_no_constraints/state_space_SISO.cpp +++ b/sample/lti_no_constraints/state_space_SISO.cpp @@ -1,3 +1,15 @@ +/** + * @file state_space_SISO.cpp + * @brief Example of discrete-time state-space simulation and SISO LTI MPC + * control in C++. + * + * This file demonstrates how to simulate a discrete-time state-space system and + * control it using a Model Predictive Controller (MPC) for a Single-Input + * Single-Output (SISO) Linear Time-Invariant (LTI) system. The code initializes + * the state-space matrices, constructs the system, and runs a simulation loop + * where the controller computes the control input at each step to track a + * reference signal. + */ #include /* CAUTION */ diff --git a/sample/lti_no_constraints/state_space_SISO.py b/sample/lti_no_constraints/state_space_SISO.py index 1ce997f..952d2fa 100644 --- a/sample/lti_no_constraints/state_space_SISO.py +++ b/sample/lti_no_constraints/state_space_SISO.py @@ -1,3 +1,8 @@ +""" +File: state_space_SISO.py + +This script demonstrates the simulation and deployment of a discrete-time state-space SISO (Single Input Single Output) system using Model Predictive Control (MPC) without constraints. The code defines a plant model, sets up an MPC controller, generates input and reference signals, simulates the closed-loop system with delay, and visualizes the results. It also provides functionality to export the MPC controller as C++ code for deployment. +""" import os import sys sys.path.append(os.getcwd()) From 9849607682c9474c671186ab4333d751bc430c1e Mon Sep 17 00:00:00 2001 From: Claude Date: Sat, 14 Jun 2025 17:05:12 +0900 Subject: [PATCH 8/9] =?UTF-8?q?=E3=83=86=E3=82=B9=E3=83=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .github/workflows/run_SIL_test.yml | 2 +- .github/workflows/run_test_vs.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/run_SIL_test.yml b/.github/workflows/run_SIL_test.yml index e687e3b..cfa37b9 100644 --- a/.github/workflows/run_SIL_test.yml +++ b/.github/workflows/run_SIL_test.yml @@ -2,7 +2,7 @@ name: Run SIL test on: push: - branches: [ develop ] + branches: [ develop, feature/* ] jobs: diff --git a/.github/workflows/run_test_vs.yml b/.github/workflows/run_test_vs.yml index 6739b2f..b98ba21 100644 --- a/.github/workflows/run_test_vs.yml +++ b/.github/workflows/run_test_vs.yml @@ -2,7 +2,7 @@ name: Run test VS on: push: - branches: [ develop ] + branches: [ develop, feature/* ] jobs: test_vs: From 5fee52fa91be37f2f15cb3dc2977e25785e45396 Mon Sep 17 00:00:00 2001 From: Claude Date: Sat, 14 Jun 2025 17:08:01 +0900 Subject: [PATCH 9/9] =?UTF-8?q?=E3=83=86=E3=82=B9=E3=83=88=E5=AE=8C?= =?UTF-8?q?=E4=BA=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .github/workflows/run_SIL_test.yml | 2 +- .github/workflows/run_test_vs.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/run_SIL_test.yml b/.github/workflows/run_SIL_test.yml index cfa37b9..e687e3b 100644 --- a/.github/workflows/run_SIL_test.yml +++ b/.github/workflows/run_SIL_test.yml @@ -2,7 +2,7 @@ name: Run SIL test on: push: - branches: [ develop, feature/* ] + branches: [ develop ] jobs: diff --git a/.github/workflows/run_test_vs.yml b/.github/workflows/run_test_vs.yml index b98ba21..6739b2f 100644 --- a/.github/workflows/run_test_vs.yml +++ b/.github/workflows/run_test_vs.yml @@ -2,7 +2,7 @@ name: Run test VS on: push: - branches: [ develop, feature/* ] + branches: [ develop ] jobs: test_vs: