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

Fixed some small bugs, tested loop with only first MPC step, result is the same as with MATLAB

parent 2e84fe3c
......@@ -145,10 +145,9 @@ void funSxSuSg_varying_affine(std::vector<MatrixAtype> A_tray, std::vector<Matri
int N_B = B_tray.size();
if(N_A != N)
std::cout << "Error: number of A matrices hsould be equal to N"<< endl << endl;
std::cout << "Error: number of A matrices hsould be equal to N, however it is equal to: "<< N_A <<endl << endl;
if(N_B != N)
std::cout << "Error: number of A matrices hsould be equal to N"<< endl << endl;
std::cout << "Error: number of B matrices hsould be equal to N, however it is equal to: "<< N_B <<endl << endl;
// int r_A = A_tray[0].rows();
// int c_A = A_tray[0].cols();
// int r_A = B_tray[0].rows();
......
......@@ -65,6 +65,8 @@
#include "MPC_functions.h"
using namespace std;
// ----------------------------------------------------------------------------------
// DDDD EEEEE FFFFF III N N EEEEE SSSS
......@@ -268,7 +270,7 @@ void perform_estimator_update_state_interial(Controller::Request &request, float
void performEstimatorUpdate_forStateInterial_viaFiniteDifference(float (&stateInertialEstimate)[12]);
void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter(float (&stateInertialEstimate)[12]);
void initialize_MPC_variables(int N);
void initialize_MPC_variables(int N, VectorXtype x0);
// ----------------------------------------------------------------------------------
......@@ -520,7 +522,10 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
get_matrices_linearization_affine(X_tray, U_tray, params, A_tray, B_tray, g_tray);
// update the current input to the middle loop
raw_input[4] = {u(0), u(1), u(2), u(3)};
raw_input[0] = u(0);
raw_input[1] = u(1);
raw_input[2] = u(2);
raw_input[3] = u(3);
}
outer_loop_counter++;
......@@ -699,7 +704,7 @@ float computeMotorPolyBackward(float thrust)
}
Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
// ----------------------------------------------------------------------------------
......@@ -725,166 +730,247 @@ void setpointCallback(const Setpoint& newSetpoint)
setpoint[2] = newSetpoint.z;
setpoint[3] = newSetpoint.yaw;
// testzone:
// ================= MPC PART ==================
VectorXtype x_vec;
x_vec.setZero();
VectorUtype u_vec;
u_vec << 0, 0, 0, cf_mass/1000*9.81;
// Make sure we run it at 50Hz
MatrixAtype A;
MatrixBtype B;
// first, we have full state measurement stored in stateInertialEstimate.
// Apply setpoint change first, and store everything in x0 vector
MatrixAtype A_d;
MatrixBtype B_d;
VectorXtype g_d;
double stateInertialEstimate[12] = {0};
params.m = cf_mass/1000.0;
params.g = 9.81;
params.Ts = 1/50.0;
x0 << -stateInertialEstimate[0] + setpoint[0],
-stateInertialEstimate[1] + setpoint[1],
-stateInertialEstimate[2] + setpoint[2],
stateInertialEstimate[3],
stateInertialEstimate[4],
stateInertialEstimate[5];
initialize_MPC_variables(N, x0);
// linearization_fast_euler_6_states(x_vec, u_vec, params, A, B);
// discretization_affine(A, B, x_vec, u_vec, params, A_d, B_d, g_d);
// 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);
VectorUtype u = U_0.tail(4); // this is the input to apply next
std::vector<VectorXtype> X_tray;
std::vector<VectorUtype> U_tray;
// prediction of next N states:
VectorXtype x_approx_next;
std::vector<VectorXtype> x_approx_temp;
x_approx_next << x0;
for(int i = 0; i < 5; i++)
for(int j = 0; j < N; j++)
{
VectorXtype x_temp;
VectorUtype u_temp;
euler_method_forward(F_crazyflie_6_states, 0, params.Ts/4, params.Ts, x_approx_next, U_0.segment(j,N_u), params, x_approx_temp);
x_approx_next = x_approx_temp.back();
X_tray[j] = x_approx_next;
// Now update U_tray using the same loop
if(j < N-1)
U_tray[j] = U_0.segment(j+1, N_u);
else
U_tray[j] = U_0.segment(j, N_u); // last element of the new U_tray is assumed to be the same as second to last
}
// X_tray and U_tray have been completely updated with the new states coming from the new sequence of inputs
x_temp << i, 0, 0.4, 0, 0, 0;
u_temp << i, 0, 0, params.m*params.g;
// Relinearize around new trayectory:
get_matrices_linearization_affine(X_tray, U_tray, params, A_tray, B_tray, g_tray);
X_tray.push_back(x_temp);
U_tray.push_back(u_temp);
}
// update the current input to the middle loop
raw_input[0] = u(0);
raw_input[1] = u(1);
raw_input[2] = u(2);
raw_input[3] = u(3);
std::vector<MatrixAtype> A_tray;
std::vector<MatrixBtype> B_tray;
std::vector<VectorXtype> g_tray;
std::cout << "U_0 for this setpoint, 1 step MPC" << std::endl;
std::cout << U_0.format(CleanFmt) << std::endl;
get_matrices_linearization_affine(X_tray, U_tray, params, A_tray, B_tray, g_tray);
// testzone:
for(int i = 0; i < A_tray.size(); i++)
{
std::cout << "index matrix" << i << std::endl;
std::cout << A_tray[i] << std::endl;
}
// VectorXtype x_vec;
// x_vec.setZero();
// debug
Eigen::IOFormat OctaveFmt(Eigen::StreamPrecision, 0, ", ", ";\n", "", "", "[", "]");
Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
// VectorUtype u_vec;
// u_vec << 0, 0, 0, cf_mass/1000*9.81;
// MatrixAtype A;
// MatrixBtype B;
// MatrixAtype A_d;
// MatrixBtype B_d;
// VectorXtype g_d;
// std::cout << A.format(OctaveFmt) << std::endl;
// params.m = cf_mass/1000.0;
// params.g = 9.81;
// params.Ts = 1/50.0;
// std::cout << B.format(OctaveFmt) << std::endl;
// // linearization_fast_euler_6_states(x_vec, u_vec, params, A, B);
// // discretization_affine(A, B, x_vec, u_vec, params, A_d, B_d, g_d);
// debug of euler method function:
// std::vector<VectorXtype> X_tray;
// std::vector<VectorUtype> U_tray;
std::vector<VectorXtype> x_out;
VectorXtype x0;
VectorUtype u;
x0 << 0, 0, 0, 0, 0, 0;
u << 0, 0, 0, params.m*params.g*2;
// for(int i = 0; i < 5; i++)
// {
// VectorXtype x_temp;
// VectorUtype u_temp;
euler_method_forward(F_crazyflie_6_states, 0, params.Ts/4, params.Ts, x0, u, params, x_out);
// x_temp << i, 0, 0.4, 0, 0, 0;
// u_temp << i, 0, 0, params.m*params.g;
for(int i = 0; i < x_out.size(); i++)
{
std::cout << "xout index:" << i << std::endl;
std::cout << x_out[i] << std::endl;
}
// X_tray.push_back(x_temp);
// U_tray.push_back(u_temp);
// }
Eigen::MatrixXd Sx_test;
Eigen::MatrixXd Su_test;
Eigen::MatrixXd Sg_test;
int N = 5;
// std::vector<MatrixAtype> A_tray;
// std::vector<MatrixBtype> B_tray;
// std::vector<VectorXtype> g_tray;
funSxSuSg_varying_affine(A_tray, B_tray, N, Sx_test, Su_test, Sg_test);
// get_matrices_linearization_affine(X_tray, U_tray, params, A_tray, B_tray, g_tray);
std::cout << "Sx test:" << std::endl << std::endl;
std::cout << Sx_test<< std::endl;
// for(int i = 0; i < A_tray.size(); i++)
// {
// std::cout << "index matrix" << i << std::endl;
// std::cout << A_tray[i] << std::endl;
// }
std::cout << "Su test:" << std::endl << std::endl;
// std::cout << Su_test<< std::endl;
std::cout << Su_test.format(CleanFmt) << std::endl;
// // debug
// Eigen::IOFormat OctaveFmt(Eigen::StreamPrecision, 0, ", ", ";\n", "", "", "[", "]");
// Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
std::cout << "Sg test:" << std::endl << std::endl;
// std::cout << Su_test<< std::endl;
std::cout << Sg_test.format(CleanFmt) << std::endl;
Eigen::MatrixXd Qbar;
Eigen::MatrixXd Rbar;
VectorXtype p_vec;
p_vec << 3, 3, 3, 2, 2, 2;
VectorXtype q_vec;
q_vec << 1, 1, 1, 4, 4, 4;
// // std::cout << A.format(OctaveFmt) << std::endl;
VectorUtype r_vec;
r_vec << 1, 1, 1, 4;
// // std::cout << B.format(OctaveFmt) << std::endl;
Eigen::MatrixXd P = p_vec.asDiagonal();
Eigen::MatrixXd Q = q_vec.asDiagonal();
Eigen::MatrixXd R = r_vec.asDiagonal();
// // debug of euler method function:
funQbar(Q, P, N, Qbar);
// std::vector<VectorXtype> x_out;
// VectorXtype x0;
// VectorUtype u;
// x0 << 0, 0, 0, 0, 0, 0;
// u << 0, 0, 0, params.m*params.g*2;
std::cout << "Qbar test:" << std::endl << std::endl;
// std::cout << Su_test<< std::endl;
std::cout << Qbar.format(CleanFmt) << std::endl;
// euler_method_forward(F_crazyflie_6_states, 0, params.Ts/4, params.Ts, x0, u, params, x_out);
funRbar(R,N, Rbar);
// for(int i = 0; i < x_out.size(); i++)
// {
// std::cout << "xout index:" << i << std::endl;
// std::cout << x_out[i] << std::endl;
// }
std::cout << "Rbar test:" << std::endl << std::endl;
// std::cout << Su_test<< std::endl;
std::cout << Rbar.format(CleanFmt) << std::endl;
// Eigen::MatrixXd Sx_test;
// Eigen::MatrixXd Su_test;
// Eigen::MatrixXd Sg_test;
// int N = 5;
Eigen::VectorXd X_ref(N_x*(N+1));
Eigen::VectorXd U_ref(N_u*N);
// funSxSuSg_varying_affine(A_tray, B_tray, N, Sx_test, Su_test, Sg_test);
VectorXtype x_ref;
VectorUtype u_ref;
x_ref << 0,0,0,0,0,0;
u_ref << 0,0,0,params.m*params.g;
// std::cout << "Sx test:" << std::endl << std::endl;
// std::cout << Sx_test<< std::endl;
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;
}
// std::cout << "Su test:" << std::endl << std::endl;
// // std::cout << Su_test<< std::endl;
// std::cout << Su_test.format(CleanFmt) << std::endl;
// std::cout << "Sg test:" << std::endl << std::endl;
// // std::cout << Su_test<< std::endl;
// std::cout << Sg_test.format(CleanFmt) << std::endl;
// Eigen::MatrixXd Qbar;
// Eigen::MatrixXd Rbar;
// VectorXtype p_vec;
// p_vec << 3, 3, 3, 2, 2, 2;
// VectorXtype q_vec;
// q_vec << 1, 1, 1, 4, 4, 4;
// VectorUtype r_vec;
// r_vec << 1, 1, 1, 4;
// Eigen::MatrixXd P = p_vec.asDiagonal();
// Eigen::MatrixXd Q = q_vec.asDiagonal();
// Eigen::MatrixXd R = r_vec.asDiagonal();
// funQbar(Q, P, N, Qbar);
// std::cout << "Qbar test:" << std::endl << std::endl;
// // std::cout << Su_test<< std::endl;
// std::cout << Qbar.format(CleanFmt) << std::endl;
// funRbar(R,N, Rbar);
// std::cout << "Rbar test:" << std::endl << std::endl;
// // std::cout << Su_test<< std::endl;
// std::cout << Rbar.format(CleanFmt) << std::endl;
Eigen::VectorXd U_0 = mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0, X_ref, U_ref, 0, 0, 0);
// Eigen::VectorXd X_ref(N_x*(N+1));
// Eigen::VectorXd 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;
// }
// Eigen::VectorXd U_0 = mympc_varying_another(A_tray, B_tray, g_tray, Q, R, P, N, x0, X_ref, U_ref, 0, 0, 0);
}
void initialize_MPC_variables(int N)
void initialize_MPC_variables(int N, VectorXtype x0)
{
cout << "called initialize MPC variables" << endl;
// params:
params.m = cf_mass/1000.0;
params.g = 9.81;
params.Ts = 1/50.0;
// Initialize initial state, 6 states
x0 << -setpoint[0], -setpoint[1], -setpoint[2], 0, 0, 0;
VectorUtype u_temp;
u_temp << 0, 0, 0, params.m*params.g;
// X_tray and U_tray
X_tray.clear();
U_tray.clear();
for(int i = 0; i < N; i++)
{
X_tray.push_back(x0);
U_tray.push_back(u_temp);
}
cout << "filled X_tray and U_tray:" << endl;
cout << "X_tray:" << endl;
for(int i=0; i < X_tray.size(); i++)
cout << X_tray[i].format(CleanFmt) << endl;
cout << "U_tray:" << endl;
for(int i=0; i < U_tray.size(); i++)
cout << U_tray[i].format(CleanFmt) << endl;
// fill A_tray, B_tray, g_tray with initial values
get_matrices_linearization_affine(X_tray, U_tray, params, A_tray, B_tray, g_tray);
cout << "filled A_tray B_tray and g_tray:" << endl;
cout << "A_tray:" << endl;
for(int i=0; i < A_tray.size(); i++)
cout << A_tray[i].format(CleanFmt) << endl;
cout << "B_tray:" << endl;
for(int i=0; i < B_tray.size(); i++)
cout << B_tray[i].format(CleanFmt) << endl;
cout << "g_tray:" << endl;
for(int i=0; i < g_tray.size(); i++)
cout << g_tray[i].format(CleanFmt) << endl;
VectorXtype q_vec;
q_vec << 5000, 5000, 5000, 500, 500, 500;
VectorUtype r_vec;
......@@ -1246,8 +1332,6 @@ int main(int argc, char* argv[]) {
// in the "DEFINES" section at the top of this file.
ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
initialize_MPC_variables(N);
// Print out some information to the user.
ROS_INFO("[MPC CONTROLLER] Service ready :-)");
......
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