mbs_equil.c

/**
* @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>
#include "mbs_errors_names.h"
#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"
#include "nrfct.h"
#include "useful_functions.h"
#include "mbs_check.h"

#include "mbs_linearipk.h"


#define MAX_NB_BUFFER (3 + 1) // maximal number of buffer 3 for equilibrium, 1 for q (animation)

MbsEquil* mbs_new_equil(MbsData* s)
{
    MbsAux* mbs_aux;

    // Initialize the local data struct
    mbs_aux = initMbsAux(s);

    return mbs_new_equil_aux(s, mbs_aux);
}

MbsEquil* mbs_new_equil_aux(MbsData* s, MbsAux* mbs_aux)
{
    MbsEquil *equil;
    MbsEquilOptions *opts;

    // 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;
    equil->nxs = 0;
    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;

    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;


    // saving variables (buffer and all)
    equil->F = NULL;
    equil->R = 0;
    equil->iter = 0;
    equil->savePeriodCounter = 0; // initial the saving counter

    equil->buffers = NULL;
    equil->user_buffer = NULL;
    equil->savedArrays = NULL;

    equil->success = 0;

    equil->Nux_saved = s->Nux;

    // Initialize the options struct with default options
    opts = (MbsEquilOptions*)malloc(sizeof(MbsEquilOptions));
    opts->method = 1;
    opts->relax = 0;
    opts->relaxcoeff = 0.9;
    opts->soft = 0;
    opts->equitol = 1e-06;
    opts->itermax = 30;
    opts->mode = 1; // static equilibrium
    opts->verbose = 1;
    opts->visualize = 0;
    opts->clearmbsglobal = 1;


    opts->save2file = 1;
    opts->compute_uxd = 1;
    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;
    opts->xe_index = NULL;
    opts->fxe_ptr = NULL;

    // saving variables (buffer and all)
    opts->saveperiod = 1;
    opts->save_anim = 1;
    opts->framerate = 1;
    opts->buffersize = -1;
    opts->max_save_user = 12;

    // 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;

    equil->options = opts;

    return equil;
}

void mbs_delete_equil(MbsEquil* eq, MbsData* s)
{
    if (eq != NULL)
    {
        free(eq->x_ptr);
        eq->x_ptr = NULL;

        free(eq->F);
        eq->F = NULL;

        free_dvec_0(eq->x);
        free_ivec_0(eq->xs);
        free_ivec_0(eq->xns);
        eq->x = NULL;
        eq->xs = NULL;
        eq->xns = NULL;

        // 5. Linearization and gradient variables
        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;
        }
        free_dmat_0(eq->grad);
        eq->grad = NULL;
        if (eq->options->grad_lpk == 1)
        {
            mbs_delete_lpk(eq->lpk);
            eq->lpk = NULL;
        }
        free(eq->qd_saved);
        free(eq->qdd_saved);
        eq->qd_saved = NULL;
        eq->qdd_saved = NULL;
        //-------
        if (eq->options->nquch != 0)
        {
            free(eq->options->xch_ptr);
            free_ivec_0(eq->options->quch);
            free(eq->xch);
            eq->options->xch_ptr = NULL;
            eq->options->quch = NULL;
            eq->xch = NULL;
        } // corresponding to malloc in mbs_equil_exchange
        if (eq->options->nxe != 0)
        {
            free(eq->options->xe_ptr);
            free(eq->xe);
            eq->options->xe_ptr = NULL;
            eq->xe = NULL;
        } // corresponding to malloc in mbs_equil_addition ??Q Fxe ?

        free(eq->options);
        eq->options = NULL;
        //-------
        freeMbsAux(eq->aux, s);
        eq->aux = NULL;
        free(eq);
    }
}

int mbs_run_equil(MbsEquil* eq, MbsData* s)
{
    int err = 0;
    // 1. Initialize the simulation
    // - - - - - - - - - - - - - -
    err = mbs_equil_init(eq, s);
    if (err < 0){
        s->DoneEquil = 0;
        mbs_equil_finish(eq, s);
        mbs_error_msg("[%d] in mbs_equil_init !! \n", _MBS_ERR_MOD_EQUIL + err);
        return err;
    }

    // 2. Run the simulation
    // - - - - - - - - - - -
    err = mbs_equil_loop(eq, s);  // another name might be more appropriate
    if (err < 0){
        s->DoneEquil = 0;
        mbs_equil_finish(eq, s);
        mbs_error_msg("[%d] in mbs_equil_loop !! \n", _MBS_ERR_MOD_EQUIL + err);
        return err;
    }

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

    return err;
}

int mbs_equil_save(MbsEquil* eq, MbsData *s, int iter)
{
    int i, err = 0;

    for (i = 0; i < eq->bufferNb; i++) {
        err = mbs_buffer_save(eq->buffers[i], iter, eq->savedArrays[i]);
        if (err < 0) return err; // Error management
    }

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

    return err;
}


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

int mbs_equil_init(MbsEquil* eq, MbsData* s)
{
    int test, i, ind_x, ind_c, ind_i, err;

    if (eq->options->verbose)
    {
        mbs_msg("\n>>EQUIL>> Starting Equilibrium module.\n");
        if (eq->options->mode == 1)
        {
            mbs_msg("       >> Mode: static\n");
        }
        else if (eq->options->mode == 2)
        {
            mbs_msg("       >> Mode: quasistatic\n");
        }
        else if (eq->options->mode == 3)
        {
            mbs_msg("       >> Mode: dynamic\n");
        }
    }

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

    // 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)
    {
        mbs_warning_msg(">>EQUIL>> mbs_equil_init : no independent variables in the equilibrium process !\n");
    }
    if (s->DonePart == 0 && s->Ncons > 0)
    {
        mbs_msg(">>EQUIL>> The coordinate Partitioning has not be performed for the system !\n");
        return _MBS_ERR_MOD_SPEC_11;
    }

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

    // check on the equil mode
    if (eq->options->mode != 1 && eq->options->mode != 2 && eq->options->mode != 3)
    {
        mbs_msg(">>EQUIL>> mode %d does not exist check documentation for equil->options->mode \n", eq->options->mode);
        return err;
    }
    // 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
    eq->qd_saved = get_dvec_0(s->njoint);
    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.
    eq->qdd_saved = get_dvec_0(s->njoint);
    if (eq->options->mode == 1 || eq->options->mode == 2)
    {
        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");
        }
    }

    if (eq->options->compute_uxd == 0 && s->Nux > 0)
    {
        if (eq->options->verbose)
        {
            mbs_warning_msg(">>EQUIL>> The uxd equations are not computed, check the option 'compute_uxd' \n");
        }
        eq->Nux_saved = s->Nux;
        s->Nux = 0;
    }

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

    if (eq->nx == 0) // Error management
    {
        // 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;

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

    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*));
    }

    eq->nxe = eq->options->nxe; // as test are performed on eq->nxe
    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]]);
        }
        else            // if the variable is a changed variable, it is replaced by an exchange variable
        {
            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
            eq->xch[ind_i] = ind_x;
            ind_i++;
        }
        ind_x++;
    }
    // 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 ?
        s->ux[i] = s->ux0[i];
        eq->x_ptr[ind_x - 1] = &(s->ux[i]);
        eq->x[ind_x - 1] = s->ux0[i];

        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
        ind_x++;
        eq->xe[i] = eq->nx + i;
    }
    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
    // 'auto_output' structure initialized
    if (eq->options->save2file)
    {
        init_set_output(eq->options->max_save_user);
    }
    user_equil_init(s, eq);

    eq->F = get_dvec_0(eq->nx);
    eq->R = 0.0;
    eq->iter = 0;

    // 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);

    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;
    }


    // 6. Equilibrium saving to file (anim, results)
    if (eq->options->save2file)
    {
        /*  if (eq->options->save_anim == 1 && (eq->options->nquch>0 || eq->options->nxe>0 ))
          {
              // warning message...
          }*/

        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();
        fnameSuffix = (char**)malloc(bufSize * sizeof(char*));
        bufferIDs = (int*)malloc(bufSize * sizeof(int));
        bufElemNb = (int*)malloc(bufSize * sizeof(int));

        // 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);
        }

        // 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
        if (get_output_vector_nb()) {
            int id = eq->bufferNb;
            for (i = 0; i < get_output_vector_nb(); i++) {
                fnameSuffix[id] = get_output_vector_label(i);    bufferIDs[id] = BUFFER_OTHER + i;    bufElemNb[id] = get_output_vector_size(i);
                id += 1;
                eq->bufferNb += 1;
            }
        }

        // set the buffer size if not specified
        if (eq->options->buffersize < 1) {
            // compute size assuming a constant time step size
            eq->options->buffersize = 100;
            // add some extra space
            eq->options->buffersize = (int)(eq->options->buffersize * 1.1);
            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);
            if (eq->buffers[bufId] == NULL)
            {
                mbs_msg(">>EQUIL>> Error while initializing buffer num %d \n", bufId);
                free(f);
                free(f_anim);
                free(respath);
                free(animpath);
                free(fnameSuffix);
                free(bufferIDs);
                free(bufElemNb);
                return _MBS_ERR_LOW_FILES;

            }
        }

        // 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*));
        bufId = 0;  eq->savedArrays[bufId] = eq->x;
        bufId++;  eq->savedArrays[bufId] = eq->F;
        bufId++;  eq->savedArrays[bufId] = &(eq->R);
        bufId++;  eq->savedArrays[bufId] = s->q + 1;

        if (get_output_vector_nb()) {
            for (i = 0; i < get_output_vector_nb(); i++) {
                bufId++;  eq->savedArrays[bufId] = get_output_vector_ptr(i);
            }
        }

        free(f);
        free(f_anim);
        free(respath);
        free(animpath);
        free(path_growing);
        free(fnameSuffix);
        free(bufferIDs);
        free(bufElemNb);
    }
    else {
        reset_flag_output();
    }
    return 0;
}

