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

added function solve_QP_ADP. Pass vector of matrices by reference to save...

added function solve_QP_ADP. Pass vector of matrices by reference to save time. Next time, do it also for the other matrices
parent c1bf8e0b
......@@ -47,3 +47,5 @@ Eigen::VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A
Eigen::VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen::VectorXd b);
// unconstrained
Eigen::VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F);
Eigen::VectorXd solve_QP_ADP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen::VectorXd b, double af, const std::vector<Eigen::MatrixXd> &Qs_adp, const std::vector<Eigen::MatrixXd> &qs_adp, const std::vector<double> &rhs_adp);
......@@ -484,20 +484,20 @@ VectorXd mympc_varying_another_ADP(std::vector<MatrixAtype> A_tray, std::vector<
std::vector<MatrixXd> qs_adp;
std::vector<double> rhs_adp;
std::cout << "Su_N:" << std::endl;
std::cout << Su_N.format(CleanFmt) << std::endl;
// std::cout << "Su_N:" << std::endl;
// std::cout << Su_N.format(CleanFmt) << std::endl;
std::cout << "Sx_N:" << std::endl;
std::cout << Sx_N.format(CleanFmt) << std::endl;
// std::cout << "Sx_N:" << std::endl;
// std::cout << Sx_N.format(CleanFmt) << std::endl;
std::cout << "Sg_N:" << std::endl;
std::cout << Sg_N.format(CleanFmt) << std::endl;
// std::cout << "Sg_N:" << std::endl;
// std::cout << Sg_N.format(CleanFmt) << std::endl;
std::cout << "g_tray_col:" << std::endl;
std::cout << g_tray_col.format(CleanFmt) << std::endl;
// std::cout << "g_tray_col:" << std::endl;
// std::cout << g_tray_col.format(CleanFmt) << std::endl;
std::cout << "X_ref_N:" << std::endl;
std::cout << X_ref_N.format(CleanFmt) << std::endl;
// std::cout << "X_ref_N:" << std::endl;
// std::cout << X_ref_N.format(CleanFmt) << std::endl;
......@@ -506,46 +506,46 @@ VectorXd mympc_varying_another_ADP(std::vector<MatrixAtype> A_tray, std::vector<
for(int i = 0; i < N_V_fun; i++)
{
std::cout << "V_functions_P[i]:" << std::endl;
std::cout << V_functions_P[i].format(CleanFmt) << std::endl;
// std::cout << "V_functions_P[i]:" << std::endl;
// std::cout << V_functions_P[i].format(CleanFmt) << std::endl;
std::cout << "V_functions_p[i]:" << std::endl;
std::cout << V_functions_p[i].format(CleanFmt) << std::endl;
// std::cout << "V_functions_p[i]:" << std::endl;
// std::cout << V_functions_p[i].format(CleanFmt) << std::endl;
std::cout << "V_functions_q[i]:" << std::endl;
std::cout << V_functions_q[i] << std::endl;
// std::cout << "V_functions_q[i]:" << std::endl;
// std::cout << V_functions_q[i] << std::endl;
temp_q << V_functions_q[i];
std::cout<< "compuing HV" << std::endl;
// std::cout<< "compuing HV" << std::endl;
H_V = Su_N.transpose()*V_functions_P[i]*Su_N;
std::cout<< "compuing FV" << std::endl;
// std::cout<< "compuing FV" << std::endl;
std::cout<< "x0" << std::endl;
std::cout << x0.format(CleanFmt) << std::endl;
// std::cout<< "x0" << std::endl;
// std::cout << x0.format(CleanFmt) << std::endl;
std::cout<< "compuing FV part 1" << std::endl;
F_V = 2*x0.transpose()*Sx_N.transpose()*V_functions_P[i]*Su_N;
std::cout << F_V.format(CleanFmt) << std::endl;
// std::cout<< "compuing FV part 1" << std::endl;
// F_V = 2*x0.transpose()*Sx_N.transpose()*V_functions_P[i]*Su_N;
// std::cout << F_V.format(CleanFmt) << std::endl;
std::cout<< "compuing FV part 2" << std::endl;
F_V = 2*g_tray_col.transpose()*Sg_N.transpose()*V_functions_P[i]*Su_N;
std::cout << F_V.format(CleanFmt) << std::endl;
// std::cout<< "compuing FV part 2" << std::endl;
// F_V = 2*g_tray_col.transpose()*Sg_N.transpose()*V_functions_P[i]*Su_N;
// std::cout << F_V.format(CleanFmt) << std::endl;
std::cout<< "compuing FV part 3" << std::endl;
F_V = V_functions_p[i].transpose()*Su_N;
std::cout << F_V.format(CleanFmt) << std::endl;
// std::cout<< "compuing FV part 3" << std::endl;
// F_V = V_functions_p[i].transpose()*Su_N;
// std::cout << F_V.format(CleanFmt) << std::endl;
std::cout<< "compuing FV part 4" << std::endl;
F_V = 2*X_ref_N.transpose()*V_functions_P[i]*Su_N;
std::cout << F_V.format(CleanFmt) << std::endl;
// std::cout<< "compuing FV part 4" << std::endl;
// F_V = 2*X_ref_N.transpose()*V_functions_P[i]*Su_N;
// std::cout << F_V.format(CleanFmt) << std::endl;
F_V = 2*x0.transpose()*Sx_N.transpose()*V_functions_P[i]*Su_N
+ 2*g_tray_col.transpose()*Sg_N.transpose()*V_functions_P[i]*Su_N
+ V_functions_p[i].transpose()*Su_N - 2*X_ref_N.transpose()*V_functions_P[i]*Su_N;
std::cout<< "compuing qV" << std::endl;
// std::cout<< "compuing qV" << std::endl;
q_v = x0.transpose()*Sx_N.transpose()*V_functions_P[i]*Sx_N*x0
+ g_tray_col.transpose()*Sg_N.transpose()*V_functions_P[i]*Sg_N*g_tray_col
+ 2*x0.transpose()*Sx_N.transpose()*V_functions_P[i]*Sg_N*g_tray_col
......@@ -569,26 +569,26 @@ VectorXd mympc_varying_another_ADP(std::vector<MatrixAtype> A_tray, std::vector<
}
std::cout << "Qs_adp_test:" << std::endl << std::endl;
for(int i = 0; i < Qs_adp.size(); i++)
{
std::cout << "index = " << i << std::endl;
std::cout << Qs_adp[i].format(CleanFmt) << std::endl << std::endl;
}
std::cout << "qs_adp_test:" << std::endl << std::endl;
for(int i = 0; i < qs_adp.size(); i++)
{
std::cout << "index = " << i << std::endl;
std::cout << qs_adp[i].format(CleanFmt) << std::endl<< std::endl;
}
std::cout << "qs_adp_test:" << std::endl << std::endl;
for(int i = 0; i < rhs_adp.size(); i++)
{
std::cout << "index = " << i << std::endl;
std::cout << rhs_adp[i] << std::endl << std::endl;
}
// std::cout << "Qs_adp_test:" << std::endl << std::endl;
// for(int i = 0; i < Qs_adp.size(); i++)
// {
// std::cout << "index = " << i << std::endl;
// std::cout << Qs_adp[i].format(CleanFmt) << std::endl << std::endl;
// }
// std::cout << "qs_adp_test:" << std::endl << std::endl;
// for(int i = 0; i < qs_adp.size(); i++)
// {
// std::cout << "index = " << i << std::endl;
// std::cout << qs_adp[i].format(CleanFmt) << std::endl<< std::endl;
// }
// std::cout << "qs_adp_test:" << std::endl << std::endl;
// for(int i = 0; i < rhs_adp.size(); i++)
// {
// std::cout << "index = " << i << std::endl;
// std::cout << rhs_adp[i] << std::endl << std::endl;
// }
// std::cout << "Au test:" << std::endl << std::endl;
// std::cout << Au.format(CleanFmt) << std::endl;
......@@ -608,6 +608,189 @@ VectorXd mympc_varying_another_ADP(std::vector<MatrixAtype> A_tray, std::vector<
return U_0;
}
VectorXd solve_QP_ADP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen::VectorXd b, double af, const std::vector<Eigen::MatrixXd> &Qs_adp, const std::vector<Eigen::MatrixXd> &qs_adp, const std::vector<double> &rhs_adp)
{
// H is assumed to be symmetric. There is NO 0.5 factor anywhere, with this we solve:
// min(x'Hx + F'x)
int size_H = H.cols();
int cols_A = A.cols();
int rows_A = A.rows();
std::vector<int> q_row;
std::vector<int> q_col;
std::vector<double> q_val;
std::vector<double> l_obj_coeff;
for(int i = 0; i < size_H; i++)
{
for(int j = i; j < size_H; j++)
{
// std::cout << "i,j -->" << i << "," << j << std::endl;
if(H(i,j) != 0)
{
q_row.push_back(i);
q_col.push_back(j);
if(i == j)
{
q_val.push_back(H(i,j));
// 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;
}
}
}
l_obj_coeff.push_back(F(i));
}
int N_vars = size_H;
int N_q_coeffs = q_val.size();
// BEGINNING C INTERFACE:
GRBenv *env = NULL;
GRBmodel *model = NULL;
int error = 0;
double sol[N_vars];
int ind[N_vars];
double val[N_vars];
int* qrow = q_row.data();
int* qcol = q_col.data();
double* qval = q_val.data();
int optimstatus;
double objval;
std::vector<double> ub(N_vars, GRB_INFINITY);
std::vector<double> lb(N_vars, -GRB_INFINITY);
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");
if (error) goto QUIT;
/* Create a model */
error = GRBnewmodel(env, &model, "qp", N_vars, l_obj_coeff.data(),lb.data(), ub.data(), NULL, NULL);
if (error) goto QUIT;
/* Quadratic objective terms */
error = GRBaddqpterms(model, N_q_coeffs, qrow, qcol, qval);
if (error) goto QUIT;
error = GRBsetintparam(GRBgetenv(model), "OutputFlag", 0);
if (error) goto QUIT;
for(int i = 0; i < rows_A; i++)
{
l_ind.clear();
l_coeff.clear();
for(int j = 0; j < cols_A; j++)
{
if(A(i,j) != 0)
{
l_ind.push_back(j);
l_coeff.push_back(A(i,j));
}
}
error = GRBaddconstr(model, l_ind.size(), l_ind.data(), l_coeff.data(), GRB_LESS_EQUAL, b(i), NULL);
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);
if (error) goto QUIT;
/* Write model to 'qp.lp' */
error = GRBwrite(model, "qp.lp");
if (error) goto QUIT;
/* Capture solution information */
error = GRBgetintattr(model, GRB_INT_ATTR_STATUS, &optimstatus);
if (error) goto QUIT;
error = GRBgetdblattr(model, GRB_DBL_ATTR_OBJVAL, &objval);
if (error) goto QUIT;
error = GRBgetdblattrarray(model, GRB_DBL_ATTR_X, 0, N_vars, sol);
if (error) goto QUIT;
// printf("\nOptimization complete\n");
if (optimstatus == GRB_OPTIMAL) {
// printf("Optimal objective: %.4e\n", objval);
// for(int i = 0; i < N_vars; i++)
// {
// printf("sol[i] = %.4f\n", sol[i]);
// }
} else if (optimstatus == GRB_INF_OR_UNBD) {
printf("Model is infeasible or unbounded\n");
} else {
printf("Optimization was stopped early\n");
}
QUIT:
/* Error reporting */
if (error) {
printf("ERROR: %s\n", GRBgeterrormsg(env));
exit(1);
}
/* Free model */
GRBfreemodel(model);
/* Free environment */
GRBfreeenv(env);
// END C INTERFACE
Map<VectorXd> U_0(sol,N_vars);
// Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
// cout << "The mapped vector U_0 is: \n" << U_0.format(CleanFmt) << "\n";
return U_0;
}
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:
......
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