MPC_functions.cpp 12.1 KB
Newer Older
1
#include "MPC_functions.h"
2
#include <unsupported/Eigen/MatrixFunctions>
roangel's avatar
roangel committed
3
#include <unsupported/Eigen/KroneckerProduct>
4

5
6
7
8
9
extern "C"
{
#include <gurobi_c.h>
}

10
11
12
using namespace Eigen;
using namespace std;

13
void linearization_fast_euler_6_states(const VectorXtype x_vec, const VectorUtype u_vec, params_t params, MatrixAtype &A, MatrixBtype &B)
14
{
15
    float m = params.m;
16
    A = MatrixAtype::Zero();
17
    A.topRightCorner(3,3) = Matrix3d::Identity();
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

    float gamma = u_vec(0);
    float beta = u_vec(1);
    float alpha = u_vec(2);
    float ft = u_vec(3);

    float c_alpha = cos(alpha);
    float s_alpha = sin(alpha);

    float s_beta = sin(beta);
    float c_beta = cos(beta);
    float c_gamma = cos(gamma);
    float s_gamma = sin(gamma);

    B = MatrixBtype::Zero();
33
34
35
    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;
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

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();

74
    MatrixXd M(c_A + c_B + 1, c_A + c_B + 1);
75
76
77


    M << A, B, g,
78
        MatrixXd::Zero(c_B+1, c_A), MatrixXd::Zero(c_B+1, c_B), MatrixXd::Zero(c_B+1,1);
79
80
81
82

    // std::cout << M << std::endl;

    // % discretization
83
    MatrixXd M_d;
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

    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);
    }
}
122
123
124
125
126
127
128



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)
{
    VectorXtype y = y0;
    VectorXtype y_next;
roangel's avatar
roangel committed
129
130
    std::vector<VectorXtype> yout_temp;
    yout_temp.push_back(y);
131
132
133
134
135
136

    for(float t = t0; t < tfinal; t = t + h)
    {
        pF(y, u, params, y_next);

        y = y + h*y_next;
roangel's avatar
roangel committed
137
        yout_temp.push_back(y);
138
    }
roangel's avatar
roangel committed
139
140
141
    yout = yout_temp;
}

142
void funSxSuSg_varying_affine(std::vector<MatrixAtype> A_tray, std::vector<MatrixBtype> B_tray, int N, MatrixXd &Sx, MatrixXd &Su, MatrixXd &Sg)
roangel's avatar
roangel committed
143
144
145
146
147
{
    int N_A = A_tray.size();
    int N_B = B_tray.size();

    if(N_A != N)
148
        std::cout << "Error: number of A matrices hsould be equal to N, however it is equal to: "<< N_A <<endl << endl;
roangel's avatar
roangel committed
149
    if(N_B != N)
150
        std::cout << "Error: number of B matrices hsould be equal to N, however it is equal to: "<< N_B <<endl << endl;
roangel's avatar
roangel committed
151
152
153
154
155
156
157
158
159
160
161
162
    // int r_A = A_tray[0].rows();
    // int c_A = A_tray[0].cols();
    // int r_A = B_tray[0].rows();
    // int c_A = B_tray[0].cols();

    int r_A = N_x;
    int c_A = N_x;
    int r_B = N_x;
    int c_B = N_u;

    // S_x is the temporal, local variable that will in the end be the output, Sx

163
164
    MatrixXd S_x = MatrixXd::Zero(r_A*(N+1), c_A);
    MatrixAtype A_mult = MatrixXd::Identity(r_A, r_A);
roangel's avatar
roangel committed
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179

    // construct S_x:
    for(int i = 0; i < N+1; i++)
    {

        S_x.middleRows(i*r_A, r_A) << A_mult;
        if (i < N)              // we cannot access index N in A_tray, protect it
        {
            A_mult = A_tray[i]*A_mult;
        }
    }


    // construct Su

180
    MatrixXd S_u = MatrixXd::Zero(r_B*(N+1), c_B*N);
roangel's avatar
roangel committed
181
182
183

    // temporal variables:

184
    MatrixXd column_Su;
roangel's avatar
roangel committed
185
186
187
188


    for(int i = 0; i < N; i++)
    {
189
190
        column_Su = MatrixXd::Ones(r_B * (N+1), c_B);
        A_mult = MatrixXd::Identity(r_A, c_A);
roangel's avatar
roangel committed
191
192
193
194
195
196
197
198
199
200

        for(int j = 0; j < N+1; j++)
        {
            if (j == i+1)
            {

                column_Su.middleRows(r_B*j, r_B) << B_tray[i];
            }
            else if (j > i+1)
            {
roangel's avatar
roangel committed
201
202
                A_mult = A_tray[j-1]  * A_mult;
                column_Su.middleRows(r_B*j, r_B) << A_mult * B_tray[i];
roangel's avatar
roangel committed
203
204
205
206
207
208
209
210
211
212
213
214
215
            }
            else
            {
                // can possibly omit this else, just for clarity. Of course first we need to initizlize to zero instead of ones columns Su
                column_Su.middleRows(r_B*j, r_B).setZero();
            }
        }

        S_u.middleCols(c_B*i, c_B) << column_Su;
    }

    // build Sg matrix

216
    MatrixXd S_g = MatrixXd::Zero(r_A*(N+1), c_A*N);
roangel's avatar
roangel committed
217
218
219

    // temporal variables:

220
    MatrixXd column_Sg;
roangel's avatar
roangel committed
221
222
223
224


    for(int i = 0; i < N; i++)
    {
225
226
        column_Sg = MatrixXd::Ones(r_A * (N+1), c_A);
        A_mult = MatrixXd::Identity(r_A, c_A);
roangel's avatar
roangel committed
227
228
229
230
231
232
233
234
235
236

        for(int j = 0; j < N+1; j++)
        {
            if (j == i+1)
            {

                column_Sg.middleRows(r_A*j, r_A).setIdentity();
            }
            else if (j > i+1)
            {
roangel's avatar
roangel committed
237
238
                A_mult = A_tray[j-1]  * A_mult;
                column_Sg.middleRows(r_A*j, r_A) << A_mult;
roangel's avatar
roangel committed
239
240
241
242
243
244
245
246
247
248
249
250
251
252
            }
            else
            {
                // can possibly omit this else, just for clarity. Of course first we need to initizlize to zero instead of ones columns Su
                column_Sg.middleRows(r_A*j, r_A).setZero();
            }
        }

        S_g.middleCols(c_A*i, c_A) << column_Sg;
    }

    Sx = S_x;
    Su = S_u;
    Sg = S_g;
253
}
roangel's avatar
roangel committed
254
255


