mbs_load_xml.c 28 KB
Newer Older
1
2
#include "mbs_load_xml.h"
#include "mbs_load_libraries.h"
3
#include "useful_functions.h"
4
#include "mbs_path.h"
5
#include "mbs_message.h"
6
#include "mbs_check.h"
7
#include "mbs_errors_names.h"
8

9
10
11
12
13
14
15
#include <string.h>


// if the flage fo accessing user/symbolic functions via pointers is active
// define pre-processor functions to access functions for user_model and user_IO
// This macro works only in a function where a MbsData is stored in a variable called s
#ifdef PRJ_FCT_PTR
16
17
18
19
20
21
22
23
24
25
#define mbs_new_user_model()                                s->fct.user.mbs_new_user_model()
#define mbs_load_user_model_xml(mbs_infos, um)              s->fct.user.mbs_load_user_model_xml(mbs_infos, um)
#define mbs_bind_user_model(mbs_infos, um)                  s->fct.user.mbs_bind_user_model(mbs_infos, um)
#define mbs_delete_user_model(um)                           s->fct.user.mbs_delete_user_model(um)
#define mbs_get_user_model_size(n_usr_model)                s->fct.user.mbs_get_user_model_size(n_usr_model)
#define mbs_get_user_model_list(user_model_list)            s->fct.user.mbs_get_user_model_list(user_model_list)
#define mbs_new_user_IO(a)                                  s->fct.user.mbs_new_user_IO(a)
#define mbs_delete_user_IO(uio)                             s->fct.user.mbs_delete_user_IO(uio)
#define mbs_get_user_IO_size(n_in, n_out, n_user_IO)        s->fct.user.mbs_get_user_IO_size(n_in, n_out, n_user_IO)
#define user_load_post(data)                                s->fct.user.user_load_post(data);
26
#else
27
#include "mbs_project_interface.h"
28
29
30
31
32
#endif

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


33
34
MbsData* mbs_load(const char* mbs_filename, const char* build_path)
{
35
    MbsData *mbs_data = NULL;
36

37
    MbsLoader *loader;
38
    loader = mbs_new_loader();
39
    mbs_data = mbs_load_with_loader(mbs_filename, build_path, loader);
40
    mbs_delete_loader(loader);
41
42
43
44
    if (mbs_data == NULL)
    {
        mbs_error_msg("in mbs_load !! \n");
    }
45

46
47
48
    return mbs_data;
}

49
50
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

