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

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
parent 5e2e11c9
......@@ -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);
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
# Mass of the crazyflie
mass : 27
mass : 32
# Frequency of the controller, in hertz
control_frequency : 200
......
#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);
}
}
......@@ -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;
}
......
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