mbs_dirdyn.c 32.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"
25
#include "mbs_check.h"
26
#include "mbs_errors_names.h"
Nicolas Docquier's avatar
Nicolas Docquier committed
27

28
#include "mbs_message.h"
29

30
 /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
Nicolas Docquier's avatar
Nicolas Docquier committed
31

32
33
MbsDirdyn* mbs_new_dirdyn(MbsData* mbs_data)
{
34
    MbsDirdyn* mbs_dirdyn;
Nicolas Docquier's avatar
Nicolas Docquier committed
35
    MbsAux* mbs_aux;
36

Nicolas Docquier's avatar
Nicolas Docquier committed
37
38
    // Initialize the local data struct
    mbs_aux = initMbsAux(mbs_data);
39
    mbs_dirdyn = mbs_new_dirdyn_aux(mbs_data, mbs_aux);
40

41
    return mbs_dirdyn;
Nicolas Docquier's avatar
Nicolas Docquier committed
42
43
}

44
45
MbsDirdyn* mbs_new_dirdyn_aux(MbsData* mbs_data, MbsAux* mbs_aux)
{
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
46
    MbsDirdyn *dirdyn;
Nicolas Docquier's avatar
Nicolas Docquier committed
47
48
49
    MbsDirdynOptions* opts;

    // Initialize the direct dynamic structure
50
    dirdyn = (MbsDirdyn*)malloc(sizeof(MbsDirdyn));
Nicolas Docquier's avatar
Nicolas Docquier committed
51
52
53
54
55

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

    // Initialize the options struct with default options
56
    opts = (MbsDirdynOptions*)malloc(sizeof(MbsDirdynOptions));
57
    dirdyn->options = opts;
58

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

75
    opts->compute_all_uxd = 1;
76

Sebastien Timmermans's avatar
Sebastien Timmermans committed
77
78
    opts->flag_oneshot = 0;

79
    opts->flag_compute_Qc = COMPUTE_ALL_QC;
80
    // Initialization for fast computation of Qc
81
    int i;
82
    opts->compute_Qc = get_ivec_1(mbs_data->njoint);
83
84
    for (i = 1; i <= mbs_data->njoint; i++) {
        opts->compute_Qc[i] = 0;
85
    }
86

87
88
89
90
91
92
93
    /*------------------------------------------*/
    /* Options related to numerical integration */
    /*------------------------------------------*/
    opts->integrator = RK4;
    // Various
    opts->verbose = 1;
    opts->flag_stop_stiff = 0;
94
    opts->flag_precise_dynamics = 1;
95
96
97
98
    //Baumgarte stabilization
    opts->flag_baumgarte_stabilization = 0;
    opts->baumgarte_alpha = 0.1;
    opts->baumgarte_beta = 0.1;
99
    // Waypoints
100
    opts->flag_waypoint = 0;
101
102
    opts->flag_solout_wp = 0;
    opts->delta_t_wp = 1.0e-3;
103

104
    // Options related to variable time-stepped methods
105
106
107
108
109
    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
110

111
112
    // Options related to integrator that need Jacobian computation
    opts->n_freeze = 0;
113
114
    // Other options
    opts->show_failed_closure = 0;
115

116
    // initial the saving counter
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132

    // Set pointer and value to default values.
    dirdyn->tsim = 0;
    dirdyn->dt = 0;
    dirdyn->nState = 0;

    dirdyn->y = NULL;
    dirdyn->yout = NULL;
    dirdyn->yd = NULL;

    dirdyn->buffers = NULL;
    dirdyn->buffer_visu = NULL;
    dirdyn->user_buffer = NULL;
    dirdyn->bufferNb = 0;
    dirdyn->savedArrays = NULL;

133
134
    dirdyn->savePeriodCounter = 0;

135
136
137
138
139
    dirdyn->integrator_struct = NULL;
    dirdyn->initialize_integrator = NULL;
    dirdyn->loop_integrator = NULL;
    dirdyn->finish_integrator = NULL;

Nicolas Docquier's avatar
Nicolas Docquier committed
140
141
142
    return dirdyn;
}

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
169
170
171
172
173
174
175

