mbs_print_user_files.c 17.7 KB
Newer Older
1
/*
2
 * Functions used to generate 'user_models.c', 'user_models.h', 'user_all_id.h' and 'user_hardcoded.h'
3
 *
4
 * author: Aubain VERLE, Nicolas Docquier, Nicolas Van der Noot
5
 *
6
 * (c) Universite catholique de Louvain
7
 *
8
9
10
11
12
13
14
15
16
17
18
19
 */
#include "mbs_print_user_files.h"

#include <time.h>
#include <string.h>



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

/**
 * Print a generic header in the given file
20
21
 *
 */
22
23
24
25
void mbs_print_header(FILE* fid){

    fprintf(fid, "/* --------------------------------------------------------\n");
    fprintf(fid, " * This code was generated automatically by MBsysC modules.\n");
26
27
28
    fprintf(fid, " * MBsysC modules are distributed as part of the ROBOTRAN \n");
    fprintf(fid, " * software. They provides functionalities for dealing with\n");
    fprintf(fid, " * symbolic equations generated by ROBOTRAN. \n");
29
    fprintf(fid, " *\n");
30
    fprintf(fid, " * More info on www.robotran.be \n");
31
    fprintf(fid, " *\n");
32
33
34
    fprintf(fid, " * Universite catholique de Louvain, Belgium \n");
    fprintf(fid, " *\n");
    fprintf(fid, " * Last update : %s", get_time_machine());
35
    fprintf(fid, " * --------------------------------------------------------\n");
36
37
    fprintf(fid, " *\n");
    fprintf(fid, " */\n");
38

39
40
41
42
43
44
45
46
}

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

/*
 * Print an empty file (just the generation date is indicated)
 */
void print_empty_file(char *fileout)
47
{
48
49
50
    FILE *fid;

    // File declaration
51
    fid = NULL;
52
53
54
55
56
57
58
59

    // Opening file
    fid = fopen(fileout, "wt");

    if(fid == NULL)
    {
        printf("Error: cannot open file '%s'\n", fileout);
        exit(1);
60
    }
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83

    fprintf(fid, "//---------------------------\n");
    fprintf(fid, "// Last update : %s", get_time_machine());
    fprintf(fid, "//---------------------------\n\n");

    fclose(fid);
}

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

/*
 * Generates 'UserModelStruct.c' and 'UserModelStruct.h'
 */
