void break_gestion(Screen_sdl *screen_sdl, Simu_real_time *real_time, MBSdataStruct *MBSdata, int *speed_last_t_usec, int init_t_sec, int init_t_usec, double tsim)
#endif
{
// variables declaration
int start_break_t_usec;
int delta_break_u_sec;
// start of the break instant
start_break_t_usec = t_usec(init_t_sec, init_t_usec);
// first break plot
update_x_min_max(screen_sdl, real_time);
update_scale_signals(screen_sdl, real_time, 1);
plot_screen_sdl(screen_sdl, real_time, tsim, 1);
while (real_time->simu_break == 1)
{
// handle events
events_sdl(screen_sdl, real_time, MBSdata);
// break plot
if (screen_sdl->break_plot_flag)
{
screen_sdl->break_plot_flag = 0;
update_scale_signals(screen_sdl, real_time, 1);
plot_screen_sdl(screen_sdl, real_time, tsim, 1);
}
#if defined(JNI) & defined (REAL_TIME)
if (real_time->change_viewpoint)
{
update_jni(jni_struct, MBSdata, real_time, MBSdata->q+1);
}
#endif
}
// update variables after the break
delta_break_u_sec = t_usec(init_t_sec, init_t_usec) - start_break_t_usec;
*speed_last_t_usec += delta_break_u_sec;
update_real_time_constraints_break(real_time, delta_break_u_sec);
screen_sdl->hor_plot_scaling = 0;
// last_break_plot
update_x_min_max(screen_sdl, real_time);
plot_screen_sdl(screen_sdl, real_time, tsim, 1);
}
Simu_real_time* init_real_time(int init_t_sec, int init_t_usec)
{
// Real-time structure
Simu_real_time *real_time;
// tab with the time constraints
double fqc_tab[NB_REAL_TIME_CONSTRAINTS];
int cur_t_usec;
cur_t_usec = t_usec(init_t_sec, init_t_usec);
fqc_tab[0] = FQC_SCREEN; // constraints
fqc_tab[1] = FQC_JNI;
real_time = init_simu_real_time(NB_REAL_TIME_CONSTRAINTS, fqc_tab, cur_t_usec, TSIM_INIT, TSIM_END);
return real_time;
}
/*
* End of the simulation
*/
void finish_simulation(Loop_inputs *loop_inputs)
{
// -- Variables declaration -- //
#ifdef PRINT_REPORT
double total_t_sec;
#endif
MBSdataStruct *MBSdata = NULL;
// -- Simulation end -- //
// for the anim file
#ifdef WRITE_FILES
#ifdef REAL_TIME
const char *fileout_anim = PROJECT_ABS_PATH"/../animationR/Model_real_time.anim";
#else
const char *fileout_anim = PROJECT_ABS_PATH"/../animationR/Model_standalone.anim";
#endif
const char generic_fileout[PATH_MAX_LENGTH] = PROJECT_ABS_PATH"/src/other/outVectors/output_vector";
#endif
// MBSdata
MBSdata = loop_inputs->MBSdata;
#ifdef PRINT_REPORT
// total real-time needed for the simulation
total_t_sec = t_usec(loop_inputs->init_t_sec, loop_inputs->init_t_usec) / 1.0e6;
// Printing simulation report
printf("... Done\n\n");
printf("-------------------------------\n");
printf("nb steps:\t%d\n", NB_SIMU_STEPS);
printf("time step:\t%.3f\t[ms]\n", DELTA_TSIM * 1000);
printf("-------------------------------\n");
printf("t_start:\t%.3f\t[s]\n", TSIM_INIT);
printf("t_end:\t\t%.3f\t[s]\n", TSIM_END);
printf("-------------------------------\n");
printf("Final time:\t%.3f\t[s]\n", MBSdata->tsim);
printf("Executed in:\t%.3f\t[s]\n", total_t_sec);
printf("-------------------------------\n\n");
#endif
// writing the .anim file
#ifdef WRITE_FILES
loop_inputs->write_files->kount = loop_inputs->write_files->kount - 1; //last increment doesn't correspond to a writting
if(write_anim_file(loop_inputs->write_files, MBSdata->njoint, fileout_anim))
{
printf("error: cannot write the anim file\n");
}
#ifdef PRINT_REPORT
else
{
printf("File '%s' successfully written\n", fileout_anim);
}
#endif
if (write_out_files(loop_inputs->write_files, generic_fileout))
{
printf("error: cannot write the output vectors files\n");
}
#ifdef PRINT_REPORT
else
{
printf("Output Files '%s' successfully written\n", generic_fileout);
}
#endif
#endif
// -- Closing operations -- //
// .anim files
#ifdef WRITE_FILES
free_write_files(loop_inputs->write_files, MBSdata->njoint);
#endif
// JNI visualization
#if defined(JNI) & defined (REAL_TIME)
free_jni(loop_inputs->jni_struct);
#endif
// Real-time constraints
#ifdef REAL_TIME
free_simu_real_time(loop_inputs->real_time);
#endif
// SDL window
#if defined(SDL) & defined(REAL_TIME)
free_screen_sdl(loop_inputs->screen_sdl);
#endif
// LocalDataStruct
#if !defined ACCELRED
freeLocalDataStruct(loop_inputs->lds,MBSdata);
#endif
#ifdef YARP
yarp_finish(loop_inputs->RobotranYarp_interface);
#endif
// MBSdata_xml
freeMBSdata_xml(MBSdata);
// simulation loop inputs
free(loop_inputs);
}
double loop_simulation(Loop_inputs *loop_inputs)
{
// -- Variables decalration -- //
int i,k;
int nvar, nstep;
int simu_go;
double x, h;
double x1, x2;
double *ystart;
double *v,*vout,*dv;
LocalDataStruct *lds;
MBSdataStruct *MBSdata;
#ifdef WRITE_FILES
Write_files *write_files;
#endif
#if defined(JNI) & defined (REAL_TIME)
JNI_struct* jni_struct;
#endif
#ifdef REAL_TIME
int cur_t_usec;
int init_t_sec, init_t_usec;
// simulation time
double tsim;
// real-time (in us) vs simulation (in s) variables
int speed_last_t_usec, speed_new_t_usec;
double speed_last_tsim, speed_new_tsim;
// Real-time constraints
Real_time_constraint **constraints;
// Real-time constraints main structure
Simu_real_time *real_time;
#endif
#if defined(SDL) & defined(REAL_TIME)
double y_vec[NB_CURVES_MAX];
Screen_sdl *screen_sdl;
#endif
// -- Variables initialization -- //
nvar = loop_inputs->nvar;
nstep = loop_inputs->nstep;
x1 = loop_inputs->x1;
x2 = loop_inputs->x2;
ystart = loop_inputs->ystart;
lds = loop_inputs->lds;
MBSdata = loop_inputs->MBSdata;
v = dvector(1,nvar);
vout = dvector(1,nvar);
dv = dvector(1,nvar);
// Load starting values
for (i=1; i <= nvar; i++)
{
v[i] = ystart[i];
}
x = x1;
h = (x2 - x1) / nstep; // [s]
#ifdef WRITE_FILES
write_files = loop_inputs->write_files;
#endif
#if defined(JNI) & defined (REAL_TIME)
jni_struct = loop_inputs->jni_struct;
#endif
#ifdef REAL_TIME
// simulation time
tsim = TSIM_INIT;
init_t_sec = loop_inputs->init_t_sec;
init_t_usec = loop_inputs->init_t_usec;
// current eral-time [us]
cur_t_usec = t_usec(init_t_sec, init_t_usec);
// real-time variables
speed_last_t_usec = cur_t_usec;
speed_new_t_usec = cur_t_usec;
speed_last_tsim = TSIM_INIT;
speed_new_tsim = TSIM_INIT;
// structures
real_time = loop_inputs->real_time;
constraints = real_time->constraints;
#endif
#if defined(SDL) & defined(REAL_TIME)
// strcuture
screen_sdl = loop_inputs->screen_sdl;
// first plot
plot_screen_sdl(screen_sdl, real_time, tsim, 2);
#endif
// flag : 0 if the simulation must be stopped (1 otherwise)
simu_go = 1;
// Simulation over nstep steps: loop
for (k = 1; (k <= nstep) && simu_go; k++)
{
// user own functions (controller_files and simulation_files)
user_compute_output(MBSdata,lds);
// .anim
#ifdef WRITE_FILES
update_write_files(write_files, MBSdata);
#endif
// SDL window
#if defined(SDL) & defined(REAL_TIME)
// assign values for the plot
get_screen_sdl_functions(y_vec, MBSdata);
// update simulation vectors for the plot
update_full_vectors(screen_sdl, tsim, y_vec);
#endif
#ifdef REAL_TIME
// check actions related to the Real-time constraints
for (i=0; i<NB_REAL_TIME_CONSTRAINTS; i++)
{
if (tsim >= constraints[i]->next_tsim)
{
// plot screen sdl
#if defined(SDL) & defined (REAL_TIME)
if (!i)
{
update_plot_vectors(screen_sdl, real_time, tsim, y_vec);
plot_screen_sdl(screen_sdl, real_time, tsim, 0);
}
#endif
// JNI visualization
#if defined(JNI) & defined (REAL_TIME)
if (i == 1)
{
update_jni(jni_struct, MBSdata, real_time);
}
#endif
// new real-time constraints
update_real_time_constraint(constraints[i], real_time->simu_speed_flag);
}
}
// handle events (coming from the keyboard...)
#if defined (SDL) & defined(JNI) & defined (REAL_TIME)
events_simu(screen_sdl, real_time, MBSdata, &simu_go, &speed_last_t_usec, init_t_sec, init_t_usec, tsim, jni_struct);
#elif defined (SDL) & defined (REAL_TIME)
events_simu(screen_sdl, real_time, MBSdata, &simu_go, &speed_last_t_usec, init_t_sec, init_t_usec, tsim);
#endif
// gate locked: waiting time
if (tsim >= real_time->next_tsim_gate)
{
// current time
cur_t_usec = t_usec(init_t_sec, init_t_usec);
// loop in order to wait to respect the real-time constraints
while (real_time->next_t_usec_gate > cur_t_usec)
{
// current time
cur_t_usec = t_usec(init_t_sec, init_t_usec);
// handle events (coming from the keyboard...)
#if defined (SDL) & defined(JNI) & defined (REAL_TIME)
events_simu(screen_sdl, real_time, MBSdata, &simu_go, &speed_last_t_usec, init_t_sec, init_t_usec, tsim, jni_struct);
#elif defined (SDL) & defined (REAL_TIME)
events_simu(screen_sdl, real_time, MBSdata, &simu_go, &speed_last_t_usec, init_t_sec, init_t_usec, tsim);
#endif
}
// in case there is no additional constraint
if (real_time->no_additional_constraint)
{
update_real_time_constraint(real_time->constraints[0], real_time->simu_speed_flag);
}
// update real-time strcuture and related variables
update_simu_real_time(real_time);
}
// computes the real time speed factor of the simulation (every REAL_TIME_SPEED_PERIOD_USEC s)
if (cur_t_usec - speed_last_t_usec > REAL_TIME_SPEED_PERIOD_USEC)
{
speed_new_t_usec = cur_t_usec;
speed_new_tsim = tsim;
real_time->real_simu_speed_factor = (speed_new_tsim - speed_last_tsim) / (1.0e-6 * (speed_new_t_usec - speed_last_t_usec));
// change the plot
#if defined(SDL) & defined (REAL_TIME)
screen_sdl->bottom_flag = 1;
#endif
speed_last_t_usec = speed_new_t_usec;
speed_last_tsim = speed_new_tsim;
}
#endif
/*
* Simbody external forces
*/
#ifdef SIMBODY
// kinematics from Robotran
update_simbody_kinematics(MBSdata->user_IO->simbodyBodies, MBSdata);
// Simbody functions
loop_Simbody(p_simbodyVariables, MBSdata->user_IO->simbodyBodies);
#endif
/*
* Main routine of the integrator.
*
* Starting from initial values ystart[1..nvar] known at x1 use fourth-order Runge-Kutta
* to advance nstep equal increments h to x2. The user-supplied routine derivs(x,v,dvdx)
* evaluates derivatives. Results are stored in the global variables y[1..nvar][1..nstep+1]
* and xx[1..nstep+1].
*/
(*derivs)(x,v,dv,lds,MBSdata);
rk4(v,dv,nvar,x,h,vout,derivs,lds,MBSdata);
if ((double)(x+h) == x)
{
nrerror("Step size too small in routine odeint");
}
x += h;
for (i=1;i<=nvar;i++)
{
v[i] = vout[i];
}
/*
* Real-time update
*/
#ifdef REAL_TIME
// update simulation time
tsim = MBSdata->tsim;
// update real time
cur_t_usec = t_usec(init_t_sec, init_t_usec);
#endif
// stop the simulation if 'stop_out == 1'
#ifdef FLAG_STOP
if (MBSdata->user_IO->stop_out)
{
simu_go = 0;
}
#endif
}
// -- Free memory -- //
free_dvector(dv,1,nvar);
free_dvector(vout,1,nvar);
free_dvector(v,1,nvar);
return MBSdata->user_IO->fitness_opti;
}
