Commit ba27989b authored by Sebastien Timmermans's avatar Sebastien Timmermans 🎹

[New] add error mngmnt in dirdyn, to be tested

parent 76e3f901
Pipeline #4338 passed with stage
in 9 minutes
......@@ -591,7 +591,7 @@ void mbs_dirdyn_write_buffers(MbsDirdyn* dd){
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
int mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn *dd)
void mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn *dd)
{
int i;
......@@ -619,21 +619,39 @@ int mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn
if(i<0){ // temporary solution
if (i == -1)
if (i >= dd->mbs_aux->MAX_NR_ITER)
{
mbs_error_msg("Loop closing Error: Newton-Raphson iteration overrun at time %g s !\n", tsim);
mbs_msg(">>DIRDYN>> ***** mbs_run_dirdyn : impossible to close the MBS : *****\n");
mbs_msg(">>DIRDYN>> after %d iteration of Newton Raphson \n", i);
mbs_msg(">>DIRDYN>> Loop closing Error: Newton-Raphson iteration overrun during direct dynamics at time %g s !\n", tsim);
mbs_msg(">>DIRDYN>>\n");
mbs_error_msg("[%d] in mbs_fct_dirdyn !! \n", 400 + i);
}
else if (i == -2)
else if (i == -22)
{
mbs_msg(">>DIRDYN>>\n");
mbs_msg(">>DIRDYN>> Error during direct dynamics at time %g s !\n", tsim);
mbs_error_msg("[%d] in mbs_fct_dirdyn !! \n", -400 + i);
}
else if (i == -55)
{
mbs_error_msg("Mass matrix Error: The reduced mass matrix has a line of zeros at time %g s !\n", tsim);
mbs_msg(">>DIRDYN>> Mass matrix Error: The reduced mass matrix has a line of zeros at time %g s !\n", tsim);
mbs_error_msg("[%d] in mbs_fct_dirdyn !! \n", -400 + i);
}
else if (i == -3)
else if (i == -33)
{
mbs_msg(">>DIRDYN>> Acceleration computation: The cholesky decomposition has failed at time %g s !\n", tsim);
mbs_error_msg("[%d] in mbs_fct_dirdyn !! \n", -400 + i);
}
else if (i == -2)
{
mbs_error_msg("Acceleration computation: The cholesky decomposition has failed at time %g s !\n", tsim);
mbs_error_msg(">>DIRDYN>> Error during Lambda computation at time %g s !\n", tsim);
mbs_error_msg("[%d] in mbs_fct_dirdyn !! \n", -400 + i);
}
else if (i == -4)
else if (i == -7)
{
mbs_error_msg("Direct dynamic computation: At least one acceleration is Nan at time %g s !\n", tsim);
mbs_msg(">>DIRDYN>> Direct dynamic computation: At least one acceleration is Nan at time %g s !\n", tsim);
mbs_error_msg("[%d] in mbs_fct_dirdyn !! \n", -470 + i);
}
}
// User Derivatives
......@@ -647,7 +665,8 @@ int mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn
// Check content of state vector
if (isnan(s->qd[s->qu[i]])) {
mbs_error_msg(">>DIRDYN>> qd[%d] is Nan at time %g s ! \n", s->qu[i], tsim );
mbs_msg(">>DIRDYN>> qd[%d] is Nan at time %g s ! \n", s->qu[i], tsim );
mbs_error_msg("[%d] in mbs_fct_dirdyn !! \n", -470 + i);
}
}
......@@ -657,7 +676,9 @@ int mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn
// Check content of state vector
if (isnan(s->uxd[i])){
mbs_error_msg("User derivative uxd[%d] is Nan\n",i);
mbs_msg(">>DIRDYN>> User derivative uxd[%d] is Nan\n",i);
mbs_msg(">>DIRDYN>> Error in state vector at time %g s ! \n", tsim);
mbs_error_msg("[%d] in mbs_fct_dirdyn !! \n", -407);
}
}
}
......
......@@ -135,13 +135,13 @@ void mbs_dirdyn_write_buffers(MbsDirdyn* dd);
* \param[in,out] s Robotran main structure
* \param[in,out] dd direct dynamic main module structure
*/
int mbs_fct_dirdyn(double t, double y[], double dydt[], MbsData *s, MbsDirdyn *dd);
void mbs_fct_dirdyn(double t, double y[], double dydt[], MbsData *s, MbsDirdyn *dd);
/*! \brief update the real-time and saving modules if requested
*
* \param[in] dd direct dynamics module
* \param[in] mbs_data Robotran main structure
*/
void save_realtime_update(MbsDirdyn* dd, MbsData* mbs_data);
#endif
......@@ -46,7 +46,13 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
if (iter >= mbs_aux->MAX_NR_ITER)
{
return -1;
mbs_msg("\t >DIRDYNARED> Impossible to close the goemetry after %d iterations \n", iter);
return iter;
}
else if (iter <0) // error during LU decomposition
{
mbs_msg("\t >DIRDYNARED> Error during LU decomposition \n");
return -22;
}
// Kinematic resolution
......@@ -256,12 +262,12 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
}
if (has_a_line_of_zeros)
{
mbs_msg("The line %d of the reduced mass matrix, associated to q(%d), is full of zeros\n", i, mbs_aux->iquc[i]);
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("mbs_aux->Mr[%d][%d] = %e;\n", i, k, mbs_aux->Mr[i][k]);
mbs_msg("\t >DIRDYNARED> mbs_aux->Mr[%d][%d] = %e;\n", i, k, mbs_aux->Mr[i][k]);
}
return -2;// temporary solution //mbs_error_msg("The reduced mass matrix has a line of zeros\n");
return -55;
}
term = 0.0;
......@@ -273,8 +279,12 @@ 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) { return err;} // temporary
err = choldc(mbs_aux->Mr, s->nqu, mbs_aux->p_Mr);
if (err < 0) {
mbs_msg("\t >DIRDYNARED> Matrix Mr is not positive definite \n");
return (err-30);
}
cholsl(mbs_aux->Mr, s->nqu, mbs_aux->p_Mr, mbs_aux->Fr, s->udd);
nL = s->nqu;
......@@ -330,15 +340,21 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
}
// compute lambda
ludcmp(mbs_aux->Jvt, s->nqv, mbs_aux->ind_Jvt, &d);
err = ludcmp(mbs_aux->Jvt, s->nqv, mbs_aux->ind_Jvt, &d);
if (err < 0)
{
mbs_msg("\t >DIRDYNARED> Error in dirdynared during LU decomposition of Jv matrix \n");
return (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(">>DIRDYN>> qdd[%d] is Not a number (Nan) \n", s->qu[i]);
return -4;
mbs_msg("\t >DIRDYNARED> qdd[%d] is Not a number (Nan) \n", s->qu[i]);
return -7;
}
}
......
......@@ -508,12 +508,13 @@ void mbs_run_part(MbsPart* mbs_part, MbsData* mbs_data)
mbs_msg(">>PART>> -> ... but have a look at the proposed {u,v} partition\n");
mbs_msg(">>PART>> Loop closing Error: Newton-Raphson iteration overrun during partitioning !\n");
mbs_msg(">>PART>>\n");
mbs_error_msg(" [%d] in mbs_run_part !! \n", 100 + nbiter); }
mbs_error_msg("[%d] in mbs_run_part !! \n", 100 + nbiter);
}
else if (nbiter < 0)
{
mbs_msg(">>PART>>\n");
mbs_msg(">>PART>> Error during mbs_part \n");
mbs_error_msg(" [%d] in mbs_run_part !! \n", -100 + nbiter);
mbs_error_msg("[%d] in mbs_run_part !! \n", -100 + nbiter);
}
else
{
......
......@@ -33,7 +33,8 @@ int choldc(double **a, int n, double p[])
if (i == j) {
if (sum <= 0.0)
{
return -3; // temporary
mbs_msg("\t >CHOL DECOMP> Error in Cholesky decomposition \n");
return -3;
}
p[i]=sqrt(sum);
} else a[j][i]=sum/p[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