void mbs_print_user_models(MDS_gen_strct* gen, char *fileoutC, char *fileoutH)
{
    int i, j;

    FILE *fidC, *fidH;

    // File declaration
    fidC = NULL; // internal filename

    // Opening file
84
    fidC = fopen(fileoutC, "wt");
85
86
87
88
89
90

    // Fill the file
    if(fidC == NULL)
    {
        printf("error: cannot open file '%s'\n", fileoutC);
        exit(1);
91
    }
92
93
94
95
96

    // File declaration
    fidH = NULL; // internal filename

    // Opening file
97
    fidH = fopen(fileoutH, "wt");
98
99
100
101
102
103

    // Fill the file
    if(fidH == NULL)
    {
        printf("error: cannot open file '%s'\n", fileoutH);
        exit(1);
104
    }
105
106

    // -- UserModelStruct.h -- //
107

108
109
    // print the generic header
    mbs_print_header(fidH);
110

Timothee Habra's avatar
Timothee Habra committed
111
112
    fprintf(fidH,"#ifndef USERMODEL_h\n");
    fprintf(fidH,"#define USERMODEL_h\n");
113
114
115
    fprintf(fidH,"\n");
    fprintf(fidH,"#include \"lut.h\"\n");
    fprintf(fidH,"\n");
116
	fprintf(fidH, "#include \"mbs_user_interface.h\"\n");
117
118
    fprintf(fidH,"// ============================================================ //\n\n");
    fprintf(fidH,"\n");
119
    fprintf(fidH,"struct UserModel \n");
120
    fprintf(fidH,"{\n");
121

122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
    for(i=0; i< gen->user_models->n_user_model; i++)
    {
        fprintf(fidH,"    struct %s{\n", gen->user_models->user_model_list[i]->name);
        for(j=0; j< gen->user_models->user_model_list[i]->n_parameter; j++)
        {
            switch( gen->user_models->user_model_list[i]->parameter_list[j]->type)
            {
                case 1: // scalar
                    fprintf(fidH,"        double %s;\n",gen->user_models->user_model_list[i]->parameter_list[j]->name);
                    break;
                case 2: // vector
                    fprintf(fidH,"        double* %s;\n",gen->user_models->user_model_list[i]->parameter_list[j]->name);
                    break;
                case 3: // look-up table 1D
                    fprintf(fidH,"        char* %s;\n",gen->user_models->user_model_list[i]->parameter_list[j]->name);
                    fprintf(fidH,"        // print_c_files.c uncompleted for lut1D\n");
                    break;
                case 4: // look-up table 2D
                    fprintf(fidH,"        char* %s;\n",gen->user_models->user_model_list[i]->parameter_list[j]->name);
                    fprintf(fidH,"        // print_c_files.c uncompleted for lut2D\n");
                    break;
                case 5: // user state
                    fprintf(fidH,"        int* %s;\n",gen->user_models->user_model_list[i]->parameter_list[j]->name);
                    fprintf(fidH,"        // index of the corresponding values in MBSdataStruct->ux/uxd/ux0\n");
                    break;
147
            }
148
149
150
151
        }
        fprintf(fidH,"    } %s;\n", gen->user_models->user_model_list[i]->name);
        fprintf(fidH," \n");
    }
152

Timothee Habra's avatar
Timothee Habra committed
153
    fprintf(fidH,"};\n");
154
155
156
157
158
    fprintf(fidH,"\n");

    fprintf(fidH,"// ============================================================ //\n \n");
    fprintf(fidH,"# endif""\n");
    fclose(fidH);
159
    printf(">> 'user_model.h' created\n");
160
161
162
163
164
165



    // -- UserModelStruct.c -- //

    // print the generic header
166
    mbs_print_header(fidC);
167

168
    fprintf(fidC,"#include \"user_model.h\"\n");
169
    fprintf(fidC,"#include \"mbs_xml_reader.h\"\n");
170
171
172
    fprintf(fidC,"\n");
    fprintf(fidC,"// ============================================================ //\n\n");
    fprintf(fidC,"\n");
173
    fprintf(fidC,"UserModel* mbs_new_user_model() \n");
174
    fprintf(fidC,"{\n");
175
176
    fprintf(fidC,"    UserModel* um;\n");
    fprintf(fidC,"    um = (UserModel*)malloc(sizeof(UserModel));\n");
177
178
179
180
181
182
183
184
185


    for(i=0; i< gen->user_models->n_user_model; i++)
    {
        for(j=0; j< gen->user_models->user_model_list[i]->n_parameter; j++)
        {
            switch( gen->user_models->user_model_list[i]->parameter_list[j]->type)
            {
                case 1:
186
                    fprintf(fidC,"    um->%s.%s = 0.0;\n",gen->user_models->user_model_list[i]->name , gen->user_models->user_model_list[i]->parameter_list[j]->name);
187
188
                    break;
                case 2:
189
190
                    fprintf(fidC,"    um->%s.%s = get_double_vec(%d+1);\n",gen->user_models->user_model_list[i]->name , gen->user_models->user_model_list[i]->parameter_list[j]->name,gen->user_models->user_model_list[i]->parameter_list[j]->n_value);
                    fprintf(fidC,"    um->%s.%s[0] = %d+1;\n",gen->user_models->user_model_list[i]->name , gen->user_models->user_model_list[i]->parameter_list[j]->name,gen->user_models->user_model_list[i]->parameter_list[j]->n_value);
191
192
                    break;
                case 3:
193
                    fprintf(fidC,"    um->%s.%s = NULL;\n",gen->user_models->user_model_list[i]->name , gen->user_models->user_model_list[i]->parameter_list[j]->name);
194
195
196
                    fprintf(fidC,"    // print_c_files.c uncompleted for lut1D\n");
                    break;
                case 4:
197
                    fprintf(fidC,"    um->%s.%s = NULL;\n",gen->user_models->user_model_list[i]->name , gen->user_models->user_model_list[i]->parameter_list[j]->name);
198
199
200
                    fprintf(fidC,"    // print_c_files.c uncompleted for lut2D\n");
                    break;
                case 5:
201
202
                    fprintf(fidC,"    um->%s.%s = get_int_vec(%d+1);\n",gen->user_models->user_model_list[i]->name , gen->user_models->user_model_list[i]->parameter_list[j]->name,gen->user_models->user_model_list[i]->parameter_list[j]->n_value);
                    fprintf(fidC,"    um->%s.%s[0] = %d+1;\n",gen->user_models->user_model_list[i]->name , gen->user_models->user_model_list[i]->parameter_list[j]->name,gen->user_models->user_model_list[i]->parameter_list[j]->n_value);
203
                    break;
204
            }
205
206
207
        }
        fprintf(fidC," \n");
    }
208
    fprintf(fidC,"    return um;\n");
209
210
211
    fprintf(fidC,"}\n");
    fprintf(fidC,"\n");

212
    fprintf(fidC,"void mbs_delete_user_model(UserModel* um) \n");
213
214
215
216
217
218
219
220
221
222
223
    fprintf(fidC,"{\n");

    for(i=0; i< gen->user_models->n_user_model; i++)
    {
        for(j=0; j< gen->user_models->user_model_list[i]->n_parameter; j++)
        {
            switch( gen->user_models->user_model_list[i]->parameter_list[j]->type)
            {
                case 1:
                    break;
                case 2:
224
                    fprintf(fidC,"    free_double_vec(um->%s.%s);\n",gen->user_models->user_model_list[i]->name , gen->user_models->user_model_list[i]->parameter_list[j]->name); // ,gen->user_models->user_model_list[i]->parameter_list[j]->n_value);
225
226
227
228
229
                    break;
                case 3:
                case 4:
                    break;
                case 5:
230
                    fprintf(fidC,"    free_int_vec(um->%s.%s);\n",gen->user_models->user_model_list[i]->name , gen->user_models->user_model_list[i]->parameter_list[j]->name); // ,gen->user_models->user_model_list[i]->parameter_list[j]->n_value);
231
                    break;
232
            }
233
234
        }
    }
235
    fprintf(fidC,"    free(um);\n");
236
237
    fprintf(fidC,"}\n");
    fprintf(fidC,"\n");
238
    fprintf(fidC," void mbs_load_user_model_xml(MDS_gen_strct* gen, UserModel* um) \n");
239
240
241
    fprintf(fidC,"{\n");
    fprintf(fidC,"\n");
    fprintf(fidC,"    int ind;\n");
242
    fprintf(fidC,"    int ind_state_value = 1;\n");
243
244
245
246
247
248
249
250
251
    fprintf(fidC,"\n");

    for(i=0; i< gen->user_models->n_user_model; i++)
    {
        for(j=0; j< gen->user_models->user_model_list[i]->n_parameter; j++)
        {
            switch( gen->user_models->user_model_list[i]->parameter_list[j]->type)
            {
                case 1:
252
                    fprintf(fidC,"    um->%s.%s = gen->user_models->user_model_list[%d]->parameter_list[%d]->value_list[0];\n",gen->user_models->user_model_list[i]->name , gen->user_models->user_model_list[i]->parameter_list[j]->name,i,j);
253
254
255
256
                    break;
                case 2:
                    fprintf(fidC,"    for(ind=0; ind<gen->user_models->user_model_list[%d]->parameter_list[%d]->n_value; ind++)\n",i,j);
                    fprintf(fidC,"    {\n");
257
                    fprintf(fidC,"        um->%s.%s[ind] = gen->user_models->user_model_list[%d]->parameter_list[%d]->value_list[ind];\n",gen->user_models->user_model_list[i]->name , gen->user_models->user_model_list[i]->parameter_list[j]->name, i, j);
258
259
260
                    fprintf(fidC,"    }\n");
                    break;
                case 3:
261
                    fprintf(fidC,"    //um->%s.%s = gen->user_models->user_model_list[i]->parameter_list[j]->lut_ref;\n",gen->user_models->user_model_list[i]->name , gen->user_models->user_model_list[i]->parameter_list[j]->name);
262
263
264
                    fprintf(fidC,"    // print_c_files.c uncompleted for lut1D\n");
                    break;
                case 4:
265
                    fprintf(fidC,"    //um->%s.%s = gen->user_models->user_model_list[i]->parameter_list[j]->lut_ref;\n",gen->user_models->user_model_list[i]->name , gen->user_models->user_model_list[i]->parameter_list[j]->name);
266
267
268
269
270
                    fprintf(fidC,"    // print_c_files.c uncompleted for lut2D\n");
                    break;
                case 5:
                    fprintf(fidC,"    for(ind=0; ind<gen->user_models->user_model_list[%d]->parameter_list[%d]->n_value; ind++)\n",i,j);
                    fprintf(fidC,"    {\n");
271
                    fprintf(fidC,"        um->%s.%s[ind+1] = ind_state_value;\n",gen->user_models->user_model_list[i]->name , gen->user_models->user_model_list[i]->parameter_list[j]->name);
272
273
274
                    fprintf(fidC,"        ind_state_value++;\n");
                    fprintf(fidC,"    }\n");
                    break;
275
            }
276
277
278
279
280
281
282
283
        }
        fprintf(fidC," \n");
    }
    fprintf(fidC,"}\n");
    fprintf(fidC,"\n");

    fprintf(fidC,"// ============================================================ //\n \n");
    fclose(fidC);
284
    printf(">> 'user_model.c' created\n");
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
}

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

