Authentication method changed. UCLouvain users, please use the "UCLouvain SSO" button to connect on the website and use ssh + ssh key (https://git.immc.ucl.ac.be/-/profile/keys) or https + personnal access token (https://git.immc.ucl.ac.be/-/profile/personal_access_tokens) in your git clients.

Commit 53070ec4 authored by Your Name's avatar Your Name
Browse files

changes in error management in dirdyn and solvekin

parent b67d4318
......@@ -53,10 +53,10 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
// Decomposition LU de la matrice -Jv
err = ludcmp(mbs_aux->mJv,s->nqv,mbs_aux->ind_mJv,&d);
if (err < 0)
if (err < 0) // Error management
{
mbs_msg("\t >CLOSE GEO> Error in mbs_close_geo during LU decomposition of Jv matrix \n");
return err;
mbs_msg("\t \t >CLOSE GEO> Error in mbs_close_geo during LU decomposition of Jv matrix \n");
return -50+err;
}
if(mbs_aux->norm_h > mbs_aux->NRerr)
......@@ -78,6 +78,14 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
}
}
}
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);
return -50;
}
return iter;
}
......
......@@ -574,7 +574,7 @@ void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data){
{
mbs_msg(">>DIRDYN>> ***** mbs_dirdyn_write_buffers : impossible to save the files *****\n");
mbs_msg(">>DIRDYN>>\n");
mbs_error_msg("[%d] in mbs_dirdyn_finish !! \n", -500 + err);
mbs_error_msg("[%d] in mbs_dirdyn_finish !! \n", -400 + err);
}
......@@ -632,6 +632,7 @@ int mbs_dirdyn_write_buffers(MbsDirdyn* dd){
void mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn *dd)
{
int err = 0;
int i;
s->tsim = tsim;
......@@ -649,51 +650,44 @@ void mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsDirdy
if (dd->options->accelred) // Full symbolic computation of qdd (including solving constraint, inv(M), ...)
{
i = mbs_accelred(s, tsim);
err = mbs_accelred(s, tsim);
}
else // Numerical solving of constraints and inv(M)
{
i = dirdynared(dd->mbs_aux,s);
err = dirdynared(dd->mbs_aux,s);
}
if (i < 0) // Error management
if (err < 0) // Error management
{
mbs_msg(">>DIRDYN>>\n");
mbs_msg(">>DIRDYN>> Error during direct dynamics at time %g s !\n", tsim);
save_realtime_update(dd, s);
mbs_dirdyn_finish(dd, s);
mbs_error_msg("[%d] in mbs_fct_dirdyn !! \n", -400 + i);
mbs_error_msg("[%d] in mbs_fct_dirdyn !! \n", -400 + err);
}
// User Derivatives
if(s->Nux>0) user_Derivative(s);
// Update state vector
for(i=1;i<=s->nqu;i++)
for (i = 1; i <= s->nqu; i++)
{
dydt[i-1] = s->qd [s->qu[i]];
dydt[i+s->nqu-1] = s->qdd[s->qu[i]];
dydt[i - 1] = s->qd[s->qu[i]];
dydt[i + s->nqu - 1] = s->qdd[s->qu[i]];
// Check content of state vector
if (isnan(s->qd[s->qu[i]])) {
mbs_msg(">>DIRDYN>> qd[%d] is Nan at time %g s ! \n", s->qu[i], tsim );
save_realtime_update(dd, s);
mbs_dirdyn_finish(dd, s);
mbs_error_msg("[%d] in mbs_fct_dirdyn !! \n", -470 + i);
}
}
// User Derivatives
if(s->Nux>0) user_Derivative(s);
for(i=1;i<=s->Nux;i++)
{
dydt[i+2*s->nqu-1] = s->uxd[i];
// Check content of state vector
if (isnan(s->uxd[i])){
mbs_msg(">>DIRDYN>> User derivative uxd[%d] is Nan\n",i);
if (isnan(s->uxd[i])) // Error management
{
mbs_msg(">>DIRDYN>> User derivative uxd[%d] is Nan\n", i);
mbs_msg(">>DIRDYN>> Error in state vector at time %g s ! \n", tsim);
save_realtime_update(dd, s);
mbs_dirdyn_finish(dd, s);
mbs_error_msg("[%d] in mbs_fct_dirdyn !! \n", -407);
mbs_error_msg("[%d] in mbs_fct_dirdyn !! \n", -447);
}
dydt[i+2*s->nqu-1] = s->uxd[i];
}
}
......
......@@ -26,10 +26,9 @@
int dirdynared(MbsAux *mbs_aux,MbsData *s)
{
int i, j, k;
int iter = 0;
int nL, nC, nk;
int err;
int err = 0;
double term;
......@@ -42,19 +41,13 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
if (s->Ncons > 0)
{
// Geometric resolution
iter = mbs_close_geo(s, mbs_aux);
if (iter >= mbs_aux->MAX_NR_ITER)
{
mbs_msg("\t >DIRDYNARED> Impossible to close the geometry after %d iterations \n", iter);
return -50;
}
else if (iter <0) // error during LU decomposition
err = mbs_close_geo(s, mbs_aux);
if (err<0) // Error management
{
mbs_msg("\t >DIRDYNARED> Error during LU decomposition \n");
return -22;
mbs_msg("\t \t in >DIRDYNARED> \n");
return err;
}
// Kinematic resolution
mbs_close_kin(s, mbs_aux);
......@@ -63,12 +56,9 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
}//if(s->Ncons > 0)
// compute forces on the system
mbs_calc_force(s);
// calcul de la matrice de masse et du vecteur des forces non lineaires
mbs_dirdyna(mbs_aux->M, mbs_aux->c, s, s->tsim);
......@@ -260,14 +250,14 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
if (mbs_aux->Mr[i][j] > 1e-16)
has_a_line_of_zeros = 0;
}
if (has_a_line_of_zeros)
if (has_a_line_of_zeros) // Error management
{
mbs_msg("\t >DIRDYNARED> The line %d of the reduced mass matrix, associated to q(%d), is full of zeros\n", i, mbs_aux->iquc[i]);
for (k = 1; k <= nC; k++)
{
mbs_msg("\t >DIRDYNARED> mbs_aux->Mr[%d][%d] = %e;\n", i, k, mbs_aux->Mr[i][k]);
}
return -55;
return -70;
}
term = 0.0;
......@@ -280,19 +270,27 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
// calcul des accelerations reduites : 'resolution' du systeme ODE = Mr*udd = Fr;
err = choldc(mbs_aux->Mr, s->nqu, mbs_aux->p_Mr);
if (err < 0) {
if (err < 0) // Error management
{
mbs_msg("\t >DIRDYNARED> Matrix Mr is not positive definite \n");
return (err-30);
return (-70+err);
}
cholsl(mbs_aux->Mr, s->nqu, mbs_aux->p_Mr, mbs_aux->Fr, s->udd);
nL = s->nqu;
for (i = 1; i <= nL; i++)
{
s->qdd[s->qu[i]] = s->udd[i];
if (isnan(s->qdd[s->qu[i]])) // Error management
{
mbs_msg("\t >DIRDYNARED> qddu[%d] is Nan \n", i);
return -77;
}
}
if (s->nqv > 0)
{
double d;
......@@ -341,23 +339,14 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
// compute lambda
err = ludcmp(mbs_aux->Jvt, s->nqv, mbs_aux->ind_Jvt, &d);
if (err < 0)
if (err < 0) // Error management
{
mbs_msg("\t >DIRDYNARED> Error in dirdynared during LU decomposition of Jv matrix \n");
return (err);
mbs_msg("\t >DIRDYNARED> Error in dirdynared during LU decomposition of Jv matrix for lambda computation\n");
return (-70+err);
}
lubksb(mbs_aux->Jvt, s->nqv, mbs_aux->ind_Jvt, s->lambda);
}
for (i = 1; i <= s->nqu; i++)
{
if (isnan(s->qdd[s->qu[i]])) {
mbs_msg("\t >DIRDYNARED> qdd[%d] is Not a number (Nan) \n", s->qu[i]);
return -7;
}
}
// Qc = Mruc_cu * udd + Mruc_cc * qddc + Fruc_c
if (s->nqc > 0)
{
......@@ -385,6 +374,6 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
}
}
return iter;
return err;
}
#endif
......@@ -203,19 +203,19 @@ void mbs_equil_init(MbsEquil* eq, MbsData* s)
// 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_msg(">>EQUIL>> mbs_equil_init : no independent variable - irrelevant process !\n");
exit(EXIT_FAILURE);
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>> mbs_equil_init : please perform a partitioning at first !\n");
exit(EXIT_FAILURE);
mbs_msg(">>EQUIL>> The coordinate Partitioning has not be performed for the system !\n");
mbs_error_msg(">>EQUIL>> [%d] in mbs_equil_init \n", -200);
}
// check on the equil mode
if(eq->options->mode != 1 && eq->options->mode != 2 && eq->options->mode != 3)
{
mbs_error_msg(">>EQUIL>> mode %d does not exist check documentation for equil->options->mode \n", eq->options->mode);
mbs_msg(">>EQUIL>> mode %d does not exist check documentation for equil->options->mode \n", eq->options->mode);
mbs_error_msg(">>EQUIL>> [%d] in mbs_equil_init \n",-200);
}
// 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)
......@@ -275,6 +275,13 @@ void mbs_equil_init(MbsEquil* eq, MbsData* s)
// 3. Memory allocation for equilibrium variables
eq->nx = s->nqu + s->Nux + eq->options->nxe;
if (eq->nx == 0) // Error management
{
mbs_msg(">>EQUIL>> There are no equilibrium variables: nqu=%d , Nux=%d, nxe=%d - irrelevant process\n", s->nqu, s->Nux, eq->options->nxe);
mbs_error_msg(">>EQUIL>> [%d] in mbs_equil_init \n", -200);
}
eq->x = get_dvec_0(eq->nx);
eq->xs = get_ivec_0(eq->nx); // oversized
eq->xns = get_ivec_0(eq->nx); // oversized
......@@ -355,7 +362,7 @@ void mbs_equil_init(MbsEquil* eq, MbsData* s)
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);
eq->lpk = mbs_new_lpk(s, eq->nx, MAX(MAX(s->nqu, s->Nux), eq->nxe));
eq->lpk = mbs_new_lpk(s, eq->nx, MAX(MAX(s->nqu, s->Nux),eq->nxe));
// 6. Equilibrium saving to file (anim, results)
if (eq->options->save2file)
......@@ -511,7 +518,7 @@ void mbs_equil_loop(MbsEquil* eq, MbsData* s)
void mbs_equil_finish(MbsEquil* eq, MbsData* s)
{
int err = 0;
if (eq->options->save2file)
{
......@@ -519,7 +526,13 @@ void mbs_equil_finish(MbsEquil* eq, MbsData* s)
// write the buffer
for (i = 0; i < eq->bufferNb; i++)
{
mbs_buffer_write(eq->buffers[i]);
err=mbs_buffer_write(eq->buffers[i]);
if (err < 0)
{
mbs_msg(">>EQUIL>>\n");
mbs_msg(">>EQUIL>> ***** mbs_buffer_write : impossible to save the files from buffers i=%d *****\n",i);
mbs_error_msg("[%d] in mbs_equil_finish !! \n", -200 + err);
}
}
// free memory used by buffers
for (i = 0; i < eq->bufferNb; i++) {
......@@ -528,7 +541,13 @@ void mbs_equil_finish(MbsEquil* eq, MbsData* s)
free(eq->buffers);
// user inputs final gestion
mbs_growing_buffer_write(eq->user_buffer);
err=mbs_growing_buffer_write(eq->user_buffer);
if (err < 0)
{
mbs_msg(">>EQUIL>>\n");
mbs_msg(">>EQUIL>> ***** mbs_growing_buffer_write : impossible to save the files *****\n");
mbs_error_msg("[%d] in mbs_equil_finish !! \n", -200 + err);
}
mbs_delete_growing_buffer(eq->user_buffer);
......@@ -629,7 +648,8 @@ int mbs_equil_fct(double x[], int nx, double fx[], MbsEquil *eq, MbsAux *aux, Mb
// 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
......@@ -642,13 +662,14 @@ int mbs_equil_fct(double x[], int nx, double fx[], MbsEquil *eq, MbsAux *aux, Mb
if(s->Nux>0) user_Derivative(s);
for(i=0; i<s->Nux;i++)
{
fx[ind_x] = s->uxd[i + 1];
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;
......@@ -690,7 +711,14 @@ int mbs_equil_fsolvepk(int(*fun_ptr)(double*, int, double*, MbsEquil*, MbsAux*,
// 1. Gradient test - search for sensitive variables
// 1.1 Gradient estimation for f(x) :
mbs_equil_grad(eq, s);
err = mbs_equil_grad(eq, s);
if (err<0) // Error management
{
mbs_msg(">>EQUIL>>\n");
mbs_msg(">>EQUIL>> Error at initial gradient computation \n");
mbs_error_msg(">>EQUIL>> [%d] in mbs_equil_fsolvepk !! \n", -200 + err);
}
// 1.2. Gradient testing (removal of non-sensitive variables AND redundant variables)
eq->nxs = 0;
......@@ -739,7 +767,8 @@ int mbs_equil_fsolvepk(int(*fun_ptr)(double*, int, double*, MbsEquil*, MbsAux*,
if (eq->nxs == 0)
{
mbs_error_msg(">>EQUIL>> mbs_equil_fsolvepk: none of the proposed parameter are sensitive !\n");
mbs_msg(">>EQUIL>> mbs_equil_fsolvepk: none of the proposed parameter are sensitive !\n");
mbs_error_msg(">>EQUIL>> [%d] in mbs_equil_fsolvepk \n", -212);
err = 0;
return err;
}
......@@ -757,7 +786,12 @@ int mbs_equil_fsolvepk(int(*fun_ptr)(double*, int, double*, MbsEquil*, MbsAux*,
// Initial values for Iterative procedure
err = (*fun_ptr)(eq->x, nx, f, eq, aux, s);
if (err<0) { mbs_error_msg("Loop closing Error: Newton-Raphson iteration overrun for equilibrium iteration %d !\n",eq->iter); }
if (err<0) // Error management
{
mbs_msg(">>EQUIL>>\n");
mbs_msg(">>EQUIL>> Error at initial function evaluatoin !\n");
mbs_error_msg(">>EQUIL>> [%d] in mbs_equil_fsolvepk !! \n", -200 + err);
}
user_equil_loop(s, eq);
eq->norm_pk = norm_dvec_0(f, nf);
// Save results of the initial step to the buffer
......@@ -768,7 +802,6 @@ int mbs_equil_fsolvepk(int(*fun_ptr)(double*, int, double*, MbsEquil*, MbsAux*,
mbs_equil_save(eq, s, eq->iter);
}
eq->iter = 0;
r_cou = 0;
normR = eq->norm_pk;
......@@ -793,7 +826,13 @@ int mbs_equil_fsolvepk(int(*fun_ptr)(double*, int, double*, MbsEquil*, MbsAux*,
{
eq->lpk->verbose = 0;
eq->options->verbose = eq->options->verbose - 1;
mbs_equil_grad(eq, s);
err=mbs_equil_grad(eq, s);
if (err<0) // Error management
{
mbs_msg(">>EQUIL>>\n");
mbs_msg(">>EQUIL>> Error during equilibrium process at iteration %d !\n", eq->iter);
mbs_error_msg("[%d] in mbs_equil_fsolvepk !! \n", -200 + err);
}
eq->options->verbose = eq->options->verbose + 1;
}
// Gradient Resizing (sensitive variables only) : [nfs * nx]
......@@ -809,8 +848,14 @@ int mbs_equil_fsolvepk(int(*fun_ptr)(double*, int, double*, MbsEquil*, MbsAux*,
slct_dvec_0(f, nf, eq->xs, eq->nxs, fs); // resizing f->fs
indx = get_ivec_0(eq->nxs); //
copy_dvec_0(fs, xds, eq->nxs); //
ludcmp_0(grads, eq->nxs, indx, &d); // those functions make the matrix division (inversion)
lubksb_0(grads, eq->nxs, indx, xds); //
err=ludcmp_0(grads, eq->nxs, indx, &d); // those functions make the matrix division (inversion)
if (err<0) // Error management
{
mbs_msg(">>EQUIL>>\n");
mbs_msg(">>EQUIL>> Error during equilibrium process at iteration %d !\n", eq->iter);
mbs_error_msg(">>EQUIL>> [%d] in mbs_equil_fsolvepk !! \n", -200 + err);
}
lubksb_0(grads, eq->nxs, indx, xds); //
free_ivec_0(indx);
for (i = 0; i < eq->nxs; i++)
{
......@@ -876,9 +921,8 @@ int mbs_equil_fsolvepk(int(*fun_ptr)(double*, int, double*, MbsEquil*, MbsAux*,
eq->x[eq->xs[i]] = eq->x[eq->xs[i]] - xd[eq->xs[i]]; // with now, xd=f(x_k)/J(x_k)
xs[i] = eq->x[eq->xs[i]];
}
err = (*fun_ptr)(eq->x, nx, f, eq, aux, s);
// THE ERROR SHOULD PROPAGATE AT LEAST TILL THE RELAXATION AS RELAXATION COULD CANCEL THIS LOOP CLOSING ERROR !
// if (err<0) { mbs_error_msg("Loop closing Error: Newton-Raphson iteration overrun for equilibrium iteration %d !\n", eq->iter); }
err = (*fun_ptr)(eq->x, nx, f, eq, aux, s); // Error management - the error should propagate at least till the relaxation (to give a chance to recover)
eq->norm_pk = norm_dvec_0(f, nf);
// 3.5 Relaxation (norm_pk is still used as the every-evaluation norm)
......@@ -898,7 +942,13 @@ int mbs_equil_fsolvepk(int(*fun_ptr)(double*, int, double*, MbsEquil*, MbsAux*,
eq->x[eq->xs[i]] = xsR[i] - xdsR[i];
}
err = (*fun_ptr)(eq->x, nx, f, eq, aux, s);
if (err < 0) { mbs_error_msg("Loop closing Error: Newton-Raphson iteration overrun for equilibrium iteration %d !\n", eq->iter); }
if (err<0) // Error management
{
mbs_msg(">>EQUIL>>\n");
mbs_msg(">>EQUIL>> Error during relaxation step %d for equilibrium iteration %d !\n", r_cou,eq->iter);
mbs_error_msg(">>EQUIL>> [%d] in mbs_equil_fsolvepk !! \n", -200 + err);
}
eq->norm_pk = norm_dvec_0(f, nf);
if (eq->options->verbose)
{
......@@ -935,6 +985,15 @@ int mbs_equil_fsolvepk(int(*fun_ptr)(double*, int, double*, MbsEquil*, MbsAux*,
mbs_equil_save(eq, s, eq->iter);
}
}
// the equil function error is not caught directly to allow the relaxation process to do something
if (err<0) // Error management
{
mbs_msg(">>EQUIL>>\n");
mbs_msg(">>EQUIL>> Equilibrium function error during equilibrium process at iteration %d !\n", eq->iter);
mbs_error_msg(">>EQUIL>> [%d] in mbs_equil_fsolvepk !! \n", -200 + err);
}
}
if (eq->options->verbose) // print the last iteration (i.e. the solution)
......@@ -944,13 +1003,13 @@ int mbs_equil_fsolvepk(int(*fun_ptr)(double*, int, double*, MbsEquil*, MbsAux*,
mbs_msg(">>EQUIL>> ---> F: "); print_dvec_0(f, eq->nx);
mbs_msg(">>EQUIL>> ---> Residue = %f\n", eq->norm_pk);
}
if (eq->iter >= eq->options->itermax)
{
mbs_error_msg(">>EQUIL>> mbs_equil_fsolvepk : Equilibrium has not converged after %d iterations\n", eq->options->itermax);
eq->success = -1;
err = -1;
return err;
if (eq->iter >= eq->options->itermax) // error management
{
err = -11;
mbs_msg(">>EQUIL>>\n");
mbs_msg(">>EQUIL>> Equilibrium process has not converged after %d iterations !\n", eq->options->itermax);
mbs_error_msg(">>EQUIL>> [%d] in mbs_equil_fsolvepk !! \n", -200 + err);
}
else
{
......@@ -980,7 +1039,6 @@ int mbs_equil_grad(MbsEquil *eq,MbsData *s)
int err = 0;
int i,j;
int EqChoice;
int lin_fail;
for (i = 0; i < eq->lpk->nx; i++) { eq->lpk->x_ptr[i] = eq->x_ptr[i]; }
if (0) // disactivated for visibility...
......@@ -993,7 +1051,12 @@ int mbs_equil_grad(MbsEquil *eq,MbsData *s)
if (0) { mbs_msg(">>EQUIL>> Computation of the tangent matrix, grad_Rr = delta Rr/delta x \n"); }
eq->lpk->nF = s->nqu;
EqChoice = 1;
lin_fail = mbs_linearipk(eq->grad_Rr, eq->lpk, eq->aux, s, EqChoice);
err = mbs_linearipk(eq->grad_Rr, eq->lpk, eq->aux, s, EqChoice);
if (err < 0) // Error management
{
mbs_msg("\t in EQUIL GRAD, computation of grad_Rr = delta Rr/delta x \n");
return err;
}
}
if (s->Nux > 0)
......@@ -1001,7 +1064,12 @@ int mbs_equil_grad(MbsEquil *eq,MbsData *s)
if (0) { mbs_msg(">>EQUIL>> Computation of the tangent matrix, grad_uxd \n"); }
eq->lpk->nF = s->Nux;
EqChoice = 2;
lin_fail = mbs_linearipk(eq->grad_uxd, eq->lpk, eq->aux, s, EqChoice);
err = mbs_linearipk(eq->grad_uxd, eq->lpk, eq->aux, s, EqChoice);
if (err < 0) // Error management
{
mbs_msg("\t in EQUIL GRAD, computation of grad_uxd = delta uxd/delta x \n");
return err;
}
}
if (eq->nxe > 0)
......@@ -1009,7 +1077,12 @@ int mbs_equil_grad(MbsEquil *eq,MbsData *s)
if (0) { mbs_msg(">>EQUIL>> Computation of the tangent matrix, grad_fxe \n"); }
eq->lpk->nF = eq->nxe;
EqChoice = 3;
lin_fail = mbs_linearipk(eq->grad_fxe, eq->lpk, eq->aux, s, EqChoice);
err = mbs_linearipk(eq->grad_fxe, eq->lpk, eq->aux, s, EqChoice);
if (err < 0) // Error management
{
mbs_msg("\t in EQUIL GRAD, computation of grad_fxe = delta fxe/delta x \n");
return err;
}
}
// assemble the 3 gradients into one matrix
......
......@@ -556,8 +556,9 @@ int mbs_solvekin_write_buffers(MbsSolvekin* sk){
int mbs_fct_solvekin(MbsData *s, MbsSolvekin *sk)
{
int err = 0;
int err1=0, err2=0, err3=0;
int i, k, index, iter, nL, nK;
int i, k, index, nL, nK;
double term;
// Allocating output vector for Look Up Table
// TODO : Storing and allocated in sk
......@@ -618,17 +619,11 @@ int mbs_fct_solvekin(MbsData *s, MbsSolvekin *sk)
// Solving constraints, Ncons checked in initialization
// Geometric resolution
iter = mbs_close_geo(s, sk->mbs_aux);
if (iter >= sk->mbs_aux->MAX_NR_ITER)
{
mbs_msg("\t >SOLVEKIN> Impossible to close the geometry after %d iterations \n", iter);
return -50;
}
else if (iter <0) // error during LU decomposition
err = mbs_close_geo(s, sk->mbs_aux);
if (err<0) // Error management
{
mbs_msg("\t >SOLVEKIN> Error during LU decomposition \n");
return -22;
mbs_msg("\t \t in >SOLVEKIN> \n");
return err;
}
// Kinematic resolution
......@@ -655,5 +650,5 @@ int mbs_fct_solvekin(MbsData *s, MbsSolvekin *sk)
free_dvec_1(lutqd);
free_dvec_1(lutqdd);
}
return iter;
return err;
}
......@@ -42,7 +42,9 @@ void ThetaSC(double y[], double dydx[], int n, double x, double h, double yout[]
}
// close loops and compute the dynamics
dirdynared(mbs_dd->mbs_aux, s);
dirdynared(mbs_dd->mbs_aux, s); // CHANGE THE CALL BY mbs_fct_dirdyn
// Second step : computing "Vfree" and final position
for (i=0;i<s->nqu;i++)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment