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

added quadratic constraints. Now we notice how the delay affects the system,...

added quadratic constraints. Now we notice how the delay affects the system, we dont have time to solve the problem
parent 7819f1e2
......@@ -37,5 +37,5 @@ 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 angle_constraint);
Eigen::VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen::VectorXd b);
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);
Eigen::VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen::VectorXd b, double af);
......@@ -271,7 +271,7 @@ void funRbar(MatrixXd R, int N, MatrixXd &Rbar)
Rbar = R_bar;
}
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)
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 af)
{
Eigen::MatrixXd Sx;
Eigen::MatrixXd Su;
......@@ -337,12 +337,17 @@ VectorXd mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<Matr
// 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::MatrixXd W = MatrixXd::Zero(4,4);
// W(1,1) = 1/(af^2);
// W(2,2) = 1/(af^2);
Eigen::VectorXd U_0 = solve_QP(H, F, Au, bu, af);
return U_0;
}
VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen::VectorXd b)
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:
// min(x'Hx + F'x)
......@@ -406,6 +411,14 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen
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");
......@@ -439,6 +452,25 @@ VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F, Eigen::MatrixXd A, Eigen
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);
......
......@@ -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.1391*4, 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.7887);
VectorUtype u = U_0.tail(4); // this is the input to apply next
// prediction of next N states:
......@@ -804,7 +804,7 @@ void setpointCallback(const Setpoint& newSetpoint)
// 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);
// 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.7887);
// 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