Commit 0e9a0cd2 by roangel

### added measurement of time, rmoved debugging prompts

parent 67f64dcc
 ... ... @@ -306,9 +306,9 @@ VectorXd mympc_varying_another(std::vector A_tray, std::vector" << i << "," << j << std::endl; // std::cout << "i,j -->" << i << "," << j << std::endl; if(H(i,j) != 0) { q_row.push_back(i); ... ... @@ -340,12 +340,12 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F) if(i == j) { q_val.push_back(H(i,j)); std::cout << "H(i,j)" << H(i,j) << endl; // std::cout << "H(i,j)" << H(i,j) << endl; } else { q_val.push_back(2*H(i,j)); std::cout << "2H(i,j)" << 2*H(i,j) << endl; // std::cout << "2H(i,j)" << 2*H(i,j) << endl; } } } ... ... @@ -387,6 +387,9 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F) error = GRBaddqpterms(model, N_q_coeffs, qrow, qcol, qval); if (error) goto QUIT; error = GRBsetintparam(GRBgetenv(model), "OutputFlag", 0); if (error) goto QUIT; // Optimize model error = GRBoptimize(model); ... ... @@ -406,9 +409,9 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F) error = GRBgetdblattrarray(model, GRB_DBL_ATTR_X, 0, N_vars, sol); if (error) goto QUIT; printf("\nOptimization complete\n"); // printf("\nOptimization complete\n"); if (optimstatus == GRB_OPTIMAL) { printf("Optimal objective: %.4e\n", objval); // printf("Optimal objective: %.4e\n", objval); // for(int i = 0; i < N_vars; i++) // { // printf("sol[i] = %.4f\n", sol[i]); ... ... @@ -441,6 +444,6 @@ QUIT: Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]"); Map U_0(sol,N_vars); cout << "The mapped vector U_0 is: \n" << U_0.format(CleanFmt) << "\n"; // cout << "The mapped vector U_0 is: \n" << U_0.format(CleanFmt) << "\n"; return U_0; }
 ... ... @@ -50,6 +50,8 @@ #include "ros/ros.h" #include #include //the generated structs from the msg-files have to be included #include "d_fall_pps/ViconData.h" #include "d_fall_pps/Setpoint.h" ... ... @@ -749,6 +751,10 @@ void setpointCallback(const Setpoint& newSetpoint) initialize_MPC_variables(N, x0); clock_t begin_time = clock(); // 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, 0); VectorUtype u = U_0.tail(4); // this is the input to apply next ... ... @@ -781,8 +787,14 @@ void setpointCallback(const Setpoint& newSetpoint) raw_input[2] = u(2); raw_input[3] = u(3); std::cout << "U_0 for this setpoint, 1 step MPC" << std::endl; std::cout << U_0.format(CleanFmt) << std::endl; clock_t end_time = clock(); double elapsed_secs = double(end_time - begin_time) / CLOCKS_PER_SEC; // std::cout << "U_0 for this setpoint, 1 step MPC" << std::endl; // std::cout << U_0.format(CleanFmt) << std::endl; std::cout << "elapsed secs: " << elapsed_secs << std::endl; // testzone: ... ... @@ -925,7 +937,6 @@ void setpointCallback(const Setpoint& newSetpoint) void initialize_MPC_variables(int N, VectorXtype x0) { cout << "called initialize MPC variables" << endl; // params: params.m = cf_mass/1000.0; params.g = 9.81; ... ... @@ -944,32 +955,32 @@ void initialize_MPC_variables(int N, VectorXtype x0) U_tray.push_back(u_temp); } cout << "filled X_tray and U_tray:" << endl; // cout << "filled X_tray and U_tray:" << endl; cout << "X_tray:" << endl; for(int i=0; i < X_tray.size(); i++) cout << X_tray[i].format(CleanFmt) << endl; // cout << "X_tray:" << endl; // for(int i=0; i < X_tray.size(); i++) // cout << X_tray[i].format(CleanFmt) << endl; cout << "U_tray:" << endl; for(int i=0; i < U_tray.size(); i++) cout << U_tray[i].format(CleanFmt) << endl; // cout << "U_tray:" << endl; // for(int i=0; i < U_tray.size(); i++) // cout << U_tray[i].format(CleanFmt) << endl; // fill A_tray, B_tray, g_tray with initial values get_matrices_linearization_affine(X_tray, U_tray, params, A_tray, B_tray, g_tray); cout << "filled A_tray B_tray and g_tray:" << endl; // cout << "filled A_tray B_tray and g_tray:" << endl; cout << "A_tray:" << endl; for(int i=0; i < A_tray.size(); i++) cout << A_tray[i].format(CleanFmt) << endl; // cout << "A_tray:" << endl; // for(int i=0; i < A_tray.size(); i++) // cout << A_tray[i].format(CleanFmt) << endl; cout << "B_tray:" << endl; for(int i=0; i < B_tray.size(); i++) cout << B_tray[i].format(CleanFmt) << endl; // cout << "B_tray:" << endl; // for(int i=0; i < B_tray.size(); i++) // cout << B_tray[i].format(CleanFmt) << endl; cout << "g_tray:" << endl; for(int i=0; i < g_tray.size(); i++) cout << g_tray[i].format(CleanFmt) << endl; // cout << "g_tray:" << endl; // for(int i=0; i < g_tray.size(); i++) // cout << g_tray[i].format(CleanFmt) << endl; VectorXtype q_vec; q_vec << 5000, 5000, 5000, 500, 500, 500; ... ...
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!