From c9efa9b17380347ade7fcf35afbaa260dca0c5cb Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 26 Sep 2025 18:50:21 +0900 Subject: [PATCH 01/11] Remove unnecessary blank line at the beginning of python_adaptive_mpc.hpp --- python_mpc/python_adaptive_mpc.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/python_mpc/python_adaptive_mpc.hpp b/python_mpc/python_adaptive_mpc.hpp index d6a575a..a919ac7 100644 --- a/python_mpc/python_adaptive_mpc.hpp +++ b/python_mpc/python_adaptive_mpc.hpp @@ -1,4 +1,3 @@ - /** * @file python_adaptive_mpc.hpp * @brief Adaptive Model Predictive Control (MPC) implementation for From da2073def446651b5087c2327f8e05aa8b8daf95 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 26 Sep 2025 19:06:28 +0900 Subject: [PATCH 02/11] Integrate MinMaxCodeGenerator into linear_mpc_deploy for active set generation --- python_mpc/common_mpc_deploy.py | 29 +++++++++++++++++++++++++++++ python_mpc/linear_mpc_deploy.py | 13 ++++++------- 2 files changed, 35 insertions(+), 7 deletions(-) diff --git a/python_mpc/common_mpc_deploy.py b/python_mpc/common_mpc_deploy.py index b57f78a..2e5fab0 100644 --- a/python_mpc/common_mpc_deploy.py +++ b/python_mpc/common_mpc_deploy.py @@ -19,3 +19,32 @@ def convert_SparseAvailable_for_deploy(SparseAvailable: sp.Matrix) -> np.ndarray SparseAvailable_ndarray[i, j] = True return SparseAvailable_ndarray + + +class MinMaxCodeGenerator: + def __init__( + self, + min_max_array: np.ndarray + ): + self.values = min_max_array + self.size = self.values.shape[0] + + def generate_active_set( + self, + is_active_function: callable = None, + is_active_array: np.ndarray = None + ): + if is_active_function is None and is_active_array is None: + raise ValueError( + "Either is_active_function or is_active_array must be provided") + + self.active_set = np.zeros((self.size, 1), dtype=bool) + + for i in range(self.size): + if is_active_function is not None and is_active_function(i): + self.active_set[i, 0] = True + + elif is_active_array is not None and is_active_array[i]: + self.active_set[i, 0] = True + + return self.active_set diff --git a/python_mpc/linear_mpc_deploy.py b/python_mpc/linear_mpc_deploy.py index 33f7600..1f79fa1 100644 --- a/python_mpc/linear_mpc_deploy.py +++ b/python_mpc/linear_mpc_deploy.py @@ -22,6 +22,7 @@ from mpc_utility.ltv_matrices_deploy import LTVMatricesDeploy from python_mpc.common_mpc_deploy import convert_SparseAvailable_for_deploy +from python_mpc.common_mpc_deploy import MinMaxCodeGenerator from external_libraries.MCAP_python_mpc.mpc_utility.linear_solver_utility import DU_U_Y_Limits from external_libraries.MCAP_python_mpc.python_mpc.linear_mpc import LTI_MPC_NoConstraints @@ -349,12 +350,10 @@ def generate_LTI_MPC_cpp_code( U_size = lti_mpc.qp_solver.U_size Y_size = lti_mpc.qp_solver.Y_size - delta_U_min_values = lti_mpc.qp_solver.DU_U_Y_Limits.delta_U_min - delta_U_min_active_set = np.zeros((U_size, 1), dtype=bool) - if delta_U_min_values is not None: - for i in range(len(delta_U_min_values)): - if lti_mpc.qp_solver.DU_U_Y_Limits.is_delta_U_min_active(i): - delta_U_min_active_set[i] = True + delta_U_min_code_generator = MinMaxCodeGenerator( + lti_mpc.qp_solver.DU_U_Y_Limits.delta_U_min) + delta_U_min_active_set = delta_U_min_code_generator.generate_active_set( + is_active_function=lti_mpc.qp_solver.DU_U_Y_Limits.is_delta_U_min_active) delta_U_max_values = lti_mpc.qp_solver.DU_U_Y_Limits.delta_U_max delta_U_max_active_set = np.zeros((U_size, 1), dtype=bool) @@ -555,7 +554,7 @@ def generate_LTI_MPC_cpp_code( for i in range(len(delta_U_min)): if delta_U_min_active_set[i]: code_text += f" delta_U_min.template set<{i}, 0>(" - code_text += f"static_cast<{type_name}>({delta_U_min_values[i, 0]})" + code_text += f"static_cast<{type_name}>({delta_U_min_code_generator.values[i, 0]})" code_text += ");\n" code_text += "\n" From 8b3af9607d6d0cba79c584f82bfc9c207e986758 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 26 Sep 2025 19:20:46 +0900 Subject: [PATCH 03/11] Enhance MinMaxCodeGenerator to include min_max_name and refactor limits code creation in LinearMPC_Deploy --- python_mpc/common_mpc_deploy.py | 28 +++++++++++++++++++++++++++- python_mpc/linear_mpc_deploy.py | 27 +++++++++++++-------------- 2 files changed, 40 insertions(+), 15 deletions(-) diff --git a/python_mpc/common_mpc_deploy.py b/python_mpc/common_mpc_deploy.py index 2e5fab0..b49397a 100644 --- a/python_mpc/common_mpc_deploy.py +++ b/python_mpc/common_mpc_deploy.py @@ -4,6 +4,9 @@ import numpy as np import sympy as sp +import copy + +from external_libraries.python_numpy_to_cpp.python_numpy.numpy_deploy import NumpyDeploy def convert_SparseAvailable_for_deploy(SparseAvailable: sp.Matrix) -> np.ndarray: @@ -24,10 +27,12 @@ def convert_SparseAvailable_for_deploy(SparseAvailable: sp.Matrix) -> np.ndarray class MinMaxCodeGenerator: def __init__( self, - min_max_array: np.ndarray + min_max_array: np.ndarray, + min_max_name: str ): self.values = min_max_array self.size = self.values.shape[0] + self.min_max_name = min_max_name def generate_active_set( self, @@ -48,3 +53,24 @@ def generate_active_set( self.active_set[i, 0] = True return self.active_set + + def create_limits_code( + self, + data_type, + variable_name: str, + caller_file_name_without_ext: str + ): + + active_set = np.array( + copy.deepcopy(self.active_set), dtype=data_type).reshape(-1, 1) + exec(f"{variable_name}_" + self.min_max_name + + " = active_set") + + file_name = eval( + f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_" + + self.min_max_name + ", " + + "file_name=caller_file_name_without_ext)") + + file_name_no_extension = file_name.split(".")[0] + + return file_name, file_name_no_extension diff --git a/python_mpc/linear_mpc_deploy.py b/python_mpc/linear_mpc_deploy.py index 1f79fa1..05819c9 100644 --- a/python_mpc/linear_mpc_deploy.py +++ b/python_mpc/linear_mpc_deploy.py @@ -351,7 +351,8 @@ def generate_LTI_MPC_cpp_code( Y_size = lti_mpc.qp_solver.Y_size delta_U_min_code_generator = MinMaxCodeGenerator( - lti_mpc.qp_solver.DU_U_Y_Limits.delta_U_min) + lti_mpc.qp_solver.DU_U_Y_Limits.delta_U_min, + "delta_U_min") delta_U_min_active_set = delta_U_min_code_generator.generate_active_set( is_active_function=lti_mpc.qp_solver.DU_U_Y_Limits.is_delta_U_min_active) @@ -391,17 +392,13 @@ def generate_LTI_MPC_cpp_code( Y_max_active_set[i] = True # create Limits code - delta_U_min = copy.deepcopy(delta_U_min_active_set) - delta_U_min = np.array( - delta_U_min, dtype=data_type).reshape(-1, 1) - exec(f"{variable_name}_delta_U_min = delta_U_min") - delta_U_min_file_name = eval( - f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_delta_U_min, " + - "file_name=caller_file_name_without_ext)") - + delta_U_min_file_name, delta_U_min_file_name_no_extension = \ + delta_U_min_code_generator.create_limits_code( + data_type=data_type, + variable_name=variable_name, + caller_file_name_without_ext=caller_file_name_without_ext + ) deployed_file_names.append(delta_U_min_file_name) - delta_U_min_file_name_no_extension = delta_U_min_file_name .split(".")[ - 0] delta_U_max = copy.deepcopy(delta_U_max_active_set) delta_U_max = np.array( @@ -550,11 +547,13 @@ def generate_LTI_MPC_cpp_code( # limits code_text += f" auto delta_U_min = {delta_U_min_file_name_no_extension}::make();\n\n" - if delta_U_min is not None and np.linalg.norm(delta_U_min_active_set) > TOL: - for i in range(len(delta_U_min)): + if delta_U_min_code_generator.active_set is not None and \ + np.linalg.norm(delta_U_min_active_set) > TOL: + for i in range(len(delta_U_min_code_generator.active_set)): if delta_U_min_active_set[i]: code_text += f" delta_U_min.template set<{i}, 0>(" - code_text += f"static_cast<{type_name}>({delta_U_min_code_generator.values[i, 0]})" + code_text += \ + f"static_cast<{type_name}>({delta_U_min_code_generator.values[i, 0]})" code_text += ");\n" code_text += "\n" From 758a8c57049ab8fa09eb4641f08b89eadbbd6dce Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 26 Sep 2025 19:41:58 +0900 Subject: [PATCH 04/11] Refactor LinearMPC_Deploy to utilize MinMaxCodeGenerator for active set generation and limits code creation --- python_mpc/common_mpc_deploy.py | 35 ++++-- python_mpc/linear_mpc_deploy.py | 206 +++++++++++--------------------- 2 files changed, 100 insertions(+), 141 deletions(-) diff --git a/python_mpc/common_mpc_deploy.py b/python_mpc/common_mpc_deploy.py index b49397a..ef86e2a 100644 --- a/python_mpc/common_mpc_deploy.py +++ b/python_mpc/common_mpc_deploy.py @@ -8,6 +8,8 @@ from external_libraries.python_numpy_to_cpp.python_numpy.numpy_deploy import NumpyDeploy +VALUE_IS_ZERO_TOL = 1e-30 + def convert_SparseAvailable_for_deploy(SparseAvailable: sp.Matrix) -> np.ndarray: @@ -34,6 +36,8 @@ def __init__( self.size = self.values.shape[0] self.min_max_name = min_max_name + self.file_name_no_extension = None + def generate_active_set( self, is_active_function: callable = None, @@ -61,16 +65,31 @@ def create_limits_code( caller_file_name_without_ext: str ): - active_set = np.array( - copy.deepcopy(self.active_set), dtype=data_type).reshape(-1, 1) - exec(f"{variable_name}_" + self.min_max_name + - " = active_set") + active_set = np.array(self.active_set, dtype=data_type).reshape(-1, 1) + exec(f"{variable_name}_{self.min_max_name} = copy.deepcopy(active_set)") file_name = eval( f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_" + - self.min_max_name + ", " + - "file_name=caller_file_name_without_ext)") + self.min_max_name + ", file_name=caller_file_name_without_ext)") + + self.file_name_no_extension = file_name.split(".")[0] - file_name_no_extension = file_name.split(".")[0] + return file_name, self.file_name_no_extension - return file_name, file_name_no_extension + def write_limits_code( + self, + code_text: str, + type_name: str + ): + code_text += f" auto {self.min_max_name} = {self.file_name_no_extension}::make();\n\n" + if self.active_set is not None and \ + np.linalg.norm(self.active_set) > VALUE_IS_ZERO_TOL: + for i in range(len(self.active_set)): + if self.active_set[i]: + code_text += f" {self.min_max_name}.template set<{i}, 0>(" + code_text += \ + f"static_cast<{type_name}>({self.values[i, 0]})" + code_text += ");\n" + code_text += "\n" + + return code_text diff --git a/python_mpc/linear_mpc_deploy.py b/python_mpc/linear_mpc_deploy.py index 05819c9..ea35849 100644 --- a/python_mpc/linear_mpc_deploy.py +++ b/python_mpc/linear_mpc_deploy.py @@ -29,8 +29,6 @@ from external_libraries.MCAP_python_mpc.python_mpc.linear_mpc import LTI_MPC from external_libraries.MCAP_python_mpc.python_mpc.linear_mpc import LTV_MPC_NoConstraints -TOL = 1e-30 - class LinearMPC_Deploy: """ @@ -356,40 +354,35 @@ def generate_LTI_MPC_cpp_code( delta_U_min_active_set = delta_U_min_code_generator.generate_active_set( is_active_function=lti_mpc.qp_solver.DU_U_Y_Limits.is_delta_U_min_active) - delta_U_max_values = lti_mpc.qp_solver.DU_U_Y_Limits.delta_U_max - delta_U_max_active_set = np.zeros((U_size, 1), dtype=bool) - if delta_U_max_values is not None: - for i in range(len(delta_U_max_values)): - if lti_mpc.qp_solver.DU_U_Y_Limits.is_delta_U_max_active(i): - delta_U_max_active_set[i] = True - - U_min_values = lti_mpc.qp_solver.DU_U_Y_Limits.U_min - U_min_active_set = np.zeros((U_size, 1), dtype=bool) - if U_min_values is not None: - for i in range(len(U_min_values)): - if lti_mpc.qp_solver.DU_U_Y_Limits.is_U_min_active(i): - U_min_active_set[i] = True - - U_max_values = lti_mpc.qp_solver.DU_U_Y_Limits.U_max - U_max_active_set = np.zeros((U_size, 1), dtype=bool) - if U_max_values is not None: - for i in range(len(U_max_values)): - if lti_mpc.qp_solver.DU_U_Y_Limits.is_U_max_active(i): - U_max_active_set[i] = True - - Y_min_values = lti_mpc.qp_solver.DU_U_Y_Limits.Y_min - Y_min_active_set = np.zeros((Y_size, 1), dtype=bool) - if Y_min_values is not None: - for i in range(len(Y_min_values)): - if lti_mpc.qp_solver.DU_U_Y_Limits.is_Y_min_active(i): - Y_min_active_set[i] = True - - Y_max_values = lti_mpc.qp_solver.DU_U_Y_Limits.Y_max - Y_max_active_set = np.zeros((Y_size, 1), dtype=bool) - if Y_max_values is not None: - for i in range(len(Y_max_values)): - if lti_mpc.qp_solver.DU_U_Y_Limits.is_Y_max_active(i): - Y_max_active_set[i] = True + delta_U_max_code_generator = MinMaxCodeGenerator( + lti_mpc.qp_solver.DU_U_Y_Limits.delta_U_max, + "delta_U_max") + delta_U_max_active_set = delta_U_max_code_generator.generate_active_set( + is_active_function=lti_mpc.qp_solver.DU_U_Y_Limits.is_delta_U_max_active) + + U_min_code_generator = MinMaxCodeGenerator( + lti_mpc.qp_solver.DU_U_Y_Limits.U_min, + "U_min") + U_min_active_set = U_min_code_generator.generate_active_set( + is_active_function=lti_mpc.qp_solver.DU_U_Y_Limits.is_U_min_active) + + U_max_code_generator = MinMaxCodeGenerator( + lti_mpc.qp_solver.DU_U_Y_Limits.U_max, + "U_max") + U_max_active_set = U_max_code_generator.generate_active_set( + is_active_function=lti_mpc.qp_solver.DU_U_Y_Limits.is_U_max_active) + + Y_min_code_generator = MinMaxCodeGenerator( + lti_mpc.qp_solver.DU_U_Y_Limits.Y_min, + "Y_min") + Y_min_active_set = Y_min_code_generator.generate_active_set( + is_active_function=lti_mpc.qp_solver.DU_U_Y_Limits.is_Y_min_active) + + Y_max_code_generator = MinMaxCodeGenerator( + lti_mpc.qp_solver.DU_U_Y_Limits.Y_max, + "Y_max") + Y_max_active_set = Y_max_code_generator.generate_active_set( + is_active_function=lti_mpc.qp_solver.DU_U_Y_Limits.is_Y_max_active) # create Limits code delta_U_min_file_name, delta_U_min_file_name_no_extension = \ @@ -400,57 +393,45 @@ def generate_LTI_MPC_cpp_code( ) deployed_file_names.append(delta_U_min_file_name) - delta_U_max = copy.deepcopy(delta_U_max_active_set) - delta_U_max = np.array( - delta_U_max, dtype=data_type).reshape(-1, 1) - exec(f"{variable_name}_delta_U_max = delta_U_max") - delta_U_max_file_name = eval( - f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_delta_U_max, " + - "file_name=caller_file_name_without_ext)") - + delta_U_max_file_name, delta_U_max_file_name_no_extension = \ + delta_U_max_code_generator.create_limits_code( + data_type=data_type, + variable_name=variable_name, + caller_file_name_without_ext=caller_file_name_without_ext + ) deployed_file_names.append(delta_U_max_file_name) - delta_U_max_file_name_no_extension = delta_U_max_file_name .split(".")[ - 0] - - U_min = copy.deepcopy(U_min_active_set) - U_min = np.array(U_min, dtype=data_type).reshape(-1, 1) - exec(f"{variable_name}_U_min = U_min") - U_min_file_name = eval( - f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_U_min, " + - "file_name=caller_file_name_without_ext)") + U_min_file_name, U_min_file_name_no_extension = \ + U_min_code_generator.create_limits_code( + data_type=data_type, + variable_name=variable_name, + caller_file_name_without_ext=caller_file_name_without_ext + ) deployed_file_names.append(U_min_file_name) - U_min_file_name_no_extension = U_min_file_name .split(".")[0] - - U_max = copy.deepcopy(U_max_active_set) - U_max = np.array(U_max, dtype=data_type).reshape(-1, 1) - exec(f"{variable_name}_U_max = U_max") - U_max_file_name = eval( - f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_U_max, " + - "file_name=caller_file_name_without_ext)") + U_max_file_name, U_max_file_name_no_extension = \ + U_max_code_generator.create_limits_code( + data_type=data_type, + variable_name=variable_name, + caller_file_name_without_ext=caller_file_name_without_ext + ) deployed_file_names.append(U_max_file_name) - U_max_file_name_no_extension = U_max_file_name .split(".")[0] - - Y_min = copy.deepcopy(Y_min_active_set) - Y_min = np.array(Y_min, dtype=data_type).reshape(-1, 1) - exec(f"{variable_name}_Y_min = Y_min") - Y_min_file_name = eval( - f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_Y_min, " + - "file_name=caller_file_name_without_ext)") + Y_min_file_name, Y_min_file_name_no_extension = \ + Y_min_code_generator.create_limits_code( + data_type=data_type, + variable_name=variable_name, + caller_file_name_without_ext=caller_file_name_without_ext + ) deployed_file_names.append(Y_min_file_name) - Y_min_file_name_no_extension = Y_min_file_name .split(".")[0] - - Y_max = copy.deepcopy(Y_max_active_set) - Y_max = np.array(Y_max, dtype=data_type).reshape(-1, 1) - exec(f"{variable_name}_Y_max = Y_max") - Y_max_file_name = eval( - f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_Y_max, " + - "file_name=caller_file_name_without_ext)") + Y_max_file_name, Y_max_file_name_no_extension = \ + Y_max_code_generator.create_limits_code( + data_type=data_type, + variable_name=variable_name, + caller_file_name_without_ext=caller_file_name_without_ext + ) deployed_file_names.append(Y_max_file_name) - Y_max_file_name_no_extension = Y_max_file_name .split(".")[0] # create cpp code code_text = "" @@ -546,61 +527,20 @@ def generate_LTI_MPC_cpp_code( code_text += f" auto Weight_U_Nc = {Weight_U_Nc_file_name_no_extension}::make();\n\n" # limits - code_text += f" auto delta_U_min = {delta_U_min_file_name_no_extension}::make();\n\n" - if delta_U_min_code_generator.active_set is not None and \ - np.linalg.norm(delta_U_min_active_set) > TOL: - for i in range(len(delta_U_min_code_generator.active_set)): - if delta_U_min_active_set[i]: - code_text += f" delta_U_min.template set<{i}, 0>(" - code_text += \ - f"static_cast<{type_name}>({delta_U_min_code_generator.values[i, 0]})" - code_text += ");\n" - code_text += "\n" - - code_text += f" auto delta_U_max = {delta_U_max_file_name_no_extension}::make();\n\n" - if delta_U_max is not None and np.linalg.norm(delta_U_max_active_set) > TOL: - for i in range(len(delta_U_max)): - if delta_U_max_active_set[i]: - code_text += f" delta_U_max.template set<{i}, 0>(" - code_text += f"static_cast<{type_name}>({delta_U_max_values[i, 0]})" - code_text += ");\n" - code_text += "\n" - - code_text += f" auto U_min = {U_min_file_name_no_extension}::make();\n\n" - if U_min is not None and np.linalg.norm(U_min_active_set) > TOL: - for i in range(len(U_min)): - if U_min_active_set[i]: - code_text += f" U_min.template set<{i}, 0>(" - code_text += f"static_cast<{type_name}>({U_min_values[i, 0]})" - code_text += ");\n" - code_text += "\n" - - code_text += f" auto U_max = {U_max_file_name_no_extension}::make();\n\n" - if U_max is not None and np.linalg.norm(U_max_active_set) > TOL: - for i in range(len(U_max)): - if U_max_active_set[i]: - code_text += f" U_max.template set<{i}, 0>(" - code_text += f"static_cast<{type_name}>({U_max_values[i, 0]})" - code_text += ");\n" - code_text += "\n" - - code_text += f" auto Y_min = {Y_min_file_name_no_extension}::make();\n\n" - if Y_min is not None and np.linalg.norm(Y_min_active_set) > TOL: - for i in range(len(Y_min)): - if Y_min_active_set[i]: - code_text += f" Y_min.template set<{i}, 0>(" - code_text += f"static_cast<{type_name}>({Y_min_values[i, 0]})" - code_text += ");\n" - code_text += "\n" - - code_text += f" auto Y_max = {Y_max_file_name_no_extension}::make();\n\n" - if Y_max is not None and np.linalg.norm(Y_max_active_set) > TOL: - for i in range(len(Y_max)): - if Y_max_active_set[i]: - code_text += f" Y_max.template set<{i}, 0>(" - code_text += f"static_cast<{type_name}>({Y_max_values[i, 0]})" - code_text += ");\n" - code_text += "\n" + code_text = delta_U_min_code_generator.write_limits_code( + code_text, type_name) + code_text = delta_U_max_code_generator.write_limits_code( + code_text, type_name) + + code_text = U_min_code_generator.write_limits_code( + code_text, type_name) + code_text = U_max_code_generator.write_limits_code( + code_text, type_name) + + code_text = Y_min_code_generator.write_limits_code( + code_text, type_name) + code_text = Y_max_code_generator.write_limits_code( + code_text, type_name) # prediction matrices code_text += f" PredictionMatrices_Type prediction_matrices(F, Phi);\n\n" From 3bb1cbadd181190d9a76198030416d715e9b1fe3 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 26 Sep 2025 19:44:59 +0900 Subject: [PATCH 05/11] Refactor LinearMPC_Deploy to streamline active set generation by removing unnecessary variable assignments for limits --- python_mpc/linear_mpc_deploy.py | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/python_mpc/linear_mpc_deploy.py b/python_mpc/linear_mpc_deploy.py index ea35849..81082ff 100644 --- a/python_mpc/linear_mpc_deploy.py +++ b/python_mpc/linear_mpc_deploy.py @@ -24,7 +24,6 @@ from python_mpc.common_mpc_deploy import convert_SparseAvailable_for_deploy from python_mpc.common_mpc_deploy import MinMaxCodeGenerator -from external_libraries.MCAP_python_mpc.mpc_utility.linear_solver_utility import DU_U_Y_Limits from external_libraries.MCAP_python_mpc.python_mpc.linear_mpc import LTI_MPC_NoConstraints from external_libraries.MCAP_python_mpc.python_mpc.linear_mpc import LTI_MPC from external_libraries.MCAP_python_mpc.python_mpc.linear_mpc import LTV_MPC_NoConstraints @@ -345,13 +344,10 @@ def generate_LTI_MPC_cpp_code( 0] # Limits code - U_size = lti_mpc.qp_solver.U_size - Y_size = lti_mpc.qp_solver.Y_size - delta_U_min_code_generator = MinMaxCodeGenerator( lti_mpc.qp_solver.DU_U_Y_Limits.delta_U_min, "delta_U_min") - delta_U_min_active_set = delta_U_min_code_generator.generate_active_set( + delta_U_min_code_generator.generate_active_set( is_active_function=lti_mpc.qp_solver.DU_U_Y_Limits.is_delta_U_min_active) delta_U_max_code_generator = MinMaxCodeGenerator( @@ -363,25 +359,25 @@ def generate_LTI_MPC_cpp_code( U_min_code_generator = MinMaxCodeGenerator( lti_mpc.qp_solver.DU_U_Y_Limits.U_min, "U_min") - U_min_active_set = U_min_code_generator.generate_active_set( + U_min_code_generator.generate_active_set( is_active_function=lti_mpc.qp_solver.DU_U_Y_Limits.is_U_min_active) U_max_code_generator = MinMaxCodeGenerator( lti_mpc.qp_solver.DU_U_Y_Limits.U_max, "U_max") - U_max_active_set = U_max_code_generator.generate_active_set( + U_max_code_generator.generate_active_set( is_active_function=lti_mpc.qp_solver.DU_U_Y_Limits.is_U_max_active) Y_min_code_generator = MinMaxCodeGenerator( lti_mpc.qp_solver.DU_U_Y_Limits.Y_min, "Y_min") - Y_min_active_set = Y_min_code_generator.generate_active_set( + Y_min_code_generator.generate_active_set( is_active_function=lti_mpc.qp_solver.DU_U_Y_Limits.is_Y_min_active) Y_max_code_generator = MinMaxCodeGenerator( lti_mpc.qp_solver.DU_U_Y_Limits.Y_max, "Y_max") - Y_max_active_set = Y_max_code_generator.generate_active_set( + Y_max_code_generator.generate_active_set( is_active_function=lti_mpc.qp_solver.DU_U_Y_Limits.is_Y_max_active) # create Limits code From 3b4ef608b73814b8d394653af0aa5333b4ebfd39 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 26 Sep 2025 19:51:03 +0900 Subject: [PATCH 06/11] Refactor limits code generation in LinearMPC_Deploy to utilize MinMaxCodeGenerator for improved clarity and maintainability --- python_mpc/linear_mpc_deploy.py | 234 ++++++++++++-------------------- 1 file changed, 85 insertions(+), 149 deletions(-) diff --git a/python_mpc/linear_mpc_deploy.py b/python_mpc/linear_mpc_deploy.py index 81082ff..80797de 100644 --- a/python_mpc/linear_mpc_deploy.py +++ b/python_mpc/linear_mpc_deploy.py @@ -1064,115 +1064,90 @@ def generate_LTV_MPC_cpp_code(ltv_mpc: LTV_MPC_NoConstraints, 0] # %% create limits code - U_size = ltv_mpc.qp_solver.U_size - Y_size = ltv_mpc.qp_solver.Y_size - - delta_U_min_values = ltv_mpc.qp_solver.DU_U_Y_Limits.delta_U_min - delta_U_min_active_set = np.zeros((U_size, 1), dtype=bool) - if delta_U_min_values is not None: - for i in range(len(delta_U_min_values)): - if ltv_mpc.qp_solver.DU_U_Y_Limits.is_delta_U_min_active(i): - delta_U_min_active_set[i] = True - - delta_U_max_values = ltv_mpc.qp_solver.DU_U_Y_Limits.delta_U_max - delta_U_max_active_set = np.zeros((U_size, 1), dtype=bool) - if delta_U_max_values is not None: - for i in range(len(delta_U_max_values)): - if ltv_mpc.qp_solver.DU_U_Y_Limits.is_delta_U_max_active(i): - delta_U_max_active_set[i] = True - - U_min_values = ltv_mpc.qp_solver.DU_U_Y_Limits.U_min - U_min_active_set = np.zeros((U_size, 1), dtype=bool) - if U_min_values is not None: - for i in range(len(U_min_values)): - if ltv_mpc.qp_solver.DU_U_Y_Limits.is_U_min_active(i): - U_min_active_set[i] = True - - U_max_values = ltv_mpc.qp_solver.DU_U_Y_Limits.U_max - U_max_active_set = np.zeros((U_size, 1), dtype=bool) - if U_max_values is not None: - for i in range(len(U_max_values)): - if ltv_mpc.qp_solver.DU_U_Y_Limits.is_U_max_active(i): - U_max_active_set[i] = True - - Y_min_values = ltv_mpc.qp_solver.DU_U_Y_Limits.Y_min - Y_min_active_set = np.zeros((Y_size, 1), dtype=bool) - if Y_min_values is not None: - for i in range(len(Y_min_values)): - if ltv_mpc.qp_solver.DU_U_Y_Limits.is_Y_min_active(i): - Y_min_active_set[i] = True - - Y_max_values = ltv_mpc.qp_solver.DU_U_Y_Limits.Y_max - Y_max_active_set = np.zeros((Y_size, 1), dtype=bool) - if Y_max_values is not None: - for i in range(len(Y_max_values)): - if ltv_mpc.qp_solver.DU_U_Y_Limits.is_Y_max_active(i): - Y_max_active_set[i] = True + delta_U_min_code_generator = MinMaxCodeGenerator( + ltv_mpc.qp_solver.DU_U_Y_Limits.delta_U_min, + "delta_U_min") + delta_U_min_code_generator.generate_active_set( + is_active_function=ltv_mpc.qp_solver.DU_U_Y_Limits.is_delta_U_min_active) - # Limits code - delta_U_min = copy.deepcopy(delta_U_min_active_set) - delta_U_min = np.array( - delta_U_min, dtype=data_type).reshape(-1, 1) - exec(f"{variable_name}_delta_U_min = delta_U_min") - delta_U_min_file_name = eval( - f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_delta_U_min, " + - "file_name=caller_file_name_without_ext)") + delta_U_max_code_generator = MinMaxCodeGenerator( + ltv_mpc.qp_solver.DU_U_Y_Limits.delta_U_max, + "delta_U_max") + delta_U_max_code_generator.generate_active_set( + is_active_function=ltv_mpc.qp_solver.DU_U_Y_Limits.is_delta_U_max_active) - deployed_file_names.append(delta_U_min_file_name) - delta_U_min_file_name_no_extension = delta_U_min_file_name .split(".")[ - 0] + U_min_code_generator = MinMaxCodeGenerator( + ltv_mpc.qp_solver.DU_U_Y_Limits.U_min, + "U_min") + U_min_code_generator.generate_active_set( + is_active_function=ltv_mpc.qp_solver.DU_U_Y_Limits.is_U_min_active) - delta_U_max = copy.deepcopy(delta_U_max_active_set) - delta_U_max = np.array( - delta_U_max, dtype=data_type).reshape(-1, 1) - exec(f"{variable_name}_delta_U_max = delta_U_max") - delta_U_max_file_name = eval( - f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_delta_U_max, " + - "file_name=caller_file_name_without_ext)") + U_max_code_generator = MinMaxCodeGenerator( + ltv_mpc.qp_solver.DU_U_Y_Limits.U_max, + "U_max") + U_max_code_generator.generate_active_set( + is_active_function=ltv_mpc.qp_solver.DU_U_Y_Limits.is_U_max_active) - deployed_file_names.append(delta_U_max_file_name) - delta_U_max_file_name_no_extension = delta_U_max_file_name .split(".")[ - 0] + Y_min_code_generator = MinMaxCodeGenerator( + ltv_mpc.qp_solver.DU_U_Y_Limits.Y_min, + "Y_min") + Y_min_code_generator.generate_active_set( + is_active_function=ltv_mpc.qp_solver.DU_U_Y_Limits.is_Y_min_active) - U_min = copy.deepcopy(U_min_active_set) - U_min = np.array(U_min, dtype=data_type).reshape(-1, 1) - exec(f"{variable_name}_U_min = U_min") - U_min_file_name = eval( - f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_U_min, " + - "file_name=caller_file_name_without_ext)") + Y_max_code_generator = MinMaxCodeGenerator( + ltv_mpc.qp_solver.DU_U_Y_Limits.Y_max, + "Y_max") + Y_max_code_generator.generate_active_set( + is_active_function=ltv_mpc.qp_solver.DU_U_Y_Limits.is_Y_max_active) - deployed_file_names.append(U_min_file_name) - U_min_file_name_no_extension = U_min_file_name .split(".")[0] + # Limits code + delta_U_min_file_name, delta_U_min_file_name_no_extension = \ + delta_U_min_code_generator.create_limits_code( + data_type=data_type, + variable_name=variable_name, + caller_file_name_without_ext=caller_file_name_without_ext + ) + deployed_file_names.append(delta_U_min_file_name) - U_max = copy.deepcopy(U_max_active_set) - U_max = np.array(U_max, dtype=data_type).reshape(-1, 1) - exec(f"{variable_name}_U_max = U_max") - U_max_file_name = eval( - f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_U_max, " + - "file_name=caller_file_name_without_ext)") + delta_U_max_file_name, delta_U_max_file_name_no_extension = \ + delta_U_max_code_generator.create_limits_code( + data_type=data_type, + variable_name=variable_name, + caller_file_name_without_ext=caller_file_name_without_ext + ) + deployed_file_names.append(delta_U_max_file_name) - deployed_file_names.append(U_max_file_name) - U_max_file_name_no_extension = U_max_file_name .split(".")[0] + U_min_file_name, U_min_file_name_no_extension = \ + U_min_code_generator.create_limits_code( + data_type=data_type, + variable_name=variable_name, + caller_file_name_without_ext=caller_file_name_without_ext + ) + deployed_file_names.append(U_min_file_name) - Y_min = copy.deepcopy(Y_min_active_set) - Y_min = np.array(Y_min, dtype=data_type).reshape(-1, 1) - exec(f"{variable_name}_Y_min = Y_min") - Y_min_file_name = eval( - f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_Y_min, " + - "file_name=caller_file_name_without_ext)") + U_max_file_name, U_max_file_name_no_extension = \ + U_max_code_generator.create_limits_code( + data_type=data_type, + variable_name=variable_name, + caller_file_name_without_ext=caller_file_name_without_ext + ) + deployed_file_names.append(U_max_file_name) + Y_min_file_name, Y_min_file_name_no_extension = \ + Y_min_code_generator.create_limits_code( + data_type=data_type, + variable_name=variable_name, + caller_file_name_without_ext=caller_file_name_without_ext + ) deployed_file_names.append(Y_min_file_name) - Y_min_file_name_no_extension = Y_min_file_name .split(".")[0] - - Y_max = copy.deepcopy(Y_max_active_set) - Y_max = np.array(Y_max, dtype=data_type).reshape(-1, 1) - exec(f"{variable_name}_Y_max = Y_max") - Y_max_file_name = eval( - f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_Y_max, " + - "file_name=caller_file_name_without_ext)") + Y_max_file_name, Y_max_file_name_no_extension = \ + Y_max_code_generator.create_limits_code( + data_type=data_type, + variable_name=variable_name, + caller_file_name_without_ext=caller_file_name_without_ext + ) deployed_file_names.append(Y_max_file_name) - Y_max_file_name_no_extension = Y_max_file_name .split(".")[0] # %% main code generation code_text = "" @@ -1285,59 +1260,20 @@ def generate_LTV_MPC_cpp_code(ltv_mpc: LTV_MPC_NoConstraints, code_text += f" auto Weight_U_Nc = {Weight_U_Nc_file_name_no_extension}::make();\n\n" # limits - code_text += f" auto delta_U_min = {delta_U_min_file_name_no_extension}::make();\n\n" - if delta_U_min is not None and np.linalg.norm(delta_U_min_active_set) > TOL: - for i in range(len(delta_U_min)): - if delta_U_min_active_set[i]: - code_text += f" delta_U_min.template set<{i}, 0>(" - code_text += f"static_cast<{type_name}>({delta_U_min_values[i, 0]})" - code_text += ");\n" - code_text += "\n" - - code_text += f" auto delta_U_max = {delta_U_max_file_name_no_extension}::make();\n\n" - if delta_U_max is not None and np.linalg.norm(delta_U_max_active_set) > TOL: - for i in range(len(delta_U_max)): - if delta_U_max_active_set[i]: - code_text += f" delta_U_max.template set<{i}, 0>(" - code_text += f"static_cast<{type_name}>({delta_U_max_values[i, 0]})" - code_text += ");\n" - code_text += "\n" - - code_text += f" auto U_min = {U_min_file_name_no_extension}::make();\n\n" - if U_min is not None and np.linalg.norm(U_min_active_set) > TOL: - for i in range(len(U_min)): - if U_min_active_set[i]: - code_text += f" U_min.template set<{i}, 0>(" - code_text += f"static_cast<{type_name}>({U_min_values[i, 0]})" - code_text += ");\n" - code_text += "\n" - - code_text += f" auto U_max = {U_max_file_name_no_extension}::make();\n\n" - if U_max is not None and np.linalg.norm(U_max_active_set) > TOL: - for i in range(len(U_max)): - if U_max_active_set[i]: - code_text += f" U_max.template set<{i}, 0>(" - code_text += f"static_cast<{type_name}>({U_max_values[i, 0]})" - code_text += ");\n" - code_text += "\n" - - code_text += f" auto Y_min = {Y_min_file_name_no_extension}::make();\n\n" - if Y_min is not None and np.linalg.norm(Y_min_active_set) > TOL: - for i in range(len(Y_min)): - if Y_min_active_set[i]: - code_text += f" Y_min.template set<{i}, 0>(" - code_text += f"static_cast<{type_name}>({Y_min_values[i, 0]})" - code_text += ");\n" - code_text += "\n" - - code_text += f" auto Y_max = {Y_max_file_name_no_extension}::make();\n\n" - if Y_max is not None and np.linalg.norm(Y_max_active_set) > TOL: - for i in range(len(Y_max)): - if Y_max_active_set[i]: - code_text += f" Y_max.template set<{i}, 0>(" - code_text += f"static_cast<{type_name}>({Y_max_values[i, 0]})" - code_text += ");\n" - code_text += "\n" + code_text = delta_U_min_code_generator.write_limits_code( + code_text, type_name) + code_text = delta_U_max_code_generator.write_limits_code( + code_text, type_name) + + code_text = U_min_code_generator.write_limits_code( + code_text, type_name) + code_text = U_max_code_generator.write_limits_code( + code_text, type_name) + + code_text = Y_min_code_generator.write_limits_code( + code_text, type_name) + code_text = Y_max_code_generator.write_limits_code( + code_text, type_name) # prediction matrices code_text += f" PredictionMatrices_Type prediction_matrices(F, Phi);\n\n" From ad4e42bd8b2a588608667568cd5457272a4d7866 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 26 Sep 2025 20:12:17 +0900 Subject: [PATCH 07/11] Refactor MinMaxCodeGenerator to handle None values and improve active set initialization --- python_mpc/adaptive_mpc_deploy.py | 238 ++++++++++++------------------ python_mpc/common_mpc_deploy.py | 28 ++-- 2 files changed, 110 insertions(+), 156 deletions(-) diff --git a/python_mpc/adaptive_mpc_deploy.py b/python_mpc/adaptive_mpc_deploy.py index 38c4e69..2eae027 100644 --- a/python_mpc/adaptive_mpc_deploy.py +++ b/python_mpc/adaptive_mpc_deploy.py @@ -23,6 +23,7 @@ from mpc_utility.adaptive_matrices_deploy import AdaptiveMatricesDeploy from python_mpc.common_mpc_deploy import convert_SparseAvailable_for_deploy +from python_mpc.common_mpc_deploy import MinMaxCodeGenerator from external_libraries.MCAP_python_mpc.python_mpc.adaptive_mpc import AdaptiveMPC_NoConstraints from external_libraries.MCAP_python_mpc.python_mpc.adaptive_mpc import AdaptiveMPC @@ -583,112 +584,96 @@ def generate_Adaptive_MPC_cpp_code( U_size = ada_mpc.qp_solver.U_size Y_size = ada_mpc.qp_solver.Y_size - delta_U_min_values = ada_mpc.qp_solver.DU_U_Y_Limits.delta_U_min - delta_U_min_active_set = np.zeros((U_size, 1), dtype=bool) - if delta_U_min_values is not None: - for i in range(len(delta_U_min_values)): - if ada_mpc.qp_solver.DU_U_Y_Limits.is_delta_U_min_active(i): - delta_U_min_active_set[i] = True - - delta_U_max_values = ada_mpc.qp_solver.DU_U_Y_Limits.delta_U_max - delta_U_max_active_set = np.zeros((U_size, 1), dtype=bool) - if delta_U_max_values is not None: - for i in range(len(delta_U_max_values)): - if ada_mpc.qp_solver.DU_U_Y_Limits.is_delta_U_max_active(i): - delta_U_max_active_set[i] = True - - U_min_values = ada_mpc.qp_solver.DU_U_Y_Limits.U_min - U_min_active_set = np.zeros((U_size, 1), dtype=bool) - if U_min_values is not None: - for i in range(len(U_min_values)): - if ada_mpc.qp_solver.DU_U_Y_Limits.is_U_min_active(i): - U_min_active_set[i] = True - - U_max_values = ada_mpc.qp_solver.DU_U_Y_Limits.U_max - U_max_active_set = np.zeros((U_size, 1), dtype=bool) - if U_max_values is not None: - for i in range(len(U_max_values)): - if ada_mpc.qp_solver.DU_U_Y_Limits.is_U_max_active(i): - U_max_active_set[i] = True - - Y_min_values = ada_mpc.qp_solver.DU_U_Y_Limits.Y_min - Y_min_active_set = np.zeros((Y_size, 1), dtype=bool) - if Y_min_values is not None: - for i in range(len(Y_min_values)): - if ada_mpc.qp_solver.DU_U_Y_Limits.is_Y_min_active(i): - Y_min_active_set[i] = True - - Y_max_values = ada_mpc.qp_solver.DU_U_Y_Limits.Y_max - Y_max_active_set = np.zeros((Y_size, 1), dtype=bool) - if Y_max_values is not None: - for i in range(len(Y_max_values)): - if ada_mpc.qp_solver.DU_U_Y_Limits.is_Y_max_active(i): - Y_max_active_set[i] = True + delta_U_min_code_generator = MinMaxCodeGenerator( + min_max_array=ada_mpc.qp_solver.DU_U_Y_Limits.delta_U_min, + min_max_name="delta_U_min", + size=U_size) + delta_U_min_code_generator.generate_active_set( + is_active_function=ada_mpc.qp_solver.DU_U_Y_Limits.is_delta_U_min_active) + + delta_U_max_code_generator = MinMaxCodeGenerator( + min_max_array=ada_mpc.qp_solver.DU_U_Y_Limits.delta_U_max, + min_max_name="delta_U_max", + size=U_size) + delta_U_max_code_generator.generate_active_set( + is_active_function=ada_mpc.qp_solver.DU_U_Y_Limits.is_delta_U_max_active) + + U_min_code_generator = MinMaxCodeGenerator( + min_max_array=ada_mpc.qp_solver.DU_U_Y_Limits.U_min, + min_max_name="U_min", + size=U_size) + U_min_code_generator.generate_active_set( + is_active_function=ada_mpc.qp_solver.DU_U_Y_Limits.is_U_min_active) + + U_max_code_generator = MinMaxCodeGenerator( + min_max_array=ada_mpc.qp_solver.DU_U_Y_Limits.U_max, + min_max_name="U_max", + size=U_size) + U_max_code_generator.generate_active_set( + is_active_function=ada_mpc.qp_solver.DU_U_Y_Limits.is_U_max_active) + + Y_min_code_generator = MinMaxCodeGenerator( + min_max_array=ada_mpc.qp_solver.DU_U_Y_Limits.Y_min, + min_max_name="Y_min", + size=Y_size) + Y_min_code_generator.generate_active_set( + is_active_function=ada_mpc.qp_solver.DU_U_Y_Limits.is_Y_min_active) + + Y_max_code_generator = MinMaxCodeGenerator( + min_max_array=ada_mpc.qp_solver.DU_U_Y_Limits.Y_max, + min_max_name="Y_max", + size=Y_size) + Y_max_code_generator.generate_active_set( + is_active_function=ada_mpc.qp_solver.DU_U_Y_Limits.is_Y_max_active) # Limits code - delta_U_min = copy.deepcopy(delta_U_min_active_set) - delta_U_min = np.array( - delta_U_min, dtype=data_type).reshape(-1, 1) - exec(f"{variable_name}_delta_U_min = delta_U_min") - delta_U_min_file_name = eval( - f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_delta_U_min, " + - "file_name=caller_file_name_without_ext)") - + delta_U_min_file_name, delta_U_min_file_name_no_extension = \ + delta_U_min_code_generator.create_limits_code( + data_type=data_type, + variable_name=variable_name, + caller_file_name_without_ext=caller_file_name_without_ext + ) deployed_file_names.append(delta_U_min_file_name) - delta_U_min_file_name_no_extension = delta_U_min_file_name .split(".")[ - 0] - - delta_U_max = copy.deepcopy(delta_U_max_active_set) - delta_U_max = np.array( - delta_U_max, dtype=data_type).reshape(-1, 1) - exec(f"{variable_name}_delta_U_max = delta_U_max") - delta_U_max_file_name = eval( - f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_delta_U_max, " + - "file_name=caller_file_name_without_ext)") + delta_U_max_file_name, delta_U_max_file_name_no_extension = \ + delta_U_max_code_generator.create_limits_code( + data_type=data_type, + variable_name=variable_name, + caller_file_name_without_ext=caller_file_name_without_ext + ) deployed_file_names.append(delta_U_max_file_name) - delta_U_max_file_name_no_extension = delta_U_max_file_name .split(".")[ - 0] - - U_min = copy.deepcopy(U_min_active_set) - U_min = np.array(U_min, dtype=data_type).reshape(-1, 1) - exec(f"{variable_name}_U_min = U_min") - U_min_file_name = eval( - f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_U_min, " + - "file_name=caller_file_name_without_ext)") + U_min_file_name, U_min_file_name_no_extension = \ + U_min_code_generator.create_limits_code( + data_type=data_type, + variable_name=variable_name, + caller_file_name_without_ext=caller_file_name_without_ext + ) deployed_file_names.append(U_min_file_name) - U_min_file_name_no_extension = U_min_file_name .split(".")[0] - - U_max = copy.deepcopy(U_max_active_set) - U_max = np.array(U_max, dtype=data_type).reshape(-1, 1) - exec(f"{variable_name}_U_max = U_max") - U_max_file_name = eval( - f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_U_max, " + - "file_name=caller_file_name_without_ext)") + U_max_file_name, U_max_file_name_no_extension = \ + U_max_code_generator.create_limits_code( + data_type=data_type, + variable_name=variable_name, + caller_file_name_without_ext=caller_file_name_without_ext + ) deployed_file_names.append(U_max_file_name) - U_max_file_name_no_extension = U_max_file_name .split(".")[0] - - Y_min = copy.deepcopy(Y_min_active_set) - Y_min = np.array(Y_min, dtype=data_type).reshape(-1, 1) - exec(f"{variable_name}_Y_min = Y_min") - Y_min_file_name = eval( - f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_Y_min, " + - "file_name=caller_file_name_without_ext)") + Y_min_file_name, Y_min_file_name_no_extension = \ + Y_min_code_generator.create_limits_code( + data_type=data_type, + variable_name=variable_name, + caller_file_name_without_ext=caller_file_name_without_ext + ) deployed_file_names.append(Y_min_file_name) - Y_min_file_name_no_extension = Y_min_file_name .split(".")[0] - - Y_max = copy.deepcopy(Y_max_active_set) - Y_max = np.array(Y_max, dtype=data_type).reshape(-1, 1) - exec(f"{variable_name}_Y_max = Y_max") - Y_max_file_name = eval( - f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_Y_max, " + - "file_name=caller_file_name_without_ext)") + Y_max_file_name, Y_max_file_name_no_extension = \ + Y_max_code_generator.create_limits_code( + data_type=data_type, + variable_name=variable_name, + caller_file_name_without_ext=caller_file_name_without_ext + ) deployed_file_names.append(Y_max_file_name) - Y_max_file_name_no_extension = Y_max_file_name .split(".")[0] # %% main code generation code_text = "" @@ -808,59 +793,20 @@ def generate_Adaptive_MPC_cpp_code( code_text += f" auto solver_factor = {solver_factor_file_name_no_extension}::make();\n\n" # limits - code_text += f" auto delta_U_min = {delta_U_min_file_name_no_extension}::make();\n\n" - if delta_U_min is not None and np.linalg.norm(delta_U_min_active_set) > TOL: - for i in range(len(delta_U_min)): - if delta_U_min_active_set[i]: - code_text += f" delta_U_min.template set<{i}, 0>(" - code_text += f"static_cast<{type_name}>({delta_U_min_values[i, 0]})" - code_text += ");\n" - code_text += "\n" - - code_text += f" auto delta_U_max = {delta_U_max_file_name_no_extension}::make();\n\n" - if delta_U_max is not None and np.linalg.norm(delta_U_max_active_set) > TOL: - for i in range(len(delta_U_max)): - if delta_U_max_active_set[i]: - code_text += f" delta_U_max.template set<{i}, 0>(" - code_text += f"static_cast<{type_name}>({delta_U_max_values[i, 0]})" - code_text += ");\n" - code_text += "\n" - - code_text += f" auto U_min = {U_min_file_name_no_extension}::make();\n\n" - if U_min is not None and np.linalg.norm(U_min_active_set) > TOL: - for i in range(len(U_min)): - if U_min_active_set[i]: - code_text += f" U_min.template set<{i}, 0>(" - code_text += f"static_cast<{type_name}>({U_min_values[i, 0]})" - code_text += ");\n" - code_text += "\n" - - code_text += f" auto U_max = {U_max_file_name_no_extension}::make();\n\n" - if U_max is not None and np.linalg.norm(U_max_active_set) > TOL: - for i in range(len(U_max)): - if U_max_active_set[i]: - code_text += f" U_max.template set<{i}, 0>(" - code_text += f"static_cast<{type_name}>({U_max_values[i, 0]})" - code_text += ");\n" - code_text += "\n" - - code_text += f" auto Y_min = {Y_min_file_name_no_extension}::make();\n\n" - if Y_min is not None and np.linalg.norm(Y_min_active_set) > TOL: - for i in range(len(Y_min)): - if Y_min_active_set[i]: - code_text += f" Y_min.template set<{i}, 0>(" - code_text += f"static_cast<{type_name}>({Y_min_values[i, 0]})" - code_text += ");\n" - code_text += "\n" - - code_text += f" auto Y_max = {Y_max_file_name_no_extension}::make();\n\n" - if Y_max is not None and np.linalg.norm(Y_max_active_set) > TOL: - for i in range(len(Y_max)): - if Y_max_active_set[i]: - code_text += f" Y_max.template set<{i}, 0>(" - code_text += f"static_cast<{type_name}>({Y_max_values[i, 0]})" - code_text += ");\n" - code_text += "\n" + code_text = delta_U_min_code_generator.write_limits_code( + code_text, type_name) + code_text = delta_U_max_code_generator.write_limits_code( + code_text, type_name) + + code_text = U_min_code_generator.write_limits_code( + code_text, type_name) + code_text = U_max_code_generator.write_limits_code( + code_text, type_name) + + code_text = Y_min_code_generator.write_limits_code( + code_text, type_name) + code_text = Y_max_code_generator.write_limits_code( + code_text, type_name) # prediction matrices code_text += f" PredictionMatrices_Type prediction_matrices(F, Phi);\n\n" diff --git a/python_mpc/common_mpc_deploy.py b/python_mpc/common_mpc_deploy.py index ef86e2a..d4998bb 100644 --- a/python_mpc/common_mpc_deploy.py +++ b/python_mpc/common_mpc_deploy.py @@ -30,12 +30,19 @@ class MinMaxCodeGenerator: def __init__( self, min_max_array: np.ndarray, - min_max_name: str + min_max_name: str, + size: int ): - self.values = min_max_array - self.size = self.values.shape[0] - self.min_max_name = min_max_name + if min_max_array is not None: + self.values = min_max_array + self.size = self.values.shape[0] + self.active_set = None + else: + self.size = size + self.values = np.ones((self.size, 1)) + self.active_set = np.zeros((self.size, 1), dtype=bool) + self.min_max_name = min_max_name self.file_name_no_extension = None def generate_active_set( @@ -47,14 +54,15 @@ def generate_active_set( raise ValueError( "Either is_active_function or is_active_array must be provided") - self.active_set = np.zeros((self.size, 1), dtype=bool) + if self.active_set is None: + self.active_set = np.zeros((self.size, 1), dtype=bool) - for i in range(self.size): - if is_active_function is not None and is_active_function(i): - self.active_set[i, 0] = True + for i in range(self.size): + if is_active_function is not None and is_active_function(i): + self.active_set[i, 0] = True - elif is_active_array is not None and is_active_array[i]: - self.active_set[i, 0] = True + elif is_active_array is not None and is_active_array[i]: + self.active_set[i, 0] = True return self.active_set From 05f079391548ce2d61a6753954210413b98286c5 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 26 Sep 2025 20:13:01 +0900 Subject: [PATCH 08/11] =?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 | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/run_SIL_test.yml b/.github/workflows/run_SIL_test.yml index 52283b6..adce048 100644 --- a/.github/workflows/run_SIL_test.yml +++ b/.github/workflows/run_SIL_test.yml @@ -4,6 +4,7 @@ on: push: branches: - develop + - feature/* jobs: test_SIL: From 638f43287c90c51b50d8a0b43c004329a11a963b Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 26 Sep 2025 20:17:49 +0900 Subject: [PATCH 09/11] Refactor workflow to trigger on feature branches and clean up imports in adaptive and linear MPC deploy modules --- .github/workflows/run_SIL_test.yml | 1 - python_mpc/adaptive_mpc_deploy.py | 3 - python_mpc/common_mpc_deploy.py | 95 +++++++++++++++++++++++++++++- python_mpc/linear_mpc_deploy.py | 1 - 4 files changed, 93 insertions(+), 7 deletions(-) diff --git a/.github/workflows/run_SIL_test.yml b/.github/workflows/run_SIL_test.yml index adce048..52283b6 100644 --- a/.github/workflows/run_SIL_test.yml +++ b/.github/workflows/run_SIL_test.yml @@ -4,7 +4,6 @@ on: push: branches: - develop - - feature/* jobs: test_SIL: diff --git a/python_mpc/adaptive_mpc_deploy.py b/python_mpc/adaptive_mpc_deploy.py index 2eae027..77ca709 100644 --- a/python_mpc/adaptive_mpc_deploy.py +++ b/python_mpc/adaptive_mpc_deploy.py @@ -15,7 +15,6 @@ import inspect import numpy as np -import copy from external_libraries.python_numpy_to_cpp.python_numpy.numpy_deploy import NumpyDeploy from external_libraries.MCAP_python_control.python_control.control_deploy import ControlDeploy @@ -28,8 +27,6 @@ from external_libraries.MCAP_python_mpc.python_mpc.adaptive_mpc import AdaptiveMPC_NoConstraints from external_libraries.MCAP_python_mpc.python_mpc.adaptive_mpc import AdaptiveMPC -TOL = 1e-30 - class AdaptiveMPC_Deploy: """ diff --git a/python_mpc/common_mpc_deploy.py b/python_mpc/common_mpc_deploy.py index d4998bb..b7aa07f 100644 --- a/python_mpc/common_mpc_deploy.py +++ b/python_mpc/common_mpc_deploy.py @@ -12,7 +12,21 @@ def convert_SparseAvailable_for_deploy(SparseAvailable: sp.Matrix) -> np.ndarray: - + """ + Converts a SymPy sparse availability matrix to a NumPy boolean ndarray for deployment. + + Parameters + ---------- + SparseAvailable : sympy.Matrix + A SymPy matrix representing sparse availability, + where nonzero entries indicate availability. + + Returns + ------- + numpy.ndarray + A boolean NumPy array of the same shape as `SparseAvailable`, + where True indicates available (nonzero) entries. + """ if not isinstance(SparseAvailable, sp.MatrixBase): raise TypeError("SparseAvailable must be a sympy Matrix") @@ -27,6 +41,26 @@ def convert_SparseAvailable_for_deploy(SparseAvailable: sp.Matrix) -> np.ndarray class MinMaxCodeGenerator: + """ + A class to generate code for min/max limits and their active sets, + typically used in Model Predictive Control (MPC) deployments. + Attributes: + values (np.ndarray): Array of min/max values. + size (int): Number of elements in the min/max array. + active_set (np.ndarray or None): Boolean array indicating active limits. + min_max_name (str): Name identifier for the min/max variable. + file_name_no_extension (str or None): File name (without extension) for generated code. + Methods: + __init__(min_max_array, min_max_name, size): + Initializes the generator with min/max values and configuration. + generate_active_set(is_active_function=None, is_active_array=None): + Generates the active set array using a function or a boolean array. + create_limits_code(data_type, variable_name, caller_file_name_without_ext): + Generates C++ code for the limits and returns the file name and its base name. + write_limits_code(code_text, type_name): + Appends code for setting active limits to the provided code text. + """ + def __init__( self, min_max_array: np.ndarray, @@ -50,6 +84,27 @@ def generate_active_set( is_active_function: callable = None, is_active_array: np.ndarray = None ): + """ + Generates and returns the active set for the current object. + + The active set is a boolean numpy array of shape (self.size, 1) + indicating which elements are active. + The active set can be determined either by a provided function or by + a provided boolean array. + + Args: + is_active_function (callable, optional): A function that takes an index + (int) and returns True if the element is active, False otherwise. + is_active_array (np.ndarray, optional): A boolean numpy array of length + self.size indicating active elements. + + Raises: + ValueError: If neither is_active_function nor is_active_array is provided. + + Returns: + np.ndarray: A boolean numpy array of shape (self.size, 1) + representing the active set. + """ if is_active_function is None and is_active_array is None: raise ValueError( "Either is_active_function or is_active_array must be provided") @@ -72,7 +127,29 @@ def create_limits_code( variable_name: str, caller_file_name_without_ext: str ): - + """ + Generates C++ code for the active set limits and returns + the generated file name and its name without extension. + + Args: + data_type: The desired NumPy data type for the active set array. + variable_name (str): The base name for the variable to store the active set. + caller_file_name_without_ext (str): The file name + (without extension) to use for the generated C++ code. + + Returns: + tuple: + - file_name (str): The name of the generated C++ code file. + - file_name_no_extension (str): The file name without its extension. + + Side Effects: + - Sets self.file_name_no_extension to the file name without extension. + + Notes: + - Uses `exec` and `eval` to dynamically create variables + and call code generation functions. + - Relies on `self.active_set` and `self.min_max_name` attributes. + """ active_set = np.array(self.active_set, dtype=data_type).reshape(-1, 1) exec(f"{variable_name}_{self.min_max_name} = copy.deepcopy(active_set)") @@ -89,6 +166,20 @@ def write_limits_code( code_text: str, type_name: str ): + """ + Appends C++ code for setting limit values to the provided code text. + + This method generates code that creates an instance of a min-max limits object, + and, if an active set is defined and non-zero, sets specific limit values using + template-based setter calls for each active index. + + Args: + code_text (str): The initial code text to append to. + type_name (str): The C++ type name to use for static casting limit values. + + Returns: + str: The updated code text with generated limit-setting code appended. + """ code_text += f" auto {self.min_max_name} = {self.file_name_no_extension}::make();\n\n" if self.active_set is not None and \ np.linalg.norm(self.active_set) > VALUE_IS_ZERO_TOL: diff --git a/python_mpc/linear_mpc_deploy.py b/python_mpc/linear_mpc_deploy.py index 80797de..9564c50 100644 --- a/python_mpc/linear_mpc_deploy.py +++ b/python_mpc/linear_mpc_deploy.py @@ -14,7 +14,6 @@ import inspect import numpy as np -import copy from external_libraries.python_numpy_to_cpp.python_numpy.numpy_deploy import NumpyDeploy from external_libraries.MCAP_python_control.python_control.control_deploy import ControlDeploy From f981303dd3e9765836a2afdac24cf3b4751c3841 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 26 Sep 2025 20:21:56 +0900 Subject: [PATCH 10/11] =?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 | 1 + python_mpc/adaptive_mpc_deploy.py | 1 - python_mpc/linear_mpc_deploy.py | 81 ++++++++++++++++++++---------- 3 files changed, 56 insertions(+), 27 deletions(-) diff --git a/.github/workflows/run_SIL_test.yml b/.github/workflows/run_SIL_test.yml index 52283b6..adce048 100644 --- a/.github/workflows/run_SIL_test.yml +++ b/.github/workflows/run_SIL_test.yml @@ -4,6 +4,7 @@ on: push: branches: - develop + - feature/* jobs: test_SIL: diff --git a/python_mpc/adaptive_mpc_deploy.py b/python_mpc/adaptive_mpc_deploy.py index 77ca709..70443df 100644 --- a/python_mpc/adaptive_mpc_deploy.py +++ b/python_mpc/adaptive_mpc_deploy.py @@ -14,7 +14,6 @@ os.getcwd(), "external_libraries", "python_control_to_cpp")) import inspect -import numpy as np from external_libraries.python_numpy_to_cpp.python_numpy.numpy_deploy import NumpyDeploy from external_libraries.MCAP_python_control.python_control.control_deploy import ControlDeploy diff --git a/python_mpc/linear_mpc_deploy.py b/python_mpc/linear_mpc_deploy.py index 9564c50..af38497 100644 --- a/python_mpc/linear_mpc_deploy.py +++ b/python_mpc/linear_mpc_deploy.py @@ -13,7 +13,6 @@ os.getcwd(), "external_libraries", "python_control_to_cpp")) import inspect -import numpy as np from external_libraries.python_numpy_to_cpp.python_numpy.numpy_deploy import NumpyDeploy from external_libraries.MCAP_python_control.python_control.control_deploy import ControlDeploy @@ -343,39 +342,54 @@ def generate_LTI_MPC_cpp_code( 0] # Limits code + U_size = lti_mpc.qp_solver.U_size + Y_size = lti_mpc.qp_solver.Y_size + delta_U_min_code_generator = MinMaxCodeGenerator( - lti_mpc.qp_solver.DU_U_Y_Limits.delta_U_min, - "delta_U_min") + min_max_array=lti_mpc.qp_solver.DU_U_Y_Limits.delta_U_min, + min_max_name="delta_U_min", + size=U_size + ) delta_U_min_code_generator.generate_active_set( is_active_function=lti_mpc.qp_solver.DU_U_Y_Limits.is_delta_U_min_active) delta_U_max_code_generator = MinMaxCodeGenerator( - lti_mpc.qp_solver.DU_U_Y_Limits.delta_U_max, - "delta_U_max") - delta_U_max_active_set = delta_U_max_code_generator.generate_active_set( + min_max_array=lti_mpc.qp_solver.DU_U_Y_Limits.delta_U_max, + min_max_name="delta_U_max", + size=U_size + ) + delta_U_max_code_generator.generate_active_set( is_active_function=lti_mpc.qp_solver.DU_U_Y_Limits.is_delta_U_max_active) U_min_code_generator = MinMaxCodeGenerator( - lti_mpc.qp_solver.DU_U_Y_Limits.U_min, - "U_min") + min_max_array=lti_mpc.qp_solver.DU_U_Y_Limits.U_min, + min_max_name="U_min", + size=U_size + ) U_min_code_generator.generate_active_set( is_active_function=lti_mpc.qp_solver.DU_U_Y_Limits.is_U_min_active) U_max_code_generator = MinMaxCodeGenerator( - lti_mpc.qp_solver.DU_U_Y_Limits.U_max, - "U_max") + min_max_array=lti_mpc.qp_solver.DU_U_Y_Limits.U_max, + min_max_name="U_max", + size=U_size + ) U_max_code_generator.generate_active_set( is_active_function=lti_mpc.qp_solver.DU_U_Y_Limits.is_U_max_active) Y_min_code_generator = MinMaxCodeGenerator( - lti_mpc.qp_solver.DU_U_Y_Limits.Y_min, - "Y_min") + min_max_array=lti_mpc.qp_solver.DU_U_Y_Limits.Y_min, + min_max_name="Y_min", + size=Y_size + ) Y_min_code_generator.generate_active_set( is_active_function=lti_mpc.qp_solver.DU_U_Y_Limits.is_Y_min_active) Y_max_code_generator = MinMaxCodeGenerator( - lti_mpc.qp_solver.DU_U_Y_Limits.Y_max, - "Y_max") + min_max_array=lti_mpc.qp_solver.DU_U_Y_Limits.Y_max, + min_max_name="Y_max", + size=Y_size + ) Y_max_code_generator.generate_active_set( is_active_function=lti_mpc.qp_solver.DU_U_Y_Limits.is_Y_max_active) @@ -1063,39 +1077,54 @@ def generate_LTV_MPC_cpp_code(ltv_mpc: LTV_MPC_NoConstraints, 0] # %% create limits code + U_size = ltv_mpc.qp_solver.U_size + Y_size = ltv_mpc.qp_solver.Y_size + delta_U_min_code_generator = MinMaxCodeGenerator( - ltv_mpc.qp_solver.DU_U_Y_Limits.delta_U_min, - "delta_U_min") + min_max_array=ltv_mpc.qp_solver.DU_U_Y_Limits.delta_U_min, + min_max_name="delta_U_min", + size=U_size + ) delta_U_min_code_generator.generate_active_set( is_active_function=ltv_mpc.qp_solver.DU_U_Y_Limits.is_delta_U_min_active) delta_U_max_code_generator = MinMaxCodeGenerator( - ltv_mpc.qp_solver.DU_U_Y_Limits.delta_U_max, - "delta_U_max") + min_max_array=ltv_mpc.qp_solver.DU_U_Y_Limits.delta_U_max, + min_max_name="delta_U_max", + size=U_size + ) delta_U_max_code_generator.generate_active_set( is_active_function=ltv_mpc.qp_solver.DU_U_Y_Limits.is_delta_U_max_active) U_min_code_generator = MinMaxCodeGenerator( - ltv_mpc.qp_solver.DU_U_Y_Limits.U_min, - "U_min") + min_max_array=ltv_mpc.qp_solver.DU_U_Y_Limits.U_min, + min_max_name="U_min", + size=U_size + ) U_min_code_generator.generate_active_set( is_active_function=ltv_mpc.qp_solver.DU_U_Y_Limits.is_U_min_active) U_max_code_generator = MinMaxCodeGenerator( - ltv_mpc.qp_solver.DU_U_Y_Limits.U_max, - "U_max") + min_max_array=ltv_mpc.qp_solver.DU_U_Y_Limits.U_max, + min_max_name="U_max", + size=U_size + ) U_max_code_generator.generate_active_set( is_active_function=ltv_mpc.qp_solver.DU_U_Y_Limits.is_U_max_active) Y_min_code_generator = MinMaxCodeGenerator( - ltv_mpc.qp_solver.DU_U_Y_Limits.Y_min, - "Y_min") + min_max_array=ltv_mpc.qp_solver.DU_U_Y_Limits.Y_min, + min_max_name="Y_min", + size=Y_size + ) Y_min_code_generator.generate_active_set( is_active_function=ltv_mpc.qp_solver.DU_U_Y_Limits.is_Y_min_active) Y_max_code_generator = MinMaxCodeGenerator( - ltv_mpc.qp_solver.DU_U_Y_Limits.Y_max, - "Y_max") + min_max_array=ltv_mpc.qp_solver.DU_U_Y_Limits.Y_max, + min_max_name="Y_max", + size=Y_size + ) Y_max_code_generator.generate_active_set( is_active_function=ltv_mpc.qp_solver.DU_U_Y_Limits.is_Y_max_active) From 21794ae1da3e4816997f4eaea83c45031d06cd19 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 26 Sep 2025 20:31:08 +0900 Subject: [PATCH 11/11] =?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 | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/run_SIL_test.yml b/.github/workflows/run_SIL_test.yml index adce048..52283b6 100644 --- a/.github/workflows/run_SIL_test.yml +++ b/.github/workflows/run_SIL_test.yml @@ -4,7 +4,6 @@ on: push: branches: - develop - - feature/* jobs: test_SIL: