ADP_matrices.cpp 6.4 KB
Newer Older
1
2
#include "ADP_matrices.h"

3
4
5
6
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;

7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168

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;
}