51
MbsData* mbs_load_with_loader(const char* mbs_filename, const char* build_path, MbsLoader *mbs_loader)
52
{
53
    int err = 0;
54
    MbsInfos *mbs_infos;
55
    MbsData *mbs_data;
56

57
58
    MbsLoadOptions *opts;
    opts = mbs_loader->opts;
59

60
    int custom_mbs_info;
61

62
    if (mbs_loader->mbs_infos == NULL)
63
    {
64
        custom_mbs_info = 0;
65
        mbs_infos = mbs_info_reader(mbs_filename);
Olivier Lantsoght's avatar
Olivier Lantsoght committed
66
        if (!mbs_infos){
67
            mbs_msg(">>LOAD>> unable to create MbsInfo.\n");
68
            mbs_msg("[%d] in mbs_load_with_loader !! \n", _MBS_ERR_MOD_LOAD + _MBS_ERR_LOW_FILES);
69
70
            return NULL;
        }
71
72
73
    }
    else
    {
74
        custom_mbs_info = 1;
75
        mbs_infos = mbs_loader->mbs_infos;
76
    }
77

78
    // local names
79
80
81
82
    char *symbolicLib_name = NULL;
    char *symbolicLib_path = NULL;
    char *userfctLib_name = NULL;
    char *userfctLib_path = NULL;
83
    //-------------------------------------------------------------------------
84
    if (opts->load_symb_user != LOAD_NONE) {
85
86
87
88
89
        // 0. build
        if (build_path == NULL) // Error management
        {
            mbs_msg(">>LOAD>> build_path is not defined \n");
            mbs_msg("\n>>LOAD>> Check your files names and paths \n\n");
90
            mbs_msg("[%d] in mbs_load_with_loader !! \n", _MBS_ERR_MOD_LOAD +_MBS_ERR_LOW_FILES);
91
92
93
94
95
96
97
98
            free(symbolicLib_name);
            free(symbolicLib_path);
            free(userfctLib_name);
            free(userfctLib_path);
            if(!custom_mbs_info){
                mbs_delete_infos(mbs_infos);
            }
            return NULL;
99
        }
100

101
102
103
        // 1. symbolic Library name (without extension)
        if (opts->symbolicLib_name == NULL)
        {
104
            symbolicLib_name = (char*)malloc(sizeof(char)*(strlen(mbs_infos->mbsfile) + 100));
105
106
107
108
109
110
111
112
113
            sprintf(symbolicLib_name, "Project_symbolic");
        }
        else
        {
            symbolicLib_name = strdup(opts->symbolicLib_name);
        }
        // 2. symbolic Library path (without name)
        if (opts->symbolicLib_path == NULL)
        {
114
            symbolicLib_path = (char*)malloc(sizeof(char)*(strlen(mbs_infos->mbsfile) + 100));
115

116
117
118
119
120
#ifdef UNIX // differences between linux and windows in the default configuration: To clean up One day !
            sprintf(symbolicLib_path, "%s/symbolicR", build_path);
#else
            strcpy(symbolicLib_path, build_path);
#endif
121
122
123
124
125
        }
        else
        {
            symbolicLib_path = strdup(opts->symbolicLib_path);
        }
126

127
        // 3. userfct Library name (without extension)
128
        if (opts->load_symb_user == LOAD_ALL)
129
        {
130
131
132
133
134
135
136
137
138
139
140
141
142
            if (opts->userfctLib_name == NULL)
            {
                userfctLib_name = (char*)malloc(sizeof(char)*(strlen(mbs_infos->mbsfile) + 100));
                sprintf(userfctLib_name, "Project_userfct");
            }
            else
            {
                userfctLib_name = strdup(opts->userfctLib_name);
            }
            // 4. userfct Library path (without name)
            if (opts->userfctLib_path == NULL)
            {
                userfctLib_path = (char*)malloc(sizeof(char)*(strlen(mbs_infos->mbsfile) + 100));
143

144
#ifdef UNIX // To clean up One day !
145
                sprintf(userfctLib_path, "%s/userfctR", build_path);
146
#else
147
                strcpy(userfctLib_path, build_path);
148
#endif
149
150
151
152
153
            }
            else
            {
                userfctLib_path = strdup(opts->userfctLib_path);
            }
154
        }
155
156
    }

157
    //-------------------------------------------------------------------------
158

159
    mbs_data = mbs_new_data();
160
    mbs_data->project_path = (char*)malloc(sizeof(char)*(strlen(mbs_filename) + 100));
161
162
163
164
165
166
167
168
169
170
171
172
    err = find_project_path(mbs_filename, mbs_data->project_path);
    if (err <0)
    {

        free(symbolicLib_name);
        free(symbolicLib_path);
        free(userfctLib_name);
        free(userfctLib_path);
        if (!custom_mbs_info) {
            mbs_delete_infos(mbs_infos);
        }
        mbs_msg("\n>>LOAD>> find_project_path \n\n");
173
174
        mbs_msg("[%d] in mbs_load_with_loader !! \n", _MBS_ERR_MOD_LOAD + err);
        mbs_delete_data(mbs_data);
175
176
177
        return NULL;

    }
178

179
    // if we call project and user function pointer, init them
180
#ifdef PRJ_FCT_PTR
181
    if (opts->load_symb_user!=LOAD_NONE) {
182
183
        err = mbs_load_symbolic_functions(mbs_data, symbolicLib_path, symbolicLib_name);
        if (err < 0){
184
            mbs_msg("[%d] in mbs_load_with_loader !! \n", _MBS_ERR_MOD_LOAD + err);
185
186
187
188
189
190
191
192
193
194
            free(symbolicLib_name);
            free(symbolicLib_path);
            free(userfctLib_name);
            free(userfctLib_path);
            if(!custom_mbs_info){
                mbs_delete_infos(mbs_infos);
            }
            mbs_delete_data(mbs_data);
            return NULL;
        }
195
196
197
198
199
200
    }
    else{
        mbs_set_symb_fun_null(mbs_data);
    }

    if(opts->load_symb_user==LOAD_ALL) {
201
202
        err = mbs_load_user_functions(mbs_data, userfctLib_path, userfctLib_name);
        if (err < 0){
203
            mbs_msg("[%d] in mbs_load_with_loader !! \n", _MBS_ERR_MOD_LOAD + err);
204
205
206
207
208
209
210
211
212
213
            free(symbolicLib_name);
            free(symbolicLib_path);
            free(userfctLib_name);
            free(userfctLib_path);
            if(!custom_mbs_info){
                mbs_delete_infos(mbs_infos);
            }
            mbs_delete_data(mbs_data);
            return NULL;
        }
214
    }
215
216
217
218
219
220
    else{
        mbs_set_user_fun_null(mbs_data);
        mbs_data->user_IO = NULL;
    }

    if (opts->load_symb_user==LOAD_NONE) {
Louis Beauloye's avatar
Louis Beauloye committed
221
222
        mbs_data->build_path = NULL;
        mbs_data->user_IO = NULL;
223
    }
224
#endif
225

226
    mbs_data = mbs_info_to_data(mbs_infos, mbs_data);
227
228
229
230
231
232
233
234
235
236
237
    if (mbs_data == NULL)
    {
        free(symbolicLib_name);
        free(symbolicLib_path);
        free(userfctLib_name);
        free(userfctLib_path);
        if (!custom_mbs_info) {
            mbs_delete_infos(mbs_infos);
        }

        mbs_msg("\n>>LOAD>> mbs_info_to_data \n\n");
238
        mbs_msg("[%d] in mbs_load_with_loader !! \n", _MBS_ERR_MOD_LOAD + _MBS_ERR_LOW_FILES);
239
240
241
        return NULL;

    }
242

243
    mbs_data->mbs_filename = (char*)malloc(1 + strlen(mbs_filename));
244
    strcpy(mbs_data->mbs_filename, mbs_filename);
245

246
    if (build_path != NULL) {
247
248
249
250
        mbs_data->build_path = (char*)malloc(1 + strlen(build_path));
        strcpy(mbs_data->build_path, build_path);
    }

251

252
253
    free(symbolicLib_name);
    free(symbolicLib_path);
254
255
    free(userfctLib_name);
    free(userfctLib_path);
256

257

258
    if (custom_mbs_info == 0)
259
    {
260
        mbs_delete_infos(mbs_infos);
261
262
263
    }
    else
    {
264
        // do nothing as the loader->mbs_infos memory could be re-used.
265
    }
266

267
268
269
    return mbs_data;
}

