mbs_dirdyn.c 21.5 KB
Newer Older
Nicolas Docquier's avatar
Nicolas Docquier committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
/**
 * @file mbs_dirdyn.c
 *
 * This file implements the  functions of the
 * dirdyn module in C.
 *
 *
 * Creation date: 19/11/2014
 * @author Nicolas Docquier (based on the work of other from CEREM: nvdn, av, ...)
 *
 *
 * (c) Universite catholique de Louvain
 */

#include "mbs_dirdyn.h"
#include <stdlib.h>
#include <stdio.h>
#include "integrator.h"
19
#include "mbs_project_interface.h"
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
20
#include "user_realtime_visu.h"
21
#include "string.h"
22
#include "realtime.h"
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
23
#include "set_output.h"
24
#include "MBSfun.h"
Nicolas Docquier's avatar
Nicolas Docquier committed
25

26
#include "mbs_message.h"
27

Nicolas Docquier's avatar
Nicolas Docquier committed
28
29
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

30
31
MbsDirdyn* mbs_new_dirdyn(MbsData* mbs_data)
{
Nicolas Docquier's avatar
Nicolas Docquier committed
32
    MbsAux* mbs_aux;
33

Nicolas Docquier's avatar
Nicolas Docquier committed
34
35
    // Initialize the local data struct
    mbs_aux = initMbsAux(mbs_data);
36

Nicolas Docquier's avatar
Nicolas Docquier committed
37
38
39
    return mbs_new_dirdyn_aux(mbs_data, mbs_aux);
}

40
41
MbsDirdyn* mbs_new_dirdyn_aux(MbsData* mbs_data, MbsAux* mbs_aux)
{
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
42
    MbsDirdyn *dirdyn;
Nicolas Docquier's avatar
Nicolas Docquier committed
43
44
45
    MbsDirdynOptions* opts;

    // Initialize the direct dynamic structure
46
    dirdyn = (MbsDirdyn*)malloc(sizeof(MbsDirdyn));
Nicolas Docquier's avatar
Nicolas Docquier committed
47
48
49
50
51

    // keep the pointer to the auxiliary data structure
    dirdyn->mbs_aux = mbs_aux;

    // Initialize the options struct with default options
52
    opts = (MbsDirdynOptions*)malloc(sizeof(MbsDirdynOptions));
53
    dirdyn->options = opts;
54

55
56
    opts->t0 = 0.0;
    opts->tf = 5.0;
Nicolas Docquier's avatar
Nicolas Docquier committed
57
    opts->dt0 = 1e-3;
58
    opts->save2file = 1;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
59
    opts->save_anim = 1;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
60
    opts->save_visu = 0;
61
    opts->framerate = 1000;
62
    opts->saveperiod = 1;
63
    opts->max_save_user = 12;
64
    opts->buffersize = -1;
65
    opts->resfilename = NULL;
66
    opts->respath = NULL;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
67
    opts->animpath = NULL;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
68
69
    opts->realtime = 0;
    opts->accelred = 0;
70
	
71
    opts->flag_compute_Qc = COMPUTE_ALL_QC;
72

73
    // Initialization for fast computation of Qc 
74
    int i;
75
76
77
    opts->compute_Qc = get_ivec_1(mbs_data->njoint); 
    for (i = 1; i <= mbs_data->njoint; i++) {
        opts->compute_Qc[i] = 0;
78
    }
79

80
81
82
83
84
85
86
    /*------------------------------------------*/
    /* Options related to numerical integration */
    /*------------------------------------------*/
    opts->integrator = RK4;
    // Various
    opts->verbose = 1;
    opts->flag_stop_stiff = 0;
87
    opts->flag_precise_dynamics = 1;
88
    // Waypoints
89
    opts->flag_waypoint = 0;
90
91
    opts->flag_solout_wp = 0;
    opts->delta_t_wp = 1.0e-3;
92

93
    // Options related to variable time-stepped methods
94
95
96
97
98
    opts->nmax = DIRDYN_INTEGRATOR_OPTION_DEFAULT_nmax;
    opts->toler = DIRDYN_INTEGRATOR_OPTION_DEFAULT_toler;
    opts->rtoler = DIRDYN_INTEGRATOR_OPTION_DEFAULT_rtoler;
    opts->atoler = DIRDYN_INTEGRATOR_OPTION_DEFAULT_atoler;
    opts->dt_max = DIRDYN_INTEGRATOR_OPTION_DEFAULT_dt_max;
Nicolas Docquier's avatar
Nicolas Docquier committed
99

100
101
	// Options related to integrator that need Jacobian computation
	opts->n_freeze = 0;
102
	
103
104
105
    // initial the saving counter
    dirdyn->savePeriodCounter = 0;

Nicolas Docquier's avatar
Nicolas Docquier committed
106
107
108
109
110
    return dirdyn;
}

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
111
112
void mbs_delete_dirdyn(MbsDirdyn* dirdyn, MbsData* mbs_data)
{
113
    free(dirdyn->options);
Nicolas Docquier's avatar
Nicolas Docquier committed
114
115
116
117
118
119
    freeMbsAux(dirdyn->mbs_aux, mbs_data);
    free(dirdyn);
}

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

