added quadratic constraints. Now we notice how the delay affects the system,...

added quadratic constraints. Now we notice how the delay affects the system, we dont have time to solve the problem

pps_ws/src/d_fall_pps/include/MPC_functions.h

@@ -37,5 +37,5 @@ void funQbar(Eigen::MatrixXd Q, Eigen::MatrixXd P, int N, Eigen::MatrixXd &Qbar)
 
 void funRbar(Eigen::MatrixXd R, int N, Eigen::MatrixXd &Rbar);
 
-Eigen::VectorXd mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, std::vector<VectorXtype> g_tray, Eigen::MatrixXd Q, Eigen::MatrixXd R, Eigen::MatrixXd P, int N, VectorXtype x0, Eigen::VectorXd X_ref, Eigen::VectorXd U_ref, float ft_min, float ft_max, float angle_constraint);
-Eigen::VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen::VectorXd b);
+Eigen::VectorXd mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, std::vector<VectorXtype> g_tray, Eigen::MatrixXd Q, Eigen::MatrixXd R, Eigen::MatrixXd P, int N, VectorXtype x0, Eigen::VectorXd X_ref, Eigen::VectorXd U_ref, float ft_min, float ft_max, float af);
+Eigen::VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen::VectorXd b, double af);

pps_ws/src/d_fall_pps/src/MPC_functions.cpp

@@ -271,7 +271,7 @@ void funRbar(MatrixXd R, int N, MatrixXd &Rbar)
     Rbar = R_bar;
 }
 
-VectorXd mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, std::vector<VectorXtype> g_tray, MatrixXd Q, MatrixXd R, MatrixXd P, int N, VectorXtype x0, VectorXd X_ref, VectorXd U_ref, float ft_min, float ft_max, float angle_constraint)
+VectorXd mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, std::vector<VectorXtype> g_tray, MatrixXd Q, MatrixXd R, MatrixXd P, int N, VectorXtype x0, VectorXd X_ref, VectorXd U_ref, float ft_min, float ft_max, float af)
 {
     Eigen::MatrixXd Sx;
     Eigen::MatrixXd Su;
@@ -337,12 +337,17 @@ VectorXd mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<Matr
     // std::cout << "bu test:" << std::endl << std::endl;
     // std::cout << bu.format(CleanFmt) << std::endl;
 
-    Eigen::VectorXd U_0 = solve_QP(H, F, Au, bu);
+    // Eigen::MatrixXd W = MatrixXd::Zero(4,4);
+
+    // W(1,1) = 1/(af^2);
+    // W(2,2) = 1/(af^2);
+
+    Eigen::VectorXd U_0 = solve_QP(H, F, Au, bu, af);
 
     return U_0;
 }
 
-VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen::VectorXd b)
+VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen::VectorXd b, double af)
 {
     // H is assumed to be symmetric. There is NO 0.5 factor anywhere, with this we solve:
     // min(x'Hx + F'x)
@@ -406,6 +411,14 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen
     std::vector<int> l_ind;
     std::vector<double> l_coeff;
 
+    int quad_row[2];
+    int quad_col[2];
+    double quad_coeff[2];
+
+    int N_horizon = size_H/N_u;
+
+
+
 
     /* Create environment */
     error = GRBloadenv(&env, "qp.log");
@@ -439,6 +452,25 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen
         if (error) goto QUIT;
     }
 
+
+
+    // Quadratic constraints.
+
+    for(int i = 0; i < N_horizon; i++)
+    {
+        quad_row[0] = N_u*i;
+        quad_col[0] = N_u*i;
+        quad_row[1] = N_u*i + 1;
+        quad_col[1] = N_u*i + 1;
+        quad_coeff[0] = 1/(af*af);
+        quad_coeff[1] = 1/(af*af);
+
+        error = GRBaddqconstr(model, 0, NULL, NULL, 2, quad_row, quad_col, quad_coeff,
+                        GRB_LESS_EQUAL, 1.0, NULL);
+        if (error) goto QUIT;
+    }
+
+
     // Optimize model
 
     error = GRBoptimize(model);

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

@@ -536,7 +536,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
             stateInertialEstimate[5];
 
         // Do the MPC step:
-        U_0 = mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0, X_ref, U_ref, 0, 0.1391*4, 0);
+        U_0 = mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0, X_ref, U_ref, 0, 0.1391*4, 0.7887);
         VectorUtype u = U_0.tail(4); // this is the input to apply next
 
         // prediction of next N states:
@@ -804,7 +804,7 @@ void setpointCallback(const Setpoint& newSetpoint)
     //     0,
     //     0;
 
-    // U_0 = mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0, X_ref, U_ref, 0, 0.1391*4, 0);
+    // U_0 = mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0, X_ref, U_ref, 0, 0.1391*4, 0.7887);
     // VectorUtype u = U_0.tail(4); // this is the input to apply next
 
     // std::cout << "U_0 opt for this setpoint, 1 step MPC" << std::endl;