/**
 * Generates 'user_all_id.h'
 */
void mbs_print_user_all_id(MDS_gen_strct* gen, char *fileoutH)
{
    int i;
    int ind;

    FILE  *fidH;

    // File declaration
    fidH = NULL; // internal filename

    // Opening file
303
    fidH = fopen(fileoutH, "wt");
304
305
306
307
308
309

    // Fill the file
    if(fidH == NULL)
    {
        printf("error: cannot open file '%s'\n", fileoutH);
        exit(1);
310
    }
311
312
313
314
315
316
317
318
319
320
321

    // -- user_all_id.h -- //

    // print the generic header
    mbs_print_header(fidH);

    fprintf(fidH,"#ifndef USER_ALL_ID_h\n");
    fprintf(fidH,"#define USER_ALL_ID_h\n");
    fprintf(fidH,"\n");
    fprintf(fidH,"// ============================================================ //\n\n");
    fprintf(fidH,"\n");
322

323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
    fprintf(fidH,"// joint\n");
    fprintf(fidH,"\n");
    for(i=0; i< gen->bodytree->n_joint; i++)
    {
        fprintf(fidH,"#define %s_id %d\n", gen->bodytree->joint_list[i]->name, i);
    }
    fprintf(fidH,"\n\n");

    fprintf(fidH,"// body\n");
    fprintf(fidH,"\n");
    ind=-1;
    for(i=0; i< gen->bodytree->n_body; i++)
    {
        ind += gen->bodytree->body_list[i]->n_joint;
        fprintf(fidH,"#define %s_id %d\n", gen->bodytree->body_list[i]->name, ind);
    }
    fprintf(fidH,"\n\n");

    fprintf(fidH,"// point\n");
    fprintf(fidH,"\n");
    for(i=0; i< gen->n_point; i++)
    {
        if(strcmp(gen->point_list[i]->name,"origin"))
        {
            fprintf(fidH,"#define %s_id %d\n", gen->point_list[i]->name, i);
        }
        else
        {
            fprintf(fidH,"//#define %s_id %d\n", gen->point_list[i]->name, i);
        }
    }
    fprintf(fidH,"\n\n");



    fprintf(fidH,"// sensor\n");
    fprintf(fidH,"\n");
    for(i=0; i< gen->n_sensor; i++)
    {
        fprintf(fidH,"#define %s_id %d\n", gen->point_list[gen->sensor_point_list[i]]->sensor->name, i+1);
    }
    fprintf(fidH,"\n\n");

    fprintf(fidH,"// extforce\n");
    fprintf(fidH,"\n");
    for(i=0; i< gen->n_extforce; i++)
    {
        fprintf(fidH,"#define %s_id %d\n", gen->point_list[gen->extforce_point_list[i]]->extforce->name, i+1);
    }
    fprintf(fidH,"\n\n");

374
375
    fprintf(fidH,"// links\n");
    fprintf(fidH,"\n");
376
    if (gen->links!= NULL)
377
    {
378
379
380
381
        for(i=0; i< gen->links->n_link; i++)
        {
            fprintf(fidH,"#define %s_id %d\n", gen->links->link_list[i]->name, i+1);
        }
382
383
384
    }
    fprintf(fidH,"\n\n");

385
386
387
388
389
390
    fprintf(fidH,"// ============================================================ //\n\n");
    fprintf(fidH,"# endif""\n");
    fclose(fidH);
    printf(">> 'user_all_id.h' created\n");

}
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
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
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536

