mbs_equil.c 47.7 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
/**
* @file mbs_equil.c
*
* This file implements the  functions of the
* equil module in C.
*
*
* Creation date: 26/11/2015
* @author Quentin Docquier (based on the work of Aubain Verle and Paul Fisette)
*
*
* (c) Universite catholique de Louvain
*/

#include <stdio.h>
16
#include "mbs_errors_names.h"
Nicolas Docquier's avatar
Nicolas Docquier committed
17
18
19
20
21
22
23
#include "mbs_equil.h"
#include "mbs_project_interface.h"
#include "mbs_equil_struct.h"
#include "set_output.h"
#include "MBSfun.h"
#include "mbs_linalg.h"
#include "mbs_message.h"
24
#include "nrfct.h"
25
#include "useful_functions.h"
26
#include "mbs_check.h"
Nicolas Docquier's avatar
Nicolas Docquier committed
27

28
29
30
#include "mbs_linearipk.h"


31
#define MAX_NB_BUFFER (3 + 1) // maximal number of buffer 3 for equilibrium, 1 for q (animation)
Nicolas Docquier's avatar
Nicolas Docquier committed
32

33
MbsEquil* mbs_new_equil(MbsData* s)
Nicolas Docquier's avatar
Nicolas Docquier committed
34
{
35
    MbsAux* mbs_aux;
Nicolas Docquier's avatar
Nicolas Docquier committed
36

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

40
    return mbs_new_equil_aux(s, mbs_aux);
Nicolas Docquier's avatar
Nicolas Docquier committed
41
42
}

43
MbsEquil* mbs_new_equil_aux(MbsData* s, MbsAux* mbs_aux)
Nicolas Docquier's avatar
Nicolas Docquier committed
44
{
45
46
    MbsEquil *equil;
    MbsEquilOptions *opts;
Nicolas Docquier's avatar
Nicolas Docquier committed
47
48
49
50
51
52
53
54
55

    // Initialize the equilibrium structure
    equil = (MbsEquil*)malloc(sizeof(MbsEquil));

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

    equil->nx = 0;
    equil->x = NULL;
56
    equil->nxs = 0;
Nicolas Docquier's avatar
Nicolas Docquier committed
57
58
59
60
61
62
63
64
    equil->xs = NULL;
    equil->nxns = 0;
    equil->xns = NULL;
    equil->x_ptr = NULL;
    equil->norm_pk = 0.0;
    equil->qd_saved = NULL;
    equil->qdd_saved = NULL;

Olivier Lantsoght's avatar
Olivier Lantsoght committed
65
66
67
68
69
70
71
72
73
74
75
76
    equil->nxch = 0;
    equil->xch = NULL;
    equil->nxe = 0;
    equil->xe = NULL;

    equil->grad = NULL;
    equil->grad_Rr = NULL;
    equil->grad_uxd = NULL;
    equil->grad_fxe = NULL;
    equil->lpk = NULL;


Nicolas Docquier's avatar
Nicolas Docquier committed
77
78
79
    // saving variables (buffer and all)
    equil->F = NULL;
    equil->R = 0;
80
    equil->iter = 0;
Nicolas Docquier's avatar
Nicolas Docquier committed
81
82
    equil->savePeriodCounter = 0; // initial the saving counter

Olivier Lantsoght's avatar
Olivier Lantsoght committed
83
84
85
86
    equil->buffers = NULL;
    equil->user_buffer = NULL;
    equil->savedArrays = NULL;

Nicolas Docquier's avatar
Nicolas Docquier committed
87
88
    equil->success = 0;

89
    equil->Nux_saved = s->Nux;
90

Nicolas Docquier's avatar
Nicolas Docquier committed
91
92
93
    // Initialize the options struct with default options
    opts = (MbsEquilOptions*)malloc(sizeof(MbsEquilOptions));
    opts->method = 1;
94
95
96
    opts->relax = 0;
    opts->relaxcoeff = 0.9;
    opts->soft = 0;
Nicolas Docquier's avatar
Nicolas Docquier committed
97
98
99
100
101
102
    opts->equitol = 1e-06;
    opts->itermax = 30;
    opts->mode = 1; // static equilibrium
    opts->verbose = 1;
    opts->visualize = 0;
    opts->clearmbsglobal = 1;
103

104

Nicolas Docquier's avatar
Nicolas Docquier committed
105
    opts->save2file = 1;
106
    opts->compute_uxd = 1;
Nicolas Docquier's avatar
Nicolas Docquier committed
107
108
109
110
111
112
113
114
115
    opts->resfilename = NULL;
    opts->respath = NULL;
    opts->animpath = NULL;

    opts->nquch = 0;
    opts->quch = NULL;
    opts->xch_ptr = NULL;
    opts->nxe = 0;
    opts->xe_ptr = NULL;
Olivier Lantsoght's avatar
Olivier Lantsoght committed
116
    opts->xe_index = NULL;
Nicolas Docquier's avatar
Nicolas Docquier committed
117
    opts->fxe_ptr = NULL;
118

Nicolas Docquier's avatar
Nicolas Docquier committed
119
120
121
122
123
124
125
    // saving variables (buffer and all)
    opts->saveperiod = 1;
    opts->save_anim = 1;
    opts->framerate = 1;
    opts->buffersize = -1;
    opts->max_save_user = 12;

126
127
128
129
130
131
132
133
134
135
136
    // gradient computation through dev
    opts->senstol = 1e-06;
    opts->devjac = 1e-06;
    // gradient computation through lpk
    opts->grad_lpk = 0;
    opts->lpk_itermax = 10;
    opts->lpk_relincr = 1e-2;
    opts->lpk_absincr = 1e-3;
    opts->lpk_equitol = 1e-6;
    opts->lpk_lintol = 1e-3;

Nicolas Docquier's avatar
Nicolas Docquier committed
137
138
139
140
141
    equil->options = opts;

    return equil;
}

