LTV MPC 制約なし を設計する機能を追加

.github/workflows/run_SIL_test.yml

@@ -44,3 +44,4 @@ jobs:
         /opt/venv_py_MCAP/bin/python3 ./test_sil/linear_mpc/state_space_SISO_trajectory_SIL.py
         /opt/venv_py_MCAP/bin/python3 ./test_sil/linear_mpc/servo_motor_SIL.py
         /opt/venv_py_MCAP/bin/python3 ./test_sil/linear_mpc/servo_motor_constraints_SIL.py
+        /opt/venv_py_MCAP/bin/python3 ./test_sil/servo_motor_ltv_mpc/servo_motor_LTV_SIL.py

README.md

@@ -16,6 +16,8 @@ C++11
   - 時不変
     - 制約なし
     - 制約あり（最適化：アクティブセット法）
+  - 時変
+    - 制約なし
 
 ## 使い方
 

mpc_utility/mpc_state_space_utility.hpp

@@ -17,9 +17,44 @@
 
 #include <functional>
 #include <type_traits>
+#include <utility>
 
 namespace PythonMPC {
 
+template <typename A_Type_In, typename B_Type_In, typename C_Type_In>
+struct EmbeddedIntegratorTypes {
+
+  using T = typename A_Type_In::Value_Type;
+
+  using O_M_T_Type =
+      PythonNumpy::SparseMatrixEmpty_Type<T, A_Type_In::COLS, C_Type_In::COLS>;
+
+  using O_M_Type =
+      PythonNumpy::SparseMatrixEmpty_Type<T, C_Type_In::COLS, A_Type_In::COLS>;
+
+  using CC_Identity_Type = PythonNumpy::DiagMatrix_Type<T, C_Type_In::COLS>;
+
+  using C_A_Type =
+      decltype(std::declval<C_Type_In>() * std::declval<A_Type_In>());
+
+  using C_B_Type =
+      decltype(std::declval<C_Type_In>() * std::declval<B_Type_In>());
+
+  using A_Type = PythonNumpy::ConcatenateBlock_Type<2, 2, A_Type_In, O_M_T_Type,
+                                                    C_A_Type, CC_Identity_Type>;
+
+  using B_Type = PythonNumpy::ConcatenateBlock_Type<2, 1, B_Type_In, C_B_Type>;
+
+  using C_Type =
+      PythonNumpy::ConcatenateBlock_Type<1, 2, O_M_Type, CC_Identity_Type>;
+
+  using D_Type =
+      PythonNumpy::SparseMatrixEmpty_Type<T, C_Type_In::COLS, B_Type_In::ROWS>;
+
+  using StateSpace_Type =
+      PythonControl::DiscreteStateSpace_Type<A_Type, B_Type, C_Type, D_Type>;
+};
+
 /* MPC Prediction Matrices */
 
 /**

python_mpc/linear_mpc_deploy.py

@@ -1,5 +1,5 @@
 """
-File: /c:/work/ModelingCodingAutomationProject/python_mpc_to_cpp/python_mpc/linear_mpc_deploy.py
+File: 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
@@ -18,11 +18,13 @@
 
 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 external_libraries.python_control_to_cpp.python_control.kalman_filter_deploy import KalmanFilterDeploy
+from python_mpc.ltv_matrices_deploy import LTVMatricesDeploy
 
 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, LTI_MPC
+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
 
 TOL = 1e-30
 
@@ -472,8 +474,6 @@ def generate_LTI_MPC_cpp_code(
 
         code_text += f"using SolverFactor_Type = {solver_factor_file_name_no_extension}::type;\n\n"
 
-        code_text += f"using Weight_U_Nc_Type = {Weight_U_Nc_file_name_no_extension}::type;\n\n"
-
         code_text += f"using Delta_U_Min_Type = {delta_U_min_file_name_no_extension}::type;\n\n"
 
         code_text += f"using Delta_U_Max_Type = {delta_U_max_file_name_no_extension}::type;\n\n"
@@ -501,7 +501,7 @@ def generate_LTI_MPC_cpp_code(
 
         code_text += f"using type = LTI_MPC_Type<\n" + \
             "  LKF_Type, PredictionMatrices_Type, ReferenceTrajectory_Type,\n" + \
-            "  Weight_U_Nc_Type, Delta_U_Min_Type, Delta_U_Max_Type,\n" + \
+            "  Delta_U_Min_Type, Delta_U_Max_Type,\n" + \
             "  U_Min_Type, U_Max_Type, Y_Min_Type, Y_Max_Type,\n" + \
             "  SolverFactor_Type>;\n\n"
 
@@ -595,3 +595,282 @@ def generate_LTI_MPC_cpp_code(
         deployed_file_names.append(code_file_name_ext)
 
         return deployed_file_names
+
+    @staticmethod
+    def generate_LTV_MPC_NC_cpp_code(ltv_mpc_nc: LTV_MPC_NoConstraints,
+                                     parameters,
+                                     file_name=None,
+                                     number_of_delay=0):
+        deployed_file_names = []
+
+        ControlDeploy.restrict_data_type(ltv_mpc_nc.kalman_filter.A.dtype.name)
+
+        type_name = NumpyDeploy.check_dtype(ltv_mpc_nc.kalman_filter.A)
+
+        # %% inspect arguments
+        # Get the caller's frame
+        frame = inspect.currentframe().f_back
+        # Get the caller's local variables
+        caller_locals = frame.f_locals
+        # Find the variable name that matches the matrix_in value
+        variable_name = None
+        for name, value in caller_locals.items():
+            if value is ltv_mpc_nc:
+                variable_name = name
+                break
+        # Get the caller's file name
+        if file_name is None:
+            caller_file_full_path = frame.f_code.co_filename
+            caller_file_name = os.path.basename(caller_file_full_path)
+            caller_file_name_without_ext = os.path.splitext(caller_file_name)[
+                0]
+        else:
+            caller_file_name_without_ext = file_name
+
+        number_of_delay = ltv_mpc_nc.Number_of_Delay
+
+        code_file_name = caller_file_name_without_ext + "_" + variable_name
+        code_file_name_ext = code_file_name + ".hpp"
+
+        # %% generate parameter class code
+        parameter_code_file_name = caller_file_name_without_ext + "_parameters.hpp"
+        parameter_code_file_name_no_extension = parameter_code_file_name.split(".")[
+            0]
+
+        parameter_code = LTVMatricesDeploy.generate_parameter_cpp_code(
+            parameters, type_name, parameter_code_file_name_no_extension)
+
+        parameter_code_file_name_ext = ControlDeploy.write_to_file(
+            parameter_code, parameter_code_file_name)
+
+        deployed_file_names.append(parameter_code_file_name_ext)
+
+        # %% generate MPC State Space Updater code
+        mpc_state_space_updater_python_name = \
+            ltv_mpc_nc.state_space_initializer.mpc_state_space_updater_file_name
+        mpc_state_space_updater_name_no_extension = \
+            mpc_state_space_updater_python_name.split(".")[0]
+        mpc_state_space_updater_cpp_name = mpc_state_space_updater_name_no_extension + ".hpp"
+
+        mpc_state_space_updater_code = LTVMatricesDeploy.generate_mpc_state_space_updater_cpp_code(
+            mpc_state_space_updater_python_name, mpc_state_space_updater_name_no_extension)
+
+        mpc_state_space_updater_cpp_name_ext = ControlDeploy.write_to_file(
+            mpc_state_space_updater_code, mpc_state_space_updater_cpp_name)
+
+        deployed_file_names.append(mpc_state_space_updater_cpp_name_ext)
+
+        # %% generate Embedded Integrator Updater code
+        embedded_integrator_updater_file_name = \
+            ltv_mpc_nc.state_space_initializer.embedded_integrator_updater_file_name
+        embedded_integrator_updater_name_no_extension = \
+            embedded_integrator_updater_file_name.split(".")[0]
+        embedded_integrator_updater_cpp_name = embedded_integrator_updater_name_no_extension + ".hpp"
+
+        # Embedded Integrator Updater is the same style as MPC State Space Updater
+        embedded_integrator_updater_code = LTVMatricesDeploy.generate_mpc_state_space_updater_cpp_code(
+            embedded_integrator_updater_file_name, embedded_integrator_updater_name_no_extension)
+
+        embedded_integrator_updater_cpp_name_ext = ControlDeploy.write_to_file(
+            embedded_integrator_updater_code, embedded_integrator_updater_cpp_name)
+
+        deployed_file_names.append(embedded_integrator_updater_cpp_name_ext)
+
+        # %% generate Prediction Matrices Phi F updater code
+        prediction_matrices_updater_file_name = \
+            ltv_mpc_nc.state_space_initializer.prediction_matrices_phi_f_updater_file_name
+        prediction_matrices_updater_name_no_extension = \
+            prediction_matrices_updater_file_name.split(".")[0]
+        prediction_matrices_updater_cpp_name = prediction_matrices_updater_name_no_extension + ".hpp"
+
+        prediction_matrices_updater_code = \
+            LTVMatricesDeploy.generate_prediction_matrices_phi_f_updater_cpp_code(
+                prediction_matrices_updater_file_name, prediction_matrices_updater_name_no_extension)
+
+        prediction_matrices_updater_cpp_name_ext = ControlDeploy.write_to_file(
+            prediction_matrices_updater_code, prediction_matrices_updater_cpp_name)
+
+        deployed_file_names.append(prediction_matrices_updater_cpp_name_ext)
+
+        # %% generate LTV MPC Phi F Updater code
+        LTV_MPC_Phi_F_updater_file_name = \
+            ltv_mpc_nc.state_space_initializer.LTV_MPC_Phi_F_updater_file_name
+        LTV_MPC_Phi_F_updater_name_no_extension = \
+            LTV_MPC_Phi_F_updater_file_name.split(".")[0]
+        LTV_MPC_Phi_F_updater_cpp_name = LTV_MPC_Phi_F_updater_name_no_extension + ".hpp"
+
+        LTV_MPC_Phi_F_updater_code = \
+            LTVMatricesDeploy.generate_ltv_mpc_phi_f_updater_cpp_code(
+                LTV_MPC_Phi_F_updater_file_name, LTV_MPC_Phi_F_updater_name_no_extension,
+                embedded_integrator_updater_cpp_name,
+                prediction_matrices_updater_cpp_name)
+
+        LTV_MPC_Phi_F_updater_cpp_name_ext = ControlDeploy.write_to_file(
+            LTV_MPC_Phi_F_updater_code, LTV_MPC_Phi_F_updater_cpp_name)
+
+        deployed_file_names.append(LTV_MPC_Phi_F_updater_cpp_name_ext)
+
+        # %% create LKF, F, Phi, solver_factor, Weight_U_Nc code
+        exec(f"{variable_name}_lkf = ltv_mpc_nc.kalman_filter")
+        lkf_file_names = eval(
+            f"KalmanFilterDeploy.generate_LKF_cpp_code({variable_name}_lkf, caller_file_name_without_ext, number_of_delay={number_of_delay})")
+
+        deployed_file_names.append(lkf_file_names)
+        lkf_file_name = lkf_file_names[-1]
+
+        lkf_file_name_no_extension = lkf_file_name.split(".")[0]
+
+        exec(f"{variable_name}_F = ltv_mpc_nc.prediction_matrices.F_ndarray")
+        F_file_name = eval(
+            f"NumpyDeploy.generate_matrix_cpp_code({variable_name}_F, caller_file_name_without_ext)")
+
+        deployed_file_names.append(F_file_name)
+        F_file_name_no_extension = F_file_name.split(".")[0]
+
+        exec(
+            f"{variable_name}_Phi = ltv_mpc_nc.prediction_matrices.Phi_ndarray")
+        Phi_file_name = eval(
+            f"NumpyDeploy.generate_matrix_cpp_code({variable_name}_Phi, caller_file_name_without_ext)")
+
+        deployed_file_names.append(Phi_file_name)
+        Phi_file_name_no_extension = Phi_file_name.split(".")[0]
+
+        exec(f"{variable_name}_solver_factor = ltv_mpc_nc.solver_factor")
+        solver_factor_file_name = eval(
+            f"NumpyDeploy.generate_matrix_cpp_code({variable_name}_solver_factor, caller_file_name_without_ext)")
+
+        deployed_file_names.append(solver_factor_file_name)
+        solver_factor_file_name_no_extension = solver_factor_file_name.split(".")[
+            0]
+
+        exec(f"{variable_name}_Weight_U_Nc = ltv_mpc_nc.Weight_U_Nc")
+        Weight_U_Nc_file_name = eval(
+            f"NumpyDeploy.generate_matrix_cpp_code({variable_name}_Weight_U_Nc, caller_file_name_without_ext)")
+
+        deployed_file_names.append(Weight_U_Nc_file_name)
+        Weight_U_Nc_file_name_no_extension = Weight_U_Nc_file_name.split(".")[
+            0]
+
+        # %% main code generation
+        code_text = ""
+
+        file_header_macro_name = "__" + code_file_name.upper() + "_HPP__"
+
+        code_text += "#ifndef " + file_header_macro_name + "\n"
+        code_text += "#define " + file_header_macro_name + "\n\n"
+
+        code_text += f"#include \"{lkf_file_name}\"\n"
+        code_text += f"#include \"{F_file_name}\"\n"
+        code_text += f"#include \"{Phi_file_name}\"\n"
+        code_text += f"#include \"{solver_factor_file_name}\"\n"
+        code_text += f"#include \"{Weight_U_Nc_file_name}\"\n"
+        code_text += f"#include \"{caller_file_name_without_ext}_parameters.hpp\"\n"
+        code_text += f"#include \"{mpc_state_space_updater_cpp_name}\"\n"
+        code_text += f"#include \"{LTV_MPC_Phi_F_updater_cpp_name}\"\n\n"
+
+        code_text += "#include \"python_mpc.hpp\"\n\n"
+
+        namespace_name = code_file_name
+
+        code_text += "namespace " + namespace_name + " {\n\n"
+
+        code_text += "using namespace PythonNumpy;\n"
+        code_text += "using namespace PythonControl;\n"
+        code_text += "using namespace PythonMPC;\n\n"
+
+        code_text += f"constexpr std::size_t NP = {ltv_mpc_nc.Np};\n"
+        code_text += f"constexpr std::size_t NC = {ltv_mpc_nc.Nc};\n\n"
+
+        code_text += f"constexpr std::size_t INPUT_SIZE = {lkf_file_name_no_extension}::INPUT_SIZE;\n"
+        code_text += f"constexpr std::size_t STATE_SIZE = {lkf_file_name_no_extension}::STATE_SIZE;\n"
+        code_text += f"constexpr std::size_t OUTPUT_SIZE = {lkf_file_name_no_extension}::OUTPUT_SIZE;\n\n"
+
+        code_text += f"constexpr std::size_t AUGMENTED_STATE_SIZE = STATE_SIZE + OUTPUT_SIZE;\n\n"
+
+        code_text += f"constexpr std::size_t NUMBER_OF_DELAY = {lkf_file_name_no_extension}::NUMBER_OF_DELAY;\n\n"
+
+        code_text += f"using LKF_Type = {lkf_file_name_no_extension}::type;\n\n"
+
+        code_text += f"using A_Type = typename LKF_Type::DiscreteStateSpace_Type::A_Type;\n\n"
+
+        code_text += f"using B_Type = typename LKF_Type::DiscreteStateSpace_Type::B_Type;\n\n"
+
+        code_text += f"using C_Type = typename LKF_Type::DiscreteStateSpace_Type::C_Type;\n\n"
+
+        code_text += f"using F_Type = {F_file_name_no_extension}::type;\n\n"
+
+        code_text += f"using Phi_Type = {Phi_file_name_no_extension}::type;\n\n"
+
+        code_text += f"using SolverFactor_Type = {solver_factor_file_name_no_extension}::type;\n\n"
+
+        code_text += f"using PredictionMatrices_Type = MPC_PredictionMatrices_Type<\n" + \
+            "  F_Type, Phi_Type, NP, NC, INPUT_SIZE, AUGMENTED_STATE_SIZE, OUTPUT_SIZE>;\n\n"
+
+        ref_row_size_text = "1"
+        if ltv_mpc_nc.is_ref_trajectory:
+            ref_row_size_text = "NP"
+
+        code_text += f"using Ref_Type = DenseMatrix_Type<{type_name}, OUTPUT_SIZE, " + \
+            ref_row_size_text + ">;\n\n"
+
+        code_text += f"using ReferenceTrajectory_Type = MPC_ReferenceTrajectory_Type<\n" + \
+            "  Ref_Type, NP>;\n\n"
+
+        code_text += f"using Parameter_Type = {parameter_code_file_name_no_extension}::Parameter;\n\n"
+
+        code_text += f"using Weight_U_Nc_Type = {Weight_U_Nc_file_name_no_extension}::type;\n\n"
+
+        code_text += f"using EmbeddedIntegratorSateSpace_Type =\n" + \
+            f"  typename EmbeddedIntegratorTypes<A_Type, B_Type, C_Type>::StateSpace_Type;\n\n"
+
+        code_text += f"using type = LTV_MPC_NoConstraints_Type<\n" + \
+            "  LKF_Type, PredictionMatrices_Type, ReferenceTrajectory_Type,\n" + \
+            "  Parameter_Type, SolverFactor_Type>;\n\n"
+
+        code_text += "inline auto make() -> type {\n\n"
+
+        code_text += f"  auto kalman_filter = {lkf_file_name_no_extension}::make();\n\n"
+
+        code_text += f"  auto F = {F_file_name_no_extension}::make();\n\n"
+
+        code_text += f"  auto Phi = {Phi_file_name_no_extension}::make();\n\n"
+
+        code_text += f"  auto solver_factor = {solver_factor_file_name_no_extension}::make();\n\n"
+
+        code_text += f"  PredictionMatrices_Type prediction_matrices(F, Phi);\n\n"
+
+        code_text += f"  ReferenceTrajectory_Type reference_trajectory;\n\n"
+
+        code_text += f"  Weight_U_Nc_Type Weight_U_Nc = {Weight_U_Nc_file_name_no_extension}::make();\n\n"
+
+        code_text += f"  MPC_StateSpace_Updater_Function_Object<\n" + \
+            f"    Parameter_Type, typename LKF_Type::DiscreteStateSpace_Type>\n" + \
+            f"    MPC_StateSpace_Updater_Function =\n" + \
+            f"    mpc_state_space_updater::MPC_StateSpace_Updater::update<\n" + \
+            f"      Parameter_Type, typename LKF_Type::DiscreteStateSpace_Type>;\n\n"
+
+        code_text += f"  LTV_MPC_Phi_F_Updater_Function_Object<\n" + \
+            f"    EmbeddedIntegratorSateSpace_Type, Parameter_Type, Phi_Type, F_Type>\n" + \
+            f"    LTV_MPC_Phi_F_Updater_Function =\n" + \
+            f"    ltv_mpc_phi_f_updater::LTV_MPC_Phi_F_Updater::update<\n" + \
+            f"      EmbeddedIntegratorSateSpace_Type, Parameter_Type, Phi_Type, F_Type>;\n\n"
+
+        code_text += f"  auto ltv_mpc_nc = make_LTV_MPC_NoConstraints(\n" + \
+            "    kalman_filter, prediction_matrices, reference_trajectory, solver_factor,\n" + \
+            "    Weight_U_Nc, MPC_StateSpace_Updater_Function,\n" + \
+            "    LTV_MPC_Phi_F_Updater_Function);\n\n"
+
+        code_text += "  return ltv_mpc_nc;\n\n"
+
+        code_text += "}\n\n"
+
+        code_text += "} // namespace " + namespace_name + "\n\n"
+
+        code_text += "#endif // " + file_header_macro_name + "\n"
+
+        code_file_name_ext = ControlDeploy.write_to_file(
+            code_text, code_file_name_ext)
+
+        deployed_file_names.append(code_file_name_ext)
+
+        return deployed_file_names