256
void funQbar(MatrixXd Q, MatrixXd P, int N, MatrixXd &Qbar)
roangel's avatar
roangel committed
257
{
258
259
    MatrixXd Q_bar;
    MatrixXd diag_mask = VectorXd::Ones(N+1).asDiagonal();
roangel's avatar
roangel committed
260
261
262
263
264
265
    Q_bar = kroneckerProduct(diag_mask, Q);
    Q_bar.bottomRightCorner(P.rows(), P.cols()) = P;

    Qbar = Q_bar;
}

266
void funRbar(MatrixXd R, int N, MatrixXd &Rbar)
roangel's avatar
roangel committed
267
{
268
269
    MatrixXd R_bar;
    MatrixXd diag_mask = VectorXd::Ones(N).asDiagonal();
roangel's avatar
roangel committed
270
271
272
273
    R_bar = kroneckerProduct(diag_mask, R);
    Rbar = R_bar;
}

274
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 angle_constraint)
275
{
276
277
278
    Eigen::MatrixXd Sx;
    Eigen::MatrixXd Su;
    Eigen::MatrixXd Sg;
279
280
281
282
283

    VectorUtype u_optimal;

    funSxSuSg_varying_affine(A_tray, B_tray, N, Sx, Su, Sg);

284
    VectorXd g_tray_col(N*N_x);
285
286
287
288
289
290
291
292
293

    for(int i = 0; i < g_tray.size(); i++)
    {
        g_tray_col.segment(i*N_x, N_x) = g_tray[i];
    }

    // std::cout << "g_tray_col" << std::endl;
    // std::cout << g_tray_col << std::endl;

294
295
    Eigen::MatrixXd Qbar;
    Eigen::MatrixXd Rbar;
296
297
298
299

    funQbar(Q, P, N, Qbar);
    funRbar(R,N, Rbar);

300
301
    Eigen::MatrixXd H;
    Eigen::MatrixXd F;
302
303
304
305
306

    H = Su.transpose()*Qbar*Su + Rbar;

    F = 2*(x0.transpose()*Sx.transpose()*Qbar*Su + g_tray_col.transpose()*Sg.transpose()*Qbar*Su - X_ref.transpose()*Qbar*Su - U_ref.transpose()*Rbar);

307
    Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
308

309
310
311
    // std::cout << "H test:" << std::endl << std::endl;
    // // std::cout << Su_test<< std::endl;
    // std::cout << H.format(CleanFmt) << std::endl;
312

313
    Eigen::VectorXd U_0 = solve_QP(H, F);
314

315
316
    return U_0;
}
317

