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 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;
typedef struct params_t {
float m;
float g;
float Ts;
double m;
double g;
double Ts;
} params_TEMPLATE;
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
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 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);
......@@ -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);
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_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(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, double ft_min, double ft_max, double af);
// function overload for solver
// quadratically constrained solver
......
......@@ -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)
{
float m = params.m;
double m = params.m;
A = MatrixAtype::Zero();
A.topRightCorner(3,3) = Matrix3d::Identity();
float gamma = u_vec(0);
float beta = u_vec(1);
float alpha = u_vec(2);
float ft = u_vec(3);
double gamma = u_vec(0);
double beta = u_vec(1);
double alpha = u_vec(2);
double ft = u_vec(3);
float c_alpha = cos(alpha);
float s_alpha = sin(alpha);
double c_alpha = cos(alpha);
double s_alpha = sin(alpha);
float s_beta = sin(beta);
float c_beta = cos(beta);
float c_gamma = cos(gamma);
float s_gamma = sin(gamma);
double s_beta = sin(beta);
double c_beta = cos(beta);
double c_gamma = cos(gamma);
double s_gamma = sin(gamma);
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;
......@@ -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)
{
float m = params.m;
float g = params.g;
double m = params.m;
double g = params.g;
float c_alpha = cos(u_vec(2));
float s_alpha = sin(u_vec(2));
float s_beta = sin(u_vec(1));
float c_beta = cos(u_vec(1));
float c_gamma = cos(u_vec(0));
float s_gamma = sin(u_vec(0));
double c_alpha = cos(u_vec(2));
double s_alpha = sin(u_vec(2));
double s_beta = sin(u_vec(1));
double c_beta = cos(u_vec(1));
double c_gamma = cos(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.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
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_next;
std::vector<VectorXtype> yout_temp;
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);
......@@ -271,7 +271,7 @@ void funRbar(MatrixXd R, int N, MatrixXd &Rbar)
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 Su;
......@@ -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 Su;
......
......@@ -196,12 +196,9 @@ params_t params;
VectorXtype x0;
std::atomic<std::array<float,N_u>> shared_raw_input;
std::atomic<array<float,N_x>> shared_x0;
std::mutex MPC_mutex;
int N = 15;
int N = 10;
// ROS Publisher for debugging variables
......@@ -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
float saturateThrust(float input)
double saturateThrust(double input)
{
float out = input;
if(input > 60000)
......@@ -520,7 +517,7 @@ void do_MPC(int* publish_rate)
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();
MPC_mutex.lock();
......@@ -782,23 +779,13 @@ void setpointCallback(const Setpoint& newSetpoint)
stateInertialEstimate[4],
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);
// clock_t begin_time = clock();
// // Do the MPC step:
// Do the MPC step:
// x0 << - setpoint[0],
// - setpoint[1],
......@@ -820,7 +807,18 @@ void setpointCallback(const Setpoint& newSetpoint)
// 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_tray[j] = x_approx_next;
......
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