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

started with MPC_ADP

parent bf3ab2c3
......@@ -38,6 +38,7 @@ 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);
Eigen::VectorXd mympc_varying_another_ADP(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
......
......@@ -326,6 +326,106 @@ VectorXd mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<Matr
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::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);
// Eigen::VectorXd U_0 = solve_QP(H, F, Au, bu);
// Eigen::VectorXd U_0 = solve_QP(H, F);
return U_0;
}
VectorXd mympc_varying_another_ADP(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;
Eigen::MatrixXd Sg;
funSxSuSg_varying_affine(A_tray, B_tray, N, Sx, Su, Sg);
VectorXd g_tray_col(N*N_x);
for(int i = 0; i < g_tray.size(); i++)
{
g_tray_col.segment(i*N_x, N_x) = g_tray[i];
}
// std::cout << "g_tray_col" << std::endl;
// std::cout << g_tray_col << std::endl;
Eigen::MatrixXd Qbar_1;
Eigen::MatrixXd Rbar;
funQbar(Q, Q, N-1, Qbar_1);
funRbar(R,N, Rbar);
Eigen::MatrixXd H;
Eigen::MatrixXd F;
int size_1 = N_x*(N-1);
Eigen::MatrixXd Su_1 = Su.topRows(size_1);
Eigen::MatrixXd Sx_1 = Sx.topRows(size_1);
Eigen::MatrixXd Sg_1 = Sg.topRows(size_1);
Eigen::VectorXd X_ref_1 = X_ref.head(size_1);
Eigen::MatrixXd Su_N = Su.bottomRows(N_x);
Eigen::MatrixXd Sx_N = Sx.bottomRows(N_x);
Eigen::MatrixXd Sg_N = Sg.bottomRows(N_x);
VectorXtype X_ref_N = X_ref.tail(N_x);
H = Su_1.transpose()*Qbar_1*Su_1 + Rbar;
// in this case, F is a row vector
F = 2*(x0.transpose()*Sx_1.transpose()*Qbar_1*Su_1 + g_tray_col.transpose()*Sg_1.transpose()*Qbar_1*Su_1 - X_ref_1.transpose()*Qbar_1*Su_1 - U_ref.transpose()*Rbar);
// include epigraph in the matrices:
Eigen::MatrixXd H_final = Eigen::MatrixXd::Zero(H.rows() + 1, H.cols() + 1);
Eigen::MatrixXd F_final = Eigen::MatrixXd::Zero(F.rows(), F.cols() + 1);
H_final.topLeftCorner(H.rows(),H.cols()) << H;
F_final.rightCols(1) << 1.0;
Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
// std::cout << "H test:" << std::endl << std::endl;
// std::cout << H.format(CleanFmt) << std::endl;
// ft_min and ft_max constraints:
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;
......
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