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

Added first part of unconstrained MPC. Need to try it feeding it to gurobi

parent d1f634bc
......@@ -36,3 +36,6 @@ void funSxSuSg_varying_affine(std::vector<MatrixAtype> A_tray, std::vector<Matri
void funQbar(Eigen::MatrixXf Q, Eigen::MatrixXf P, int N, Eigen::MatrixXf &Qbar);
void funRbar(Eigen::MatrixXf R, int N, Eigen::MatrixXf &Rbar);
VectorUtype mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, std::vector<VectorXtype> g_tray, Eigen::MatrixXf Q, Eigen::MatrixXf R, Eigen::MatrixXf P, int N, VectorXtype x0, Eigen::VectorXf X_ref, Eigen::VectorXf U_ref, float ft_min, float ft_max, float angle_constraint);
......@@ -268,3 +268,47 @@ void funRbar(MatrixXf R, int N, MatrixXf &Rbar)
Rbar = R_bar;
}
VectorUtype mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, std::vector<VectorXtype> g_tray, MatrixXf Q, MatrixXf R, MatrixXf P, int N, VectorXtype x0, VectorXf X_ref, VectorXf U_ref, float ft_min, float ft_max, float angle_constraint)
{
Eigen::MatrixXf Sx;
Eigen::MatrixXf Su;
Eigen::MatrixXf Sg;
VectorUtype u_optimal;
funSxSuSg_varying_affine(A_tray, B_tray, N, Sx, Su, Sg);
VectorXf 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::MatrixXf Qbar;
Eigen::MatrixXf Rbar;
funQbar(Q, P, N, Qbar);
funRbar(R,N, Rbar);
Eigen::MatrixXf H;
Eigen::MatrixXf F;
H = Su.transpose()*Qbar*Su + Rbar;
F = 2*(x0.transpose()*Sx.transpose()*Qbar*Su + g_tray_col.transpose()*Sg.transpose()*Qbar*Su - X_ref.transpose()*Qbar*Su - U_ref.transpose()*Rbar);
// Eigen::IOFormat CleanFmt(4, 0, ", ", "\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;
return u_optimal;
}
......@@ -693,6 +693,8 @@ void setpointCallback(const Setpoint& newSetpoint)
Eigen::IOFormat OctaveFmt(Eigen::StreamPrecision, 0, ", ", ";\n", "", "", "[", "]");
Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
// std::cout << A.format(OctaveFmt) << std::endl;
// std::cout << B.format(OctaveFmt) << std::endl;
......@@ -757,6 +759,23 @@ void setpointCallback(const Setpoint& newSetpoint)
std::cout << "Rbar test:" << std::endl << std::endl;
// std::cout << Su_test<< std::endl;
std::cout << Rbar.format(CleanFmt) << std::endl;
Eigen::VectorXf X_ref(N_x*(N+1));
Eigen::VectorXf U_ref(N_u*N);
VectorXtype x_ref;
VectorUtype u_ref;
x_ref << 0,0,0,0,0,0;
u_ref << 0,0,0,params.m*params.g;
for(int i = 0; i < N+1; i++)
{
X_ref.segment(i*N_x, N_x) = x_ref;
if(i < N)
U_ref.segment(i*N_u, N_u) = u_ref;
}
mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0, X_ref, U_ref, 0, 0, 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