int mbs_equil_loop(MbsEquil* eq, MbsData* s)
{
    int err = 0;
    switch (eq->options->method)
    {
        case 1:
        {
            err = mbs_equil_fsolvepk(&(mbs_equil_fct), eq, eq->aux, s);
            if (err < 0)
            {
                mbs_msg(">>EQUIL>> [%d] in mbs_equil_fsolvepk.\n", err);
                return(err);
            }
            break;
        }
        default:
        {
            err =  _MBS_ERR_INIT;
            mbs_msg(">>EQUIL>>\n");
            mbs_msg(">>EQUIL>> Invalid option value: MbsEquil->options->method=%d.\n", eq->options->method);
            mbs_msg(">>EQUIL>> [%d] Choose a method listed in the documentation.\n", err);
            return(err);
        }
    }
    return err;
}

int mbs_equil_finish(MbsEquil* eq, MbsData* s)
{
    int err = 0;
    if (!eq){
        return err;
    }

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

        int i;
        // write the buffer
        if(eq->buffers){
            for (i = 0; i < eq->bufferNb; i++)
            {
                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);
                }
            }
            // free memory used by buffers
            for (i = 0; i < eq->bufferNb; i++) {
                mbs_delete_buffer(eq->buffers[i]);
            }
            free(eq->buffers);
            eq->buffers = NULL;
        }

        // user inputs final gestion
        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");
            }

            mbs_delete_growing_buffer(eq->user_buffer);
            eq->user_buffer = NULL;
        }

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

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

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

            mbs_msg("[%d] in mbs_equil_finish !! \n", err);
            return(err);
        }
    }

    // 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;
    }
    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;
    }
    else if (eq->options->mode == 3)
    {
        s->DoneEquil = 1;
    }

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

    if (eq->options->compute_uxd == 0 && s->Nux == 0)
    {
        s->Nux = eq->Nux_saved;
    }

    if (eq->options->verbose)
    {
        mbs_msg("\n>>EQUIL>> Equilibrium analysis finished.\n");
    }
    return err;
}

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