int check_user_dirdyn_options(MbsDirdyn* dd, MbsData* mbs_data)
{
    int i;
    int err = 0;
    // errors generated
    if (dd->options->t0 > dd->options->tf)
    {
        err = _MBS_ERR_INIT;
        mbs_msg("\t >Dirdyn> options error: t0 (%f) > tf (%f).\n", dd->options->t0, dd->options->tf);
        mbs_msg("[%d] in check_user_dirdyn_options !! \n", err);
        return err;
    }

    if (dd->options->dt0 <= 0.0)
    {
        err = _MBS_ERR_INIT;
        mbs_msg("\t >Dirdyn> options error: dt0 (%f) is not strictly positive.\n", dd->options->dt0);
        mbs_msg("[%d] in check_user_dirdyn_options !! \n", err);
        return err;
    }

    if (dd->options->t0 == dd->options->tf)
    {
        err = _MBS_ERR_INIT;
        mbs_msg("\t >Dirdyn> options error: t0 (%f) == tf (%f).\n", dd->options->t0, dd->options->tf);
        mbs_msg("\t >Dirdyn> if you need one evaluation of the derivative function, see the oneshot option in dirdyn. \n");
        mbs_msg("[%d] in check_user_dirdyn_options !! \n", err);
        return err;
    }

    return err;
}
Nicolas Docquier's avatar
Nicolas Docquier committed
176
177
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
178
179
void mbs_delete_dirdyn(MbsDirdyn* dirdyn, MbsData* mbs_data)
{
180
181
182
183
184
185
186
    if (!dirdyn){
        return;
    }
    if (dirdyn->options){
        free_ivec_1(dirdyn->options->compute_Qc);
        free(dirdyn->options);
    }
Nicolas Docquier's avatar
Nicolas Docquier committed
187
188
189
190
191
192
    freeMbsAux(dirdyn->mbs_aux, mbs_data);
    free(dirdyn);
}

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

193
int mbs_run_dirdyn(MbsDirdyn* dd, MbsData* mbs_data)
194
{
195
    int err = 0;
Sebastien Timmermans's avatar
Sebastien Timmermans committed
196
197
198
199
200

    if (dd->options->flag_oneshot)
    {
        mbs_msg("\n>>DIRDYN>> One shot evaluation of the direct dynamics is activated.\n");
        dd->options->resfilename = "oneshot";
201
        mbs_msg("\n>>DIRDYN>> Output filename is called oneshot by default.\n");
Sebastien Timmermans's avatar
Sebastien Timmermans committed
202
203
    }

204
    // 1. Initialize the simulation
205
    // - - - - - - - - - - - - - -
206
    err = mbs_dirdyn_init(dd, mbs_data);
Sebastien Timmermans's avatar
Sebastien Timmermans committed
207
    if (err < 0) {
208
        // Freeing the buffers, without error catching
209
        mbs_dirdyn_finish(dd, mbs_data);
210
211
        mbs_error_msg("[%d] in mbs_run_dirdyn !! \n", _MBS_ERR_MOD_DIRDYN + err);
        return _MBS_ERR_MOD_DIRDYN + err;
212
    }
213

214
    // 2. Run the simulation
215
    // - - - - - - - - - - -
Sebastien Timmermans's avatar
Sebastien Timmermans committed
216
217
218
219
220
221
222
223
224
    if (dd->options->flag_oneshot)
    {
        err = mbs_fct_dirdyn(dd->tsim, dd->y, dd->yd, mbs_data, dd);
    }
    else
    {
        err = mbs_dirdyn_loop(dd, mbs_data);
    }
    if (err < 0) {
225
226
        // Freeing the buffers, without error catching
        mbs_dirdyn_finish(dd, mbs_data);
227
228
        mbs_error_msg("[%d] in mbs_run_dirdyn !! \n", _MBS_ERR_MOD_DIRDYN + err);
        return _MBS_ERR_MOD_DIRDYN + err;
229
    }
230

Sebastien Timmermans's avatar
Sebastien Timmermans committed
231

232
    // 3. Finish the simulation
233
    // - - - - - - - - - - - -
234
    err = mbs_dirdyn_finish(dd, mbs_data);
235
236
237
238
    if (err < 0) {
        mbs_error_msg("[%d] in mbs_run_dirdyn !! \n", _MBS_ERR_MOD_DIRDYN + err);
        return _MBS_ERR_MOD_DIRDYN + err;
    }
239
240
241
242
243
244
    
    if (dd->options->verbose)
    {
        mbs_msg("\n>>DIRDYN>> Direct dynamics analysis finished.\n");
    }

245
    return err;
246
247
}