120
121
void mbs_run_dirdyn(MbsDirdyn* dd, MbsData* mbs_data)
{
122
    // 1. Initialize the simulation
123
    // - - - - - - - - - - - - - -
124
125
126
    mbs_dirdyn_init(dd, mbs_data);


127
    // 2. Run the simulation
128
    // - - - - - - - - - - -
129
130
    mbs_dirdyn_loop(dd, mbs_data);

131
132

    // 3. Finish the simulation
133
    // - - - - - - - - - - - -
134
    mbs_dirdyn_finish(dd, mbs_data);
135
136
}

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
137
void mbs_dirdyn_save(MbsDirdyn* dd, MbsData *mbs_data, double t){
138

139
    int i, err =0;
140
#ifdef VISU_3D
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
141
    Simu_realtime *realtime;
142
    Realtime_visu *visu;
143
#endif
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
144

145
    for (i=0 ; i < dd->bufferNb ; i++){
146
147
148
149
150
151
152
153
154
        err = mbs_buffer_save(dd->buffers[i], t, dd->savedArrays[i]);
    }
    
    if (err < 0)
    {
        mbs_msg(">>DIRDYN>> ***** mbs_dirdyn_save : impossible to save the files *****\n");
        mbs_msg(">>DIRDYN>> ***** during buffer save *****\n");
        mbs_msg(">>DIRDYN>>\n");
        mbs_error_msg("[%d] in mbs_fct_dirdyn !! \n", -400 + err);
155
    }
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
156

157
    err = mbs_growing_buffer_save(dd->user_buffer, t);
158
159
160
161
162
163
164
165
166
   
    if (err < 0)
    {
        mbs_msg(">>DIRDYN>> ***** mbs_dirdyn_save : impossible to save the files *****\n");
        mbs_msg(">>DIRDYN>> ***** during growing buffer save *****\n");
        mbs_msg(">>DIRDYN>>\n");
        mbs_error_msg("[%d] in mbs_fct_dirdyn !! \n", -400 + err);
    }
   
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
167
168
    if (dd->options->save_visu)
    {
169
170
#ifdef VISU_3D
        realtime = (Simu_realtime*)mbs_data->realtime;
171
        visu = realtime->ext->visu;
172

173
        user_realtime_visu(mbs_data, visu->nb_models, visu->nb_q, visu->anim_q);
174

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
175
176
        for(i=0; i<realtime->options->nb_models; i++)
        {
177
            mbs_buffer_save(dd->buffer_visu[i], t, visu->anim_q[i]);
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
178
        }
179
#endif
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
180
    }
181
}
182