270
MbsLoadOptions* mbs_new_load_options()
271
272
273
{
    MbsLoadOptions *opts;

274
    opts = (MbsLoadOptions*)malloc(sizeof(MbsLoadOptions));
275
276
    opts->symbolicLib_name = NULL;
    opts->symbolicLib_path = NULL;
277
278
    opts->userfctLib_name = NULL;
    opts->userfctLib_path = NULL;
279
    opts->load_symb_user = LOAD_ALL;
280
281
282
283

    return opts;
}

284
void mbs_delete_load_options(MbsLoadOptions* opts) {
285
286
287
    free(opts);
}

288
289
290
291
MbsLoader* mbs_new_loader()
{
    MbsLoader *loader;

292
    loader = (MbsLoader*)malloc(sizeof(MbsLoader));
293
    loader->opts = mbs_new_load_options();
294
    loader->mbs_infos = NULL; // in order to perform a test on it !
295
296
297

    return loader;
}
298

299
300
301
302
303
void mbs_delete_loader(MbsLoader* loader)
{
    mbs_delete_load_options(loader->opts);
    free(loader);
}
304
#ifdef PRJ_FCT_PTR
305
void mbs_set_symb_fun_null(MbsData *s) {
306
    s->fct.symbolicLibHandle = NULL;
307
308
309
310
311
312
313
314
315
316
317
318
319
320
    // Symbolic functions
    s->fct.symb.mbs_link = NULL;
    s->fct.symb.mbs_link3D = NULL;
    s->fct.symb.mbs_extforces = NULL;
    s->fct.symb.mbs_accelred = NULL;
    s->fct.symb.mbs_dirdyna = NULL;
    s->fct.symb.mbs_invdyna = NULL;
    s->fct.symb.mbs_cons_hJ = NULL;
    s->fct.symb.mbs_cons_jdqd = NULL;
    s->fct.symb.mbs_sensor = NULL;
    s->fct.symb.mbs_gensensor = NULL;
}

void mbs_set_user_fun_null(MbsData *s) {
321
    s->fct.userfctLibHandle = NULL;
322
    // User functions
323
324
325
326
    s->fct.user.user_JointForces = NULL;
    s->fct.user.user_load_post = NULL;
    s->fct.user.user_dirdyn_init = NULL;
    s->fct.user.user_dirdyn_loop = NULL;
327
    s->fct.user.user_dirdyn_finish = NULL;
328
329
330
331
332
333
334
335
336
337
338
    s->fct.user.user_equil_init = NULL;
    s->fct.user.user_equil_loop = NULL;
    s->fct.user.user_equil_finish = NULL;
    s->fct.user.user_equil_fxe = NULL;
    s->fct.user.user_Derivative = NULL;
    s->fct.user.user_DrivenJoints = NULL;
    s->fct.user.user_cons_hJ = NULL;
    s->fct.user.user_cons_jdqd = NULL;
    s->fct.user.user_LinkForces = NULL;
    s->fct.user.user_Link3DForces = NULL;
    s->fct.user.user_ExtForces = NULL;
339
    // function generated from mbs file
340
341
342
343
344
    s->fct.user.mbs_new_user_IO = NULL;
    s->fct.user.mbs_new_user_model = NULL;
    s->fct.user.mbs_delete_user_model = NULL;
    s->fct.user.mbs_delete_user_IO = NULL;
    s->fct.user.mbs_get_user_IO_size = NULL;
345
    s->fct.user.mbs_load_user_model_xml = NULL;
346
347
    s->fct.user.mbs_bind_user_model = NULL;
    s->fct.user.mbs_print_user_model = NULL;
348
349
    s->fct.user.mbs_get_user_model_size = NULL;
    s->fct.user.mbs_get_user_model_list = NULL;
350

351
352
353
354
355
356
357
358
359
360
361
362
363
#ifdef REAL_TIME
    s->fct.user.user_realtime_options = NULL;
#ifdef SDL
    s->fct.user.user_keyboard = NULL;
    s->fct.user.user_realtime_plot = NULL;
    s->fct.user.user_joystick_axes = NULL;
    s->fct.user.user_joystick_buttons = NULL;
#endif

#ifdef VISU_3D
    s->fct.user.user_realtime_visu = NULL;
#endif
#endif
364
365
}
#endif
366
367
368