248
int mbs_dirdyn_save(MbsDirdyn* dd, MbsData *mbs_data, double t) {
249

250
    int i, err = 0;
251
#ifdef VISU_3D
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
252
    Simu_realtime *realtime;
253
    Realtime_visu *visu;
254
#endif
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
255

256
    for (i = 0; i < dd->bufferNb; i++) {
257
258
        err = mbs_buffer_save(dd->buffers[i], t, dd->savedArrays[i]);
    }
259

260
261
262
263
    if (err < 0)
    {
        mbs_msg(">>DIRDYN>> ***** mbs_dirdyn_save : impossible to save the files *****\n");
        mbs_msg(">>DIRDYN>> ***** during buffer save *****\n");
264
265
        mbs_msg("[%d] in mbs_dirdyn_save !! \n", err);
        return err;
266
    }
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
267

268
    err = mbs_growing_buffer_save(dd->user_buffer, t);
269

270
271
272
273
    if (err < 0)
    {
        mbs_msg(">>DIRDYN>> ***** mbs_dirdyn_save : impossible to save the files *****\n");
        mbs_msg(">>DIRDYN>> ***** during growing buffer save *****\n");
274
275
        mbs_msg("[%d] in mbs_dirdyn_save !! \n", err);
        return  err;
276
    }
277

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
278
279
    if (dd->options->save_visu)
    {
280
281
#ifdef VISU_3D
        realtime = (Simu_realtime*)mbs_data->realtime;
282
        visu = realtime->ext->visu;
283

284
        user_realtime_visu(mbs_data, visu->nb_models, visu->nb_q, visu->anim_q);
285

286
        for (i = 0; i < realtime->options->nb_models; i++)
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
287
        {
288
289
290
291
292
293
294
            err = mbs_buffer_save(dd->buffer_visu[i], t, visu->anim_q[i]);
        }
        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>> in the REAL-TIME process \n");
295
296
            mbs_msg("[%d] in mbs_dirdyn_save !! \n", err);
            return err;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
297
        }
298
#endif
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
299
    }
300
301

    return 0;
302
}
303

304
int mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
305
{
306
    int i, err;
307

308
#ifdef VISU_3D
309
310
    Simu_realtime *realtime;
    Realtime_visu *visu;
311
#endif
312
313
314
315
316
317

    if (dd->options->verbose)
    {
        mbs_msg("\n>>DIRDYN>> Starting direct dynamics module.\n");
    }

318
319
320
    // Checking mbs_data coherence
    err = mbs_check_mbs_data_values(dd->mbs_aux, mbs_data);
    if (err < 0) {
321
        mbs_msg("\t >Dirdyn> Incoherences detected during module initialization! (See message above) \n");
322
323
        mbs_msg("[%d] in mbs_dirdyn_init !! \n", err);
        return err;
324
325
    }

326
327
    // Check the usefulness of dirdyn
    //----------------
328
    if (mbs_data->nqu + mbs_data->nqc == 0)
329
330
331
    {
        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");
332
333
        err =  _MBS_ERR_MOD_SPEC_11;
        mbs_msg("[%d] in mbs_dirdyn_init !! \n", err);
334
        return err;
335
336
337
338
339
    }
    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");
    }
340

341
342
    // INITIALIZATION
    // - - - - - - - -
Nicolas Docquier's avatar
Nicolas Docquier committed
343

344
345
    // Checking constraints and dependant variable coherence
    err = mbs_check_nhu_nqv(mbs_data);
346
    if (err < 0) {
347
348
        mbs_msg("\t >Dirdyn> Incoherence detected during module initialization! \n");
        mbs_msg("[%d] in mbs_dirdyn_init !! \n", err);
349
        return err;
350
    }
351
352
    // Set the selected integrator
    set_integrator(dd);
353

354
355
356
357
358
    // 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;
359
360
    // Other
    dd->mbs_aux->close_anim = dd->options->show_failed_closure;
361

362
363
    /* TODO : Move in initialize_integrator when 2nd order integrator will be implemented */
    // Allocate state vectors
364
    dd->nState = (dd->options->compute_all_uxd) ? 2 * mbs_data->nqu + mbs_data->Nux : 2 * mbs_data->nqu; // depending if we want to compute the uxd
365
    dd->y = get_dvec_0(dd->nState);
366
    dd->yout = get_dvec_0(dd->nState);