void mbs_equil_exchange(MbsEquilOptions *options)
{
    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;
}

void mbs_equil_set_variable(MbsEquilOptions *options, double *address,
    int id_exchanged, int replaced_variable_id) {
    options->quch[id_exchanged] = replaced_variable_id;
    options->xch_ptr[id_exchanged] = address;
}

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


void mbs_equil_addition(MbsEquilOptions *options)
{
    int i;
    options->xe_ptr = (double**)malloc((options->nxe + 1) * sizeof(double*));

    for (i = 0; i <= options->nxe; i++)
    {
        options->xe_ptr[i] = NULL;
    }
}

void mbs_equil_ignorance(MbsEquilOptions *options) // useless function without the qui
{
    /*
    int i;
    options->qui=(int*) malloc((options->nqui+1)*sizeof(int));
    for(i=0;i<=options->nxe;i++)
    {
        options->qui[i]=0;
    }
    */
}

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

int mbs_equil_fct(double x[], int nx, double fx[], MbsEquil *eq, MbsAux *aux, MbsData *s)
{
    int err = 0;
    int i;
    int ind_x;

    // 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];
    }

    // 2. Compute Fruc
    err = mbs_Rred(aux, s);
    if (err < 0){
        return err; // Error management
    }

    // 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++;
    }
    // 3.2 Express Fxu, the equations defined by the user in user_defined.
    if (s->Nux > 0) user_Derivative(s);
    for (i = 0; i < s->Nux; i++)
    {
        fx[ind_x] = s->uxd[i + 1];  // Error management TO DO
        ind_x++;
    }
    // 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
                                                    // Error management TO DO
    }

    return err;
}

