Commit d4d4dc8e authored by Nicolas Van der Noot's avatar Nicolas Van der Noot
Browse files

dopri5 full options

parent 7489f460
......@@ -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;
......
......@@ -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;
}
}
}
......@@ -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
......@@ -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)
......
......@@ -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
......
Supports Markdown
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