mbs_close_loops.c 5.84 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17
//-------------------------------
// UCL-CEREM-MBS
//
// @version MBsysLab_s 1.7.a
//
// Creation : 2008 by JF Collard
// Last update : 01/10/2008
//-------------------------------
//
// Gestion via Bugzilla :
// 01/10/2008 : JFC : Bug n°40
//

#include "MBSfun.h"
#include "nrfct.h"
#include "mbs_project_interface.h"
#include "mbs_message.h"
18 19
#include "mbs_solvekin.h"

20 21 22 23 24 25 26 27

double norm_vector(double *v, int n);
double norminf_vector(double *v, int n);



int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
{
28
    int iter = 0;
29 30
    int err = 0;

31
    // if failed closure animation are activated, backup q vector
32
    if (mbs_aux->close_anim) {
33
        copy_dvec_1(s->q, mbs_aux->q_save);
34 35
    }

36
    iter = 0;
37 38
    mbs_aux->norm_h = 1.0;
    while ((mbs_aux->norm_h > mbs_aux->NRerr) && (iter++ <= mbs_aux->MAX_NR_ITER))
39 40
    {
        err = mbs_step_close_geo(s, mbs_aux);
41
        if (err < 0) {
42
            mbs_msg("\t \t >CLOSE GEO> Error in mbs_close_geo during a NR iteration \n");
43
            return -50 + err;
44 45 46 47 48 49
        }
    }

    if (iter >= mbs_aux->MAX_NR_ITER) // Error management
    {
        mbs_msg("\t \t >CLOSE GEO> Impossible to close the geometry after %d iterations \n", iter);
50 51 52
        if (mbs_aux->close_anim) {
            mbs_anim_close_geo(s, mbs_aux);
        }
53
        mbs_anim_close_geo(s, mbs_aux);
54 55 56 57 58 59
        return -50;
    }

    return iter;
}

60 61 62
int mbs_step_close_geo(MbsData *s, MbsAux *mbs_aux) {
    int i, j;
    int nL, nC;
63 64 65
    int err = 0;
    double d;

66
    // Computation of the constraints (h) and the Jacobian (Jac)
67
    mbs_calc_hJ(s, mbs_aux);
68

69
    // Maximal value of the independant constraint
Olivier Lantsoght's avatar
Olivier Lantsoght committed
70
    mbs_aux->norm_h = norminf_vector(mbs_aux->mJv_h, s->nhu);
71 72 73 74

    // -Jv
    nL = s->nhu;
    nC = s->nqv;
75
    for (i = 1; i <= nL; i++)
76
    {
77
        for (j = 1; j <= nC; j++)
78 79 80 81 82 83
        {
            mbs_aux->mJv[i][j] = -mbs_aux->Jac[s->hu[i]][s->qv[j]];
        }
    }

    // Decomposition LU de la matrice -Jv
84
    err = ludcmp(mbs_aux->mJv, s->nqv, mbs_aux->ind_mJv, &d);
85 86 87 88 89 90 91

    if (err < 0) // Error management
    {
        mbs_msg("\t \t >CLOSE GEO> Error in mbs_step_close_geo during LU decomposition of Jv matrix \n");
        return err;
    }

92
    if (mbs_aux->norm_h > mbs_aux->NRerr)
93
    {
94
        lubksb(mbs_aux->mJv, s->nqv, mbs_aux->ind_mJv, mbs_aux->mJv_h);
95 96

        // Correction des qv
97
        for (i = 1; i <= s->nhu; i++)
98 99 100 101 102 103 104
        {
            s->q[s->qv[i]] += mbs_aux->mJv_h[i];
        }
    }
    return 0;
}

105
int mbs_anim_close_geo(MbsData *mbs_data, MbsAux *mbs_aux) {
106 107
    mbs_msg("\t \t           > Attempting to generate animation of the failed loop closing procedure.\n");
    // Restoring qv
108
    copy_dvec_1(mbs_aux->q_save, mbs_data->q);
109 110 111 112 113 114
    // Creating the animation with the solvekin module
    MbsSolvekin *mbs_solvekin;
    mbs_solvekin = mbs_new_solvekin(mbs_data);
    mbs_solvekin->options->motion = closeloop;
    mbs_solvekin->options->framerate = 1;
    mbs_solvekin->options->resfilename = "Failed_loop_closing_procedure";
115 116
    mbs_solvekin->mbs_aux->MAX_NR_ITER = mbs_aux->MAX_NR_ITER; // Retrieve max iteration from previous module
    // Run the solvekin module
117 118 119
    mbs_run_solvekin(mbs_solvekin, mbs_data);
    mbs_delete_solvekin(mbs_solvekin, mbs_data);
    mbs_msg("\t \t           > Loop closure procedure animated in file \"Failed_loop_closing_procedure_q.anim\".\n");
120
    return 1;
121 122
}

