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

debugged the creation of ADP related matrices. Next step, put it inside GUROBI

parent bc09e0aa
# Mass of the crazyflie
mass : 30
mass : 32
# Frequency of the controller, in hertz
control_frequency : 200
......
#include "MPC_functions.h"
#include "ADP_matrices.h"
#include <unsupported/Eigen/MatrixFunctions>
#include <unsupported/Eigen/KroneckerProduct>
......@@ -353,6 +354,9 @@ VectorXd mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<Matr
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, double ft_min, double ft_max, double af)
{
Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
Eigen::MatrixXd Sx;
Eigen::MatrixXd Su;
Eigen::MatrixXd Sg;
......@@ -378,7 +382,7 @@ VectorXd mympc_varying_another_ADP(std::vector<MatrixAtype> A_tray, std::vector<
Eigen::MatrixXd H;
Eigen::MatrixXd F;
int size_1 = N_x*(N-1);
int size_1 = N_x*N;
Eigen::MatrixXd Su_1 = Su.topRows(size_1);
Eigen::MatrixXd Sx_1 = Sx.topRows(size_1);
......@@ -392,21 +396,49 @@ VectorXd mympc_varying_another_ADP(std::vector<MatrixAtype> A_tray, std::vector<
VectorXtype X_ref_N = X_ref.tail(N_x);
// std::cout << "computing H" << std::endl;
// std::cout << "Q_bar_1 test:" << std::endl << std::endl;
// std::cout << Qbar_1.format(CleanFmt) << std::endl;
// std::cout << "size Qbar_1" << std::endl;
// std::cout << Qbar_1.rows() << "," << Qbar_1.cols() << std::endl;
// std::cout << "Su_1 test:" << std::endl << std::endl;
// std::cout << Su_1.format(CleanFmt) << std::endl;
// std::cout << "size Su_1" << std::endl;
// std::cout << Su_1.rows() << "," << Su_1.cols() << std::endl;
H = Su_1.transpose()*Qbar_1*Su_1 + Rbar;
std::cout << "H:" << std::endl;
std::cout << H.format(CleanFmt) << std::endl;
// in this case, F is a row vector
std::cout << "computing F" << std::endl;
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);
std::cout << "F:" << std::endl;
std::cout << F.format(CleanFmt) << std::endl;
// 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;
std::cout << "H_final:" << std::endl;
std::cout << H_final.format(CleanFmt) << std::endl;
F_final.leftCols(N*N_u) << F;
F_final.leftCols(F.cols()) << F;
F_final.rightCols(1) << 1.0;
Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
std::cout << "F_final:" << std::endl;
std::cout << F_final.format(CleanFmt) << std::endl;
// std::cout << "H test:" << std::endl << std::endl;
// std::cout << H.format(CleanFmt) << std::endl;
......@@ -428,12 +460,136 @@ VectorXd mympc_varying_another_ADP(std::vector<MatrixAtype> A_tray, std::vector<
MatrixXd Au = kroneckerProduct(Au_kernel, pattern);
MatrixXd Au_final = Eigen::MatrixXd::Zero(Au.rows(), Au.cols()+1);
Au_final.leftCols(Au.cols()) << Au;
std::cout << "Au_final:" << std::endl;
std::cout << Au_final.format(CleanFmt) << std::endl;
VectorXd bu(2*N);
bu.topRows(N) = VectorXd::Ones(N)*ft_max;
bu.bottomRows(N) = VectorXd::Ones(N)*(-ft_min);
// ADP part
// create and fill the vectors of lind, lval, qrows, qcols, qvals
Eigen::MatrixXd H_V(N_u*N, N_u*N);
Eigen::MatrixXd F_V(1, N_u*N);
Eigen::MatrixXd q_v(1,1);
Eigen::MatrixXd temp_q(1,1);
std::vector<MatrixXd> Qs_adp;
std::vector<MatrixXd> qs_adp;
std::vector<double> rhs_adp;
std::cout << "Su_N:" << std::endl;
std::cout << Su_N.format(CleanFmt) << std::endl;
std::cout << "Sx_N:" << std::endl;
std::cout << Sx_N.format(CleanFmt) << std::endl;
std::cout << "Sg_N:" << std::endl;
std::cout << Sg_N.format(CleanFmt) << std::endl;
std::cout << "g_tray_col:" << std::endl;
std::cout << g_tray_col.format(CleanFmt) << std::endl;
std::cout << "X_ref_N:" << std::endl;
std::cout << X_ref_N.format(CleanFmt) << std::endl;
std::cout << "entered adp part" << std::endl;
for(int i = 0; i < N_V_fun; i++)
{
std::cout << "V_functions_P[i]:" << std::endl;
std::cout << V_functions_P[i].format(CleanFmt) << std::endl;
std::cout << "V_functions_p[i]:" << std::endl;
std::cout << V_functions_p[i].format(CleanFmt) << std::endl;
std::cout << "V_functions_q[i]:" << std::endl;
std::cout << V_functions_q[i] << std::endl;
temp_q << V_functions_q[i];
std::cout<< "compuing HV" << std::endl;
H_V = Su_N.transpose()*V_functions_P[i]*Su_N;
std::cout<< "compuing FV" << std::endl;
std::cout<< "x0" << std::endl;
std::cout << x0.format(CleanFmt) << std::endl;
std::cout<< "compuing FV part 1" << std::endl;
F_V = 2*x0.transpose()*Sx_N.transpose()*V_functions_P[i]*Su_N;
std::cout << F_V.format(CleanFmt) << std::endl;
std::cout<< "compuing FV part 2" << std::endl;
F_V = 2*g_tray_col.transpose()*Sg_N.transpose()*V_functions_P[i]*Su_N;
std::cout << F_V.format(CleanFmt) << std::endl;
std::cout<< "compuing FV part 3" << std::endl;
F_V = V_functions_p[i].transpose()*Su_N;
std::cout << F_V.format(CleanFmt) << std::endl;
std::cout<< "compuing FV part 4" << std::endl;
F_V = 2*X_ref_N.transpose()*V_functions_P[i]*Su_N;
std::cout << F_V.format(CleanFmt) << std::endl;
F_V = 2*x0.transpose()*Sx_N.transpose()*V_functions_P[i]*Su_N
+ 2*g_tray_col.transpose()*Sg_N.transpose()*V_functions_P[i]*Su_N
+ V_functions_p[i].transpose()*Su_N - 2*X_ref_N.transpose()*V_functions_P[i]*Su_N;
std::cout<< "compuing qV" << std::endl;
q_v = x0.transpose()*Sx_N.transpose()*V_functions_P[i]*Sx_N*x0
+ g_tray_col.transpose()*Sg_N.transpose()*V_functions_P[i]*Sg_N*g_tray_col
+ 2*x0.transpose()*Sx_N.transpose()*V_functions_P[i]*Sg_N*g_tray_col
+ V_functions_p[i].transpose()*Sx_N*x0 + V_functions_p[i].transpose()*Sg_N*g_tray_col
+ temp_q - 2*x0.transpose()*Sx_N.transpose()*V_functions_P[i]*X_ref_N
- 2*X_ref_N.transpose()*V_functions_P[i]*Sg_N*g_tray_col - V_functions_p[i].transpose()*X_ref_N;
Eigen::MatrixXd Q_temp = MatrixXd::Zero(H_V.rows() + 1, H_V.cols() + 1);
Q_temp.topLeftCorner(H_V.rows(), H_V.cols()) << H_V;
Q_temp << Q_temp + (MatrixXd::Identity(Q_temp.rows(), Q_temp.cols())*1e-12);
Qs_adp.push_back(Q_temp);
Eigen::MatrixXd q_temp = MatrixXd::Zero(F_V.rows(), F_V.cols() + 1);
q_temp.leftCols(F_V.cols()) << F_V;
q_temp.rightCols(1) << -1;
qs_adp.push_back(q_temp);
rhs_adp.push_back(-q_v.value());
}
std::cout << "Qs_adp_test:" << std::endl << std::endl;
for(int i = 0; i < Qs_adp.size(); i++)
{
std::cout << "index = " << i << std::endl;
std::cout << Qs_adp[i].format(CleanFmt) << std::endl << std::endl;
}
std::cout << "qs_adp_test:" << std::endl << std::endl;
for(int i = 0; i < qs_adp.size(); i++)
{
std::cout << "index = " << i << std::endl;
std::cout << qs_adp[i].format(CleanFmt) << std::endl<< std::endl;
}
std::cout << "qs_adp_test:" << std::endl << std::endl;
for(int i = 0; i < rhs_adp.size(); i++)
{
std::cout << "index = " << i << std::endl;
std::cout << rhs_adp[i] << std::endl << std::endl;
}
// std::cout << "Au test:" << std::endl << std::endl;
// std::cout << Au.format(CleanFmt) << std::endl;
......
......@@ -199,7 +199,7 @@ VectorXtype x0;
std::mutex MPC_mutex;
int N = 8;
int N = 4;
// ROS Publisher for debugging variables
......@@ -783,9 +783,10 @@ void setpointCallback(const Setpoint& newSetpoint)
stateInertialEstimate[5];
MPC_mutex.unlock();
initialize_MPC_variables(N, x0);
U_0 = mympc_varying_another_ADP(A_tray, B_tray, g_tray, Q, R, P, N, x0, X_ref, U_ref, 0, 0.1391*4, 0.7887);
// debug
// fill_V_function_variables();
// std::cout << "V_functions_P[0]" << 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