367
    dd->yd = get_dvec_0(dd->nState);
368

369
370
    // initialyze integrator
    dd->initialize_integrator(mbs_data, dd);
371

372
373
    // check (and print) warnings for users options of integrator
    print_warnings_integrator(dd, dd->options->integrator);
374

375
    // real-time modules activated
376
#ifdef REAL_TIME
377
378
379
    if (dd->options->realtime)
    {
        mbs_realtime_reset();
380
381
382
383
        err = mbs_realtime_init(mbs_data, dd->options->t0, dd->options->tf, dd->options->dt0);
        if (err < 0)
        {
            mbs_msg("\t >Real-time> error during activation !\n");
384
            mbs_msg("[%d] in mbs_dirdyn_init, shutting down the process !! \n", err);
385
            return err;
386
        }
387
    }
388
#else
389
390
    if (dd->options->realtime)
    {
391
        err = _MBS_ERR_INIT;
392
        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");
393
        mbs_msg("[%d] in mbs_dirdyn_init, shutting down the process !! \n", err);
394
        return err;
395
    }
396
#endif
397

398
399
400
401
402
403
404
405
406
407
    // Setting the Qc to be cumputed in dirdynared
    if (dd->options->flag_compute_Qc == COMPUTE_NO_QC)
    {
        for (i = 1; i <= mbs_data->njoint; i++)
        {
            dd->mbs_aux->compute_Qc_vec[i] = 0;
        }
    }
    else if (dd->options->flag_compute_Qc == COMPUTE_CUSTOM_QC)
    {
408
        copy_ivec_1(dd->options->compute_Qc, dd->mbs_aux->compute_Qc_vec, mbs_data->njoint);
409
410
411
412
413
414
415
416
    }
    else
    {
        for (i = 1; i <= mbs_data->njoint; i++)
        {
            dd->mbs_aux->compute_Qc_vec[i] = 1;

        }
417
    }
418

419
420
    // init time
    dd->tsim = dd->options->t0;
421
    dd->dt = dd->options->dt0;
422

423
    // user initialization
424
    if (dd->options->save2file)
425
426
427
428
    {
        // 'auto_output' structure initialized, needed for add_output_vector
        init_set_output(dd->options->max_save_user);
    }
429
    user_dirdyn_init(mbs_data, dd);
Nicolas Docquier's avatar
Nicolas Docquier committed
430

431
432
433
434
435
436
437
438
    // Checking time options
    err = check_user_dirdyn_options(dd, mbs_data);
    if (err < 0) {
        mbs_msg("\t >Dirdyn> Incoherence detected during module initialization! \n");
        mbs_msg("[%d] in mbs_dirdyn_init !! \n", err);
        return err;
    }

439
    /* TODO : Move in initialize_integrator when 2nd order integrator will be implemented */
Nicolas Docquier's avatar
Nicolas Docquier committed
440
    // Simulation state initialization
441
    for (i = 1; i <= mbs_data->nqu; i++)
Nicolas Docquier's avatar
Nicolas Docquier committed
442
    {
443
444
        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
445
    }
446
    if (dd->options->compute_all_uxd) // do not add uxd, if user asks to not compute them during dd
Nicolas Docquier's avatar
Nicolas Docquier committed
447
    {
448
449
450
451
        for (i = 1; i <= mbs_data->Nux; i++)
        {
            dd->y[i - 1 + 2 * mbs_data->nqu] = mbs_data->ux[i];
        }
Nicolas Docquier's avatar
Nicolas Docquier committed
452
453
    }

454
    if (dd->options->save2file)
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
455
    {
456
        char *resfilename, *respath, *animpath;
457
        int bufId, bufSize, njoint, Nux, Nlink, nqc, n_hu;
458
        char* f;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
459
        char* f_anim;
460

461
462
463
        char **fnameSuffix;
        int *bufferIDs;
        int *bufElemNb;
464

465
        njoint = mbs_data->njoint;
466
        Nux = mbs_data->Nux;
467
        Nlink = mbs_data->Nlink;
468
        nqc = mbs_data->nqc;
469
        n_hu = mbs_data->nhu;
470
471

        // allocate buffer at needed size
472
        int compute_uxd = Nux && dd->options->compute_all_uxd; // if user do not want to compute uxd
473
        bufSize = 4 + (compute_uxd ? 2 : 0) + (Nlink ? 3 : 0) + (nqc ? 1 : 0) + (n_hu ? 1 : 0) + get_output_vector_nb();
474
475
476
477
        fnameSuffix = (char**)malloc(bufSize * sizeof(char*));
        bufferIDs = (int*)malloc(bufSize * sizeof(int));
        bufElemNb = (int*)malloc(bufSize * sizeof(int));

478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
        // 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);
        }

