main.c 6.7 KB
Newer Older
1
2
3
4
5
6
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
   /**
    *
    *   Universite catholique de Louvain
    *   CEREM : Centre for research in mechatronics
    *   http://www.robotran.be  
    *   Contact : info@robotran.be
    *
    *
    *   MBsysC main script template for simple model:
    *   -----------------------------------------------
    *    This template loads the data file *.mbs and execute:
    *      - the coordinate partitioning module
    *      - the direct dynamic module (time integration of
    *        equations of motion).
    *    It may be adapted and completed by the user.
    * 
    *    (c) Universite catholique de Louvain
    *
    * To turn this file as a C++ file, just change its extension to .cc (or .cpp).
    * If you plan to use some C++ files, it is usually better that the main is compiled as a C++ function.
    * Currently, most compilers do not require this, but it is a safer approach to port your code to other computers.
    */

#include <stdio.h>
#include "mbs_data.h"
#include "mbs_dirdyn.h"
#include "mbs_part.h"
#include "realtime.h"
#include "mbs_set.h"
#include "mbs_load_xml.h"
#include "cmake_config.h"
#include "mbs_modal.h"
#include "mbs_equil.h"

#include "user_model.h"
#include "user_all_id.h"

Your Name's avatar
Your Name committed
38
#include "mbs_linalg.h"
39
#include "mbs_lqr.h"
Your Name's avatar
Your Name committed
40
#include "mbs_kalman.h"
41
42
int main(int argc, char const *argv[])
{
Your Name's avatar
Your Name committed
43

44
    MbsData *mbs_data;
Your Name's avatar
Your Name committed
45

46
47
48
49
    MbsPart *mbs_part;
    MbsDirdyn *mbs_dirdyn;
    MbsModal *mbs_modal;

Your Name's avatar
Your Name committed
50
51
52
53
54
55
56







57
58
59
60
61
62
63
64
65
66
67
    printf("Starting MPC_CartPendulum MBS project!\n");


    /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
    /*                     LOADING                               *
    /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
    
    printf("Loading the MPC_CartPendulum data file !\n");
    mbs_data = mbs_load(PROJECT_SOURCE_DIR"/../dataR/CartPendulum.mbs", BUILD_PATH);
    printf("*.mbs file loaded!\n");

Your Name's avatar
Your Name committed
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
    /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
    /*              COORDINATE PARTITIONING                      *
    /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

    mbs_part = mbs_new_part(mbs_data);
    mbs_part->options->rowperm=1;
    mbs_part->options->verbose = 1;
    mbs_run_part(mbs_part, mbs_data);
    
    mbs_delete_part(mbs_part);


    MbsEquil *mbs_equil;

    /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
    /*                   STATIC EQUILIBRIUM                      *
    /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

    mbs_equil = mbs_new_equil(mbs_data);
    // equil options (see documentations for additional options)
    mbs_equil->options->senstol = 1e-06;
    mbs_equil->options->verbose = 0;
    // equilibrium procedure
    mbs_run_equil(mbs_equil, mbs_data);
    mbs_print_equil(mbs_equil);
    mbs_delete_equil(mbs_equil, mbs_data);

Olivier Lantsoght's avatar
Olivier Lantsoght committed
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
    /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
    /*                   MODAL ANALYSIS (UNCONTROLLED)           *
    /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

    mbs_modal = mbs_new_modal(mbs_data);
    // modal options (see documentations for additional options)
    mbs_modal->options->save_mat = 1;
    mbs_modal->options->save_eval = 1;
    mbs_modal->options->save_evec = 1;
    mbs_modal->options->save_result = 1;
    mbs_modal->options->save_anim = 1;
    mbs_modal->options->mode_ampl = 0.5;
    mbs_modal->options->verbose = 1;
    mbs_modal->options->resfilename = "modal_uncontrolled";
    mbs_run_modal(mbs_modal, mbs_data);
    mbs_delete_modal(mbs_modal, mbs_data);
114
115
116
117
118
119
120
121
122
123
124
125

    /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
    /*                 STATE SPACE AND LQR                       *
    /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
    MbsAux *aux = initMbsAux(mbs_data);
    MbsLpk *lpk = mbs_new_lpk(mbs_data, mbs_data->nqu, mbs_data->nqu);
    MbsSS *ss= mbs_new_statespace(mbs_data, 1, 1, 0, 0);
    ss->iu[1] = 1;
    ss->iy[1] = 2;
    mbs_compute_linearMatrices(ss, mbs_data, lpk, aux);
    mbs_compute_statespace(ss, mbs_data);

126
127
128
    save_dmat_0(ss->A, ss->nx, ss->nx, PROJECT_SOURCE_DIR"/../resultsR/A_cartPendulum.txt");
    save_dmat_0(ss->B, ss->nx, ss->nu, PROJECT_SOURCE_DIR"/../resultsR/B_cartPendulum.txt");
    save_dmat_0(ss->C, ss->ny, ss->nx, PROJECT_SOURCE_DIR"/../resultsR/C_cartPendulum.txt");
Your Name's avatar
Your Name committed
129
130
131
132
133
134
135
136

    MbsSS *sskal = mbs_new_statespace(mbs_data, 1, 1, 0, 0);
    double **T = get_dmat_0(ss->nx, ss->nx);
    int nxc=0;
    mbs_compute_kalmanDcpt(ss, sskal, T,&nxc);
    free_dmat_0(T);


137
138
139
140
141
142
143
144
145
146
    int i;
    MbsLqr *lqr = mbs_new_lqr(mbs_data->nqu * 2, 1, 1);
    for (i = 0; i < 4; i++)
    {
        lqr->Q[i][i] = 1.0;
    }
    for (i = 0; i < 1; i++)
    {
        lqr->R[i][i] = 1.0;
    }
Your Name's avatar
Your Name committed
147
    lqr->Q[0][0] = 1;
148
    lqr_compute_gain(ss, lqr);
Your Name's avatar
Your Name committed
149
150
151

    print_dmat_0(lqr->K, ss->nu, ss->nx);
    system("pause");
152
153
154
155
156
157
158
159
160
161
    mbs_data->user_model->lqr.K[0] = lqr->K[0][0];
    mbs_data->user_model->lqr.K[1] = lqr->K[0][1];
    mbs_data->user_model->lqr.K[2] = lqr->K[0][2];
    mbs_data->user_model->lqr.K[3] = lqr->K[0][3];

    mbs_delete_statespace(ss);
    mbs_delete_lqr(lqr);
    mbs_delete_lpk(lpk);
    freeMbsAux(aux, mbs_data);

Olivier Lantsoght's avatar
Olivier Lantsoght committed
162
163
164
165
    /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
    /*                   MODAL ANALYSIS (CONTROLLED)             *
    /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

166
167
168
169
170
171
172
173
174
    mbs_modal = mbs_new_modal(mbs_data);
    // modal options (see documentations for additional options)
    mbs_modal->options->save_mat = 1;
    mbs_modal->options->save_eval = 1;
    mbs_modal->options->save_evec = 1;
    mbs_modal->options->save_result = 1;
    mbs_modal->options->save_anim = 1;
    mbs_modal->options->mode_ampl = 0.5;
    mbs_modal->options->verbose = 1;
Olivier Lantsoght's avatar
Olivier Lantsoght committed
175
    mbs_modal->options->resfilename = "modal_controlled";
176
177
178
179
    mbs_run_modal(mbs_modal, mbs_data);
    mbs_delete_modal(mbs_modal, mbs_data);


Olivier Lantsoght's avatar
Olivier Lantsoght committed
180

181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
    /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
    /*                   DIRECT DYNANMICS                        *
    /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */


    mbs_data->q[T1_Cart_id] = .0;
    mbs_data->q[R2_Pendulum_id] = .0;
    mbs_data->process = 3;

    mbs_dirdyn = mbs_new_dirdyn(mbs_data);
    mbs_dirdyn->options->accelred = 0;
    // dirdyn options (see documentations for additional options)
    mbs_dirdyn->options->dt0 = 1e-3;
    mbs_dirdyn->options->tf  = 10.0;
    mbs_dirdyn->options->save2file = 1;
196
    mbs_dirdyn->options->realtime = 1;
197
198
199
200
201
202
203
204
205
206
207
208
209
210
    
    mbs_run_dirdyn(mbs_dirdyn, mbs_data);
    
    mbs_delete_dirdyn(mbs_dirdyn, mbs_data);
    
    
    /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
    /*                   CLOSING OPERATIONS                      *
    /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
    mbs_delete_data(mbs_data);
    
    return 0;
}