183
184
void mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
{
185
    int i;
186

187
#ifdef VISU_3D
188
189
    Simu_realtime *realtime;
    Realtime_visu *visu;
190
#endif
191
192
193
194
195
196
197
198
199
200
201
202
    // Check the usefulness of dirdyn
    //----------------
    if (mbs_data->nqu+mbs_data->nqc == 0)
    {
        mbs_msg("\t >Dirdyn> If no independant nor driven joints are present, the direct dynamic process has no reason to be used !\n");
        mbs_msg("\t >Dirdyn> Irrelevant process !\n");
        mbs_error_msg("[%d] in mbs_dirdyn_init !! \n", -410);
    }
    else if (mbs_data->nqu == 0)
    {
        mbs_warning_msg("\t >Dirdyn> You are using the direct dynamic process without any not-driven independant joint ! \n");
    }
203

204
205
    // INITIALIZATION
    // - - - - - - - -
Nicolas Docquier's avatar
Nicolas Docquier committed
206

207
208
    // Set the selected integrator
    set_integrator(dd);
209

210
211
212
213
214
    // options (indications for the user)
    mbs_data->t0 = dd->options->t0;
    mbs_data->tsim = dd->options->t0;
    mbs_data->tf = dd->options->tf;
    mbs_data->dt0 = dd->options->dt0;
215

216
217
218
219
220
221
    /* TODO : Move in initialize_integrator when 2nd order integrator will be implemented */
    // Allocate state vectors
    dd->nState = 2 * mbs_data->nqu + mbs_data->Nux;
    dd->y    = get_dvec_0(dd->nState);
    dd->yout = get_dvec_0(dd->nState);
    dd->yd   = get_dvec_0(dd->nState);
222

223
224
    // initialyze integrator
    dd->initialize_integrator(mbs_data, dd);
225

226
227
    // check (and print) warnings for users options of integrator
    print_warnings_integrator(dd, dd->options->integrator);
228

229
    // real-time modules activated
230
#ifdef REAL_TIME
231
232
233
234
235
    if (dd->options->realtime)
    {
        mbs_realtime_reset();
        mbs_realtime_init(mbs_data, dd->options->t0, dd->options->tf, dd->options->dt0);
    }
236
#else
237
238
    if (dd->options->realtime)
    {
239
        mbs_msg("\t >Real-time> To use the real-time features (mbs_dirdyn->options->realtime activated), set the CMake flag 'FLAG_REAL_TIME' to ON !\n");
240
        mbs_error_msg("[%d] in mbs_dirdyn_init !! \n", -410);
241
    }
242
#endif
243

244
	// Setting the Qc to be cumputed in dirdynared
245
246
	if (dd->options->flag_compute_Qc == COMPUTE_NO_QC)
	{
247
		for (i = 1; i <= mbs_data->njoint; i++)
248
		{
249
			dd->mbs_aux->compute_Qc_vec[i] = 0;
250
251
252
253
		}
	}
	else if (dd->options->flag_compute_Qc == COMPUTE_CUSTOM_QC)
	{
254
		for (i = 1; i <= mbs_data->njoint; i++)
255
		{
256
			dd->mbs_aux->compute_Qc_vec[i] = dd->options->compute_Qc[i];
257
258
259
260
		}
	}
	else
	{
261
		for (i = 1; i <= mbs_data->njoint; i++)
262
		{
263
264
			dd->mbs_aux->compute_Qc_vec[i] = 1;
			
265
266
267
		}
	}

268
269
270
271
    // init time
    dd->tsim = dd->options->t0;
    dd->dt   = dd->options->dt0;

272
    // user initialization
273
274
275
276
277
    if(dd->options->save2file)
    {
        // 'auto_output' structure initialized, needed for add_output_vector
        init_set_output(dd->options->max_save_user);
    }
278
    user_dirdyn_init(mbs_data, dd);
Nicolas Docquier's avatar
Nicolas Docquier committed
279

280
    /* TODO : Move in initialize_integrator when 2nd order integrator will be implemented */
Nicolas Docquier's avatar
Nicolas Docquier committed
281
282
283
    // Simulation state initialization
    for(i=1; i<=mbs_data->nqu; i++)
    {
284
285
        dd->y[i-1]               = mbs_data->q[mbs_data->qu[i]];
        dd->y[i-1+mbs_data->nqu] = mbs_data->qd[mbs_data->qu[i]];
Nicolas Docquier's avatar
Nicolas Docquier committed
286
287
288
    }
    for(i=1; i<=mbs_data->Nux; i++)
    {
289
        dd->y[i-1+2*mbs_data->nqu] = mbs_data->ux[i];
Nicolas Docquier's avatar
Nicolas Docquier committed
290
291
    }

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
292
293
    if(dd->options->save2file)
    {
294
        char *resfilename, *respath, *animpath;
295
        int bufId, bufSize, njoint, Nux, Nlink, nqc;
296
        char* f;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
297
        char* f_anim;
298

299
300
301
        char **fnameSuffix;
        int *bufferIDs;
        int *bufElemNb;
302

303
        njoint = mbs_data->njoint;
304
        Nux = mbs_data->Nux;
305
        Nlink = mbs_data->Nlink;
306
        nqc = mbs_data->nqc;
307
308
309
310
311
312

        // allocate buffer at needed size
        bufSize = 4 + (Nux?2:0) + (Nlink?3:0) + (nqc?1:0) + get_output_vector_nb();
        fnameSuffix = (char**) malloc(bufSize*sizeof(char*));
        bufferIDs = (int*) malloc(bufSize*sizeof(int));
        bufElemNb = (int*) malloc(bufSize*sizeof(int));
313
        
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
        // set the filename if not specified
        if (dd->options->resfilename == NULL) {
            resfilename = "dirdyn";
        }
        else {
            resfilename = dd->options->resfilename;
        }

        // set the path to result files if not specified
        if (dd->options->respath == NULL)
        {
            respath = (char*)malloc(sizeof(char)*(strlen(mbs_data->project_path) + 100));
            strcpy(respath, mbs_data->project_path);
            strcat(respath, "/resultsR");
        }
        else
        {
            respath = strdup(dd->options->respath);
        }

        // set the path to anim file if not specified
        if (dd->options->animpath == NULL)
        {
            animpath = (char*)malloc(sizeof(char)*(strlen(mbs_data->project_path) + 100));
            strcpy(animpath, mbs_data->project_path);
            strcat(animpath, "/animationR");
        }
        else
        {
            animpath = strdup(dd->options->animpath);
        }

346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
        // generic buffer properties
        fnameSuffix[0] = "q";    bufferIDs[0] = BUFFER_Q;    bufElemNb[0]=njoint;
        fnameSuffix[1] = "qd";   bufferIDs[1] = BUFFER_QD;   bufElemNb[1]=njoint;
        fnameSuffix[2] = "qdd";  bufferIDs[2] = BUFFER_QDD;  bufElemNb[2]=njoint;
        fnameSuffix[3] = "Qq";   bufferIDs[3] = BUFFER_QQ;   bufElemNb[3]=njoint;
        dd->bufferNb=4;
        
        // buffer properties for user states, if any
        if(Nux){
            int id = dd->bufferNb;
            fnameSuffix[id+0] = "ux";    bufferIDs[id+0] = BUFFER_UX;    bufElemNb[id+0]=Nux;
            fnameSuffix[id+1] = "uxd";   bufferIDs[id+1] = BUFFER_UXD;   bufElemNb[id+1]=Nux;
            dd->bufferNb += 2;
        }
       
Nicolas Docquier's avatar
Nicolas Docquier committed
361
        // buffer properties for links (link 1D only)
362
363
364
365
366
367
368
        if(mbs_data->Nlink){
            int id = dd->bufferNb;
            fnameSuffix[id+0] = "linkZ";    bufferIDs[id+0] = BUFFER_LINK_Z;    bufElemNb[id+0]=Nlink;
            fnameSuffix[id+1] = "linkZD";   bufferIDs[id+1] = BUFFER_LINK_ZD;   bufElemNb[id+1]=Nlink;
            fnameSuffix[id+2] = "linkF";    bufferIDs[id+2] = BUFFER_LINK_F;    bufElemNb[id+2]=Nlink;
            dd->bufferNb += 3;            
        }
369

Nicolas Docquier's avatar
Nicolas Docquier committed
370
        // buffer properties for joint/force associated to driven joints
371
372
373
374
375
376
        if(nqc){
            int id = dd->bufferNb;
            // A column is saved for each joint, even if there are not driven !
            fnameSuffix[id+0] = "Qc";    bufferIDs[id+0] = BUFFER_QC;    bufElemNb[id+0]=njoint;
            dd->bufferNb += 1;            
        }
377
378
379
380
381
382
383
384
385
386
        // buffer properties for the user specified vectors
        if (get_output_vector_nb()){
            int id = dd->bufferNb;
            for (i=0; i<get_output_vector_nb() ; i++){
                fnameSuffix[id] = get_output_vector_label(i);    bufferIDs[id] = BUFFER_OTHER+i;    bufElemNb[id]=get_output_vector_size(i);
                id +=1;
                dd->bufferNb += 1;
            }

        }
387

388
389
390
        // set the buffer size if not specified
        if (dd->options->buffersize<1){
            // compute size assuming a constant time step size
391
            dd->options->buffersize = (int) ( (dd->options->tf - dd->options->t0)/
392
                                      (dd->dt * dd->options->saveperiod)
393
                                      + 1 ) ;
394
            // add some extra space
395
            dd->options->buffersize = (int) (dd->options->buffersize * 1.1);
396
397
            dd->options->buffersize += 10;
        }
398
        
399
400
        // initialize the buffers for saving results
        dd->buffers = (MbsBuffer**)malloc(dd->bufferNb*sizeof(MbsBuffer*));
401

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
402
403
        f      = (char*)malloc(sizeof(char)*(strlen(respath) +strlen(resfilename)+30));
        f_anim = (char*)malloc(sizeof(char)*(strlen(animpath)+strlen(resfilename)+30));
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
404

405
        for(bufId=0 ; bufId < dd->bufferNb; bufId++){
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
406
407
            sprintf(f,      "%s/%s_%s.res",  respath,  resfilename, fnameSuffix[bufId]);
            sprintf(f_anim, "%s/%s_%s.anim", animpath, resfilename, fnameSuffix[bufId]);
408

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
409
410
411
412
413
            dd->buffers[bufId] = mbs_new_buffer(f, f_anim, bufElemNb[bufId], dd->options->buffersize, bufferIDs[bufId], dd->options->save_anim, dd->options->save_visu, 1./(double)dd->options->framerate);
        }

        if (dd->options->save_visu)
        {
414
#ifdef VISU_3D
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
415
416
            if (!dd->options->realtime)
            {
417
                mbs_msg("\t >Real-time> Error: real-time features must be activated to set 'save_visu' to 1 ! \n");
418
                mbs_error_msg("[%d] in mbs_dirdyn_init !! \n", -410);
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
419
420
421
422
423
424
            }

            realtime = (Simu_realtime*) mbs_data->realtime;

            if (!realtime->options->flag_visu)
            {
425
                mbs_msg("\t >Real-time> Error: flag_visu must be set to 1 to set 'save_value' to 1 ! \n");
426
                mbs_error_msg("[%d] in mbs_dirdyn_init !! \n", -410);
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
427
428
            }

429
            visu = realtime->ext->visu;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
430
            dd->buffer_visu = (MbsBuffer**) malloc(realtime->options->nb_models*sizeof(MbsBuffer*));
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
431

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
432
433
434
435
            for(i=0; i<realtime->options->nb_models; i++)
            {
                sprintf(f,      "%s/visu_%d_q.res",  respath, i);
                sprintf(f_anim, "%s/visu_%d_q.anim", animpath, i);
436

437
                dd->buffer_visu[i] = mbs_new_buffer(f, f_anim, visu->nb_q[i], dd->options->buffersize, BUFFER_VISU, dd->options->save_anim, dd->options->save_visu, 1./(double)dd->options->framerate);
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
438
            }
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
439

440
#else
441
            mbs_msg("\t >Real-time> Error: Java libraries must be activated to set 'save_visu' to 1 ! \n");
442
            mbs_error_msg("[%d] in mbs_dirdyn_init !! \n", -410);
443
#endif
444
        }
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
445

446
        // growing buffer for user variables
447
448
449
450
        char * path_growing;
        path_growing = (char*) malloc((strlen(respath)+strlen(resfilename)+10) * sizeof(char));
        sprintf(path_growing, "%s/%s", respath, resfilename);
        dd->user_buffer = mbs_new_growing_buffer(dd->options->max_save_user, dd->options->buffersize, path_growing);
451

Nicolas Docquier's avatar
Nicolas Docquier committed
452
        // binding buffers to data that must be saved
453
        dd->savedArrays = (double**) malloc(dd->bufferNb*sizeof(double*));
454
455
456
457
458
459
460
461
        bufId = 0;  dd->savedArrays[bufId] = mbs_data->q+1;
        bufId++  ;  dd->savedArrays[bufId] = mbs_data->qd+1;
        bufId++  ;  dd->savedArrays[bufId] = mbs_data->qdd+1;
        bufId++  ;  dd->savedArrays[bufId] = mbs_data->Qq+1;
        if(Nux){
            bufId++  ;  dd->savedArrays[bufId] = mbs_data->ux+1;
            bufId++  ;  dd->savedArrays[bufId] = mbs_data->uxd+1;
        }
462
463
464
465
466
        if(Nlink){
            bufId++  ;  dd->savedArrays[bufId] = mbs_data->Z +1;
            bufId++  ;  dd->savedArrays[bufId] = mbs_data->Zd+1;            
            bufId++  ;  dd->savedArrays[bufId] = mbs_data->Fl+1;            
        }
467
        if(nqc){
468
469
470
471
472
473
            bufId++  ;  dd->savedArrays[bufId] = mbs_data->Qc +1;
        }
        if(get_output_vector_nb()){
            for (i=0; i<get_output_vector_nb() ; i++){
                bufId++  ;  dd->savedArrays[bufId] = get_output_vector_ptr(i);
            }
474
        }
475

476
        // save the initial config
477
       // dd->options->save2file = 0; // Uncomment this line when compiling with simulink and do not want to save results in files 
478

479
480
       // Save first state of the system
        mbs_fct_dirdyn(dd->tsim, dd->y, dd->yd, mbs_data, dd);
481
        mbs_dirdyn_save(dd, mbs_data, dd->tsim);
482

483

484
        // release memory
485
486
487
488
        free(f_anim);
        free(f);
        free(respath);
        free(animpath);
489
        free(path_growing);
490
491
492
        free(fnameSuffix);
        free(bufferIDs);
        free(bufElemNb);
493
    }
494
495
496
    else{
        reset_flag_output();
    }
497
498
}