142
void mbs_delete_equil(MbsEquil* eq, MbsData* s)
Nicolas Docquier's avatar
Nicolas Docquier committed
143
144
145
146
{
    if (eq != NULL)
    {
        free(eq->x_ptr);
Olivier Lantsoght's avatar
Olivier Lantsoght committed
147
        eq->x_ptr = NULL;
148

Nicolas Docquier's avatar
Nicolas Docquier committed
149
        free(eq->F);
Olivier Lantsoght's avatar
Olivier Lantsoght committed
150
        eq->F = NULL;
Nicolas Docquier's avatar
Nicolas Docquier committed
151
152
153
154

        free_dvec_0(eq->x);
        free_ivec_0(eq->xs);
        free_ivec_0(eq->xns);
Olivier Lantsoght's avatar
Olivier Lantsoght committed
155
156
157
        eq->x = NULL;
        eq->xs = NULL;
        eq->xns = NULL;
Nicolas Docquier's avatar
Nicolas Docquier committed
158

159
        // 5. Linearization and gradient variables
Olivier Lantsoght's avatar
Olivier Lantsoght committed
160
161
162
163
164
165
166
167
168
169
170
171
        if (s->nqu > 0){
            free_dmat_0(eq->grad_Rr);
            eq->grad_Rr = NULL;
        }
        if (eq->options->compute_uxd == 1 && s->Nux > 0){
            free_dmat_0(eq->grad_uxd);
            eq->grad_uxd = NULL;
        }
        if (eq->nxe > 0){
            free_dmat_0(eq->grad_fxe);
            eq->grad_fxe = NULL;
        }
172
        free_dmat_0(eq->grad);
Olivier Lantsoght's avatar
Olivier Lantsoght committed
173
        eq->grad = NULL;
174
175
176
        if (eq->options->grad_lpk == 1)
        {
            mbs_delete_lpk(eq->lpk);
Olivier Lantsoght's avatar
Olivier Lantsoght committed
177
            eq->lpk = NULL;
178
        }
Nicolas Docquier's avatar
Nicolas Docquier committed
179
180
        free(eq->qd_saved);
        free(eq->qdd_saved);
Olivier Lantsoght's avatar
Olivier Lantsoght committed
181
182
        eq->qd_saved = NULL;
        eq->qdd_saved = NULL;
Nicolas Docquier's avatar
Nicolas Docquier committed
183
184
185
186
        //-------
        if (eq->options->nquch != 0)
        {
            free(eq->options->xch_ptr);
187
            free_ivec_0(eq->options->quch);
Nicolas Docquier's avatar
Nicolas Docquier committed
188
            free(eq->xch);
Olivier Lantsoght's avatar
Olivier Lantsoght committed
189
190
191
            eq->options->xch_ptr = NULL;
            eq->options->quch = NULL;
            eq->xch = NULL;
Nicolas Docquier's avatar
Nicolas Docquier committed
192
193
194
195
196
        } // corresponding to malloc in mbs_equil_exchange
        if (eq->options->nxe != 0)
        {
            free(eq->options->xe_ptr);
            free(eq->xe);
Olivier Lantsoght's avatar
Olivier Lantsoght committed
197
198
            eq->options->xe_ptr = NULL;
            eq->xe = NULL;
Nicolas Docquier's avatar
Nicolas Docquier committed
199
200
201
        } // corresponding to malloc in mbs_equil_addition ??Q Fxe ?

        free(eq->options);
Olivier Lantsoght's avatar
Olivier Lantsoght committed
202
        eq->options = NULL;
Nicolas Docquier's avatar
Nicolas Docquier committed
203
        //-------
204
        freeMbsAux(eq->aux, s);
Olivier Lantsoght's avatar
Olivier Lantsoght committed
205
        eq->aux = NULL;
Nicolas Docquier's avatar
Nicolas Docquier committed
206
207
208
209
        free(eq);
    }
}

210
int mbs_run_equil(MbsEquil* eq, MbsData* s)
Nicolas Docquier's avatar
Nicolas Docquier committed
211
{
212
    int err = 0;
213
214
    // 1. Initialize the simulation
    // - - - - - - - - - - - - - -
215
216
    err = mbs_equil_init(eq, s);
    if (err < 0){
217
218
219
        s->DoneEquil = 0;
        mbs_equil_finish(eq, s);
        mbs_error_msg("[%d] in mbs_equil_init !! \n", _MBS_ERR_MOD_EQUIL + err);
220
221
        return err;
    }
Nicolas Docquier's avatar
Nicolas Docquier committed
222

223
224
    // 2. Run the simulation
    // - - - - - - - - - - -
225
226
    err = mbs_equil_loop(eq, s);  // another name might be more appropriate
    if (err < 0){
227
        s->DoneEquil = 0;
228
229
        mbs_equil_finish(eq, s);
        mbs_error_msg("[%d] in mbs_equil_loop !! \n", _MBS_ERR_MOD_EQUIL + err);
230
231
        return err;
    }
Nicolas Docquier's avatar
Nicolas Docquier committed
232

233
234
    // 3. Finish the simulation
    // - - - - - - - - - - - -
235
    err = mbs_equil_finish(eq, s);
236
237
238
239
240
    if (err < 0) {
        s->DoneEquil = 0;
        mbs_error_msg("[%d] in mbs_equil_loop !! \n", _MBS_ERR_MOD_EQUIL + err);
        return err;
    }
241

242
    return err;
Nicolas Docquier's avatar
Nicolas Docquier committed
243
244
}