369
370
371
372
373
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */

/**
 * Implementation of utility function for user_IO ==> should be moved
 * (see def in mbs_user_interface.h)
374
 */
375
376
377
void mbs_set_user_IO(UserIoInfo* ioInfo, double val) {
    int i, j;

378
    // set input initial value to 0.0
379
380
    for (i = 0; i < ioInfo->n_in; i++) {
        for (j = 0; j < ioInfo->size_in[i]; j++) {
381
382
383
            ioInfo->ptr_in[i][j] = val;
        }
    }
384

385
    // set output initial value to 0.0
386
387
    for (i = 0; i < ioInfo->n_out; i++) {
        for (j = 0; j < ioInfo->size_out[i]; j++) {
388
            ioInfo->ptr_out[i][j] = val;
389
        }
390
391
392
393
394
395
    }
}


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

396
UserIoInfo* mbs_xml_create_user_IO_info(MbsInfos* mbs_infos, MbsData* s) {
397

398
    int i, i_in, i_out;
399
    UserIoInfo* ioInfo = (UserIoInfo*)malloc(sizeof(UserIoInfo));
400
    MbsInfoUserIOs *mmbs_info_IOs = mbs_infos->user_IOs;
401

402
    // number of input/output
403
    ioInfo->n_in = mmbs_info_IOs->n_in;
404

405
    ioInfo->n_out = mmbs_info_IOs->n_out;
406
407

    // sizes of input/output
408
409
    ioInfo->size_in = (int*)malloc(sizeof(int)*ioInfo->n_in);
    ioInfo->size_out = (int*)malloc(sizeof(int)*ioInfo->n_out);
410
411

    i_in = 0; i_out = 0;
412
    for (i = 0; i < mmbs_info_IOs->n_user_IO; i++) {
413
        int io_type = mmbs_info_IOs->user_IO_list[i]->type;
414

415
        if (io_type == 1) {
416
            ioInfo->size_in[i_in] = mmbs_info_IOs->user_IO_list[i]->size;
417
418
            i_in++;
        }
419
        else if (io_type == 2) {
420
            ioInfo->size_out[i_out] = mmbs_info_IOs->user_IO_list[i]->size;
421
422
423
424
425
            i_out++;
        }
    }

    // data pointer
426
427
    ioInfo->ptr_in = (double**)malloc(sizeof(double*)*ioInfo->n_in);
    ioInfo->ptr_out = (double**)malloc(sizeof(double*)*ioInfo->n_out);
428

429
430
431
432
433
434
    // initialization of data point is project dependent. UserIO must be initialized.


    return ioInfo;
}

435
void mbs_delete_user_IO_info(UserIoInfo* ioInfo) {
436

437
438
    free(ioInfo->ptr_in);
    free(ioInfo->ptr_out);
439
440


441
442
    free(ioInfo->size_in);
    free(ioInfo->size_out);
443

444
    free(ioInfo);
445

446
447
448
449
450
451
452
453
454
455
456
457
}

/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*! \brief return a (nj sized) vector (starting at index 1) of integer which is a copy of the provided (nq sized) vector (starting at index 0) with all value incremented by 1.
 *  If nj is greater than nq, the unused value are set to 0.
 *
 * \param[in] q_mds the original (nq sized) vector of integers, starting at index 0
 * \param[in] nq    the size of the q_mds vector
 * \param[in] nj    the size of the returned (starting at index 1) vector (0 element not taken into account)
 *
 * \return requested vector (element 0 receives nj: the size without this element)
 */
458
int* MDS_translate_q(int* q_mds, int nq, int nj)
459
460
461
{
    int* q_MBSdataStruct;
    q_MBSdataStruct = get_ivec_1(nj);
462

463
    if (nq)
464
465
    {
        int i;
466
        for (i = 0; i < nq; i++)
467
        {
468
            q_MBSdataStruct[i + 1] = q_mds[i] + 1;
469
470
471
472
473
        }
    }
    return q_MBSdataStruct;
}

