Skip to content
GitLab
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
Hide whitespace changes
Inline Side-by-side
pps_ws/src/d_fall_pps/src/MPC_functions.cpp
View file @ 67f64dcc
......@@ -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();
......
pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
View file @ 67f64dcc
......@@ -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 :-)");
......
Supports Markdown
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