245
int mbs_equil_save(MbsEquil* eq, MbsData *s, int iter)
Nicolas Docquier's avatar
Nicolas Docquier committed
246
{
247
    int i, err = 0;
Nicolas Docquier's avatar
Nicolas Docquier committed
248
249

    for (i = 0; i < eq->bufferNb; i++) {
250
251
        err = mbs_buffer_save(eq->buffers[i], iter, eq->savedArrays[i]);
        if (err < 0) return err; // Error management
Nicolas Docquier's avatar
Nicolas Docquier committed
252
253
    }

254
255
256
    err = mbs_growing_buffer_save(eq->user_buffer, iter);

    return err;
Nicolas Docquier's avatar
Nicolas Docquier committed
257
258
259
260
261
}


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

262
int mbs_equil_init(MbsEquil* eq, MbsData* s)
Nicolas Docquier's avatar
Nicolas Docquier committed
263
{
264
    int test, i, ind_x, ind_c, ind_i, err;
Nicolas Docquier's avatar
Nicolas Docquier committed
265
266
267

    if (eq->options->verbose)
    {
268
        mbs_msg("\n>>EQUIL>> Starting Equilibrium module.\n");
269
270
        if (eq->options->mode == 1)
        {
271
            mbs_msg("       >> Mode: static\n");
272
273
274
        }
        else if (eq->options->mode == 2)
        {
275
            mbs_msg("       >> Mode: quasistatic\n");
276
277
278
        }
        else if (eq->options->mode == 3)
        {
279
            mbs_msg("       >> Mode: dynamic\n");
Nicolas Docquier's avatar
Nicolas Docquier committed
280
281
        }
    }
282
283
284
285

    // Checking mbs_data coherence
    err = mbs_check_mbs_data_values(eq->aux, s);
    if (err < 0) {
286
        mbs_msg(">>EQUIL>> Incoherences detected during module initialization! (See message above) \n");
287
        return err;
288
289
    }

Nicolas Docquier's avatar
Nicolas Docquier committed
290
291
292
    // 1.1 test if an Equilibrium is needed AND if, in a case of a closed-loop system, partionning has been made
    if (s->nqu == 0)
    {
293
        mbs_warning_msg(">>EQUIL>> mbs_equil_init : no independent variables in the equilibrium process !\n");
Nicolas Docquier's avatar
Nicolas Docquier committed
294
295
296
    }
    if (s->DonePart == 0 && s->Ncons > 0)
    {
297
        mbs_msg(">>EQUIL>> The coordinate Partitioning has not be performed for the system !\n");
298
        return _MBS_ERR_MOD_SPEC_11;
Nicolas Docquier's avatar
Nicolas Docquier committed
299
    }
300
301
302

    // Checking constraints and dependant variable coherence
    err = mbs_check_nhu_nqv(s);
303
    if (err < 0) {
304
        mbs_msg(">>EQUIL>> Inchoherence detected during module initialization! \n");
305
        return err;
306
    }
307

Nicolas Docquier's avatar
Nicolas Docquier committed
308
    // check on the equil mode
309
    if (eq->options->mode != 1 && eq->options->mode != 2 && eq->options->mode != 3)
Nicolas Docquier's avatar
Nicolas Docquier committed
310
    {
311
        mbs_msg(">>EQUIL>> mode %d does not exist check documentation for equil->options->mode \n", eq->options->mode);
312
        return err;
Nicolas Docquier's avatar
Nicolas Docquier committed
313
314
315
316
317
318
319
320
    }
    // 1.2 Verify whether a static equilibrium has been performed before a quasistatic or dynamic equilibrium
    if ((eq->options->mode == 2 || eq->options->mode == 3) && s->DoneEquil != 1)
    {
        mbs_warning_msg(">>EQUIL>> mbs_equil_init : It might be appropriate to make a static equilibrium before a quasistatic or dynamic one !\n");
    }

    // 2.1 For static equilibrium, set velocities to 0 when necessary
321
    eq->qd_saved = get_dvec_0(s->njoint);
Nicolas Docquier's avatar
Nicolas Docquier committed
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
    copy_dvec_0(&(s->qd[1]), eq->qd_saved, s->njoint);  // save the mbsPad initial values
    if (eq->options->mode == 1)// for static equilibrium
    {
        test = 0;
        for (i = 1; i <= s->njoint; i++)
        {
            if (s->qd[i] != 0.0)
            {
                test = 1;
                s->qd[i] = 0.0;
            }
        }
        if (test && eq->options->verbose)
        {
            mbs_msg(">>EQUIL>> Non-zero initial velocities for all joints have been forced to 0 !\n");
        }
    }

    // 2.2 For static and quasistatic equilibrium: set all accelerations to zero.
341
    eq->qdd_saved = get_dvec_0(s->njoint);
342
    if (eq->options->mode == 1 || eq->options->mode == 2)
Nicolas Docquier's avatar
Nicolas Docquier committed
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
    {
        copy_dvec_0(&(s->qdd[1]), eq->qdd_saved, s->njoint);
        test = 0;
        for (i = 1; i <= s->njoint; i++)
        {
            if (s->qdd[i] != 0.0)
            {
                test = 1;
                s->qdd[i] = 0.0;
            }
        }
        if (test && eq->options->verbose)
        {
            mbs_msg(">>EQUIL>> Non-zero initial accelerations for all joints have been forced to 0 !\n");
        }
    }
359
360

    if (eq->options->compute_uxd == 0 && s->Nux > 0)
361
362
363
    {
        if (eq->options->verbose)
        {
364
            mbs_warning_msg(">>EQUIL>> The uxd equations are not computed, check the option 'compute_uxd' \n");
365
366
367
368
        }
        eq->Nux_saved = s->Nux;
        s->Nux = 0;
    }
369
370
371

    // 3. Memory allocation for equilibrium variables
    eq->nx = s->nqu + s->Nux + eq->options->nxe;
372
373
374

    if (eq->nx == 0) // Error management
    {
375
376
377
378
379
380
381
382
383
384
        // Restore MbsData, free memory
        copy_dvec_0(eq->qd_saved, s->qd + 1, s->njoint);
        if (eq->options->mode == 1 || eq->options->mode == 2){
            copy_dvec_0(eq->qdd_saved, s->qdd + 1, s->njoint);
        }
        free_dvec_0(eq->qd_saved);
        free_dvec_0(eq->qdd_saved);
        eq->qd_saved = NULL;
        eq->qdd_saved = NULL;

385
        mbs_msg(">>EQUIL>> There are no equilibrium variables: nqu=%d , Nux=%d, nxe=%d - irrelevant process\n", s->nqu, s->Nux, eq->options->nxe);
386
        return _MBS_ERR_MOD_SPEC_12;
387
388
    }

389
390
391
392
393
394
395
396
397
398
399
    eq->x = get_dvec_0(eq->nx);
    eq->xs = get_ivec_0(eq->nx);  // oversized
    eq->xns = get_ivec_0(eq->nx); // oversized
    eq->x_ptr = (double**)malloc((eq->nx + 1) * sizeof(double*));

    if (eq->options->nquch != 0)
    {
        eq->nxch = eq->options->nquch;
        eq->xch = (int*)malloc((eq->nxch + 1) * sizeof(double*));
    }

400
    eq->nxe = eq->options->nxe; // as test are performed on eq->nxe
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
    if (eq->options->nxe != 0)
    {
        eq->xe = (int*)malloc((eq->nxe + 1) * sizeof(double*));
    }

    // 4. Definition of equilibrium variables: x, initial values and x_ptr, pointers
    ind_x = 1;
    ind_c = 0;
    ind_i = 1;
    // 4.1 Independent variables and exchange variables, qu and quch(->xch)
    for (i = 1; i <= s->nqu; i++)
    {
        ind_c = find_ivec_1(eq->options->quch, eq->options->nquch, s->qu[i]);

        if (ind_c == -1) // if the variable is not a changed variable
        {
            eq->x_ptr[ind_x - 1] = &(s->q[s->qu[i]]);
            eq->x[ind_x - 1] = (s->q[s->qu[i]]);
        }
420
        else            // if the variable is a changed variable, it is replaced by an exchange variable
421
        {
422
423
            eq->x_ptr[ind_x - 1] = eq->options->xch_ptr[ind_c];        // get the exchange variable pointer
            eq->x[ind_x - 1] = *(eq->options->xch_ptr[ind_c]);        // get the exchange variable initial value
424
425
426
427
428
            eq->xch[ind_i] = ind_x;
            ind_i++;
        }
        ind_x++;
    }
429
430
431
432
    // 4.2. User defined variables, ux.
    for (i = 1; i <= s->Nux; i++)
    {
        // It would be nice to allow variable exchange for the ux as well. using x=[qu ux] for the index ?
433
        s->ux[i] = s->ux0[i];
434
435
        eq->x_ptr[ind_x - 1] = &(s->ux[i]);
        eq->x[ind_x - 1] = s->ux0[i];
436
437
438
439
440
441
442
443

        ind_x++;
    }
    // 4.3 Added extra equilibrium variables, xe
    for (i = 1; i <= eq->options->nxe; i++)
    {
        eq->x_ptr[ind_x - 1] = eq->options->xe_ptr[i];
        eq->x[ind_x - 1] = *(eq->options->xe_ptr[i]);    // might add test to check whether non null on ptr
444
        ind_x++;
445
        eq->xe[i] = eq->nx + i;
446
    }
447
448
449
450
    eq->options->fxe_ptr = &(user_equil_fxe); // extra equilibrium functions, Fxe(xe)=0
    // might add test to check whether non null on ptr

    // user initialization
Nicolas Docquier's avatar
Nicolas Docquier committed
451
452
453
454
455
    // 'auto_output' structure initialized
    if (eq->options->save2file)
    {
        init_set_output(eq->options->max_save_user);
    }
456
457
    user_equil_init(s, eq);

Nicolas Docquier's avatar
Nicolas Docquier committed
458
459
460
461
    eq->F = get_dvec_0(eq->nx);
    eq->R = 0.0;
    eq->iter = 0;

462
463
464
465
466
    // 5. Linearization and gradient variables
    if (s->nqu > 0) { eq->grad_Rr = get_dmat_0(s->nqu, eq->nx); }
    if (s->Nux > 0) { eq->grad_uxd = get_dmat_0(s->Nux, eq->nx); }
    if (eq->nxe > 0) { eq->grad_fxe = get_dmat_0(eq->nxe, eq->nx); }
    eq->grad = get_dmat_0(eq->nx, eq->nx);
467

468
469
470
471
472
473
474
475
476
    if (eq->options->grad_lpk == 1)
    {
        eq->lpk = mbs_new_lpk(s, eq->nx, MAX(MAX(s->nqu, s->Nux), eq->nxe));
        eq->lpk->itermax = eq->options->lpk_itermax;
        eq->lpk->relincr = eq->options->lpk_relincr;
        eq->lpk->absincr = eq->options->lpk_absincr;
        eq->lpk->equitol = eq->options->lpk_equitol;
        eq->lpk->lintol = eq->options->lpk_lintol;
    }
477

478
479

    // 6. Equilibrium saving to file (anim, results)
Nicolas Docquier's avatar
Nicolas Docquier committed
480
481
    if (eq->options->save2file)
    {
482
483
484
485
        /*  if (eq->options->save_anim == 1 && (eq->options->nquch>0 || eq->options->nxe>0 ))
          {
              // warning message...
          }*/
Nicolas Docquier's avatar
Nicolas Docquier committed
486
487
488
489
490
491
492
493
494
495
496
497

        char *resfilename, *respath, *animpath;
        int bufId, bufSize;
        char* f;
        char* f_anim;

        char **fnameSuffix;
        int  *bufferIDs;
        int  *bufElemNb;

        // allocate buffer at needed size
        bufSize = 4 + get_output_vector_nb();
498
499
500
        fnameSuffix = (char**)malloc(bufSize * sizeof(char*));
        bufferIDs = (int*)malloc(bufSize * sizeof(int));
        bufElemNb = (int*)malloc(bufSize * sizeof(int));
Nicolas Docquier's avatar
Nicolas Docquier committed
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

        // set the filename if not specified
        if (eq->options->resfilename == NULL) {
            resfilename = "equil";
        }
        else {
            resfilename = eq->options->resfilename;
        }

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

        // set the path to animation files if not specified
        if (eq->options->animpath == NULL)
        {
            animpath = (char*)malloc(sizeof(char)*(strlen(s->project_path) + 100));
            strcpy(animpath, s->project_path);
            strcat(animpath, "/animationR");
        }
        else
        {
            animpath = strdup(eq->options->animpath);
        }
533

Nicolas Docquier's avatar
Nicolas Docquier committed
534
535
536
537
538
539
540
541
        // generic buffer properties
        fnameSuffix[0] = "x";   bufferIDs[0] = BUFFER_X;  bufElemNb[0] = eq->nx;
        fnameSuffix[1] = "F";   bufferIDs[1] = BUFFER_F;  bufElemNb[1] = eq->nx;
        fnameSuffix[2] = "R";   bufferIDs[2] = BUFFER_R;  bufElemNb[2] = 1;
        fnameSuffix[3] = "q";   bufferIDs[3] = BUFFER_Q;  bufElemNb[3] = s->njoint;
        eq->bufferNb = 4;

        // buffer properties for the user specified vectors
542
        if (get_output_vector_nb()) {
Nicolas Docquier's avatar
Nicolas Docquier committed
543
            int id = eq->bufferNb;
544
545
546
            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;
Nicolas Docquier's avatar
Nicolas Docquier committed
547
548
549
550
551
                eq->bufferNb += 1;
            }
        }

        // set the buffer size if not specified
552
        if (eq->options->buffersize < 1) {
Nicolas Docquier's avatar
Nicolas Docquier committed
553
554
555
            // compute size assuming a constant time step size
            eq->options->buffersize = 100;
            // add some extra space
556
            eq->options->buffersize = (int)(eq->options->buffersize * 1.1);
Nicolas Docquier's avatar
Nicolas Docquier committed
557
558
559
560
561
562
563
564
565
566
567
568
569
570
            eq->options->buffersize += 10;
        }

        // initialize the buffers for saving results
        eq->buffers = (MbsBuffer**)malloc(eq->bufferNb * sizeof(MbsBuffer*));

        f = (char*)malloc(sizeof(char)*(strlen(respath) + strlen(resfilename) + 30));
        f_anim = (char*)malloc(sizeof(char)*(strlen(animpath) + strlen(resfilename) + 30));

        for (bufId = 0; bufId < eq->bufferNb; bufId++) {
            sprintf(f, "%s/%s_%s.res", respath, resfilename, fnameSuffix[bufId]);
            sprintf(f_anim, "%s/%s_%s.anim", animpath, resfilename, fnameSuffix[bufId]);

            eq->buffers[bufId] = mbs_new_buffer(f, f_anim, bufElemNb[bufId], eq->options->buffersize, bufferIDs[bufId], eq->options->save_anim, 0, 1. / (double)eq->options->framerate);
571
572
            if (eq->buffers[bufId] == NULL)
            {
573
                mbs_msg(">>EQUIL>> Error while initializing buffer num %d \n", bufId);
574
575
576
577
578
579
580
                free(f);
                free(f_anim);
                free(respath);
                free(animpath);
                free(fnameSuffix);
                free(bufferIDs);
                free(bufElemNb);
581
                return _MBS_ERR_LOW_FILES;
582
583

            }
Nicolas Docquier's avatar
Nicolas Docquier committed
584
585
586
587
588
589
590
591
592
593
        }

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

        // binding buffers to data that must be saved
        eq->savedArrays = (double**)malloc(eq->bufferNb * sizeof(double*));
594
595
        bufId = 0;  eq->savedArrays[bufId] = eq->x;
        bufId++;  eq->savedArrays[bufId] = eq->F;
Nicolas Docquier's avatar
Nicolas Docquier committed
596
597
598
        bufId++;  eq->savedArrays[bufId] = &(eq->R);
        bufId++;  eq->savedArrays[bufId] = s->q + 1;

599
600
601
        if (get_output_vector_nb()) {
            for (i = 0; i < get_output_vector_nb(); i++) {
                bufId++;  eq->savedArrays[bufId] = get_output_vector_ptr(i);
Nicolas Docquier's avatar
Nicolas Docquier committed
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
            }
        }

        free(f);
        free(f_anim);
        free(respath);
        free(animpath);
        free(path_growing);
        free(fnameSuffix);
        free(bufferIDs);
        free(bufElemNb);
    }
    else {
        reset_flag_output();
    }