474
MbsData* mbs_info_to_data(MbsInfos* mbs_infos, MbsData* s)
475
{
476
    int i, j;
477
478
479
    int ind_joint = 0;
    int ind_state_value = 0;

480
    s->mbs_name = strdup(mbs_infos->mbsname);
481

482
483
    s->nbody = mbs_infos->bodytree->n_body;
    s->njoint = mbs_infos->bodytree->n_joint;
484

485
    s->npt = mbs_infos->n_point;
486

487
488
489
    s->nqu = mbs_infos->bodytree->n_qu;
    s->nqv = mbs_infos->bodytree->n_qv;
    s->nqc = mbs_infos->bodytree->n_qc;
490
491
    s->nqlocked = mbs_infos->bodytree->n_qlocked;
    s->nqdriven = mbs_infos->bodytree->n_qdriven;
492
    s->nqa = mbs_infos->bodytree->n_qa;
493

494
495
496
497
498
    if (mbs_infos->cuts == NULL) {
        s->Nloopc = 0;
    }
    else {
        s->Nloopc = mbs_infos->cuts->n_rod + 3 * mbs_infos->cuts->n_ball + 6 * mbs_infos->cuts->n_solid;
499
500
    }

501
502
    s->Nuserc = 0;
    s->Ncons = s->Nloopc + s->Nuserc;
503
504
505

    s->nhu = s->Ncons;

506
507
508
    if (mbs_infos->links != NULL) {
        s->Nlink = mbs_infos->links->n_link;
        s->Nlink3D = mbs_infos->links->n_link3D;
509
    }
510
511
512
    else {
        s->Nlink = 0;
        s->Nlink3D = 0;
513
514
    }

515
516
    s->Nsensor = mbs_infos->n_sensor;
    s->Nxfrc = mbs_infos->n_extforce;
517
518

#if !defined SENSORKIN
519
    s->Nuser_model = mbs_infos->user_models->n_user_model;
520
    s->n_user_IO = mbs_infos->user_IOs->n_user_IO;
521
#endif
522
    s->Nux = mbs_infos->n_state_value;
523
524


525
    //    geometric ans dynamic data
526

527
        //pt
528

Augustin Ledoyen's avatar
Augustin Ledoyen committed
529
    s->dpt = NULL;
530
531
    if (s->npt)
    {
532
533
        s->dpt = get_dmat_1(3, s->npt);
        for (i = 0; i < mbs_infos->n_point; i++)
534
        {
535
            for (j = 0; j < 3; j++)
536
            {
537
538
                s->dpt[j + 1][i + 1] = mbs_infos->point_list[i]->pt[j];
                mbs_infos->point_list[i]->d_pt[j]->MBSdata_d_ptr = &(s->dpt[j + 1][i + 1]);
539
540
541
542
543
544
545
            }
        }
    }

    // l, m, In

    ind_joint = 0;
546
    s->l = get_dmat_1(3, s->njoint);
547

Augustin Ledoyen's avatar
Augustin Ledoyen committed
548
    s->m = get_dvec_1(s->njoint);
549

550
    s->In = get_dmat_1(9, s->njoint);
551

552
    for (i = 0; i < mbs_infos->bodytree->n_body; i++)
553
    {
554
        ind_joint = ind_joint + mbs_infos->bodytree->body_list[i]->n_joint;
555
        for (j = 0; j < 3; j++)
556
        {
557
            s->l[j + 1][ind_joint] = mbs_infos->bodytree->body_list[i]->com[j];
558
559
        }

560
        s->m[ind_joint] = mbs_infos->bodytree->body_list[i]->mass;
561

562
563
564
565
566
567
568
569
570
        s->In[1][ind_joint] = mbs_infos->bodytree->body_list[i]->inertia[0];
        s->In[2][ind_joint] = mbs_infos->bodytree->body_list[i]->inertia[1];
        s->In[3][ind_joint] = mbs_infos->bodytree->body_list[i]->inertia[2];
        s->In[4][ind_joint] = mbs_infos->bodytree->body_list[i]->inertia[1];
        s->In[5][ind_joint] = mbs_infos->bodytree->body_list[i]->inertia[3];
        s->In[6][ind_joint] = mbs_infos->bodytree->body_list[i]->inertia[4];
        s->In[7][ind_joint] = mbs_infos->bodytree->body_list[i]->inertia[2];
        s->In[8][ind_joint] = mbs_infos->bodytree->body_list[i]->inertia[4];
        s->In[9][ind_joint] = mbs_infos->bodytree->body_list[i]->inertia[5];
571
572
573
574
    }

    // g
    s->g[0] = 0.0;
575
    copy_dvec_0(mbs_infos->base->gravity, &(s->g[1]), 3);
576
577
578

    // partitioning infos, the allocated vector are sized to have the possibility to handle all joint index.

579
580
581
582
583
584
    s->qu = MDS_translate_q(mbs_infos->bodytree->qu, mbs_infos->bodytree->n_qu, s->njoint);
    s->qv = MDS_translate_q(mbs_infos->bodytree->qv, mbs_infos->bodytree->n_qv, s->njoint);
    s->qc = MDS_translate_q(mbs_infos->bodytree->qc, mbs_infos->bodytree->n_qc, s->njoint);
    s->qlocked = MDS_translate_q(mbs_infos->bodytree->qlocked, mbs_infos->bodytree->n_qlocked, s->njoint);
    s->qdriven = MDS_translate_q(mbs_infos->bodytree->qdriven, mbs_infos->bodytree->n_qdriven, s->njoint);
    s->qa = MDS_translate_q(mbs_infos->bodytree->qa, mbs_infos->bodytree->n_qa, s->njoint);
585
586
587
588
589



    if (s->nhu) // to change
    {
590
        s->hu = get_ivec_1(s->nhu);
591
        for (i = 0; i < s->nhu; i++)
592
        {
593
            s->hu[i + 1] = i + 1;
594
595
596
597
598
599
        }
    }
    else
        s->hu = NULL;

    // joints data
600
601
    s->q0 = get_dvec_1(s->njoint);
    s->qd0 = get_dvec_1(s->njoint);
602
603
    s->qdd0 = get_dvec_1(s->njoint);

604
605
    s->q = get_dvec_1(s->njoint);
    s->qd = get_dvec_1(s->njoint);
606
607
608
    s->qdd = get_dvec_1(s->njoint);


609
    for (i = 0; i < mbs_infos->bodytree->n_joint; i++)
610
    {
611
612
613
        s->q0[i + 1] = mbs_infos->bodytree->joint_list[i]->q0;
        s->qd0[i + 1] = mbs_infos->bodytree->joint_list[i]->qd0;
        s->qdd0[i + 1] = mbs_infos->bodytree->joint_list[i]->qdd0;
614

615
        mbs_infos->bodytree->joint_list[i]->d_qf->MBSdata_d_ptr = &(s->q[i + 1]);
616
617
618
619
620
621
    }

    copy_dvec_1(s->q0, s->q);
    copy_dvec_1(s->qd0, s->qd);
    copy_dvec_1(s->qdd0, s->qdd);

622
    // Frc, Trq, Qq, tsim
623

624
625
        // frc
    s->frc = get_dmat_1(3, s->njoint);
626
627

    // trq
628
    s->trq = get_dmat_1(3, s->njoint);
629

630
    // Qq  (Qa)    (... and Qc)
Augustin Ledoyen's avatar
Augustin Ledoyen committed
631
    s->Qq = get_dvec_1(s->njoint);
632
    s->Qa = get_dvec_1(s->njoint); // oversized on purpose
633
    s->Qc = get_dvec_1(s->njoint); // oversized on purpose
634
635
636
637
638

    // tsim
    s->tsim = 0.0;

    // correctly updated later
639
640
    s->t0 = 0.0;
    s->tf = 0.0;
641
642
643
644
645
    s->dt0 = 0.0;

    // 1 to stop the simulation, 0 otherwise
    s->flag_stop = 0;

646
    // Constraint data
647

648
649
        // lrod
    if (mbs_infos->cuts != NULL && mbs_infos->cuts->n_rod)
650
    {
651
        s->lrod = get_dvec_1(mbs_infos->cuts->n_rod);
652
        for (i = 0; i < mbs_infos->cuts->n_rod; i++)
653
        {
654
655
            s->lrod[i + 1] = mbs_infos->cuts->rod_list[i]->length;
            mbs_infos->cuts->rod_list[i]->d_length->MBSdata_d_ptr = &(s->lrod[i + 1]);
656
657
658
659
660
661
        }
    }
    else
        s->lrod = NULL;

    // NRerr
662
    s->NRerr = 1.0e-9; //default value
663

664
665
    // Always initialized at max size, allows user constraints and dependant variable to be declared in the project main functions.
    s->lambda = get_dvec_1(s->njoint);
666
667


668
    // Link Data
669

670
        // Z, Zd, Fl
671
672
    if (s->Nlink)
    {
673
674
675
        s->Z = get_dvec_1(s->Nlink);
        s->Zd = get_dvec_1(s->Nlink);
        s->Fl = get_dvec_1(s->Nlink);
676
        for (i = 1; i <= s->Nlink; i++)
677
        {
678
            s->Z[i] = 0.0;
679
680
681
682
683
684
            s->Zd[i] = 0.0;
            s->Fl[i] = 0.0;
        }
    }
    else
    {
685
        s->Z = NULL;
686
687
688
689
690
691
        s->Zd = NULL;
        s->Fl = NULL;
    }

    // l3DWr
    if (s->Nlink3D)
692
        s->l3DWr = get_dmat_1(s->Nlink3D, 6);
693
694
695
    else
        s->l3DWr = NULL;

696
    // Ext. Forces Data
697
698
699

    if (s->Nxfrc)
    {
700
701
        //xfidpt
        s->xfidpt = get_ivec_1(s->Nxfrc);
702
        for (i = 0; i < mbs_infos->n_extforce; i++)
703
        {
704
            s->xfidpt[i + 1] = mbs_infos->extforce_point_list[i] + 1;
705
        }
706
        //SWr
707
        s->SWr = get_dmat_1(s->Nxfrc, 9);
708
    }
709
    else {
710
711
        s->xfidpt = NULL;
        s->SWr = NULL;
712
    }
713
714

#if !defined SENSORKIN
715
    // User Model
716

717
        // user_model
718
    if (s->Nuser_model
719
720
721
#ifdef PRJ_FCT_PTR
        && s->fct.user.mbs_new_user_model != NULL
#endif
722
        )
723
724
    {
        s->user_model = mbs_new_user_model();
725
726
        mbs_load_user_model_xml(mbs_infos, s->user_model);
        mbs_bind_user_model(mbs_infos, s->user_model);
727
728
729
730
731
732
733
734
735
736
737
738

        // Error management if user_model files are not correctly re-generated by the user (check on number of user_model and their type)
        int n_usr_model = 0;
        mbs_get_user_model_size(&n_usr_model);
        int* user_model_list_c = get_ivec_1(n_usr_model);
        mbs_get_user_model_list(user_model_list_c);

        // Retrieve mbs parameters list
        int* user_model_list_mbs = get_ivec_1(s->Nuser_model);
        int k = 0;
        for (k = 0; k < s->Nuser_model; k++)
        {
739
            user_model_list_mbs[k + 1] = mbs_infos->user_models->user_model_list[k]->n_parameter;
740
741
742
        }

        // Do the check
743
        int err_usr_model = mbs_check_n_user_model(user_model_list_mbs, s->Nuser_model, user_model_list_c, n_usr_model);
744
745
746
747
        if (err_usr_model < 0)
        {
            mbs_msg(">>LOAD>> Error while fetching user_model number \n");
            mbs_msg("\n>>LOAD>> Please Re-generated your user_model Files !!! \n\n");
748
            mbs_msg("[%d] in mbs_load_with_loader \n", _MBS_ERR_MOD_LOAD);
749
            return NULL;
750
751
752
753
        }
        free_ivec_1(user_model_list_c);
        free_ivec_1(user_model_list_mbs);

754
755
756
757
758
759
760
761
762
    }
    else
    {
        s->user_model = NULL;
    }

    // ux, uxd, ux0
    if (s->Nux)
    {
763
764
765
        s->ux = get_dvec_1(s->Nux);
        s->uxd = get_dvec_1(s->Nux);
        s->ux0 = get_dvec_1(s->Nux);
766
        for (i = 0; i < mbs_infos->n_state; i++)
767
        {
768
            for (j = 1; j <= mbs_infos->state_list[i]->n_value; j++)
769
            {
770
                s->ux0[ind_state_value + 1] = mbs_infos->state_list[i]->value_list[j];
771
772
773
                ind_state_value++;
            }
        }
774
        copy_dvec_1(s->ux0, s->ux);
775
776
777
    }
    else
    {
778
        s->ux = NULL;
779
780
781
        s->uxd = NULL;
        s->ux0 = NULL;
    }
782

783
#ifdef PRJ_FCT_PTR
784
    if (s->fct.user.mbs_new_user_IO != NULL) {
785
#endif
786
787
788
789
790
791
792
793
794
795
796
797
798
799
        s->__user_IO_info = mbs_xml_create_user_IO_info(mbs_infos, s);
        s->user_IO = mbs_new_user_IO(s->__user_IO_info);

        // Error management if user_IO files are not correctly re-generated by the user (check on number of inputs/ouputs)
        int n_in = 0;
        int n_out = 0;
        int n_user_IO = 0;
        mbs_get_user_IO_size(&n_in, &n_out, &n_user_IO);

        int err_IO = mbs_check_n_IO(s->__user_IO_info, n_in, n_out, n_user_IO);
        if (err_IO < 0)
        {
            mbs_msg(">>LOAD>> Error while fetching user_IO number \n");
            mbs_msg("\n>>LOAD>> Please Re-generated your user_IO Files !!! \n\n");
800
801
            mbs_msg("[%d] in mbs_load_with_loader \n", _MBS_ERR_MOD_LOAD);
            return NULL;
802
803
804
805
806
        }

        if (s->n_user_IO) {
            mbs_set_user_IO(s->__user_IO_info, 0.);
        }
807
808
809
#ifdef PRJ_FCT_PTR
    }
#endif
810

811
812
#endif

813
    //    Other
814

815
        // udd is sized to have the possibility to handle all joints acceleration.
816
817
818
819
820
821
    s->udd = get_dvec_1(s->njoint);

    s->DonePart = 0;
    s->DoneEquil = 0;
    s->DoneModal = 0;

822
    s->process = 1;
823
824
825
    s->simu_end = 0;


826
827
    // call the user_load_post function
#ifdef PRJ_FCT_PTR
828
    if (s->fct.user.user_load_post != NULL) {
829
830
831
832
833
834
835
#endif

        user_load_post(s);

#ifdef PRJ_FCT_PTR
    }
#endif
836
837
838
839
840
841
842

    return s;
}


