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

Added wrapper function, cleaned, C gurobi interface ready for next steps

parent 49e69367
......@@ -37,5 +37,6 @@ void funQbar(Eigen::MatrixXd Q, Eigen::MatrixXd P, int N, Eigen::MatrixXd &Qbar)
void funRbar(Eigen::MatrixXd R, int N, Eigen::MatrixXd &Rbar);
VectorUtype 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 angle_constraint);
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 angle_constraint);
Eigen::VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F);
......@@ -10,22 +10,6 @@ extern "C"
using namespace Eigen;
using namespace std;
static int
dense_optimize(GRBenv *env,
int rows,
int cols,
double *c, /* linear portion of objective function */
double *Q, /* quadratic portion of objective function */
double *A, /* constraint matrix */
char *sense, /* constraint senses */
double *rhs, /* RHS vector */
double *lb, /* variable lower bounds */
double *ub, /* variable upper bounds */
char *vtype, /* variable types (continuous, binary, etc.) */
double *solution,
double *objvalP);
void linearization_fast_euler_6_states(const VectorXtype x_vec, const VectorUtype u_vec, params_t params, MatrixAtype &A, MatrixBtype &B)
{
float m = params.m;
......@@ -288,7 +272,7 @@ void funRbar(MatrixXd R, int N, MatrixXd &Rbar)
Rbar = R_bar;
}
VectorUtype mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, std::vector<VectorXtype> g_tray, MatrixXd Q, MatrixXd R, MatrixXd P, int N, VectorXtype x0, VectorXd X_ref, VectorXd U_ref, float ft_min, float ft_max, float angle_constraint)
VectorXd mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, std::vector<VectorXtype> g_tray, MatrixXd Q, MatrixXd R, MatrixXd P, int N, VectorXtype x0, VectorXd X_ref, VectorXd U_ref, float ft_min, float ft_max, float angle_constraint)
{
Eigen::MatrixXd Sx;
Eigen::MatrixXd Su;
......@@ -327,174 +311,18 @@ VectorUtype mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<M
// std::cout << Su_test<< std::endl;
std::cout << H.format(CleanFmt) << std::endl;
Eigen::VectorXd U_0 = solve_QP(H, F);
// F and H have been computed properly. Now we need to feed them to the solver.
// Copy and paste again the QP example: --------------------------------------
// GRBenv *env = NULL;
// GRBmodel *model = NULL;
// int error = 0;
// double sol[3];
// int ind[3];
// double val[3];
// int qrow[5];
// int qcol[5];
// double qval[5];
// char vtype[3];
// int optimstatus;
// double objval;
// double obj_1[] = {2.0, 0.0, 0.0};
// double lb[] = {-GRB_INFINITY, -GRB_INFINITY, -GRB_INFINITY};
// /* Create environment */
// error = GRBloadenv(&env, "qp.log");
// if (error) goto QUIT;
// /* Create an empty model */
// error = GRBnewmodel(env, &model, "qp", 3, obj_1, lb, NULL, NULL, NULL);
// if (error) goto QUIT;
// /* Add variables */
// // error = GRBaddvars(model, 3, 0, NULL, NULL, NULL, NULL, NULL, NULL, NULL,
// // NULL);
// // if (error) goto QUIT;
// /* Quadratic objective terms */
// qrow[0] = 0; qrow[1] = 0; qrow[2] = 1; qrow[3] = 1; qrow[4] = 2;
// qcol[0] = 0; qcol[1] = 1; qcol[2] = 1; qcol[3] = 2; qcol[4] = 2;
// qval[0] = 1; qval[1] = 1; qval[2] = 1; qval[3] = 1; qval[4] = 1;
// error = GRBaddqpterms(model, 5, qrow, qcol, qval);
// if (error) goto QUIT;
// /* Linear objective term */
// // error = GRBsetdblattrelement(model, GRB_DBL_ATTR_OBJ, 0, 2.0);
// // if (error) goto QUIT;
// // // First constraint: x + 2 y + 3 z <= 4
// // ind[0] = 0; ind[1] = 1; ind[2] = 2;
// // val[0] = 0; val[1] = 0; val[2] = 0;
// // error = GRBaddconstr(model, 3, ind, val, GRB_GREATER_EQUAL, 0, "c0");
// // if (error) goto QUIT;
// // /* Second constraint: x + y >= 1 */
// // ind[0] = 0; ind[1] = 1;
// // val[0] = 1; val[1] = 1;
// // error = GRBaddconstr(model, 2, ind, val, GRB_GREATER_EQUAL, 1.0, "c1");
// // 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, 3, sol);
// if (error) goto QUIT;
// printf("\nOptimization complete\n");
// if (optimstatus == GRB_OPTIMAL) {
// printf("Optimal objective: %.4e\n", objval);
// printf(" x=%.4f, y=%.4f, z=%.4f\n", sol[0], sol[1], sol[2]);
// } else if (optimstatus == GRB_INF_OR_UNBD) {
// printf("Model is infeasible or unbounded\n");
// } else {
// printf("Optimization was stopped early\n");
// }
/* Modify variable types */
// vtype[0] = GRB_INTEGER; vtype[1] = GRB_INTEGER; vtype[2] = GRB_INTEGER;
// error = GRBsetcharattrarray(model, GRB_CHAR_ATTR_VTYPE, 0, 3, vtype);
// if (error) goto QUIT;
// /* Optimize model */
// error = GRBoptimize(model);
// if (error) goto QUIT;
// /* Write model to 'qp2.lp' */
// error = GRBwrite(model, "qp2.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, 3, sol);
// if (error) goto QUIT;
// printf("\nOptimization complete\n");
// if (optimstatus == GRB_OPTIMAL) {
// printf("Optimal objective: %.4e\n", objval);
// printf(" x=%.4f, y=%.4f, z=%.4f\n", sol[0], sol[1], sol[2]);
// } 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);
return U_0;
}
// Trial with QP example: --------------------------------------------------------------------------
VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F)
{
int size_H = H.cols();
std::vector<int> q_row;
std::vector<int> q_col;
std::vector<double> q_val;
std::vector<int> l_obj_ind;
std::vector<double> l_obj_coeff;
for(int i = 0; i < size_H; i++)
......@@ -524,9 +352,10 @@ VectorUtype mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<M
int N_vars = N_u*N;
int N_vars = size_H;
int N_q_coeffs = q_val.size();
// BEGINNING C INTERFACE:
GRBenv *env = NULL;
GRBmodel *model = NULL;
......@@ -537,11 +366,6 @@ VectorUtype mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<M
int* qrow = q_row.data();
int* qcol = q_col.data();
double* qval = q_val.data();
// int qrow[] = {0,1};
// int qcol[] = {0,1};
// double qval[] = {1,2};
// double c[] = {2,2};
// char vtype[N_vars];
int optimstatus;
double objval;
......@@ -549,71 +373,28 @@ VectorUtype mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<M
std::vector<double> lb(N_vars, -GRB_INFINITY);
/* Create environment */
error = GRBloadenv(&env, "qp.log");
if (error) goto QUIT;
/* Create an empty model */
/* Create a model */
error = GRBnewmodel(env, &model, "qp", N_vars, l_obj_coeff.data(),lb.data(), ub.data(), NULL, NULL);
if (error) goto QUIT;
/* Add variables */
// error = GRBaddvars(model, N_vars, 0, NULL, NULL, NULL, NULL, NULL, NULL, NULL,
// NULL);
// if (error) goto QUIT;
/* Quadratic objective terms */
error = GRBaddqpterms(model, N_q_coeffs, qrow, qcol, qval);
if (error) goto QUIT;
/* Linear objective term */
// for(int i = 0; i < l_obj_ind.size(); i++)
// {
// error = GRBsetdblattrelement(model, GRB_DBL_ATTR_OBJ, l_obj_ind[i] , l_obj_coeff[i]);
// std::cout << "l_obj_ind[i]" << std::endl;
// std::cout << l_obj_ind[i] << std::endl;
// std::cout << "l_obj_coeff[i]" << std::endl;
// std::cout << l_obj_coeff[i] << std::endl;
// if (error) goto QUIT;
// }
// error = GRBsetdblattrelement(model, GRB_DBL_ATTR_OBJ, 0, 2.0);
// if (error) goto QUIT;
// error = GRBsetdblattrelement(model, GRB_DBL_ATTR_OBJ, 1, 2.0);
// if (error) goto QUIT;
// First constraint: x + 2 y + 3 z <= 4
// ind[0] = 0; ind[1] = 1; ind[2] = 2;
// val[0] = 1; val[1] = 2; val[2] = 3;
// error = GRBaddconstr(model, 3, ind, val, GRB_GREATER_EQUAL, 4.0, "c0");
// if (error) goto QUIT;
/* Second constraint: x + y >= 1 */
// ind[0] = 0; ind[1] = 1;
// val[0] = 1; val[1] = 1;
// error = GRBaddconstr(model, 2, ind, val, GRB_GREATER_EQUAL, 1.0, "c1");
// 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;
......@@ -626,10 +407,10 @@ VectorUtype mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<M
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]);
}
// 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 {
......@@ -654,170 +435,10 @@ QUIT:
GRBfreeenv(env);
// END C INTERFACE
// Trial with Dense.c example: ----------------------------------------------------------------------
// Eigen::MatrixXd H_d = H.cast <double> (); // Matrix of floats to matrix of doubles
// std::vector<double> H_vec(H_d.size());
// Eigen::Map<Eigen::MatrixXd>(H_vec.data(), H_d.rows(), H_d.cols()) = H_d;
// std::cout<< "vector row major order check" << std::endl;
// for (const auto& x : H_vec)
// cout << x << ", ";
// cout << "\n";
// Eigen::MatrixXd F_d = F.cast <double> (); // Matrix of floats to matrix of doubles
// std::vector<double> F_vec(F_d.size());
// Eigen::Map<Eigen::MatrixXd>(F_vec.data(), F_d.rows(), F_d.cols()) = F_d;
// std::cout<< "vector row major order check" << std::endl;
// for (const auto& x : F_vec)
// cout << x << ", ";
// cout << "\n";
// std::cout << "H: " << std::endl;
// std::cout << H.format(CleanFmt) << std::endl;
// std::cout << "F: " << std::endl;
// std::cout << F.format(CleanFmt) << std::endl;
// GRBenv *env = NULL;
// int error = 0;
// // double* f = F_vec.data();
// // double* h = H_vec.data();
// double A[2][3] = {{1, 2, 3}, {1, 1, 0}};
// char sense[] = {'>', '>'};
// double rhs[] = {4, 1};
// double lb[] = {0, 0, 0};
// double sol[20];
// int solved;
// double objval;
// double h[2][2] ={{1,0},{0,2}};
// double f[2] = {2,2};
// /* Create environment */
// error = GRBloadenv(&env, "dense.log");
// if (error) goto QUIT;
// /* Solve the model */
// solved = dense_optimize(env, 0, 2, &f[0], &h[0][0], NULL, NULL, NULL, NULL,
// NULL, NULL, sol, &objval);
// if (solved)
// for(int i =0; i < 2; i++)
// {
// printf("Solved: x=%.4f\n", sol[i]);
// }
// QUIT:
// /* Free environment */
// GRBfreeenv(env);
return u_optimal;
}
static int
dense_optimize(GRBenv *env,
int rows,
int cols,
double *c, /* linear portion of objective function */
double *Q, /* quadratic portion of objective function */
double *A, /* constraint matrix */
char *sense, /* constraint senses */
double *rhs, /* RHS vector */
double *lb, /* variable lower bounds */
double *ub, /* variable upper bounds */
char *vtype, /* variable types (continuous, binary, etc.) */
double *solution,
double *objvalP)
{
GRBmodel *model = NULL;
int i, j, optimstatus;
int error = 0;
int success = 0;
/* Create an empty model */
error = GRBnewmodel(env, &model, "dense", cols, c, lb, ub, vtype, NULL);
if (error) goto QUIT;
error = GRBaddconstrs(model, rows, 0, NULL, NULL, NULL, sense, rhs, NULL);
if (error) goto QUIT;
/* Populate A matrix */
for (i = 0; i < rows; i++) {
for (j = 0; j < cols; j++) {
if (A[i*cols+j] != 0) {
error = GRBchgcoeffs(model, 1, &i, &j, &A[i*cols+j]);
if (error) goto QUIT;
}
}
}
/* Populate Q matrix */
if (Q) {
for (i = 0; i < cols; i++) {
for (j = 0; j < cols; j++) {
if (Q[i*cols+j] != 0) {
error = GRBaddqpterms(model, 1, &i, &j, &Q[i*cols+j]);
if (error) goto QUIT;
}
}
}
}
/* Optimize model */
error = GRBoptimize(model);
if (error) goto QUIT;
/* Write model to 'dense.lp' */
error = GRBwrite(model, "dense.lp");
if (error) goto QUIT;
/* Capture solution information */
error = GRBgetintattr(model, GRB_INT_ATTR_STATUS, &optimstatus);
if (error) goto QUIT;
if (optimstatus == GRB_OPTIMAL) {
error = GRBgetdblattr(model, GRB_DBL_ATTR_OBJVAL, objvalP);
if (error) goto QUIT;
error = GRBgetdblattrarray(model, GRB_DBL_ATTR_X, 0, cols, solution);
if (error) goto QUIT;
success = 1;
}
QUIT:
/* Error reporting */
if (error) {
printf("ERROR: %s\n", GRBgeterrormsg(env));
exit(1);
}
/* Free model */
GRBfreemodel(model);
return success;
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";
return U_0;
}
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