510
        // generic buffer properties
511
512
513
514
515
516
        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;

517
        // buffer properties for user states, if any
518
        if (Nux&&dd->options->compute_all_uxd) {
519
            int id = dd->bufferNb;
520
521
            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;
522
523
            dd->bufferNb += 2;
        }
524

Nicolas Docquier's avatar
Nicolas Docquier committed
525
        // buffer properties for links (link 1D only)
526
        if (mbs_data->Nlink) {
527
            int id = dd->bufferNb;
528
529
530
531
            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;
532
        }
533

Nicolas Docquier's avatar
Nicolas Docquier committed
534
        // buffer properties for joint/force associated to driven joints
535
        if (nqc) {
536
537
            int id = dd->bufferNb;
            // A column is saved for each joint, even if there are not driven !
538
539
            fnameSuffix[id + 0] = "Qc";    bufferIDs[id + 0] = BUFFER_QC;    bufElemNb[id + 0] = njoint;
            dd->bufferNb += 1;
540
        }
541
542

        // buffer properties for Lagrange multipliers associated to constraints
543
        if (n_hu) {
544
545
546
547
548
549
            int id = dd->bufferNb;
            // There is as many columns as there is joint, only the first MbsData::Ncons columns will be pupulated !
            fnameSuffix[id + 0] = "Lambda";    bufferIDs[id + 0] = BUFFER_LAMBDA;    bufElemNb[id + 0] = njoint;
            dd->bufferNb += 1;
        }

550
        // buffer properties for the user specified vectors
551
        if (get_output_vector_nb()) {
552
            int id = dd->bufferNb;
553
554
555
            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;
556
557
558
559
                dd->bufferNb += 1;
            }

        }
560

561
        // set the buffer size if not specified
562
        if (dd->options->buffersize < 1) {
563
            // compute size assuming a constant time step size
564
565
566
            dd->options->buffersize = (int)((dd->options->tf - dd->options->t0) /
                (dd->dt * dd->options->saveperiod)
                + 1);
567
            // add some extra space
568
            dd->options->buffersize = (int)(dd->options->buffersize * 1.1);
569
570
            dd->options->buffersize += 10;
        }
571

572
        // initialize the buffers for saving results
573
        dd->buffers = (MbsBuffer**)malloc(dd->bufferNb * sizeof(MbsBuffer*));
574

575
576
        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
577

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

582
            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);
583
            if (dd->buffers[bufId] == NULL)
584
585
586
587
588
589
590
591
592
593
            {
                free(f);
                free(f_anim);
                free(respath);
                free(animpath);
                free(fnameSuffix);
                free(bufferIDs);
                free(bufElemNb);

                mbs_msg("\t >Dirdyn> Error during buffer visu initialization num %d! \n", i);
594
595
                mbs_msg("[%d] in mbs_dirdyn_init !! \n", _MBS_ERR_MOD_SPEC_11);
                return _MBS_ERR_MOD_SPEC_11;
596
            }
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
597
598
599
600
        }

        if (dd->options->save_visu)
        {
601
#ifdef VISU_3D
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
602
603
            if (!dd->options->realtime)
            {
604
605
606
607
608
609
610
611
                free(f);
                free(f_anim);
                free(respath);
                free(animpath);
                free(fnameSuffix);
                free(bufferIDs);
                free(bufElemNb);

612
                mbs_msg("\t >Real-time> Error: real-time features must be activated to set 'save_visu' to 1 ! \n");
613
                mbs_msg("[%d] in mbs_dirdyn_init, shutting down the process !! \n",  _MBS_ERR_INIT);
614
                return _MBS_ERR_INIT;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
615
616
            }

617
            realtime = (Simu_realtime*)mbs_data->realtime;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
618
619
620

            if (!realtime->options->flag_visu)
            {
621
622
623
624
625
626
627
628
                free(f);
                free(f_anim);
                free(respath);
                free(animpath);
                free(fnameSuffix);
                free(bufferIDs);
                free(bufElemNb);

629
                mbs_msg("\t >Real-time> Error: flag_visu must be set to 1 to set 'save_value' to 1 ! \n");
630
                mbs_msg("[%d] in mbs_dirdyn_init, shutting down the process !! \n", _MBS_ERR_INIT);
631
                return _MBS_ERR_INIT;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
632
633
            }

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

637
            for (i = 0; i < realtime->options->nb_models; i++)
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
638
            {
639
                sprintf(f, "%s/visu_%d_q.res", respath, i);
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
640
                sprintf(f_anim, "%s/visu_%d_q.anim", animpath, i);
641

642
                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);
643
644
645
646
647
648
649
650
651
652
653
                if (dd->buffer_visu[i] == NULL)
                {
                    free(f);
                    free(f_anim);
                    free(respath);
                    free(animpath);
                    free(fnameSuffix);
                    free(bufferIDs);
                    free(bufElemNb);

                    mbs_msg("\t >Dirdyn> [REAL-TIME] Error during buffer visu initialization num %d! \n", i);
654
                    mbs_msg("[%d] in mbs_dirdyn_init, shutting down the process !! \n", _MBS_ERR_MOD_SPEC_11);
655
                    return _MBS_ERR_MOD_SPEC_11;
656
                }
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
657
            }
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
658

