dopri5 full options

MBprojects/PendulumSpringC/workR/src/main.c

@@ -69,11 +69,16 @@ int main(int argc, char const *argv[])
     mbs_dirdyn->options->save2file = 1;
     mbs_dirdyn->options->respath = PROJECT_SOURCE_DIR"/../resultsR";
 
-    mbs_dirdyn->options->dopri5->flag = 1;
+    mbs_dirdyn->options->dopri5->flag_use = 1;
+
     mbs_dirdyn->options->dopri5->rtoler = 1.0e-14;
     mbs_dirdyn->options->dopri5->atoler = 1.0e-14;
     mbs_dirdyn->options->dopri5->dt_max = 0.001;
     mbs_dirdyn->options->dopri5->verbose = 1;
+
+    mbs_dirdyn->options->dopri5->flag_waypoint = 1;
+    mbs_dirdyn->options->dopri5->flag_solout_wp = 1;
+    mbs_dirdyn->options->dopri5->delta_t_wp = 1.0e-3;
     
     mbs_dirdyn->options->realtime = 1;
 

MBsysC/mbs_common/mbs_module/mbs_dirdyn.c

@@ -64,12 +64,15 @@ MbsDirdyn* mbs_new_dirdyn_aux(MbsData* mbs_data, MbsAux* mbs_aux){
     
     // adaptive time step integrator
     dopr_opt = (DirdynOptDopri5*) malloc(sizeof(DirdynOptDopri5));
-    dopr_opt->flag = 0;
+    dopr_opt->flag_use = 0;
+    dopr_opt->flag_waypoint = 0;
+    dopr_opt->flag_solout_wp = 0;
     dopr_opt->verbose = 1;
     dopr_opt->rtoler = 1.0e-3;
     dopr_opt->atoler = 1.0e-3;
     dopr_opt->dt_max = 1.0e-3;
     dopr_opt->nmax = 1.0e9;
+    dopr_opt->delta_t_wp = 1.0e-3;
 
     opts->dopri5 = dopr_opt;
     dirdyn->options = opts;
@@ -269,16 +272,19 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data){
 
     FILE *animFile;
     FILE *fileout_dopri5;
+    DirdynOptDopri5 *dopr_opt;
     int i;
     double **dopri5_alloc_tab;
-    double cur_t0, cur_tf, delta_dopri5_dt;
+    double cur_t0, cur_tf;
 
     // NUMERICAL INTEGRATION
     // - - - - - - - - - - -
 
-    if (dd->options->dopri5->flag)
+    dopr_opt = dd->options->dopri5;
+
+    if (dopr_opt->flag_use)
     {
-        if (dd->options->dopri5->verbose)
+        if (dopr_opt->verbose)
         {
             fileout_dopri5 = stdout;
         }
@@ -295,17 +301,23 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data){
             dopri5_alloc_tab[i] = (double*) malloc (dd->nState*sizeof(double));
         }
 
-        delta_dopri5_dt = 0.001;
-
         cur_t0 = dd->options->t0;
-        cur_tf = cur_t0 + delta_dopri5_dt;
+
+        if (dopr_opt->flag_waypoint)
+        {
+            cur_tf = cur_t0 + dopr_opt->delta_t_wp;
+        }
+        else
+        {
+            cur_tf = dd->options->tf;
+        }
 
         while (cur_tf <= dd->options->tf)
         {
             loop_dopri5(cur_t0, cur_tf, dd->options->dt0, dopri5_alloc_tab, mbs_data, dd, fileout_dopri5);
 
             cur_t0 = cur_tf;
-            cur_tf += delta_dopri5_dt;
+            cur_tf += dopr_opt->delta_t_wp;
 
             // stop the simulation if 'flag_stop' set to 1
             if (mbs_data->flag_stop)
@@ -399,8 +411,6 @@ void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data){
     }
 }
 
-
-
 /* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
 
 /*! \brief direct dynamics derivative computation
@@ -569,11 +579,19 @@ void fcn_dopri5(unsigned n, long nr, double tsim, double y[], double dydt[], Mbs
  * \param[in] y state vector of size n
  * \param[in] n dimension of the system [-]
  * \param[out] irtrn set a negative value to stop the simulation
+ * \param[in] init_flag 1 if initial call to solout, 0 otherwise
  * \param[in,out] s Robotran main structure
  * \param[in,out] dd direct dynamic main module structure
  */