int get_index_parent_joint(MDS_body_strct *body, MDS_bodytree_strct *bodytree)
{
    int i;
    char *joint_name;
    
    joint_name = body->joint_list[body->n_joint-1]->name;

    for(i=0; i< bodytree->n_joint; i++)
    {
        if (!strcmp(bodytree->joint_list[i]->name ,joint_name))
        {
            return i+1;
        }
    }

    printf("Error: unknown joint %s !\n", joint_name);
    return 0;
}

/*! \brief print the 'user_hardcoded.h' file
 * 
 * \param[in] gen MDS generated structure (from the .mbs file)
 * \param[in] fileoutH file to create
 */
void mbs_print_user_hardcoded(MDS_gen_strct* gen, char *fileoutH)
{
    int i, j, k;
    int ind;
    int cur_index;

    FILE  *fidH;
    MDS_bodytree_strct *bodytree;
    MDS_body_strct *cur_body;
    MDS_point_strct *cur_point;

    int inertia_tab[6] = {1, 2, 3, 5, 6, 9};

    // File declaration
    fidH = NULL; // internal filename

    // Opening file
    fidH = fopen(fileoutH, "wt");

    // Fill the file
    if(fidH == NULL)
    {
        printf("error: cannot open file '%s'\n", fileoutH);
        exit(1);
    }

    bodytree = gen->bodytree;

    // -- user_hardcoded.h -- //

    // print the generic header
    mbs_print_header(fidH);

    fprintf(fidH,"#ifndef USER_HARDCODED_h\n");
    fprintf(fidH,"#define USER_HARDCODED_h\n");
    fprintf(fidH,"\n");
    fprintf(fidH,"// ============================================================ //\n\n");
    fprintf(fidH,"\n");

    // gravity
    fprintf(fidH,"// gravity\n");
    for(i=0; i<3; i++)
    {
        if (gen->base->gravity[i])
        {
            fprintf(fidH,"#define G_%d %f\n", i+1, gen->base->gravity[i]);
        }        
    }
    fprintf(fidH,"\n");

    // mass
    fprintf(fidH,"// mass\n");
    for(i=0; i<bodytree->n_body; i++)
    {
        cur_body = bodytree->body_list[i];
        cur_index = get_index_parent_joint(cur_body, bodytree);

        if (cur_body->mass)
        {
            fprintf(fidH,"#define M_%d %f\n", cur_index, cur_body->mass);
        }
    }
    fprintf(fidH,"\n");

    // com
    fprintf(fidH,"// center of mass\n");
    for(i=0; i<bodytree->n_body; i++)
    {
        cur_body = bodytree->body_list[i];
        cur_index = get_index_parent_joint(cur_body, bodytree);

        //x y z
        for(j=0; j<3; j++)
        {
            if (cur_body->com[j])
            {
                fprintf(fidH,"#define L_%d_%d %f\n", j+1, cur_index, cur_body->com[j]);
            }            
        }        
    }
    fprintf(fidH,"\n");

    // inertia
    fprintf(fidH,"// inertia\n");
    for(i=0; i<bodytree->n_body; i++)
    {
        cur_body = bodytree->body_list[i];
        cur_index = get_index_parent_joint(cur_body, bodytree);

        // Ixx Ixy Ixz Iyy Iyz Izz
        for(j=0; j<6; j++)
        {
            if (cur_body->inertia[j])
            {
                fprintf(fidH,"#define IN_%d_%d %f\n", inertia_tab[j], cur_index, cur_body->inertia[j]);
            }            
        }        
    }
    fprintf(fidH,"\n");

    // body point
    fprintf(fidH,"// body point\n");
    for(i=0; i< gen->n_point; i++)
    {
        cur_point = gen->point_list[i];

        for(j=0; j<3; j++)
        {
            if (cur_point->pt[j])
            {
                fprintf(fidH,"#define DPT_%d_%d %f\n", j+1, i+1, cur_point->pt[j]);
            }
        }
    }
    fprintf(fidH,"\n");

    fprintf(fidH,"// ============================================================ //\n\n");
    fprintf(fidH,"# endif""\n");
    fclose(fidH);
    printf(">> 'user_hardcoded.h' created\n");
}