123 124
void mbs_close_kin(MbsData *s, MbsAux *mbs_aux)
{
125 126
    int i, j, k;
    int nL, nC, nk;
127 128 129 130 131

    double term;

    nL = s->nhu;
    nC = s->nqu + s->nqc;
132
    for (i = 1; i <= nL; i++)
133
    {
134
        for (j = 1; j <= nC; j++)
135 136
        {
            mbs_aux->Juct[j][i] = mbs_aux->Jac[s->hu[i]][mbs_aux->iquc[j]];
137
            //          mbs_aux->Juc[i][j] = mbs_aux->Jac[s->hu[i]][mbs_aux->iquc[j]];
138 139 140 141
        }
    }

    // calcul de la matrice de couplage des vitesses
142
    for (j = 1; j <= nC; j++)
143
    {
144
        lubksb(mbs_aux->mJv, s->nqv, mbs_aux->ind_mJv, mbs_aux->Juct[j]);
145
    }
146 147
    //  gaussj(mbs_aux->mJv, s->nqv, mbs_aux->Juc, nC);
    for (i = 1; i <= nL; i++)
148
    {
149
        for (j = 1; j <= nC; j++)
150 151
        {
            mbs_aux->Bvuc[i][j] = mbs_aux->Juct[j][i];
152
            //          mbs_aux->Bvuc[i][j] = mbs_aux->Juc[i][j];
153 154 155 156 157 158
        }
    }

    // calcul des vitesses dependantes (qdv = Bvuc * qduc)
    nL = s->nqv;
    nk = s->nqu + s->nqc;
159
    for (i = 1; i <= nL; i++)
160 161
    {
        term = 0.0;
162
        for (k = 1; k <= nk; k++)
163
        {
164
            term += mbs_aux->Bvuc[i][k] * s->qd[mbs_aux->iquc[k]];
165 166 167 168 169 170 171 172
        }
        s->qd[s->qv[i]] = term;
    }

}

void mbs_calc_hJ(MbsData *s, MbsAux *mbs_aux)
{
173
    int i, j;
174 175 176


    // contraintes de fermeture de boucles
177 178
    if (s->Nloopc > 0) {
        mbs_cons_hJ(mbs_aux->h, mbs_aux->Jac, s, s->tsim);
Olivier Lantsoght's avatar
Olivier Lantsoght committed
179
    }
180
    // contraintes user
181
    if (s->Nuserc > 0) {
182 183 184
        user_cons_hJ(mbs_aux->huserc, mbs_aux->Juserc, s, s->tsim);

        // ajout des contraintes user aux contraintes de fermeture
185 186 187 188
        for (i = 1; i <= s->Nuserc; i++) {
            mbs_aux->h[s->Nloopc + i] = mbs_aux->huserc[i];
            for (j = 1; j <= s->njoint; j++)
                mbs_aux->Jac[s->Nloopc + i][j] = mbs_aux->Juserc[i][j];
189 190
        }
    }
Olivier Lantsoght's avatar
Olivier Lantsoght committed
191
    // Retrieving independant constraint value
192
    slct_dvec_1(mbs_aux->h, s->hu, s->nhu, mbs_aux->mJv_h);
193 194 195 196
}

void mbs_calc_jdqd(MbsData *s, MbsAux *mbs_aux)
{
197
    int i;
198

199 200 201 202 203 204 205 206
    // compute contribution of symbolic constraints to jdqd
    if (s->Nloopc > 0) {
        mbs_cons_jdqd(mbs_aux->jdqd, s, s->tsim);
    }

    // compute contribution of user constraints to jdqd
    if (s->Nuserc > 0) {
        user_cons_jdqd(mbs_aux->jdqduserc, s, s->tsim);
207

208 209 210 211 212
        // ajout des contraintes user aux contraintes de fermeture
        for (i = 1; i <= s->Nuserc; i++) {
            mbs_aux->jdqd[s->Nloopc + i] = mbs_aux->jdqduserc[i];
        }
    }
213

214 215 216 217
    // bp = (-Jv)\jdqd
    for (i = 1; i <= s->nhu; i++)
    {
        mbs_aux->bp[i] = mbs_aux->jdqd[s->hu[i]];
218
    }
219
    lubksb(mbs_aux->mJv, s->nqv, mbs_aux->ind_mJv, mbs_aux->bp);
220 221

}