Commit 7819f1e2 authored by roangel's avatar roangel
Browse files

Added linear constraints in thrust

parent 030d7e2e
...@@ -307,20 +307,36 @@ VectorXd mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<Matr ...@@ -307,20 +307,36 @@ VectorXd mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<Matr
Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]"); Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
// std::cout << "H test:" << std::endl << std::endl; // std::cout << "H test:" << std::endl << std::endl;
// // std::cout << Su_test<< std::endl;
// std::cout << H.format(CleanFmt) << std::endl; // std::cout << H.format(CleanFmt) << std::endl;
// ft_min and ft_max constraints: // ft_min and ft_max constraints:
RowVectorXd pattern; RowVectorXd pattern(4);
pattern << 0, 0, 0, 1; pattern << 0, 0, 0, 1;
// std::cout << "pattern test:" << std::endl << std::endl;
// std::cout << pattern.format(CleanFmt) << std::endl;
MatrixXd Au_kernel(2*N, N); MatrixXd Au_kernel(2*N, N);
Au_kernel.topRows(N) = MatrixXd::Identity(N,N); Au_kernel.topRows(N) = MatrixXd::Identity(N,N);
Au_kernel.bottomRows(N) = (-1)*MatrixXd::Identity(N,N); Au_kernel.bottomRows(N) = (-1)*MatrixXd::Identity(N,N);
// std::cout << "Au_kernel test:" << std::endl << std::endl;
// std::cout << Au_kernel.format(CleanFmt) << std::endl;
MatrixXd Au = kroneckerProduct(Au_kernel, pattern); MatrixXd Au = kroneckerProduct(Au_kernel, pattern);
VectorXd bu(2*N); VectorXd bu(2*N);
bu.topRows(N) = VectorXd::Ones(N)*ft_max; bu.topRows(N) = VectorXd::Ones(N)*ft_max;
bu.bottomRows(N) = VectorXd::Ones(N)*(-ft_min); bu.bottomRows(N) = VectorXd::Ones(N)*(-ft_min);
// std::cout << "Au test:" << std::endl << std::endl;
// std::cout << Au.format(CleanFmt) << std::endl;
// std::cout << "bu test:" << std::endl << std::endl;
// std::cout << bu.format(CleanFmt) << std::endl;
Eigen::VectorXd U_0 = solve_QP(H, F, Au, bu); Eigen::VectorXd U_0 = solve_QP(H, F, Au, bu);
return U_0; return U_0;
...@@ -332,6 +348,8 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen ...@@ -332,6 +348,8 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen
// min(x'Hx + F'x) // min(x'Hx + F'x)
int size_H = H.cols(); int size_H = H.cols();
int cols_A = A.cols();
int rows_A = A.rows();
std::vector<int> q_row; std::vector<int> q_row;
std::vector<int> q_col; std::vector<int> q_col;
...@@ -385,6 +403,10 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen ...@@ -385,6 +403,10 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen
std::vector<double> ub(N_vars, GRB_INFINITY); std::vector<double> ub(N_vars, GRB_INFINITY);
std::vector<double> lb(N_vars, -GRB_INFINITY); std::vector<double> lb(N_vars, -GRB_INFINITY);
std::vector<int> l_ind;
std::vector<double> l_coeff;
/* Create environment */ /* Create environment */
error = GRBloadenv(&env, "qp.log"); error = GRBloadenv(&env, "qp.log");
if (error) goto QUIT; if (error) goto QUIT;
...@@ -401,6 +423,22 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen ...@@ -401,6 +423,22 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen
error = GRBsetintparam(GRBgetenv(model), "OutputFlag", 0); error = GRBsetintparam(GRBgetenv(model), "OutputFlag", 0);
if (error) goto QUIT; 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 // Optimize model
error = GRBoptimize(model); error = GRBoptimize(model);
......
...@@ -536,7 +536,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -536,7 +536,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
stateInertialEstimate[5]; stateInertialEstimate[5];
// Do the MPC step: // 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); 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);
VectorUtype u = U_0.tail(4); // this is the input to apply next VectorUtype u = U_0.tail(4); // this is the input to apply next
// prediction of next N states: // prediction of next N states:
...@@ -799,12 +799,12 @@ void setpointCallback(const Setpoint& newSetpoint) ...@@ -799,12 +799,12 @@ void setpointCallback(const Setpoint& newSetpoint)
// x0 << - setpoint[0], // x0 << - setpoint[0],
// - setpoint[1], // - setpoint[1],
// 0, // - setpoint[2],
// 0, // 0,
// 0, // 0,
// 0; // 0;
// U_0 = mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0, X_ref, U_ref, 0, 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);
// VectorUtype u = U_0.tail(4); // this is the input to apply next // 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; // std::cout << "U_0 opt for this setpoint, 1 step MPC" << std::endl;
......
Supports Markdown
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