added a lot of matrix functions that linearize/discretize/form the matrices...

added a lot of matrix functions that linearize/discretize/form the matrices linearized along a trayectory. Need to do next the SxSuSg function

pps_ws/src/d_fall_pps/include/MPC_functions.h

@@ -14,5 +14,14 @@ typedef Eigen::Matrix<float, N_x, N_u> MatrixBtype;
 typedef Eigen::Matrix<float, N_x, 1> VectorXtype;
 typedef Eigen::Matrix<float, N_u, 1> VectorUtype;
 
+typedef struct params_t {
+    float m;
+    float g;
+    float Ts;
+} params;
 
-void linearization_fast_euler_6_states(VectorXtype x_vec, VectorUtype u_vec, float cf_mass, MatrixAtype &A, MatrixBtype &B);
+
+void linearization_fast_euler_6_states(const VectorXtype x_vec, const VectorUtype u_vec, const params_t params, MatrixAtype &A, MatrixBtype &B);
+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);

pps_ws/src/d_fall_pps/param/Crazyflie.db

-7,PPS_CF01,0/0/2M/E7E7E7E701,0,1.56976,-0.10861,-0.2,2.79907,0.808677,1.2
+1,PPS_CF01,0/0/2M/E7E7E7E701,0,-0.62,-0.63,-0.2,0.7,0.83,1.2

pps_ws/src/d_fall_pps/param/MpcController.yaml

 # Mass of the crazyflie
-mass : 27
+mass : 32
 
 # Frequency of the controller, in hertz
 control_frequency : 200

pps_ws/src/d_fall_pps/src/MPC_functions.cpp

 #include "MPC_functions.h"
+#include <unsupported/Eigen/MatrixFunctions>
 
 using namespace Eigen;
 using namespace std;
 
 
-void linearization_fast_euler_6_states(VectorXtype x_vec, VectorUtype u_vec, float cf_mass, 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;
     A = MatrixAtype::Zero();
     A.topRightCorner(3,3) = Matrix3f::Identity();
 
@@ -23,8 +25,92 @@ void linearization_fast_euler_6_states(VectorXtype x_vec, VectorUtype u_vec, flo
     float s_gamma = sin(gamma);
 
     B = MatrixBtype::Zero();
-    B.row(3) << (ft*(c_gamma*s_alpha - c_alpha*s_beta*s_gamma))/cf_mass, (ft*c_alpha*c_beta*c_gamma)/cf_mass, (ft*(c_alpha*s_gamma - c_gamma*s_alpha*s_beta))/cf_mass,  (s_alpha*s_gamma + c_alpha*c_gamma*s_beta)/cf_mass;
-    B.row(4) << -(ft*(c_alpha*c_gamma + s_alpha*s_beta*s_gamma))/cf_mass, (ft*c_beta*c_gamma*s_alpha)/cf_mass, (ft*(s_alpha*s_gamma + c_alpha*c_gamma*s_beta))/cf_mass, -(c_alpha*s_gamma - c_gamma*s_alpha*s_beta)/cf_mass;
-    B.row(5) << -(ft*c_beta*s_gamma)/cf_mass,           -(ft*c_gamma*s_beta)/cf_mass,                                                                0,                                     (c_beta*c_gamma)/cf_mass;
+    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(4) << -(ft*(c_alpha*c_gamma + s_alpha*s_beta*s_gamma))/m, (ft*c_beta*c_gamma*s_alpha)/m, (ft*(s_alpha*s_gamma + c_alpha*c_gamma*s_beta))/m, -(c_alpha*s_gamma - c_gamma*s_alpha*s_beta)/m;
+    B.row(5) << -(ft*c_beta*s_gamma)/m,           -(ft*c_gamma*s_beta)/m,                                                                0,                                     (c_beta*c_gamma)/m;
 
 }
+
+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;
+
+    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));
+
+    float 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),
+                      c*(s_alpha*s_beta*c_gamma - c_alpha*s_gamma),
+                      c*(c_beta*c_gamma) - g;
+
+}
+
+
+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)
+{
+    VectorXtype g;
+    VectorXtype temp;
+
+    F_crazyflie_6_states(x_tray,u_tray, params, temp);
+
+    g = temp - A*x_tray - B*u_tray;
+
+    int c_A = A.cols();
+    int c_B = B.cols();
+
+    MatrixXf M(c_A + c_B + 1, c_A + c_B + 1);
+
+
+    M << A, B, g,
+        MatrixXf::Zero(c_B+1, c_A), MatrixXf::Zero(c_B+1, c_B), MatrixXf::Zero(c_B+1,1);
+
+    // std::cout << M << std::endl;
+
+    // % discretization
+    MatrixXf M_d;
+
+    M = M*params.Ts;
+    M_d = M.exp();
+
+
+    A_d = M_d.topLeftCorner(c_A, c_A);
+    B_d = M_d.block(0,c_A,c_A,c_B);
+    g_d = M_d.topRightCorner(c_A, 1);
+
+
+    // Eigen::IOFormat OctaveFmt(Eigen::StreamPrecision, 0, ", ", ";\n", "", "", "[", "]");
+    // std::cout << A_d.format(OctaveFmt) << std::endl << std::endl;
+    // std::cout << B_d.format(OctaveFmt) << std::endl << std::endl;
+    // std::cout << g_d.format(OctaveFmt) << std::endl << std::endl;
+}
+
+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)
+{
+    int N = X_tray.size();
+
+    A_tray.clear();
+    B_tray.clear();
+    g_tray.clear();
+
+    for(int i = 0; i < N; i++)
+    {
+        MatrixAtype A_temp;
+        MatrixBtype B_temp;
+        VectorXtype g_temp;
+
+        linearization_fast_euler_6_states(X_tray[i], U_tray[i], params, A_temp, B_temp);
+        discretization_affine(A_temp, B_temp, X_tray[i], U_tray[i], params, A_temp, B_temp, g_temp);
+
+        A_tray.push_back(A_temp);
+        B_tray.push_back(B_temp);
+        g_tray.push_back(g_temp);
+    }
+}

pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp

@@ -648,14 +648,54 @@ void setpointCallback(const Setpoint& newSetpoint)
     MatrixAtype A;
     MatrixBtype B;
 
-    linearization_fast_euler_6_states(x_vec, u_vec, cf_mass/1000, A, B);
+    MatrixAtype A_d;
+    MatrixBtype B_d;
+    VectorXtype g_d;
+
+
+    params_t params;
+
+    params.m = cf_mass/1000.0;
+    params.g = 9.81;
+    params.Ts = 1/50.0;
+
+    // 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);
+
+    std::vector<VectorXtype> X_tray;
+    std::vector<VectorUtype> U_tray;
+
+    for(int i = 0; i < 5; i++)
+    {
+        VectorXtype x_temp;
+        VectorUtype u_temp;
+
+        x_temp << i, 0, 0.4, 0, 0, 0;
+        u_temp << i, 0, 0, params.m*params.g;
+
+        X_tray.push_back(x_temp);
+        U_tray.push_back(u_temp);
+    }
+
+    std::vector<MatrixAtype> A_tray;
+    std::vector<MatrixBtype> B_tray;
+    std::vector<VectorXtype> g_tray;
+
+    get_matrices_linearization_affine(X_tray, U_tray, params, A_tray, B_tray, g_tray);
+
+    for(int i = 0; i < B_tray.size(); i++)
+    {
+        std::cout << "index matrix" << i << std::endl;
+        std::cout << B_tray[i] << std::endl;
+    }
 
     // debug
     Eigen::IOFormat OctaveFmt(Eigen::StreamPrecision, 0, ", ", ";\n", "", "", "[", "]");
 
-    std::cout << A.format(OctaveFmt) << std::endl;
+    // std::cout << A.format(OctaveFmt) << std::endl;
+
+    // std::cout << B.format(OctaveFmt) << std::endl;
 
-    std::cout << B.format(OctaveFmt) << std::endl;
 
 
 }