To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 0e9a0cd2 authored by roangel's avatar roangel
Browse files

added measurement of time, rmoved debugging prompts

parent 67f64dcc
......@@ -306,9 +306,9 @@ VectorXd mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<Matr
Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
std::cout << "H test:" << std::endl << std::endl;
// std::cout << Su_test<< std::endl;
std::cout << H.format(CleanFmt) << std::endl;
// std::cout << "H test:" << std::endl << std::endl;
// // std::cout << Su_test<< std::endl;
// std::cout << H.format(CleanFmt) << std::endl;
Eigen::VectorXd U_0 = solve_QP(H, F);
......@@ -331,7 +331,7 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F)
{
for(int j = i; j < size_H; j++)
{
std::cout << "i,j -->" << 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<VectorXd> 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 <ros/package.h>
#include <ctime>
//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;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment