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 ed2750e6 authored by roangel's avatar roangel
Browse files

ADP controller running, need to make some time measurements now to properly compare

parent c2696646
# Mass of the crazyflie
mass : 32
mass : 30
# Frequency of the controller, in hertz
control_frequency : 200
......
......@@ -409,18 +409,24 @@ VectorXd mympc_varying_another_ADP(std::vector<MatrixAtype> A_tray, std::vector<
// std::cout << "size Su_1" << std::endl;
// std::cout << Su_1.rows() << "," << Su_1.cols() << std::endl;
// std::cout << "Sx_1 test:" << std::endl << std::endl;
// std::cout << Sx_1.format(CleanFmt) << std::endl;
// std::cout << "Rbar test:" << std::endl << std::endl;
// std::cout << Rbar.format(CleanFmt) << std::endl;
H = Su_1.transpose()*Qbar_1*Su_1 + Rbar;
std::cout << "H:" << std::endl;
std::cout << H.format(CleanFmt) << std::endl;
// std::cout << "H:" << std::endl;
// std::cout << H.format(CleanFmt) << std::endl;
// in this case, F is a row vector
std::cout << "computing F" << std::endl;
// std::cout << "computing F" << std::endl;
F = 2*(x0.transpose()*Sx_1.transpose()*Qbar_1*Su_1 + g_tray_col.transpose()*Sg_1.transpose()*Qbar_1*Su_1 - X_ref_1.transpose()*Qbar_1*Su_1 - U_ref.transpose()*Rbar);
std::cout << "F:" << std::endl;
std::cout << F.format(CleanFmt) << std::endl;
// std::cout << "F:" << std::endl;
// std::cout << F.format(CleanFmt) << std::endl;
......@@ -429,15 +435,15 @@ VectorXd mympc_varying_another_ADP(std::vector<MatrixAtype> A_tray, std::vector<
Eigen::MatrixXd F_final = Eigen::MatrixXd::Zero(F.rows(), F.cols() + 1);
H_final.topLeftCorner(H.rows(),H.cols()) << H;
std::cout << "H_final:" << std::endl;
std::cout << H_final.format(CleanFmt) << std::endl;
// std::cout << "H_final:" << std::endl;
// std::cout << H_final.format(CleanFmt) << std::endl;
F_final.leftCols(F.cols()) << F;
F_final.rightCols(1) << 1.0;
std::cout << "F_final:" << std::endl;
std::cout << F_final.format(CleanFmt) << std::endl;
// std::cout << "F_final:" << std::endl;
// std::cout << F_final.format(CleanFmt) << std::endl;
// std::cout << "H test:" << std::endl << std::endl;
......@@ -464,8 +470,8 @@ VectorXd mympc_varying_another_ADP(std::vector<MatrixAtype> A_tray, std::vector<
MatrixXd Au_final = Eigen::MatrixXd::Zero(Au.rows(), Au.cols()+1);
Au_final.leftCols(Au.cols()) << Au;
std::cout << "Au_final:" << std::endl;
std::cout << Au_final.format(CleanFmt) << std::endl;
// std::cout << "Au_final:" << std::endl;
// std::cout << Au_final.format(CleanFmt) << std::endl;
VectorXd bu(2*N);
bu.topRows(N) = VectorXd::Ones(N)*ft_max;
......@@ -501,7 +507,7 @@ VectorXd mympc_varying_another_ADP(std::vector<MatrixAtype> A_tray, std::vector<
std::cout << "entered adp part" << std::endl;
// std::cout << "entered adp part" << std::endl;
for(int i = 0; i < N_V_fun; i++)
{
......@@ -601,7 +607,7 @@ VectorXd mympc_varying_another_ADP(std::vector<MatrixAtype> A_tray, std::vector<
// W(1,1) = 1/(af^2);
// W(2,2) = 1/(af^2);
Eigen::VectorXd U_0 = solve_QP(H, F, Au, bu, af);
Eigen::VectorXd U_0 = solve_QP_ADP(H_final, F_final, Au_final, bu, af, Qs_adp, qs_adp, rhs_adp);
// Eigen::VectorXd U_0 = solve_QP(H, F, Au, bu);
// Eigen::VectorXd U_0 = solve_QP(H, F);
......@@ -672,6 +678,13 @@ VectorXd solve_QP_ADP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, E
std::vector<int> l_ind;
std::vector<double> l_coeff;
std::vector<int> q_ind_adp_row;
std::vector<int> q_ind_adp_col;
std::vector<double> q_val_adp;
std::vector<int> l_ind_adp;
std::vector<double> l_val_adp;
int quad_row[2];
int quad_col[2];
double quad_coeff[2];
......@@ -735,15 +748,37 @@ VectorXd solve_QP_ADP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, E
for(int i = 0; i < N_V_fun; i++)
{
q_ind_adp_row.clear();
q_ind_adp_col.clear();
q_val_adp.clear();
l_ind_adp.clear();
l_val_adp.clear();
for(int j = 0; j < Qs_adp[i].rows(); j++)
{
for(int k = 0; k < Qs_adp[i].cols(); k++)
{
// here create q_row, q_col, q_val if the element of Qs_adp[i](j,k) != 0
if(Qs_adp[i](j,k) != 0)
{
q_ind_adp_row.push_back(j);
q_ind_adp_col.push_back(k);
q_val_adp.push_back(Qs_adp[i](j,k));
}
}
// here create l_ind, l_val if the elemt qs_adp[i](j) != 0
if(qs_adp[i](j) != 0)
{
l_ind_adp.push_back(j);
l_val_adp.push_back(qs_adp[i](j));
}
}
// here add the constraint using the arrays crated before
error = GRBaddqconstr(model, l_ind_adp.size(), l_ind_adp.data(), l_val_adp.data(), q_ind_adp_row.size(), q_ind_adp_col.data(), q_ind_adp_row.data(), q_val_adp.data(),
GRB_LESS_EQUAL, rhs_adp[i], NULL);
if (error) goto QUIT;
}
......@@ -799,9 +834,11 @@ QUIT:
// END C INTERFACE
Map<VectorXd> U_0(sol,N_vars);
Map<VectorXd> solution(sol,N_vars);
// Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
// cout << "The mapped vector U_0 is: \n" << U_0.format(CleanFmt) << "\n";
VectorXd U_0 = solution.head(size_H - 1);
return U_0;
}
......
......@@ -199,7 +199,7 @@ VectorXtype x0;
std::mutex MPC_mutex;
int N = 4;
int N = 1;
// ROS Publisher for debugging variables
......@@ -508,7 +508,7 @@ void do_MPC(int* publish_rate)
MPC_mutex.unlock();
// Do the MPC step:
U_0 = mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0_local, X_ref, U_ref, 0, 0.1391*4, 0.7887);
U_0 = mympc_varying_another_ADP(A_tray, B_tray, g_tray, Q, R, P, N, x0_local, 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:
......@@ -785,7 +785,7 @@ void setpointCallback(const Setpoint& newSetpoint)
initialize_MPC_variables(N, x0);
U_0 = mympc_varying_another_ADP(A_tray, B_tray, g_tray, Q, R, P, N, x0, X_ref, U_ref, 0, 0.1391*4, 0.7887);
// U_0 = mympc_varying_another_ADP(A_tray, B_tray, g_tray, Q, R, P, N, x0, X_ref, U_ref, 0, 0.1391*4, 0.7887);
// debug
// fill_V_function_variables();
......@@ -803,6 +803,8 @@ void setpointCallback(const Setpoint& newSetpoint)
// Do the MPC step:
// MPC_mutex.lock();
// x0 << - setpoint[0],
// - setpoint[1],
// - setpoint[2],
......@@ -810,7 +812,9 @@ 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.7887);
// MPC_mutex.unlock();
// U_0 = mympc_varying_another_ADP(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;
......
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