659
#else
660
661
662
663
664
665
666
667
            free(f);
            free(f_anim);
            free(respath);
            free(animpath);
            free(fnameSuffix);
            free(bufferIDs);
            free(bufElemNb);

668
            mbs_msg("\t >Real-time> Error: Java libraries must be activated to set 'save_visu' to 1 ! \n");
669
670
            mbs_msg("[%d] in mbs_dirdyn_init !! \n",  _MBS_ERR_INIT);
            return _MBS_ERR_INIT;
671
#endif
672
        }
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
673

674
        // growing buffer for user variables
675
        char * path_growing;
676
        path_growing = (char*)malloc((strlen(respath) + strlen(resfilename) + 10) * sizeof(char));
677
678
        sprintf(path_growing, "%s/%s", respath, resfilename);
        dd->user_buffer = mbs_new_growing_buffer(dd->options->max_save_user, dd->options->buffersize, path_growing);
679

Nicolas Docquier's avatar
Nicolas Docquier committed
680
        // binding buffers to data that must be saved
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
        dd->savedArrays = (double**)malloc(dd->bufferNb * sizeof(double*));
        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&&dd->options->compute_all_uxd) {
            bufId++;  dd->savedArrays[bufId] = mbs_data->ux + 1;
            bufId++;  dd->savedArrays[bufId] = mbs_data->uxd + 1;
        }
        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;
        }
        if (nqc) {
            bufId++;  dd->savedArrays[bufId] = mbs_data->Qc + 1;
        }
698
        if (n_hu) {
699
700
            bufId++;  dd->savedArrays[bufId] = mbs_data->lambda + 1;
        }
701
702
703
        if (get_output_vector_nb()) {
            for (i = 0; i < get_output_vector_nb(); i++) {
                bufId++;  dd->savedArrays[bufId] = get_output_vector_ptr(i);
704
            }
705
        }
706

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

710
       // Save first state of the system
711
712
        err = mbs_fct_dirdyn(dd->tsim, dd->y, dd->yd, mbs_data, dd);
        if (err < 0){
713

714
            // Just this scope to clean.
715
716
717
718
719
720
721
722
723
            free(f);
            free(f_anim);
            free(respath);
            free(animpath);
            free(fnameSuffix);
            free(bufferIDs);
            free(bufElemNb);
            free(path_growing);

724
            mbs_msg("[%d] in mbs_dirdyn_init !! \n", err);
725
726
727
            return err;
        }
        err = mbs_dirdyn_save(dd, mbs_data, dd->tsim);
728
729
        if (err < 0)
        {
730
731
732
733
734
735
736
737
738
            free(f);
            free(f_anim);
            free(respath);
            free(animpath);
            free(fnameSuffix);
            free(bufferIDs);
            free(bufElemNb);
            free(path_growing);

739
            mbs_msg("[%d] in mbs_dirdyn_init !! \n", err);
740
741
            return err;
        }
742

743
        // release memory
744
745
746
747
        free(f_anim);
        free(f);
        free(respath);
        free(animpath);
748
        free(path_growing);
