Commit 25451e9a authored by roangel's avatar roangel
Browse files

fixed a small bug in prediction indexing

parent ead53ec7
...@@ -16,9 +16,9 @@ typedef Eigen::Matrix<double, N_u, 1> VectorUtype; ...@@ -16,9 +16,9 @@ typedef Eigen::Matrix<double, N_u, 1> VectorUtype;
typedef struct params_t { typedef struct params_t {
float m; double m;
float g; double g;
float Ts; double Ts;
} params_TEMPLATE; } params_TEMPLATE;
typedef void (*F_callback_type)(const VectorXtype x_vec, const VectorUtype u_vec, params_t params, VectorXtype &dx_vec); typedef void (*F_callback_type)(const VectorXtype x_vec, const VectorUtype u_vec, params_t params, VectorXtype &dx_vec);
...@@ -29,7 +29,7 @@ void linearization_fast_euler_6_states(const VectorXtype x_vec, const VectorUtyp ...@@ -29,7 +29,7 @@ void linearization_fast_euler_6_states(const VectorXtype x_vec, const VectorUtyp
void F_crazyflie_6_states(const VectorXtype x_vec, const VectorUtype u_vec, params_t params, VectorXtype &dx_vec); void F_crazyflie_6_states(const VectorXtype x_vec, const VectorUtype u_vec, params_t params, VectorXtype &dx_vec);
void discretization_affine(const MatrixAtype A, const MatrixBtype B, const VectorXtype x_tray, const VectorUtype u_tray, params_t params, MatrixAtype &A_d, MatrixBtype &B_d, VectorXtype &g_d); void discretization_affine(const MatrixAtype A, const MatrixBtype B, const VectorXtype x_tray, const VectorUtype u_tray, params_t params, MatrixAtype &A_d, MatrixBtype &B_d, VectorXtype &g_d);
void get_matrices_linearization_affine(std::vector<VectorXtype> X_tray, std::vector<VectorUtype> U_tray, params_t params, std::vector<MatrixAtype> &A_tray, std::vector<MatrixBtype> &B_tray, std::vector<VectorXtype> &g_tray); void get_matrices_linearization_affine(std::vector<VectorXtype> X_tray, std::vector<VectorUtype> U_tray, params_t params, std::vector<MatrixAtype> &A_tray, std::vector<MatrixBtype> &B_tray, std::vector<VectorXtype> &g_tray);
void euler_method_forward(F_callback_type pF, float t0, float h, float tfinal, VectorXtype y0, VectorUtype u, params_t params, std::vector<VectorXtype> &yout); void euler_method_forward(F_callback_type pF, double t0, double h, double tfinal, VectorXtype y0, VectorUtype u, params_t params, std::vector<VectorXtype> &yout);
void funSxSuSg_varying_affine(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, int N, Eigen::MatrixXd &Sx, Eigen::MatrixXd &Su, Eigen::MatrixXd &Sg); void funSxSuSg_varying_affine(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, int N, Eigen::MatrixXd &Sx, Eigen::MatrixXd &Su, Eigen::MatrixXd &Sg);
...@@ -37,8 +37,8 @@ void funQbar(Eigen::MatrixXd Q, Eigen::MatrixXd P, int N, Eigen::MatrixXd &Qbar) ...@@ -37,8 +37,8 @@ void funQbar(Eigen::MatrixXd Q, Eigen::MatrixXd P, int N, Eigen::MatrixXd &Qbar)
void funRbar(Eigen::MatrixXd R, int N, Eigen::MatrixXd &Rbar); 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(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, double ft_min, double ft_max, double 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); 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, double ft_min, double ft_max, double af);
// function overload for solver // function overload for solver
// quadratically constrained solver // quadratically constrained solver
......
...@@ -12,22 +12,22 @@ using namespace std; ...@@ -12,22 +12,22 @@ using namespace std;
void linearization_fast_euler_6_states(const VectorXtype x_vec, const VectorUtype u_vec, params_t params, MatrixAtype &A, MatrixBtype &B) void linearization_fast_euler_6_states(const VectorXtype x_vec, const VectorUtype u_vec, params_t params, MatrixAtype &A, MatrixBtype &B)
{ {
float m = params.m; double m = params.m;
A = MatrixAtype::Zero(); A = MatrixAtype::Zero();
A.topRightCorner(3,3) = Matrix3d::Identity(); A.topRightCorner(3,3) = Matrix3d::Identity();
float gamma = u_vec(0); double gamma = u_vec(0);
float beta = u_vec(1); double beta = u_vec(1);
float alpha = u_vec(2); double alpha = u_vec(2);
float ft = u_vec(3); double ft = u_vec(3);
float c_alpha = cos(alpha); double c_alpha = cos(alpha);
float s_alpha = sin(alpha); double s_alpha = sin(alpha);
float s_beta = sin(beta); double s_beta = sin(beta);
float c_beta = cos(beta); double c_beta = cos(beta);
float c_gamma = cos(gamma); double c_gamma = cos(gamma);
float s_gamma = sin(gamma); double s_gamma = sin(gamma);
B = MatrixBtype::Zero(); B = MatrixBtype::Zero();
B.row(3) << (ft*(c_gamma*s_alpha - c_alpha*s_beta*s_gamma))/m, (ft*c_alpha*c_beta*c_gamma)/m, (ft*(c_alpha*s_gamma - c_gamma*s_alpha*s_beta))/m, (s_alpha*s_gamma + c_alpha*c_gamma*s_beta)/m; B.row(3) << (ft*(c_gamma*s_alpha - c_alpha*s_beta*s_gamma))/m, (ft*c_alpha*c_beta*c_gamma)/m, (ft*(c_alpha*s_gamma - c_gamma*s_alpha*s_beta))/m, (s_alpha*s_gamma + c_alpha*c_gamma*s_beta)/m;
...@@ -39,17 +39,17 @@ void linearization_fast_euler_6_states(const VectorXtype x_vec, const VectorUtyp ...@@ -39,17 +39,17 @@ void linearization_fast_euler_6_states(const VectorXtype x_vec, const VectorUtyp
void F_crazyflie_6_states(const VectorXtype x_vec, const VectorUtype u_vec, params_t params, VectorXtype &dx_vec) void F_crazyflie_6_states(const VectorXtype x_vec, const VectorUtype u_vec, params_t params, VectorXtype &dx_vec)
{ {
float m = params.m; double m = params.m;
float g = params.g; double g = params.g;
float c_alpha = cos(u_vec(2)); double c_alpha = cos(u_vec(2));
float s_alpha = sin(u_vec(2)); double s_alpha = sin(u_vec(2));
float s_beta = sin(u_vec(1)); double s_beta = sin(u_vec(1));
float c_beta = cos(u_vec(1)); double c_beta = cos(u_vec(1));
float c_gamma = cos(u_vec(0)); double c_gamma = cos(u_vec(0));
float s_gamma = sin(u_vec(0)); double s_gamma = sin(u_vec(0));
float c = u_vec(3)/m; double c = u_vec(3)/m;
dx_vec.head(3) = x_vec.tail(3); dx_vec.head(3) = x_vec.tail(3);
dx_vec.tail(3) << c*(c_alpha*s_beta*c_gamma + s_alpha*s_gamma), dx_vec.tail(3) << c*(c_alpha*s_beta*c_gamma + s_alpha*s_gamma),
...@@ -122,14 +122,14 @@ void get_matrices_linearization_affine(std::vector<VectorXtype> X_tray, std::vec ...@@ -122,14 +122,14 @@ void get_matrices_linearization_affine(std::vector<VectorXtype> X_tray, std::vec
void euler_method_forward(F_callback_type pF, float t0, float h, float tfinal, VectorXtype y0, VectorUtype u, params_t params, std::vector<VectorXtype> &yout) void euler_method_forward(F_callback_type pF, double t0, double h, double tfinal, VectorXtype y0, VectorUtype u, params_t params, std::vector<VectorXtype> &yout)
{ {
VectorXtype y = y0; VectorXtype y = y0;
VectorXtype y_next; VectorXtype y_next;
std::vector<VectorXtype> yout_temp; std::vector<VectorXtype> yout_temp;
yout_temp.push_back(y); yout_temp.push_back(y);
for(float t = t0; t < tfinal; t = t + h) for(double t = t0; t < tfinal; t = t + h)
{ {
pF(y, u, params, y_next); pF(y, u, params, y_next);
...@@ -271,7 +271,7 @@ void funRbar(MatrixXd R, int N, MatrixXd &Rbar) ...@@ -271,7 +271,7 @@ void funRbar(MatrixXd R, int N, MatrixXd &Rbar)
Rbar = R_bar; Rbar = R_bar;
} }
VectorXd mympc_varying_another(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) VectorXd mympc_varying_another(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::MatrixXd Sx; Eigen::MatrixXd Sx;
Eigen::MatrixXd Su; Eigen::MatrixXd Su;
...@@ -350,7 +350,7 @@ VectorXd mympc_varying_another(std::vector<MatrixAtype> A_tray, std::vector<Matr ...@@ -350,7 +350,7 @@ 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, float ft_min, float ft_max, float af) 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::MatrixXd Sx; Eigen::MatrixXd Sx;
Eigen::MatrixXd Su; Eigen::MatrixXd Su;
......
...@@ -196,12 +196,9 @@ params_t params; ...@@ -196,12 +196,9 @@ params_t params;
VectorXtype x0; VectorXtype x0;
std::atomic<std::array<float,N_u>> shared_raw_input;
std::atomic<array<float,N_x>> shared_x0;
std::mutex MPC_mutex; std::mutex MPC_mutex;
int N = 15; int N = 10;
// ROS Publisher for debugging variables // ROS Publisher for debugging variables
...@@ -407,7 +404,7 @@ void initialize_MPC_variables(int N, VectorXtype x0); ...@@ -407,7 +404,7 @@ void initialize_MPC_variables(int N, VectorXtype x0);
// //
// This function WILL NEED TO BE edited for successful completion of the PPS exercise // This function WILL NEED TO BE edited for successful completion of the PPS exercise
float saturateThrust(float input) double saturateThrust(double input)
{ {
float out = input; float out = input;
if(input > 60000) if(input > 60000)
...@@ -520,7 +517,7 @@ void do_MPC(int* publish_rate) ...@@ -520,7 +517,7 @@ void do_MPC(int* publish_rate)
for(int j = 0; j < N; j++) for(int j = 0; j < N; j++)
{ {
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); euler_method_forward(F_crazyflie_6_states, 0, params.Ts/4, params.Ts, x_approx_next, U_0.segment(N_u*j,N_u), params, x_approx_temp);
x_approx_next = x_approx_temp.back(); x_approx_next = x_approx_temp.back();
MPC_mutex.lock(); MPC_mutex.lock();
...@@ -782,23 +779,13 @@ void setpointCallback(const Setpoint& newSetpoint) ...@@ -782,23 +779,13 @@ void setpointCallback(const Setpoint& newSetpoint)
stateInertialEstimate[4], stateInertialEstimate[4],
stateInertialEstimate[5]; stateInertialEstimate[5];
std::array<float, N_x> x0_temp;
x0_temp[0] = x0(0);
x0_temp[1] = x0(1);
x0_temp[2] = x0(2);
x0_temp[3] = x0(3);
x0_temp[4] = x0(4);
x0_temp[5] = x0(5);
shared_x0.store(x0_temp, std::memory_order_release);
initialize_MPC_variables(N, x0); initialize_MPC_variables(N, x0);
// clock_t begin_time = clock(); // clock_t begin_time = clock();
// // Do the MPC step: // Do the MPC step:
// x0 << - setpoint[0], // x0 << - setpoint[0],
// - setpoint[1], // - setpoint[1],
...@@ -820,7 +807,18 @@ void setpointCallback(const Setpoint& newSetpoint) ...@@ -820,7 +807,18 @@ void setpointCallback(const Setpoint& newSetpoint)
// for(int j = 0; j < N; j++) // for(int j = 0; j < N; j++)
// { // {
// 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); // euler_method_forward(F_crazyflie_6_states, 0, params.Ts/4, params.Ts, x_approx_next, U_0.segment(N_u*j,N_u), params, x_approx_temp);
// // std::cout << "for x_0 as input :" << std::endl;
// // std::cout << x_approx_next.format(CleanFmt);
// // std::cout << "and U as input :" << std::endl;
// // std::cout << U_0.segment(N_u*j,N_u).format(CleanFmt) << std::endl;
// // for(int i = 0; i < x_approx_temp.size(); i++)
// // {
// // std::cout << "x_approx_temp i = " << i << std::endl;
// // std::cout << x_approx_temp[i].format(CleanFmt) << std::endl << std::endl;
// // }
// x_approx_next = x_approx_temp.back(); // x_approx_next = x_approx_temp.back();
// X_tray[j] = x_approx_next; // X_tray[j] = x_approx_next;
......
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