318
319
VectorXd solve_QP(Eigen::MatrixXd H, Eigen::MatrixXd F)
{
roangel's avatar
roangel committed
320
321
322
    // H is assumed to be symmetric. There is NO 0.5 factor anywhere, with this we solve:
    // min(x'Hx + F'x)

323
    int size_H = H.cols();
324

325
326
327
328
    std::vector<int> q_row;
    std::vector<int> q_col;
    std::vector<double> q_val;
    std::vector<double> l_obj_coeff;
329

330
331
332
333
    for(int i = 0; i < size_H; i++)
    {
        for(int j = i; j < size_H; j++)
        {
334
            // std::cout << "i,j -->" << i << "," << j << std::endl;
335
336
337
338
339
340
341
342
            if(H(i,j) != 0)
            {
                q_row.push_back(i);
                q_col.push_back(j);

                if(i == j)
                {
                    q_val.push_back(H(i,j));
343
                    // std::cout << "H(i,j)" << H(i,j) << endl;
344
345
346
347
                }
                else
                {
                    q_val.push_back(2*H(i,j));
348
                    // std::cout << "2H(i,j)" << 2*H(i,j) << endl;
349
350
351
352
353
                }
            }
        }
        l_obj_coeff.push_back(F(i));
    }
354
355
356



357
    int N_vars = size_H;
358
    int N_q_coeffs = q_val.size();
359

360
    // BEGINNING C INTERFACE:
361

362
363
364
365
366
367
368
369
370
371
372
    GRBenv   *env   = NULL;
    GRBmodel *model = NULL;
    int       error = 0;
    double    sol[N_vars];
    int       ind[N_vars];
    double    val[N_vars];
    int*       qrow = q_row.data();
    int*       qcol = q_col.data();
    double*    qval = q_val.data();
    int       optimstatus;
    double    objval;
373

374
375
    std::vector<double> ub(N_vars, GRB_INFINITY);
    std::vector<double> lb(N_vars, -GRB_INFINITY);
376

377
378
379
    /* Create environment */
    error = GRBloadenv(&env, "qp.log");
    if (error) goto QUIT;
380

381
    /* Create a model */
382
383
    error = GRBnewmodel(env, &model, "qp", N_vars, l_obj_coeff.data(),lb.data(), ub.data(), NULL, NULL);
    if (error) goto QUIT;
384

385
    /* Quadratic objective terms */
386

387
388
    error = GRBaddqpterms(model, N_q_coeffs, qrow, qcol, qval);
    if (error) goto QUIT;
389

390
391
392
    error = GRBsetintparam(GRBgetenv(model), "OutputFlag", 0);
    if (error) goto QUIT;

393
    // Optimize model
394

395
396
    error = GRBoptimize(model);
    if (error) goto QUIT;
397

398
399
400
    /* Write model to 'qp.lp' */
    error = GRBwrite(model, "qp.lp");
    if (error) goto QUIT;
401

402
403
404
    /* Capture solution information */
    error = GRBgetintattr(model, GRB_INT_ATTR_STATUS, &optimstatus);
    if (error) goto QUIT;
405

406
407
    error = GRBgetdblattr(model, GRB_DBL_ATTR_OBJVAL, &objval);
    if (error) goto QUIT;
408

409
410
    error = GRBgetdblattrarray(model, GRB_DBL_ATTR_X, 0, N_vars, sol);
    if (error) goto QUIT;
411

412
    // printf("\nOptimization complete\n");
413
    if (optimstatus == GRB_OPTIMAL) {
414
        // printf("Optimal objective: %.4e\n", objval);
415
416
417
418
        // for(int i = 0; i < N_vars; i++)
        // {
        //     printf("sol[i] = %.4f\n", sol[i]);
        // }
419
420
421
422
423
    } else if (optimstatus == GRB_INF_OR_UNBD) {
        printf("Model is infeasible or unbounded\n");
    } else {
        printf("Optimization was stopped early\n");
    }
424
425


426
QUIT:
427

428
    /* Error reporting */
429

430
431
432
433
    if (error) {
        printf("ERROR: %s\n", GRBgeterrormsg(env));
        exit(1);
    }
434

435
    /* Free model */
436

437
438
439
440
441
    GRBfreemodel(model);

    /* Free environment */

    GRBfreeenv(env);
442

443
    // END C INTERFACE
444

445
446
    Eigen::IOFormat CleanFmt(4, 0, ", ", "\n", "[", "]");
    Map<VectorXd> U_0(sol,N_vars);
447
    // cout << "The mapped vector U_0 is: \n" << U_0.format(CleanFmt) << "\n";
448
    return U_0;
449
}