749
750
751
        free(fnameSuffix);
        free(bufferIDs);
        free(bufElemNb);
752
    }
753
    else {
754
755
        reset_flag_output();
    }
756
757

    return 0;
758
759
}

760
int mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data)
761
{
762
    MbsDirdynOptions* opts;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
763
    double cur_t0, cur_tf;
764
    int err = 0, err2 =0;
Nicolas Docquier's avatar
Nicolas Docquier committed
765

766
    opts = dd->options;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
767

768
769
    // Set initial time
    cur_t0 = opts->t0;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
770

771
772
773
774
775
776
777
778
    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;
    }
779

780
781
    while (cur_tf <= opts->tf)
    {
782
        err = dd->loop_integrator(cur_t0, cur_tf, mbs_data, dd);
783
784
        if (err < 0)
        {
785
786
787
788
            err2 = save_realtime_update(dd, mbs_data);
            if (err2 < 0) {
                mbs_msg("\t >Dirdyn> Error: in dirdyn loop during save realtime update  ! \n");
            }
789
            mbs_msg(">>DIRDYN>> [%d] during direct dynamics loop.\n", err);
Sebastien Timmermans's avatar
Sebastien Timmermans committed
790
            return (err < 0) ? err : err2;
791
        }
792
793
794
        // Update times
        cur_t0 = cur_tf;
        cur_tf += opts->delta_t_wp;
795

796
797
798
        // stop the simulation if 'flag_stop' set to 1
        if (mbs_data->flag_stop)
        {
799
            return 0;
800
        }
Nicolas Docquier's avatar
Nicolas Docquier committed
801
    }
Sebastien Timmermans's avatar
Sebastien Timmermans committed
802

803
    return 0;
804
805
}

806
int mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data) {
807
    if (!dd){
808
        return 0;
809
    }
810
    int i;
811
    int err = 0; // error number
812

813
#ifdef REAL_TIME
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
814
    Simu_realtime *realtime;
815
#endif
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
816

817
818
    // ENDING SIMULATION
    // - - - - - - - -
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
819

Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
820
    // real-time structure release
821
822
#ifdef REAL_TIME
    realtime = (Simu_realtime*)mbs_data->realtime;
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
823

824
    if (dd->options->realtime && !realtime)
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
825
    {
826
827
828
829
        err = mbs_realtime_finish((Simu_realtime*)mbs_data->realtime);
        if (err < 0)
        {
            mbs_msg(">>DIRDYN>> ***** realtime : impossible to finish !\n");
830
            mbs_msg("[%d] in mbs_dirdyn_finish !! \n", err);
831
            return (err);
832
        }
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
833
    }
834
#endif
835
836

    // user finalization
837
    user_dirdyn_finish(mbs_data, dd);
838
    if (dd->finish_integrator){
839
840
        dd->finish_integrator(mbs_data, dd);
    }
841
    free(dd->integrator_struct);
842
843
844
845
    // Set pointers to NULL
    dd->initialize_integrator = NULL;
    dd->loop_integrator = NULL;
    dd->finish_integrator = NULL;
846
    dd->integrator_struct = NULL;
847

848
849
    /* TODO : Move in finish_integrator when 2nd order integrator will be implemented */
    // Free state vectors
850
851
852
    free(dd->y);
    free(dd->yd);
    free(dd->yout);
853
854
855
    dd->y = NULL;
    dd->yd = NULL;
    dd->yout = NULL;
856

857
    if (dd->options->save2file)
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
858
    {
859
860
        // both buffers write are called and memory released before raising error.
        int err2, err1;
861

862
863
864
        // write the buffer (in case they were not saved automatically)
        err1 = mbs_dirdyn_write_buffers(dd);
        if (err1 < 0)
865
        {
866
            mbs_msg(">>DIRDYN>> ***** mbs_dirdyn_write_buffers : impossible to save the files!\n");
867
            mbs_msg("[%d] in mbs_dirdyn_finish !! \n", err);
868
            return(err);
869
870
        }

871
        // free memory used by buffers
872
        for (i = 0; i < dd->bufferNb; i++) {
873
874
875
            mbs_delete_buffer(dd->buffers[i]);
        }
        free(dd->buffers);
876
        dd->buffers = NULL;
877

878
#ifdef REAL_TIME
Nicolas Van der Noot's avatar
Nicolas Van der Noot committed
879
880
        if (dd->options->save_visu<