617
    return 0;
Nicolas Docquier's avatar
Nicolas Docquier committed
618
619
}

620
int mbs_equil_loop(MbsEquil* eq, MbsData* s)
Nicolas Docquier's avatar
Nicolas Docquier committed
621
{
622
    int err = 0;
Nicolas Docquier's avatar
Nicolas Docquier committed
623
624
    switch (eq->options->method)
    {
Olivier Lantsoght's avatar
Olivier Lantsoght committed
625
626
        case 1:
        {
627
            err = mbs_equil_fsolvepk(&(mbs_equil_fct), eq, eq->aux, s);
628
629
630
631
632
            if (err < 0)
            {
                mbs_msg(">>EQUIL>> [%d] in mbs_equil_fsolvepk.\n", err);
                return(err);
            }
Olivier Lantsoght's avatar
Olivier Lantsoght committed
633
634
635
636
            break;
        }
        default:
        {
637
            err =  _MBS_ERR_INIT;
Olivier Lantsoght's avatar
Olivier Lantsoght committed
638
639
            mbs_msg(">>EQUIL>>\n");
            mbs_msg(">>EQUIL>> Invalid option value: MbsEquil->options->method=%d.\n", eq->options->method);
640
            mbs_msg(">>EQUIL>> [%d] Choose a method listed in the documentation.\n", err);
641
            return(err);
Olivier Lantsoght's avatar
Olivier Lantsoght committed
642
        }
Nicolas Docquier's avatar
Nicolas Docquier committed
643
    }
644
    return err;
Nicolas Docquier's avatar
Nicolas Docquier committed
645
646
}