int mbs_equil_fsolvepk(int(*fun_ptr)(double*, int, double*, MbsEquil*, MbsAux*, MbsData*), MbsEquil *eq, MbsAux *aux, MbsData *s)
{
    int i, j;
    int err = 0;

    void* grad_fun_ptr = NULL; // in case of Analythical expression for the gradient

    int nx, nf;
    double *xd = NULL, *f = NULL, *fd = NULL; // xd is used for different calculations.
    double **grad = NULL;

    double  normR;
    int r_cou;

    double **grads = NULL;

    // s stands for sensitive , R stands for Relaxation
    double *xds = NULL, *xs = NULL, *pgrads = NULL, *fs = NULL, *xdsR = NULL, *xsR = NULL;
    double norm1, norm2, inorm2, dnorm2, norm3, sc_prdct;

    int *indx = NULL;
    double d;

    // 0. initialization (and allocation)
    nx = eq->nx;
    nf = nx;
    xd = get_dvec_0(nx);
    f = get_dvec_0(nx);
    fd = get_dvec_0(nx);
    grad = eq->grad;    // Grad(f) [nf*nx]  with f [nf*1] and x [nx*1]

    // 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

    // 1. Gradient test - search for sensitive variables

    // 1.1 Gradient estimation for f(x) :
    if (eq->options->grad_lpk == 1)
    {
        err = mbs_equil_grad_lpk(eq, s);
    }
    else
    {
        err = mbs_equil_grad_dev(eq, s);
    }
    if (err < 0) // Error management
    {
        free_dvec_0(xd);
        free_dvec_0(f);
        free_dvec_0(fd);
        mbs_msg(">>EQUIL>> Error at initial gradient computation \n");
        mbs_msg(">>EQUIL>> [%d] in mbs_equil_fsolvepk !! \n",  err);
        return(err);
    }


    // 1.2. Gradient testing (removal of non-sensitive variables AND redundant variables)
    eq->nxs = 0;
    eq->nxns = 0;
    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
    {
        //copy the column
        for (i = 0; i < nf; i++)
        {
            if (fabs(grad[j][i]) > fabs(eq->options->senstol)) // sensitive if there is a non-zero element among the ith column
            {
                grad_fill[i][j] = 0.0;
            }
            grad_fill[i][j] = eq->grad[i][j];
        }
        rank_fill = mbs_rank_0(grad_fill, nf, j + 1);
        if (rank_fill == rank_iter)
        {
            rank_iter = rank_iter + 1;
            // the column is indpt
            eq->xs[eq->nxs] = j;        // write the vector, eq->xs, with the index of sensitive variables
            (eq->nxs)++;                // increment the number of sensitive variables

        }
        else
        {
            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

        }
    }
    free_dmat_0(grad_fill);

    // 1.3 Print a summary of sensitive and non sensitive variables
    if (eq->options->verbose)
    {
        mbs_msg(">>EQUIL>> Number of sensitive variables detected, nxs = %d / %d  \n", eq->nxs, eq->nx);
        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");
    }

    if (eq->nxs == 0)
    {
        free_dvec_0(xd);
        free_dvec_0(f);
        free_dvec_0(fd);

        err = _MBS_ERR_MOD_SPEC_12;

        mbs_msg(">>EQUIL>> mbs_equil_fsolvepk: none of the proposed parameter are sensitive !\n");
        mbs_msg(">>EQUIL>> [%d] in mbs_equil_fsolvepk \n", err);
        return(err);
    }

    // 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);

    // 3. Solution searching

    // Initial values for Iterative procedure
    err = (*fun_ptr)(eq->x, nx, f, eq, aux, s);
    if (err < 0) // Error management
    {
        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);

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

    user_equil_loop(s, eq);
    eq->norm_pk = norm_dvec_0(f, nf);
    // Save results of the initial step to the buffer
    eq->R = eq->norm_pk;
    copy_dvec_0(f, eq->F, eq->nx);
    if (eq->options->save2file)
    {
        err = mbs_equil_save(eq, s, eq->iter);