////////////////////////////////////////////////////////////////////////////        CAUTION  CAUTION

843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
MbsData *mbs_new_data(){
    MbsData *s = (MbsData*)malloc(sizeof(MbsData));

    // Setting all pointer to NULL and variables to 0
    s->mbs_name = NULL;
    // Geometrical and dynamical datas
    s->npt = 0;
    s->dpt = NULL;
    s->l = NULL;
    s->m = NULL;
    s->In = NULL;

    // partitioning informations, always allocated
    s->qu = NULL;
    s->qv = NULL;
    s->qc = NULL;
    s->qlocked = NULL;
    s->qdriven = NULL;
    s->qa = NULL;

    // Joints
    s->q = NULL;
    s->qd = NULL;
    s->qdd = NULL;
    s->q0 = NULL;
    s->qd0 = NULL;
    s->qdd0 = NULL;

    // Forces, torques
    s->frc = NULL;
    s->trq = NULL;
    s->Qq = NULL;
    s->Qa = NULL;
    s->Qc = NULL;

    // Constraints
    s->lrod = NULL;
    s->lambda = NULL;

    // Links
    s->Nlink = 0;
    s->Z = NULL;
    s->Zd = NULL;
    s->Fl = NULL;

    s->Nlink3D = 0;
    s->l3DWr = NULL;

    // Ext. forces
    s->Nxfrc = 0;
    s->xfidpt = NULL;
    s->SWr = NULL;

    // User state
    s->Nux = 0;
    s->ux = NULL;
    s->uxd = NULL;
    s->ux0 = NULL;

#ifndef SENSORKIN
    // User models
    s->Nuser_model = 0;
    s->user_model = NULL;

    s->user_IO = NULL;
#endif

    // Other
    s->udd = NULL;
    s->nhu = 0;
    s->hu = NULL;
    s->mbs_filename = NULL;
    s->build_path = NULL;
    s->project_path = NULL;

#ifdef PRJ_FCT_PTR
    // dynamics libraries associated to s (symbolic and userfct)
    s->fct.symbolicLibHandle = NULL;
    s->fct.userfctLibHandle = NULL;
#endif

    return s;
}
926
927
928

