適応MPCバグ修正、行列コード生成機能を改善

.github/workflows/run_SIL_test.yml

@@ -40,9 +40,10 @@ jobs:
 
     - name: Run SIL tests
       run: |
-        /opt/venv_py_MCAP/bin/python3 ./test_sil/linear_mpc/state_space_SISO_SIL.py
-        /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/lti_mpc/state_space_SISO_SIL.py
+        /opt/venv_py_MCAP/bin/python3 ./test_sil/lti_mpc/state_space_SISO_trajectory_SIL.py
+        /opt/venv_py_MCAP/bin/python3 ./test_sil/lti_mpc/servo_motor_SIL.py
+        /opt/venv_py_MCAP/bin/python3 ./test_sil/lti_mpc/servo_motor_constraints_SIL.py
         /opt/venv_py_MCAP/bin/python3 ./test_sil/servo_motor_ltv_mpc/servo_motor_LTV_SIL.py
         /opt/venv_py_MCAP/bin/python3 ./test_sil/servo_motor_ltv_mpc_constraints/servo_motor_LTV_constraints_SIL.py
+        /opt/venv_py_MCAP/bin/python3 ./test_sil/adaptive_mpc_two_wheel_vehicle/two_wheel_vehicle_model_SIL.py

.vscode/launch.json

@@ -4,7 +4,7 @@
   "version": "0.2.0",
   "configurations": [
     {
-      "name": "Debug Active File",
+      "name": "Debug Active C++ File",
       "type": "cppdbg",
       "request": "launch",
       "program": "${workspaceFolder}/${fileBasenameNoExtension}",
@@ -21,7 +21,7 @@
           "ignoreFailures": true
         }
       ],
-      "preLaunchTask": "Build Active File"
+      "preLaunchTask": "Build Active C++ File"
     }
   ]
 }

.vscode/tasks.json

@@ -4,11 +4,12 @@
   "version": "2.0.0",
   "tasks": [
     {
-      "label": "Build Active File",
+      "label": "Build Active C++ File",
       "type": "shell",
       "command": "/usr/bin/g++",
       "args": [
         "-g",
+        "-O0",
         "-std=c++11",
         "-I${workspaceFolder}",
         "-I${workspaceFolder}/mpc_utility",
@@ -29,7 +30,7 @@
         "isDefault": true
       },
       "problemMatcher": ["$gcc"],
-      "detail": "Build the active C++ file"
+      "detail": "Build Active C++ File"
     }
   ]
 }

mpc_utility/mpc_matrices_deploy.py

@@ -200,12 +200,16 @@ def visit_FunctionDef(self, node):
             unused_args = [
                 arg for arg in update_args if arg not in sympy_func_args]
 
+            self.indent += "    "
             for arg in unused_args:
                 self.cpp_code += f"{self.indent}static_cast<void>({arg});\n"
+            self.indent = self.indent[:-4]
             self.cpp_code += "\n"
 
         if node.name == "sympy_function":
-            self.cpp_code += f"{self.indent}    {ret_type} result;\n\n"
+            self.indent += "    "
+            self.cpp_code += f"{self.indent}{ret_type} result;\n\n"
+            self.indent = self.indent[:-4]
 
         # function body
         self.indent += "    "
@@ -247,11 +251,11 @@ def visit_Return(self, node):
             return_code = np_array_extractor.convert_to_cpp()
             self.SparseAvailable = np_array_extractor.SparseAvailable
             return_code = return_code.replace(
-                "\n", "\n" + self.indent + "    ")
-            self.cpp_code += self.indent + "    " + return_code + "\n"
-            self.cpp_code += self.indent + "    return result;\n"
+                "\n", "\n" + self.indent)
+            self.cpp_code += self.indent + return_code + "\n"
+            self.cpp_code += self.indent + "return result;\n"
         else:
