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

added hardcoded value function matrices. Need to test for runtime seg faults

parent 25451e9a
......@@ -288,7 +288,7 @@ add_executable(PPSClient src/nodes/PPSClient.cpp)
add_executable(SafeControllerService src/nodes/SafeControllerService.cpp)
add_executable(DemoControllerService src/nodes/DemoControllerService.cpp)
add_executable(StudentControllerService src/nodes/StudentControllerService.cpp)
add_executable(MpcControllerService src/nodes/MpcControllerService.cpp src/MPC_functions.cpp)
add_executable(MpcControllerService src/nodes/MpcControllerService.cpp src/MPC_functions.cpp src/ADP_matrices.cpp)
add_executable(CentralManagerService src/nodes/CentralManagerService.cpp src/CrazyflieIO.cpp)
add_executable(ParameterService src/nodes/ParameterService.cpp)
......
#pragma once
#include <Eigen/Dense>
#include "MPC_functions.h"
#define N_V_fun 9
std::array<Eigen::Matrix<double, N_x, N_x>, N_V_fun> V_functions_P;
std::array<Eigen::Matrix<double, N_x, 1>, N_V_fun> V_functions_p;
std::array<double, N_V_fun> V_functions_q;
void fill_V_function_variables();
#include "ADP_matrices.h"
void fill_V_function_variables()
{
// loaded value functions: 45 deg constraint, 0.02s Ts, 0.98 gamma
// first value function: 0.01, 0.1 sigma
V_functions_P[0] << 95894.043204, -0.000000, 0.000000, 11762.945358, 0.000000, 0.000000,
-0.000000, 95895.336458, -0.000000, 0.000000, 11763.233619, 0.000000,
0.000000, -0.000000, 111507.101343, 0.000000, 0.000000, 21340.261640,
11762.945358, 0.000000, 0.000000, 5993.674048, 0.000000, 0.000000,
0.000000, 11763.233619, 0.000000, 0.000000, 5993.894503, -0.000000,
0.000000, 0.000000, 21340.261640, 0.000000, -0.000000, 13242.175033;
V_functions_p[0] << -0.000000,
0.000000,
-153.090757,
-0.000000,
0.000000,
-140.633098;
V_functions_q[0] = -0.902164;
// 2nd value function: 0.001, 0.01
V_functions_P[1] << 95759.720373, -0.000000, 0.000000, 11708.900086, -0.000000, -0.000000,
-0.000000, 95760.889519, -0.000000, -0.000000, 11709.205738, -0.000000,
0.000000, -0.000000, 111285.307369, -0.000000, 0.000000, 21222.414800,
11708.900086, -0.000000, -0.000000, 5962.576825, -0.000000, 0.000000,
-0.000000, 11709.205738, 0.000000, -0.000000, 5962.790743, -0.000000,
-0.000000, -0.000000, 21222.414800, 0.000000, -0.000000, 13158.349556;
V_functions_p[1] <<-0.000000,
-0.000000,
-1.565436,
0.000000,
-0.000000,
-1.427705;
V_functions_q[1] = -0.000085;
// 3rd value function: 0.03, 0.3
V_functions_P[2] << 96898.869633, 0.000000, 0.000000, 12328.042909, -0.000000, 0.000000,
0.000000, 96899.481925, -0.000000, -0.000000, 12328.367643, -0.000000,
0.000000, -0.000000, 112608.081726, 0.000000, -0.000000, 22077.194777,
12328.042909, -0.000000, 0.000000, 6331.438567, -0.000000, 0.000000,
-0.000000, 12328.367643, -0.000000, -0.000000, 6331.657284, -0.000000,
0.000000, -0.000000, 22077.194777, 0.000000, -0.000000, 13774.854462;
V_functions_p[2] <<-0.000000,
0.000000,
-1403.939507,
-0.000000,
0.000000,
-1289.101451;
V_functions_q[2] = -76.933950;
// 4th value function: 0.05, 0.5
V_functions_P[3] << 98581.380332, 0.000000, -0.000000, 13299.606107, -0.000000, -0.000000,
0.000000, 98582.058632, 0.000000, -0.000000, 13299.967933, 0.000000,
-0.000000, 0.000000, 114616.331808, -0.000000, 0.000000, 23466.408233,
13299.606107, -0.000000, -0.000000, 6925.507179, -0.000000, -0.000000,
-0.000000, 13299.967933, 0.000000, -0.000000, 6925.751528, 0.000000,
-0.000000, 0.000000, 23466.408233, -0.000000, 0.000000, 14806.067082;
V_functions_p[3] << 0.000000,
-0.000000,
-3502.707133,
0.000000,
-0.000000,
-3309.615390;
V_functions_q[3] = -562.627115;
// 5th value function: 0.07, 0.7
V_functions_P[4] << 100649.613076, -0.000000, 0.000000, 14523.662049, -0.000000, 0.000000,
-0.000000, 100650.297822, -0.000000, -0.000000, 14524.067930, -0.000000,
0.000000, -0.000000, 116882.828574, 0.000000, -0.000000, 25079.750540,
14523.662049, -0.000000, 0.000000, 7698.121362, -0.000000, 0.000000,
-0.000000, 14524.067930, -0.000000, -0.000000, 7698.400903, -0.000000,
0.000000, -0.000000, 25079.750540, 0.000000, -0.000000, 16051.091196;
V_functions_p[4] << -0.000000,
0.000000,
-5720.357097,
-0.000000,
0.000000,
-5657.793108;
V_functions_q[4] = -1936.868935;
// 6th value function: 0.1, 1
V_functions_P[5] << 103755.564658, -0.000000, 0.000000, 16412.308155, -0.000000, 0.000000,
-0.000000, 103756.359287, -0.000000, -0.000000, 16412.787731, -0.000000,
0.000000, -0.000000, 119805.622177, 0.000000, -0.000000, 27227.046965,
16412.308155, -0.000000, 0.000000, 8938.379991, -0.000000, 0.000000,
-0.000000, 16412.787731, -0.000000, -0.000000, 8938.718350, -0.000000,
0.000000, -0.000000, 27227.046965, 0.000000, -0.000000, 17803.004855;
V_functions_p[5] << -0.000000,
0.000000,
-7954.226741,
-0.000000,
0.000000,
-8661.125770;
V_functions_q[5] = -6284.083834;
// 7th value function: 0.13, 1.3
V_functions_P[6] << 106727.833717, -0.000000, 0.000000, 18290.737925, -0.000000, -0.000000,
-0.000000, 106728.717864, -0.000000, -0.000000, 18291.288835, -0.000000,
0.000000, -0.000000, 122064.101558, -0.000000, -0.000000, 28951.777740,
18290.737925, -0.000000, -0.000000, 10236.322124, -0.000000, -0.000000,
-0.000000, 18291.288835, -0.000000, -0.000000, 10236.720923, -0.000000,
-0.000000, -0.000000, 28951.777740, -0.000000, -0.000000, 19311.206251;
V_functions_p[6] <<0.000000,
0.000000,
-8708.039468,
0.000000,
0.000000,
-10684.631444;
V_functions_q[6] = -14162.420109;
// 8th value function: 0.2, 2
V_functions_P[7] << 113957.103857, -0.000000, 0.000000, 23290.584350, -0.000000, 0.000000,
-0.000000, 113961.537703, -0.000000, -0.000000, 23293.592366, -0.000000,
0.000000, -0.000000, 129236.970349, 0.000000, -0.000000, 34796.162819,
23290.584350, -0.000000, 0.000000, 14040.053864, -0.000000, 0.000000,
-0.000000, 23293.592366, -0.000000, -0.000000, 14042.176136, -0.000000,
0.000000, -0.000000, 34796.162819, 0.000000, -0.000000, 24555.621443;
V_functions_p[7] <<0.000000,
-0.000000,
-3596.304460,
0.000000,
-0.000000,
-9265.240481;
V_functions_q[7] = -61502.005814;
// 9th value function: 0.15, 1.5
V_functions_P[8] <<109415.180481, -0.000000, -0.000000, 20043.784978, 0.000000, -0.000000,
-0.000000, 109416.258830, -0.000000, 0.000000, 20044.480448, -0.000000,
-0.000000, -0.000000, 124466.933919, -0.000000, -0.000000, 30801.518919,
20043.784978, 0.000000, -0.000000, 11467.344049, 0.000000, -0.000000,
0.000000, 20044.480448, -0.000000, 0.000000, 11467.850877, -0.000000,
-0.000000, -0.000000, 30801.518919, -0.000000, -0.000000, 20870.888674;
V_functions_p[8] << 0.000000,
0.000000,
-7279.147202,
0.000000,
0.000000,
-10546.120706;
V_functions_q[8] = -24998.704399;
}
......@@ -10,6 +10,7 @@ extern "C"
using namespace Eigen;
using namespace std;
void linearization_fast_euler_6_states(const VectorXtype x_vec, const VectorUtype u_vec, params_t params, MatrixAtype &A, MatrixBtype &B)
{
double m = params.m;
......@@ -402,6 +403,7 @@ VectorXd mympc_varying_another_ADP(std::vector<MatrixAtype> A_tray, std::vector<
H_final.topLeftCorner(H.rows(),H.cols()) << H;
F_final.leftCols(N*N_u) << F;
F_final.rightCols(1) << 1.0;
Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
......@@ -410,6 +412,7 @@ VectorXd mympc_varying_another_ADP(std::vector<MatrixAtype> A_tray, std::vector<
// std::cout << H.format(CleanFmt) << std::endl;
// ft_min and ft_max constraints:
RowVectorXd pattern(4);
pattern << 0, 0, 0, 1;
......
......@@ -198,7 +198,7 @@ VectorXtype x0;
std::mutex MPC_mutex;
int N = 10;
int N = 8;
// ROS Publisher for debugging variables
......
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