-void solout_dopri5(long nr, double tsim_old, double tsim, double y[], unsigned n, int* irtrn, MbsData *s, MbsDirdyn *dd)
+void solout_dopri5(long nr, double tsim_old, double tsim, double y[], unsigned n, int* irtrn, int init_flag, MbsData *s, MbsDirdyn *dd)
 {
+    // when using waypoints, only waypoints are used if asked
+    if ((!init_flag) && dd->options->dopri5->flag_waypoint && dd->options->dopri5->flag_solout_wp)
+    {
+        return;
+    }
+
+    // only call new values (used for waypoints method)
     if (tsim > dd->dopri5->solout_last_t)
     {
         dd->dopri5->solout_last_t = tsim;
@@ -586,5 +604,5 @@ void solout_dopri5(long nr, double tsim_old, double tsim, double y[], unsigned n
 
         // quit the simulation if requested
         *irtrn = (s->flag_stop) ? -1 : 1;
-    }  
+    }
 }

MBsysC/mbs_common/mbs_module/mbs_dirdyn.h

@@ -24,13 +24,17 @@
   */
 typedef struct DirdynOptDopri5
 {
-    int flag;    ///< 1 to use dopri5 (adaptive time step), 0 to use runge kutta 4 (fixed time step)
+    int flag_use; ///< 1 to use dopri5 (adaptive time step), 0 to use runge kutta 4 (fixed time step)
+    int flag_waypoint; ///< 1 to use waypoints, 0 otherwise
+    int flag_solout_wp; ///< 1 to call solout only at required waypoints (only if flag_waypoint activated)
+
     int nmax;    ///< maximal number of stpes [-]
     int verbose; ///< 1 to get print indications related to time adaptive integrator, 0 otherwise
-
+     
     double rtoler; ///< relative error tolerances [-]
     double atoler; ///< absolute error tolerances [-]
     double dt_max; ///< maximal time step [s]
+    double delta_t_wp; ///< time interval between two waypoints [s]
 
 } DirdynOptDopri5;
 
@@ -211,6 +215,6 @@ void mbs_fct_dirdyn(double t, double y[], double dydt[], MbsData *s, MbsDirdyn *
 void save_realtime_update(MbsDirdyn* dd, MbsData* mbs_data);
 double loop_dopri5(double cur_t0, double cur_tf, double dt0, double **dopri5_alloc_tab, MbsData *s, MbsDirdyn *dd, FILE *fileout_dopri5);
 void fcn_dopri5(unsigned n, long nr, double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn *dd);
-void solout_dopri5(long nr, double tsim_old, double tsim, double y[], unsigned n, int* irtrn, MbsData *s, MbsDirdyn *dd);
+void solout_dopri5(long nr, double tsim_old, double tsim, double y[], unsigned n, int* irtrn, int init_flag, MbsData *s, MbsDirdyn *dd);
 
 #endif

MBsysC/mbs_common/mbs_numerics/dopri5.c

@@ -224,7 +224,7 @@ static int dopcor (unsigned n, FcnEqDiff fcn, double x, double* y, double xend,
         irtrn = 1;
         hout = h;
         xout = x;
-        solout (naccpt+1, xold, x, y, n, &irtrn, s, dd);
+        solout (naccpt+1, xold, x, y, n, &irtrn, 1, s, dd); // modif: 1, s, dd added
         if (irtrn < 0)
         {
             if (fileout)
@@ -413,7 +413,7 @@ static int dopcor (unsigned n, FcnEqDiff fcn, double x, double* y, double xend,
             {
         hout = h;
         xout = x;
-        solout (naccpt+1, xold, x, y, n, &irtrn, s, dd);
+        solout (naccpt+1, xold, x, y, n, &irtrn, 0, s, dd); // modif: 0, s, dd added
         if (irtrn < 0)
         {
             if (fileout)

MBsysC/mbs_common/mbs_numerics/dopri5.h

@@ -183,8 +183,8 @@ nfcnRead    Number of function calls.
 // modif: long nr, MbsData *s, MbsDirdyn *dd added
 typedef void (*FcnEqDiff)(unsigned n, long nr, double x, double *y, double *f, MbsData *s, MbsDirdyn *dd);
 
-// modif, MbsData *s, MbsDirdyn *dd added
-typedef void (*SolTrait)(long nr, double xold, double x, double* y, unsigned n, int* irtrn, MbsData *s, MbsDirdyn *dd);
+// modif, MbsData *s, MbsDirdyn *dd, int init_flag added
+typedef void (*SolTrait)(long nr, double xold, double x, double* y, unsigned n, int* irtrn, int init_flag, MbsData *s, MbsDirdyn *dd);
 
 
 extern int dopri5