-            self.cpp_code += self.indent + "    return " + return_code + ";\n"
+            self.cpp_code += self.indent + "return " + return_code + ";\n"
 
     def visit_Assign(self, node):
         """

python_mpc/adaptive_mpc_deploy.py

@@ -20,7 +20,9 @@
 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 mpc_utility.adaptive_matrices_deploy import AdaptiveMatricesDeploy
+from python_mpc.common_mpc_deploy import convert_SparseAvailable_for_deploy
 
 from external_libraries.MCAP_python_mpc.python_mpc.adaptive_mpc import AdaptiveMPC_NoConstraints
 
@@ -114,8 +116,8 @@ def generate_Adaptive_MPC_NC_cpp_code(
         exec(
             f"{B_matrix_name} = B_symbolic_SparseAvailable_list[0]")
         B_file_name = eval(
-            f"NumpyDeploy.generate_matrix_cpp_code({B_matrix_name}," +
-            " caller_file_name_without_ext)")
+            f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={B_matrix_name}, " +
+            "file_name=caller_file_name_without_ext)")
 
         deployed_file_names.append(B_file_name)
         B_file_name_no_extension = B_file_name.split(".")[0]
@@ -193,32 +195,48 @@ def generate_Adaptive_MPC_NC_cpp_code(
             raise ValueError(
                 "No parameter file found in EKF deployment files.")
 
+        # create F
+        F_SparseAvailable = convert_SparseAvailable_for_deploy(
+            ada_mpc_nc.prediction_matrices.F_SparseAvailable)
         exec(f"{variable_name}_F = ada_mpc_nc.prediction_matrices.F_ndarray")
         F_file_name = eval(
-            f"NumpyDeploy.generate_matrix_cpp_code({variable_name}_F, caller_file_name_without_ext)")
+            f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_F, " +
+            "SparseAvailable=F_SparseAvailable, " +
+            "file_name=caller_file_name_without_ext)")
 
         deployed_file_names.append(F_file_name)
         F_file_name_no_extension = F_file_name.split(".")[0]
 
+        # create Phi
+        Phi_SparseAvailable = convert_SparseAvailable_for_deploy(
+            ada_mpc_nc.prediction_matrices.Phi_SparseAvailable)
         exec(
             f"{variable_name}_Phi = ada_mpc_nc.prediction_matrices.Phi_ndarray")
         Phi_file_name = eval(
-            f"NumpyDeploy.generate_matrix_cpp_code({variable_name}_Phi, caller_file_name_without_ext)")
+            f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_Phi, " +
+            "SparseAvailable=Phi_SparseAvailable, " +
+            "file_name=caller_file_name_without_ext)")
 
         deployed_file_names.append(Phi_file_name)
         Phi_file_name_no_extension = Phi_file_name.split(".")[0]
 
+        # create solver_factor
+        solver_factor_SparseAvailable = convert_SparseAvailable_for_deploy(
+            ada_mpc_nc.solver_factor_SparseAvailable)
         exec(f"{variable_name}_solver_factor = ada_mpc_nc.solver_factor")
         solver_factor_file_name = eval(
-            f"NumpyDeploy.generate_matrix_cpp_code({variable_name}_solver_factor, caller_file_name_without_ext)")
+            f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_solver_factor, " +
+            "SparseAvailable=solver_factor_SparseAvailable, " +
+            "file_name=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 = ada_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)")
+            f"NumpyDeploy.generate_matrix_cpp_code(matrix_in={variable_name}_Weight_U_Nc, " +
+            "file_name=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(".")[

python_mpc/common_mpc_deploy.py

@@ -0,0 +1,21 @@
+import os
+import sys
+sys.path.append(os.getcwd())
+
+import numpy as np
+import sympy as sp
+
+
+def convert_SparseAvailable_for_deploy(SparseAvailable: sp.Matrix) -> np.ndarray:
+
+    if not isinstance(SparseAvailable, sp.MatrixBase):
+        raise TypeError("SparseAvailable must be a sympy Matrix")
+
+    SparseAvailable_ndarray = np.zeros(SparseAvailable.shape, dtype=bool)
+
+    for i in range(SparseAvailable.shape[0]):
+        for j in range(SparseAvailable.shape[1]):
+            if int(SparseAvailable[i, j]) != 0:
+                SparseAvailable_ndarray[i, j] = True
+
+    return SparseAvailable_ndarray