647
int mbs_equil_finish(MbsEquil* eq, MbsData* s)
Nicolas Docquier's avatar
Nicolas Docquier committed
648
{
649
    int err = 0;
650
    if (!eq){
651
        return err;
652
    }
Nicolas Docquier's avatar
Nicolas Docquier committed
653
654
655

    if (eq->options->save2file)
    {
656
657
658
659
        // both buffers write are called and memory released before raising error.
        int err1 = 0;
        int err2 = 0;

Nicolas Docquier's avatar
Nicolas Docquier committed
660
        int i;
661
        // write the buffer
662
663
        if(eq->buffers){
            for (i = 0; i < eq->bufferNb; i++)
664
            {
665
666
667
668
669
670
                err1 = mbs_buffer_write(eq->buffers[i]);
                if (err1 < 0)
                {
                    mbs_msg(">>EQUIL>>\n");
                    mbs_msg(">>EQUIL>> ***** mbs_buffer_write : impossible to save the files from buffers i=%d *****\n", i);
                }
671
            }
672
673
674
675
676
677
            // free memory used by buffers
            for (i = 0; i < eq->bufferNb; i++) {
                mbs_delete_buffer(eq->buffers[i]);
            }
            free(eq->buffers);
            eq->buffers = NULL;
Nicolas Docquier's avatar
Nicolas Docquier committed
678
679
680
        }

        // user inputs final gestion
681
682
683
684
685
686
687
        if(eq->user_buffer){
            err2 = mbs_growing_buffer_write(eq->user_buffer);
            if (err2 < 0)
            {
                mbs_msg(">>EQUIL>>\n");
                mbs_msg(">>EQUIL>> ***** mbs_growing_buffer_write : impossible to save the files *****\n");
            }
Nicolas Docquier's avatar
Nicolas Docquier committed
688

689
690
691
            mbs_delete_growing_buffer(eq->user_buffer);
            eq->user_buffer = NULL;
        }
Nicolas Docquier's avatar
Nicolas Docquier committed
692
693
694
695
696

        // release memory for the 'auto_output' structure
        free_set_output();
        reset_flag_output();
        free(eq->savedArrays);
697
698
699
700
701
        eq->savedArrays = NULL;

        // memory has been released, raise error
        if (err1 < 0 && err2 <0)
        {
702
703
            mbs_msg("[%d] and [%d] in mbs_equil_finish !! \n", err1,  err2);
            return err1; // arbitrary choice
704
705
        }
        else if (err1 < 0){
706
            err = err1;
707

708
            mbs_msg("[%d] in mbs_equil_finish !! \n", err);
709
            return(err);
710
711
        }
        else if (err2 < 0){
712
            err = err2;
713

714
            mbs_msg("[%d] in mbs_equil_finish !! \n", err);
715
            return(err);
716
        }
Nicolas Docquier's avatar
Nicolas Docquier committed
717
718
    }

719
720
721
722
723
724
725
726
    // free qd_saved qd_saved
    // free x xs xns
    if (eq->options->mode == 1)  // if static equilibrium: get the velocities and acceleration back from the pad.
    {
        copy_dvec_0(eq->qd_saved, &(s->qd[1]), s->njoint);
        copy_dvec_0(eq->qdd_saved, &(s->qdd[1]), s->njoint);
        s->DoneEquil = 1;
    }
Nicolas Docquier's avatar
Nicolas Docquier committed
727
728
729
730
731
    else if (eq->options->mode == 2) // if quasistatic equilibrium: get the acceleration back from the pad.
    {
        copy_dvec_0(eq->qdd_saved, &(s->qdd[1]), s->njoint);
        s->DoneEquil = 1;
    }
732
733
734
735
    else if (eq->options->mode == 3)
    {
        s->DoneEquil = 1;
    }
Nicolas Docquier's avatar
Nicolas Docquier committed
736
737
738
739

    // user finishing procedure
    user_equil_finish(s, eq);

740
741
    if (eq->options->compute_uxd == 0 && s->Nux == 0)
    {
742
        s->Nux = eq->Nux_saved;
743
744
    }

745
746
    if (eq->options->verbose)
    {
747
        mbs_msg("\n>>EQUIL>> Equilibrium analysis finished.\n");
748
    }
749
    return err;
Nicolas Docquier's avatar
Nicolas Docquier committed
750
751
752
753
754
755
}

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

void mbs_equil_exchange(MbsEquilOptions *options)
{
756
757
758
759
760
761
762
763
764
765
    int i;
    options->quch = get_ivec_0(options->nquch + 1);
    options->xch_ptr = (double**)malloc((options->nquch + 1) * sizeof(double*));

    for (i = 0; i <= options->nquch; i++)
    {
        options->quch[i] = 0;
        options->xch_ptr[i] = NULL;
    }
    options->quch[0] = options->nquch;
Nicolas Docquier's avatar
Nicolas Docquier committed
766
767
}

768
void mbs_equil_set_variable(MbsEquilOptions *options, double *address,
769
    int id_exchanged, int replaced_variable_id) {
770
771
772
773
    options->quch[id_exchanged] = replaced_variable_id;
    options->xch_ptr[id_exchanged] = address;
}

774
void mbs_equil_add_variable(MbsEquilOptions *options, double *address, int id_added) {
775
776
777
778
    options->xe_ptr[id_added] = address;
}


Nicolas Docquier's avatar
Nicolas Docquier committed
779
780
void mbs_equil_addition(MbsEquilOptions *options)
{
781
782
    int i;
    options->xe_ptr = (double**)malloc((options->nxe + 1) * sizeof(double*));
Nicolas Docquier's avatar
Nicolas Docquier committed
783

784
785
786
787
    for (i = 0; i <= options->nxe; i++)
    {
        options->xe_ptr[i] = NULL;
    }
Nicolas Docquier's avatar
Nicolas Docquier committed
788
789
790
791
}

