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

added overloaded functions for different type of inputs for the solver

parent 772f6285
......@@ -38,4 +38,11 @@ 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 af);
// function overload for solver
// quadratically constrained solver
Eigen::VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen::VectorXd b, double af);
// only with linear constraints
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);
......@@ -342,7 +342,8 @@ VectorXd mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<Matr
// 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(H, F, Au, bu, af);
Eigen::VectorXd U_0 = solve_QP(H, F, Au, bu);
return U_0;
}
......@@ -523,8 +524,303 @@ QUIT:
// END C INTERFACE
Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
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)
{
// 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;
/* 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;
}
// 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)
{
// 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();
std::vector<int> q_row;
std::vector<int> q_col;
std::vector<double> q_val;
std::vector<double> l_obj_coeff;
int N_vars = size_H;
int N_q_coeffs = q_val.size();
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));
}
// 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);
/* 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;
// 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;
}
......@@ -483,11 +483,15 @@ clock_t begin_time = 0;
clock_t end_time = 0;
ros::Time begin_t;
DebugMsg debugMsg;
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
// debugMsg.vicon_x = request.ownCrazyflie.x;
// debugPublisher.publish(debugMsg);
// end_time = clock();
// double elapsed_secs = double(end_time - begin_time) / CLOCKS_PER_SEC;
......
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