499
500
void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data)
{
501
    MbsDirdynOptions* opts;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
502
    double cur_t0, cur_tf;
Nicolas Docquier's avatar
Nicolas Docquier committed
503

504
    opts = dd->options;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
505

506
507
    // Set initial time
    cur_t0 = opts->t0;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
508

509
510
511
512
513
514
515
516
    if (opts->flag_waypoint) // Set intermediate final time
    {
        cur_tf = cur_t0 + opts->delta_t_wp;
    }
    else // Set ending time of simulation
    {
        cur_tf = opts->tf;
    }
517

518
519
    while (cur_tf <= opts->tf)
    {
520
        dd->loop_integrator(cur_t0, cur_tf, mbs_data, dd);
521
522
523
        // Update times
        cur_t0 = cur_tf;
        cur_tf += opts->delta_t_wp;
524

525
526
527
528
        // stop the simulation if 'flag_stop' set to 1
        if (mbs_data->flag_stop)
        {
            break;
529
        }
Nicolas Docquier's avatar
Nicolas Docquier committed
530
    }
531
532
}

533
void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data){
534
    int i;
535
    int err = 0; // error number
536

537
#ifdef REAL_TIME
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
538
    Simu_realtime *realtime;
539
#endif
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
540

541
542
    // ENDING SIMULATION
    // - - - - - - - -
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
543

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
544
    // real-time structure release
545
546
#ifdef REAL_TIME
    realtime = (Simu_realtime*)mbs_data->realtime;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
547

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
548
549
    if (dd->options->realtime)
    {
550
        mbs_realtime_finish((Simu_realtime*) mbs_data->realtime);
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
551
    }
552
#endif
553
554

    // user finalization
555
    user_dirdyn_finish(mbs_data, dd);
556
    dd->finish_integrator(mbs_data, dd);
557
    free(dd->integrator_struct);
558

559
560
    /* TODO : Move in finish_integrator when 2nd order integrator will be implemented */
    // Free state vectors
561
562
563
    free(dd->y);
    free(dd->yd);
    free(dd->yout);
564

565
	// free Qc vector
566
    free_ivec_1(dd->options->compute_Qc);
567

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
568
569
    if(dd->options->save2file)
    {
570
        // write the buffer (in case they were not saved automatically)
571
572
573
574
        err = mbs_dirdyn_write_buffers(dd);

        if (err < 0)
        {
575
            mbs_msg(">>DIRDYN>> ***** mbs_dirdyn_write_buffers : impossible to save the files *****\n");
576
            mbs_msg(">>DIRDYN>>\n");
577
            mbs_error_msg("[%d] in mbs_dirdyn_finish !! \n", -400 + err);
578
579
        }

580

581
582
583
584
585
        // free memory used by buffers
        for(i=0 ; i < dd->bufferNb; i++){
            mbs_delete_buffer(dd->buffers[i]);
        }
        free(dd->buffers);
586

587
#ifdef REAL_TIME
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
588
589
        if (dd->options->save_visu)
        {
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
590
591
592
593
594
595
            for(i=0; i<realtime->options->nb_models; i++)
            {
                mbs_buffer_write(dd->buffer_visu[i]);
                mbs_delete_buffer(dd->buffer_visu[i]);
            }
            free(dd->buffer_visu);
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
596
        }
597
#endif
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
598

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
599
        // user inputs final gestion
600
601
602
603
604
605
606
607
        err = mbs_growing_buffer_write(dd->user_buffer);

        if (err < 0)
        {
            mbs_msg(">>DIRDYN>>\n");
            mbs_msg(">>DIRDYN>> in mbs_dirdyn_finish : while saving results to disk ! !\n");
            mbs_error_msg("[%d] in mbs_fct_dirdyn !! \n", -400 + err);
        }
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
608

609
610
        mbs_delete_growing_buffer(dd->user_buffer);

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
611
612
        // release memory for the 'auto_output' structure
        free_set_output();
613
        reset_flag_output();
614
        free(dd->savedArrays);
615
    }
616
617
}