void mbs_delete_data(MbsData *s)
{
929
930
931
    if (!s){
        return;
    }
932

933
934
935
936
937
    if (s->mbs_name)
    {
        free(s->mbs_name);
    }

938
    // Donnees geometriques et dynamiques
939
    if (s->npt){
Augustin Ledoyen's avatar
Augustin Ledoyen committed
940
        free_dmat_1(s->dpt);
941
    }
942

943
    free_dmat_1(s->l);
944

945
    free_dvec_1(s->m);
946

Olivier Lantsoght's avatar
Olivier Lantsoght committed
947
    free_dmat_1(s->In);
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965

    // partitioning informations, always allocated
    free_ivec_1(s->qu);
    free_ivec_1(s->qv);
    free_ivec_1(s->qc);
    free_ivec_1(s->qlocked);
    free_ivec_1(s->qdriven);
    free_ivec_1(s->qa);

    // Variables articulaires, valeures initiales, limites
    free_dvec_1(s->q);
    free_dvec_1(s->qd);
    free_dvec_1(s->qdd);
    free_dvec_1(s->q0);
    free_dvec_1(s->qd0);
    free_dvec_1(s->qdd0);

    // frc, trq, Qq
966
967
968
969
970
    free_dmat_1(s->frc);
    free_dmat_1(s->trq);
    free_dvec_1(s->Qq);
    free_dvec_1(s->Qa);
    free_dvec_1(s->Qc);
971
972

    // Constraints
973
    if (s->lrod != NULL){
974
        free_dvec_1(s->lrod);
975
    }
976

977
    if (s->lambda != NULL){
978
        free_dvec_1(s->lambda);
979
    }
980
981
982
983

    // Links
    if (s->Nlink)
    {
984
985
986
        free_dvec_1(s->Z);
        free_dvec_1(s->Zd);
        free_dvec_1(s->Fl);
987
    }
988
    if (s->Nlink3D){