Added first part of unconstrained MPC. Need to try it feeding it to gurobi

pps_ws/src/d_fall_pps/include/MPC_functions.h

@@ -36,3 +36,6 @@ void funSxSuSg_varying_affine(std::vector<MatrixAtype> A_tray, std::vector<Matri
 void funQbar(Eigen::MatrixXf Q, Eigen::MatrixXf P, int N, Eigen::MatrixXf &Qbar);
 
 void funRbar(Eigen::MatrixXf R, int N, Eigen::MatrixXf &Rbar);
+
+VectorUtype mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, std::vector<VectorXtype> g_tray, Eigen::MatrixXf Q, Eigen::MatrixXf R, Eigen::MatrixXf P, int N, VectorXtype x0, Eigen::VectorXf X_ref, Eigen::VectorXf U_ref, float ft_min, float ft_max, float angle_constraint);
+

pps_ws/src/d_fall_pps/src/MPC_functions.cpp

@@ -268,3 +268,47 @@ void funRbar(MatrixXf R, int N, MatrixXf &Rbar)
     Rbar = R_bar;
 }
 
+VectorUtype mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, std::vector<VectorXtype> g_tray, MatrixXf Q, MatrixXf R, MatrixXf P, int N, VectorXtype x0, VectorXf X_ref, VectorXf U_ref, float ft_min, float ft_max, float angle_constraint)
+{
+    Eigen::MatrixXf Sx;
+    Eigen::MatrixXf Su;
+    Eigen::MatrixXf Sg;
+
+    VectorUtype u_optimal;
+
+    funSxSuSg_varying_affine(A_tray, B_tray, N, Sx, Su, Sg);
+
+    VectorXf g_tray_col(N*N_x);
+
+    for(int i = 0; i < g_tray.size(); i++)
+    {
+        g_tray_col.segment(i*N_x, N_x) = g_tray[i];
+    }
+
+    // std::cout << "g_tray_col" << std::endl;
+    // std::cout << g_tray_col << std::endl;
+
+    Eigen::MatrixXf Qbar;
+    Eigen::MatrixXf Rbar;
+
+    funQbar(Q, P, N, Qbar);
+    funRbar(R,N, Rbar);
+
+    Eigen::MatrixXf H;
+    Eigen::MatrixXf F;
+
+    H = Su.transpose()*Qbar*Su + Rbar;
+
+    F = 2*(x0.transpose()*Sx.transpose()*Qbar*Su + g_tray_col.transpose()*Sg.transpose()*Qbar*Su - X_ref.transpose()*Qbar*Su - U_ref.transpose()*Rbar);
+
+    // Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
+
+
+    // std::cout << "H: " << std::endl;
+    // std::cout << H.format(CleanFmt) << std::endl;
+
+    // std::cout << "F: " << std::endl;
+    // std::cout << F.format(CleanFmt) << std::endl;
+
+    return u_optimal;
+}

pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp

@@ -693,6 +693,8 @@ void setpointCallback(const Setpoint& newSetpoint)
     Eigen::IOFormat OctaveFmt(Eigen::StreamPrecision, 0, ", ", ";\n", "", "", "[", "]");
     Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
 
+
+
     // std::cout << A.format(OctaveFmt) << std::endl;
 
     // std::cout << B.format(OctaveFmt) << std::endl;
@@ -757,6 +759,23 @@ void setpointCallback(const Setpoint& newSetpoint)
     std::cout << "Rbar test:" << std::endl << std::endl;
     // std::cout << Su_test<< std::endl;
     std::cout << Rbar.format(CleanFmt) << std::endl;
+
+    Eigen::VectorXf X_ref(N_x*(N+1));
+    Eigen::VectorXf U_ref(N_u*N);
+
+    VectorXtype x_ref;
+    VectorUtype u_ref;
+    x_ref << 0,0,0,0,0,0;
+    u_ref << 0,0,0,params.m*params.g;
+
+    for(int i = 0; i < N+1; i++)
+    {
+        X_ref.segment(i*N_x, N_x) = x_ref;
+        if(i < N)
+            U_ref.segment(i*N_u, N_u) = u_ref;
+    }
+
+    mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0, X_ref, U_ref, 0, 0, 0);
 }