void mbs_equil_ignorance(MbsEquilOptions *options) // useless function without the qui
{
792
793
794
795
796
797
798
799
    /*
    int i;
    options->qui=(int*) malloc((options->nqui+1)*sizeof(int));
    for(i=0;i<=options->nxe;i++)
    {
        options->qui[i]=0;
    }
    */
Nicolas Docquier's avatar
Nicolas Docquier committed
800
801
802
803
804
805
}

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

int mbs_equil_fct(double x[], int nx, double fx[], MbsEquil *eq, MbsAux *aux, MbsData *s)
{
806
807
808
    int err = 0;
    int i;
    int ind_x;
Nicolas Docquier's avatar
Nicolas Docquier committed
809

810
811
812
813
814
    // 1. Update equilibrium variables (in mbs_data) through the equilibrium pointers
    for (i = 0; i < nx; i++) // numerical process convention (starts at 0)
    {
        eq->x_ptr[i][0] = x[i];
    }
Nicolas Docquier's avatar
Nicolas Docquier committed
815

816
    // 2. Compute Fruc
Nicolas Docquier's avatar
Nicolas Docquier committed
817
    err = mbs_Rred(aux, s);
818
819
820
    if (err < 0){
        return err; // Error management
    }
821

822
823
824
825
826
827
828
829
    // 3. Express the equilibrium function, f(x) (i.e. the vector that has to equals 0)
    ind_x = 0;
    // 3.1 Express Rred, the reduced equations of motion (after the two reductions) on its residual form
    for (i = 0; i < s->nqu; i++)
    {
        fx[ind_x] = aux->Rred[i + 1]; //  +1 necessary to match Num and Rob convention
        ind_x++;
    }
830
    // 3.2 Express Fxu, the equations defined by the user in user_defined.
831
832
    if (s->Nux > 0) user_Derivative(s);
    for (i = 0; i < s->Nux; i++)
833
    {
834
        fx[ind_x] = s->uxd[i + 1];  // Error management TO DO
835
836
        ind_x++;
    }
837
838
839
840
    // 3.3 Express Fxe, the extra equilibrium equations added by the user
    if (eq->options->nxe != 0)
    {
        (*eq->options->fxe_ptr)(s, &(fx[ind_x - 1])); // -1 necessary to match Num and Rob convention
841
                                                    // Error management TO DO
842
    }
Nicolas Docquier's avatar
Nicolas Docquier committed
843

844
    return err;
Nicolas Docquier's avatar
Nicolas Docquier committed
845
846
}

