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 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
Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
// std::cout << "H test:" << std::endl << std::endl;
// // std::cout << Su_test<< std::endl;
// std::cout << H.format(CleanFmt) << std::endl;
// ft_min and ft_max constraints:
RowVectorXd pattern;
RowVectorXd pattern(4);
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);
Au_kernel.topRows(N) = 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);
VectorXd bu(2*N);
bu.topRows(N) = VectorXd::Ones(N)*ft_max;
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);
return U_0;
......@@ -332,6 +348,8 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen
// 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;
......@@ -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> 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;
......@@ -401,6 +423,22 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen
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);
......
......@@ -536,7 +536,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
stateInertialEstimate[5];
// 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
// prediction of next N states:
......@@ -799,12 +799,12 @@ void setpointCallback(const Setpoint& newSetpoint)
// x0 << - setpoint[0],
// - setpoint[1],
// 0,
// - setpoint[2],
// 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
// 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