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

soulout safety

parent d86690be
......@@ -70,8 +70,8 @@ int main(int argc, char const *argv[])
mbs_dirdyn->options->respath = PROJECT_SOURCE_DIR"/../resultsR";
mbs_dirdyn->options->adapt_time_step = 1;
mbs_dirdyn->options->adapt_rtoler = 1.0e-3;//1.0e-14;
mbs_dirdyn->options->adapt_atoler = 1.0e-3;//1.0e-14;
mbs_dirdyn->options->adapt_rtoler = 1.0e-14;
mbs_dirdyn->options->adapt_atoler = 1.0e-14;
mbs_dirdyn->options->adapt_dt_max = 0.001;
mbs_dirdyn->options->adapt_verbose = 1;
......
......@@ -142,6 +142,7 @@ void mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data){
// adaptive time step
dd->adapt_dydt_save = (double*) malloc(dd->nState*sizeof(double));
dd->adapt_solout_last_t = mbs_data->t0 - mbs_data->dt0;
// real-time modules activated
#ifdef REAL_TIME
......@@ -291,7 +292,7 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data){
while (cur_tf <= dd->options->tf)
{
loop_dopri5(cur_t0, cur_tf, dopri5_alloc_tab, mbs_data, dd, fileout_dopri5);
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;
......@@ -327,7 +328,7 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data){
{
dd->y[i] = dd->yout[i];
}
// save data + real-time features
save_realtime_update(dd, mbs_data);
......@@ -388,6 +389,8 @@ void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data){
}
}
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/*! \brief direct dynamics derivative computation
......@@ -400,7 +403,7 @@ void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data){
*/
void mbs_fct_dirdyn(double tsim, double y[], double dydt[], MbsData *s, MbsDirdyn *dd)
{
int i;
int i;
s->tsim = tsim;
......@@ -487,19 +490,25 @@ void save_realtime_update(MbsDirdyn* dd, MbsData* mbs_data)
}
/*! \brief dopri5 loop (one call to dopri 5)
*
*
* \param[in] cur_t0 initial time [s]
* \param[in] cur_tf final time [s]
* \param[in] dt0 first time step [s]
* \param[in,out] dopri5_alloc_tab allocated vectors for dopri5
* \param[in,out] s Robotran main structure
* \param[in,out] dd direct dynamic main module structure
* \param[out] fileout_dopri5 fileout for dopri5 fprintf
* \return last time step used [s]
*/
void loop_dopri5(double cur_t0, double cur_tf, double **dopri5_alloc_tab, MbsData *s, MbsDirdyn *dd, FILE *fileout_dopri5)
double loop_dopri5(double cur_t0, double cur_tf, double dt0, double **dopri5_alloc_tab, MbsData *s, MbsDirdyn *dd, FILE *fileout_dopri5)
{
double hout;
dopri5(dd->nState, fcn_dopri5, cur_t0, dd->y, cur_tf, &(dd->options->adapt_rtoler), &(dd->options->adapt_atoler),
0, solout_dopri5, 1, fileout_dopri5, 0.0, 0.0, 0.0, 0.0, 0.0, dd->options->adapt_dt_max, dd->options->dt0, dd->options->adapt_nmax,
1, 0, 0, NULL, 0, dopri5_alloc_tab, s, dd);
0, solout_dopri5, 1, fileout_dopri5, 0.0, 0.0, 0.0, 0.0, 0.0, dd->options->adapt_dt_max, dt0, dd->options->adapt_nmax,
1, 0, 0, NULL, 0, dopri5_alloc_tab, s, dd, &hout);
return hout;
}
/*! \brief dopri5 derivative function
......@@ -552,14 +561,17 @@ void fcn_dopri5(unsigned n, long nr, double tsim, double y[], double dydt[], Mbs
*/
void solout_dopri5(long nr, double tsim_old, double tsim, double y[], unsigned n, int* irtrn, MbsData *s, MbsDirdyn *dd)
{
//printf("%f\n", tsim*1.0e3);
if (tsim > dd->adapt_solout_last_t)
{
dd->adapt_solout_last_t = tsim;
// user loop
user_loop(s, dd);
// user loop
user_loop(s, dd);
// save data + real-time features
save_realtime_update(dd, s);
// save data + real-time features
save_realtime_update(dd, s);
// quit the simulation if requested
*irtrn = (s->flag_stop) ? -1 : 1;
// quit the simulation if requested
*irtrn = (s->flag_stop) ? -1 : 1;
}
}
......@@ -110,6 +110,7 @@ typedef struct MbsDirdyn
// adaptive time step
int adapt_flag_save;
double *adapt_dydt_save;
double adapt_solout_last_t;
} MbsDirdyn;
......@@ -191,7 +192,7 @@ void mbs_delete_dirdyn(MbsDirdyn* dirdyn, MbsData* mbs_data);
void mbs_fct_dirdyn(double t, double y[], double dydt[], MbsData *s, MbsDirdyn *dd);
void save_realtime_update(MbsDirdyn* dd, MbsData* mbs_data);
void loop_dopri5(double cur_t0, double cur_tf, double **dopri5_alloc_tab, MbsData *s, MbsDirdyn *dd, FILE *fileout_dopri5);
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);
......
......@@ -7,7 +7,7 @@
static long nfcn, nstep, naccpt, nrejct, n_fcn; // modif: n_fcn added
static double hout, xold, xout;
static double hout, xold, xout, last_hnew; // modif: last_hnew added
static unsigned nrds, *indir;
double *yy1, *k1, *k2, *k3, *k4, *k5, *k6, *ysti;
static double *rcont1, *rcont2, *rcont3, *rcont4, *rcont5;
......@@ -448,19 +448,20 @@ static int dopcor (unsigned n, FcnEqDiff fcn, double x, double* y, double xend,
}
h = hnew;
last_hnew = hnew; // modif: line added
}
} /* dopcor */
/* front-end */
// modif: double **dopri5_alloc_tab, MbsData *s, MbsDirdyn *dd added
// modif: double **dopri5_alloc_tab, MbsData *s, MbsDirdyn *dd double *last_h, added
int dopri5
(unsigned n, FcnEqDiff fcn, double x, double* y, double xend, double* rtoler,
double* atoler, int itoler, SolTrait solout, int iout, FILE* fileout, double uround,
double safe, double fac1, double fac2, double beta, double hmax, double h,
long nmax, int meth, long nstiff, unsigned nrdens, unsigned* icont, unsigned licont,
double **dopri5_alloc_tab, MbsData *s, MbsDirdyn *dd)
double **dopri5_alloc_tab, MbsData *s, MbsDirdyn *dd, double *last_h)
{
int arret, idid;
unsigned i;
......@@ -642,9 +643,13 @@ int dopri5
}
else
{
last_hnew = h; // modif: line added
idid = dopcor (n, fcn, x, y, xend, hmax, h, rtoler, atoler, itoler, fileout,
solout, iout, nmax, uround, meth, nstiff, safe, beta, fac1, fac2, icont, s, dd); // modif: s and dd added
*last_h = last_hnew; // modif: line added
/* modif: remove yy1, k1, k2, k3, k4, k5, k6, ysti memory release (free) */
if (indir)
......
......@@ -214,7 +214,8 @@ extern int dopri5
unsigned licont, /* declared length of icon */
double **dopri5_alloc_tab, /* modif: tabular of allocated vectors */
MbsData *s, /* modif: Robotran main fields */
MbsDirdyn *dd /* modif: direct dynamics */
MbsDirdyn *dd, /* modif: direct dynamics */
double *last_h /* modif: last step used */
); // modif: MbsData *s, MbsDirdyn *dd added
extern double contd5
......
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