要素積を修正

optimization_utility/python_optimization_sqp_matrix_operation.hpp

@@ -195,50 +195,6 @@ inline auto get_row(const Matrix_In_Type &in_matrix,
   return out;
 }
 
-/**
- * @brief Performs element-wise multiplication of two matrices.
- *
- * This function takes two matrices, A and B, of the same dimensions and returns
- * a new matrix where each element is the product of the corresponding elements
- * in A and B.
- *
- * @tparam Matrix_A_Type Type of the first input matrix. Must define Value_Type,
- * COLS, and ROWS.
- * @tparam Matrix_B_Type Type of the second input matrix. Must define COLS and
- * ROWS.
- * @param A The first input matrix.
- * @param B The second input matrix.
- * @return PythonNumpy::DenseMatrix_Type<typename Matrix_A_Type::Value_Type,
- * Matrix_A_Type::COLS, Matrix_A_Type::ROWS> A matrix containing the
- * element-wise products of A and B.
- *
- * @note The function enforces at compile time that both matrices have the same
- * dimensions.
- */
-template <typename Matrix_A_Type, typename Matrix_B_Type>
-inline auto element_wise_multiply(const Matrix_A_Type &A,
-                                  const Matrix_B_Type &B)
-    -> PythonNumpy::DenseMatrix_Type<typename Matrix_A_Type::Value_Type,
-                                     Matrix_A_Type::COLS, Matrix_A_Type::ROWS> {
-
-  static_assert(Matrix_A_Type::COLS == Matrix_B_Type::COLS,
-                "Matrix_A_Type::COLS != Matrix_B_Type::COLS");
-  static_assert(Matrix_A_Type::ROWS == Matrix_B_Type::ROWS,
-                "Matrix_A_Type::ROWS != Matrix_B_Type::ROWS");
-
-  PythonNumpy::DenseMatrix_Type<typename Matrix_A_Type::Value_Type,
-                                Matrix_A_Type::COLS, Matrix_A_Type::ROWS>
-      out;
-
-  for (std::size_t i = 0; i < Matrix_A_Type::COLS; i++) {
-    for (std::size_t j = 0; j < Matrix_A_Type::ROWS; j++) {
-      out(i, j) = A(i, j) * B(i, j);
-    }
-  }
-
-  return out;
-}
-
 /**
  * @brief Calculates the quadratic form X * W * X^T for a given row vector X and
  * matrix W.

optimization_utility/python_optimization_sqp_matrix_utility.hpp

@@ -1042,9 +1042,10 @@ class SQP_CostMatrices_NMPC {
    * @return X_Horizon_Type The matrix containing the state trajectory over the
    * horizon.
    */
-  inline auto
-  simulate_trajectory(const X_Type &X_initial, const U_Horizon_Type &U_horizon,
-                      const _Parameter_Type &parameter) -> X_Horizon_Type {
+  inline auto simulate_trajectory(const X_Type &X_initial,
+                                  const U_Horizon_Type &U_horizon,
+                                  const _Parameter_Type &parameter)
+      -> X_Horizon_Type {
 
     X_Horizon_Type X_horizon;
     X_Type X = X_initial;
@@ -1074,8 +1075,8 @@ class SQP_CostMatrices_NMPC {
    * @return Y_Horizon_Type The resulting penalty matrix for Y horizon limit
    * violations.
    */
-  inline auto
-  calculate_Y_limit_penalty(const Y_Horizon_Type &Y_horizon) -> Y_Horizon_Type {
+  inline auto calculate_Y_limit_penalty(const Y_Horizon_Type &Y_horizon)
+      -> Y_Horizon_Type {
     Y_Horizon_Type Y_limit_penalty;
 
     MatrixOperation::calculate_Y_limit_penalty(
@@ -1457,8 +1458,10 @@ class SQP_CostMatrices_NMPC {
     auto CX_N_T_Py_Cx_N_dx = PythonNumpy::ATranspose_mul_B(
         Cx_N, (static_cast<_T>(2) * this->_Py * Cx_N_dx));
 
-    auto YN_limit_active_CX_N_dx = MatrixOperation::element_wise_multiply(
-        MatrixOperation::get_row(Y_limit_active, NP), Cx_N_dx);
+    Y_Type YN_limit_active_CX_N_dx;
+    PythonNumpy::element_wise_multiply(
+        YN_limit_active_CX_N_dx, MatrixOperation::get_row(Y_limit_active, NP),
+        Cx_N_dx);
 
     auto Y_min_max_rho_YN_limit_active_CX_N_dx =
         static_cast<_T>(2) * this->_Y_min_max_rho * YN_limit_active_CX_N_dx;
@@ -1506,8 +1509,10 @@ class SQP_CostMatrices_NMPC {
           static_cast<_T>(2) * this->_Py * ek_y,
           MatrixOperation::get_row(dx, k));
 
-      auto YN_limit_active_CX_k_dx = MatrixOperation::element_wise_multiply(
-          MatrixOperation::get_row(Y_limit_active, k), Cx_dx_k);
+      Y_Type YN_limit_active_CX_k_dx;
+      PythonNumpy::element_wise_multiply(
+          YN_limit_active_CX_k_dx, MatrixOperation::get_row(Y_limit_active, k),
+          Cx_dx_k);
 
       auto Y_min_max_rho_Yk_limit_active_Cx_dx_k =
           static_cast<_T>(2) * this->_Y_min_max_rho * YN_limit_active_CX_k_dx;