Nicolas Docquier's avatar
Nicolas Docquier committed
618
619
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

620

621
int mbs_dirdyn_write_buffers(MbsDirdyn* dd){
622
    int i, err =0;
623
    for (i=0 ; i < dd->bufferNb ; i++){
624
625
        err = mbs_buffer_write(dd->buffers[i]);
        if (err < 0) { return err; }
626
    }   
627
    return err;
628
629
630
631
}

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

632

633
void mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn *dd)
Nicolas Docquier's avatar
Nicolas Docquier committed
634
{
635
    int err = 0;
636
    int i;
Nicolas Docquier's avatar
Nicolas Docquier committed
637
638
639
640
641
642
643
644
645
646
647
648
649
650

    s->tsim = tsim;

    // Update state variables
    for(i=1;i<=s->nqu;i++)
    {
        s->q[s->qu[i]] = y[i-1];
        s->qd[s->qu[i]] = y[i+s->nqu-1];
    }
    for(i=1;i<=s->Nux;i++)
    {
        s->ux[i] = y[i+2*s->nqu-1];
    }

651
    if (dd->options->accelred) // Full symbolic computation of qdd (including solving constraint, inv(M), ...)
Nicolas Docquier's avatar
Nicolas Docquier committed
652
    {
653
        err = mbs_accelred(s, tsim);
Nicolas Docquier's avatar
Nicolas Docquier committed
654
655
    }
    else // Numerical solving of constraints and inv(M)
656
    {
657
        err = dirdynared(dd->mbs_aux,s);
Nicolas Docquier's avatar
Nicolas Docquier committed
658
659
    }

660
    if (err < 0) // Error management
661
662
    {
        mbs_msg(">>DIRDYN>>\n");
663
        mbs_msg(">>DIRDYN>>  Error during direct dynamics at time %g s !\n", tsim);
664
        save_realtime_update(dd, s);
665
        mbs_dirdyn_finish(dd, s);
666
        mbs_error_msg("[%d] in mbs_fct_dirdyn !! \n", -400 + err);
667
    }
Nicolas Docquier's avatar
Nicolas Docquier committed
668
    // Update state vector
669
    for (i = 1; i <= s->nqu; i++)
Nicolas Docquier's avatar
Nicolas Docquier committed
670
    {
671
672
        dydt[i - 1] = s->qd[s->qu[i]];
        dydt[i + s->nqu - 1] = s->qdd[s->qu[i]];
673

Nicolas Docquier's avatar
Nicolas Docquier committed
674
    }
675

676
677
678
    // User Derivatives
    if(s->Nux>0) user_Derivative(s);

Nicolas Docquier's avatar
Nicolas Docquier committed
679
680
    for(i=1;i<=s->Nux;i++)
    {
681
682
683
        if (isnan(s->uxd[i])) // Error management
        {
            mbs_msg(">>DIRDYN>> User derivative uxd[%d] is Nan\n", i);
684
            mbs_msg(">>DIRDYN>> Error in state vector at time %g s ! \n", tsim);
685
            save_realtime_update(dd, s);
686
            mbs_dirdyn_finish(dd, s);
687
            mbs_error_msg("[%d] in mbs_fct_dirdyn !! \n", -447);
688
        }
689
690

        dydt[i+2*s->nqu-1] = s->uxd[i];
Nicolas Docquier's avatar
Nicolas Docquier committed
691
692
693
    }
}

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
694
void save_realtime_update(MbsDirdyn* dd, MbsData* mbs_data)
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
695
{
696
#ifdef REAL_TIME
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
697
    Simu_realtime *realtime;
698
#endif
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
699

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
700
    // save results to the buffer
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
701
    if(dd->options->save2file)
702
    {
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
703
        dd->savePeriodCounter++;
704

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
705
        if(dd->savePeriodCounter%dd->options->saveperiod == 0)
706
        {
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
707
            mbs_dirdyn_save(dd, mbs_data, mbs_data->tsim);
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
708
709
710
711
        }
    }

    // real-time modules
712
#ifdef REAL_TIME
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
713
    if (dd->options->realtime)
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
714
    {
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
715
        realtime = (Simu_realtime*) mbs_data->realtime;
716

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
717
        // update real-time buffers
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
718
        mbs_realtime_update(realtime, mbs_data->tsim);
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
719
720

        // real-time loop iteration
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
721
        mbs_realtime_loop(realtime, mbs_data->tsim);
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
722

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
723
        // quit the simulation
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
724
725
        if (realtime->simu_quit)
        {
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
726
            mbs_data->flag_stop = 1;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
727
        }
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
728
    }
729
#endif
730
}