847
int mbs_equil_fsolvepk(int(*fun_ptr)(double*, int, double*, MbsEquil*, MbsAux*, MbsData*), MbsEquil *eq, MbsAux *aux, MbsData *s)
Nicolas Docquier's avatar
Nicolas Docquier committed
848
{
849
850
    int i, j;
    int err = 0;
Nicolas Docquier's avatar
Nicolas Docquier committed
851

852
    void* grad_fun_ptr = NULL; // in case of Analythical expression for the gradient
Nicolas Docquier's avatar
Nicolas Docquier committed
853

854
    int nx, nf;
855
856
    double *xd = NULL, *f = NULL, *fd = NULL; // xd is used for different calculations.
    double **grad = NULL;
Nicolas Docquier's avatar
Nicolas Docquier committed
857

858
859
    double  normR;
    int r_cou;
Nicolas Docquier's avatar
Nicolas Docquier committed
860

861
    double **grads = NULL;
Nicolas Docquier's avatar
Nicolas Docquier committed
862

863
864
    // s stands for sensitive , R stands for Relaxation
    double *xds = NULL, *xs = NULL, *pgrads = NULL, *fs = NULL, *xdsR = NULL, *xsR = NULL;
865
    double norm1, norm2, inorm2, dnorm2, norm3, sc_prdct;
Nicolas Docquier's avatar
Nicolas Docquier committed
866

867
    int *indx = NULL;
868
    double d;
Nicolas Docquier's avatar
Nicolas Docquier committed
869

870
871
872
873
874
875
    // 0. initialization (and allocation)
    nx = eq->nx;
    nf = nx;
    xd = get_dvec_0(nx);
    f = get_dvec_0(nx);
    fd = get_dvec_0(nx);
876
    grad = eq->grad;    // Grad(f) [nf*nx]  with f [nf*1] and x [nx*1]
Nicolas Docquier's avatar
Nicolas Docquier committed
877

878
879
    // 0.1 Solver might either use a analythical evaluation or a numerical estimation for the Jacobian
    // in case of analythical evaluation a additional test might be necessary
Nicolas Docquier's avatar
Nicolas Docquier committed
880

881
    // 1. Gradient test - search for sensitive variables
Nicolas Docquier's avatar
Nicolas Docquier committed
882

883
    // 1.1 Gradient estimation for f(x) :
884
885
886
887
888
889
890
891
    if (eq->options->grad_lpk == 1)
    {
        err = mbs_equil_grad_lpk(eq, s);
    }
    else
    {
        err = mbs_equil_grad_dev(eq, s);
    }
892
    if (err < 0) // Error management
893
    {
894
895
896
        free_dvec_0(xd);
        free_dvec_0(f);
        free_dvec_0(fd);
897
        mbs_msg(">>EQUIL>> Error at initial gradient computation \n");
898
        mbs_msg(">>EQUIL>> [%d] in mbs_equil_fsolvepk !! \n",  err);
899
        return(err);
900
901
    }

Nicolas Docquier's avatar
Nicolas Docquier committed
902

903
    // 1.2. Gradient testing (removal of non-sensitive variables AND redundant variables)
904
905
    eq->nxs = 0;
    eq->nxns = 0;
906
907
908
909
910
911
    double **grad_fill = get_dmat_0(nf, nx);
    zeros_dmat_0(grad_fill, nf, nx);
    int rank_fill;
    int rank_iter;
    rank_iter = 1;
    for (j = 0; j < nx; j++)   // there might be a better way to do that which does no imply to compute nx times the rank of the gradient
912
    {
913
914
        //copy the column
        for (i = 0; i < nf; i++)
915
916
917
        {
            if (fabs(grad[j][i]) > fabs(eq->options->senstol)) // sensitive if there is a non-zero element among the ith column
            {
918
                grad_fill[i][j] = 0.0;
919
            }
920
            grad_fill[i][j] = eq->grad[i][j];
921
        }
922
        rank_fill = mbs_rank_0(grad_fill, nf, j + 1);
923
        if (rank_fill == rank_iter)
924
        {
925
926
            rank_iter = rank_iter + 1;
            // the column is indpt
927
928
            eq->xs[eq->nxs] = j;        // write the vector, eq->xs, with the index of sensitive variables
            (eq->nxs)++;                // increment the number of sensitive variables
929

930
        }
931
        else
932
        {
933
934
            eq->xns[eq->nxns] = j;    // write the vector, eq->xs, with the index of non sensitive variables
            (eq->nxns)++;            // increment the number of non sensitive variables
935

936
937
        }
    }
938
    free_dmat_0(grad_fill);
939

940
    // 1.3 Print a summary of sensitive and non sensitive variables
941
942
943
    if (eq->options->verbose)
    {
        mbs_msg(">>EQUIL>> Number of sensitive variables detected, nxs = %d / %d  \n", eq->nxs, eq->nx);
944
945
946
947
        mbs_msg(">>EQUIL>> xs   = [");  for (i = 0; i < eq->nxs; i++) { mbs_msg(" %d ", eq->xs[i] + 1); } mbs_msg(" ] \t (index in the x vector)\n");
        mbs_msg(">>EQUIL>> xns  = [");  for (i = 0; i < eq->nxns; i++) { mbs_msg(" %d ", eq->xns[i] + 1); }mbs_msg(" ] \t (index in the x vector)\n");
    }

948
949
    if (eq->nxs == 0)
    {
950
951
952
953
        free_dvec_0(xd);
        free_dvec_0(f);
        free_dvec_0(fd);

954
        err = _MBS_ERR_MOD_SPEC_12;
955

956
        mbs_msg(">>EQUIL>> mbs_equil_fsolvepk: none of the proposed parameter are sensitive !\n");
957
        mbs_msg(">>EQUIL>> [%d] in mbs_equil_fsolvepk \n", err);
958
        return(err);
959
    }
Nicolas Docquier's avatar
Nicolas Docquier committed
960

961
962
963
964
965
966
967
968
    // 2. Second initialization (and allocation)
    grads = get_dmat_0(eq->nxs, eq->nxs);
    xds = get_dvec_0(eq->nxs);
    xs = get_dvec_0(eq->nxs);
    pgrads = get_dvec_0(eq->nxs);
    fs = get_dvec_0(eq->nxs);
    xdsR = get_dvec_0(eq->nxs);
    xsR = get_dvec_0(eq->nxs);
Nicolas Docquier's avatar
Nicolas Docquier committed
969

970
    // 3. Solution searching
Nicolas Docquier's avatar
Nicolas Docquier committed
971

972
973
974
    // Initial values for Iterative procedure
    err = (*fun_ptr)(eq->x, nx, f, eq, aux, s);
    if (err < 0) // Error management
975
    {
976
977
978
979
980
981
982
983
984
985
986
987
        free_dmat_0(grads);

        free_dvec_0(xd);
        free_dvec_0(f);
        free_dvec_0(fd);
        free_dvec_0(xds);
        free_dvec_0(xs);
        free_dvec_0(pgrads);
        free_dvec_0(fs);
        free_dvec_0(xdsR);
        free_dvec_0(xsR);

988
        mbs_msg(">>EQUIL>>  Error at initial function evaluatoin !\n");
989
        mbs_msg(">>EQUIL>> [%d] in mbs_equil_fsolvepk !! \n", err);
990
        return(err);
991
    }
992

993
    user_equil_loop(s, eq);
994
    eq->norm_pk = norm_dvec_0(f, nf);
995
    // Save results of the initial step to the buffer
Nicolas Docquier's avatar
Nicolas Docquier committed
996
997
998
999
    eq->R = eq->norm_pk;
    copy_dvec_0(f, eq->F, eq->nx);
    if (eq->options->save2file)
    {
1000
        err = mbs_equil_save(eq, s, eq->iter);
For faster browsing, not all history is shown. View entire blame