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

MBsysC/mbs_common/mbs_module/mbs_dirdyn.c

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

MBsysC/mbs_common/mbs_module/mbs_dirdyn.h

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

MBsysC/mbs_common/mbs_module/mbs_dirdynared.c

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

MBsysC/mbs_common/mbs_module/mbs_part.c

@@ -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
         {

MBsysC/mbs_common/mbs_numerics/choldc.c

@@ -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];