Commit 5f012da0 authored by Olivier Lantsoght's avatar Olivier Lantsoght
Browse files

[MbsDirdyn] Error code in deriv, integrators and lot of stuff for dirdyn_loop correct catching

parent 0de0e84e
......@@ -162,7 +162,10 @@ int mbs_run_dirdyn(MbsDirdyn* dd, MbsData* mbs_data)
// 2. Run the simulation
// - - - - - - - - - - -
mbs_dirdyn_loop(dd, mbs_data);
err = mbs_dirdyn_loop(dd, mbs_data);
if (err < 0){
return -1;
}
// 3. Finish the simulation
......@@ -547,6 +550,7 @@ int mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
dd->finish_integrator = NULL;
free(dd->integrator_struct);
dd->integrator_struct = NULL;
free_dvec_0(dd->y);
dd->y = NULL;
......@@ -625,6 +629,7 @@ int mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
dd->finish_integrator = NULL;
free(dd->integrator_struct);
dd->integrator_struct = NULL;
free_dvec_0(dd->y);
dd->y = NULL;
......@@ -667,6 +672,7 @@ int mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
dd->finish_integrator = NULL;
free(dd->integrator_struct);
dd->integrator_struct = NULL;
free_dvec_0(dd->y);
dd->y = NULL;
......@@ -718,10 +724,11 @@ int mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
return 0;
}
void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data)
int mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data)
{
MbsDirdynOptions* opts;
double cur_t0, cur_tf;
int err = 0;
opts = dd->options;
......@@ -739,7 +746,10 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data)
while (cur_tf <= opts->tf)
{
dd->loop_integrator(cur_t0, cur_tf, mbs_data, dd);
err = dd->loop_integrator(cur_t0, cur_tf, mbs_data, dd);
if (err < 0){
return err;
}
// Update times
cur_t0 = cur_tf;
cur_tf += opts->delta_t_wp;
......@@ -747,9 +757,10 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data)
// stop the simulation if 'flag_stop' set to 1
if (mbs_data->flag_stop)
{
break;
return 0;
}
}
return 0;
}
void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data) {
......@@ -777,12 +788,16 @@ void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data) {
user_dirdyn_finish(mbs_data, dd);
dd->finish_integrator(mbs_data, dd);
free(dd->integrator_struct);
dd->integrator_struct = NULL;
/* TODO : Move in finish_integrator when 2nd order integrator will be implemented */
// Free state vectors
free(dd->y);
free(dd->yd);
free(dd->yout);
dd->y = NULL;
dd->yd = NULL;
dd->yout = NULL;
if (dd->options->save2file)
{
......@@ -803,6 +818,7 @@ void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data) {
mbs_delete_buffer(dd->buffers[i]);
}
free(dd->buffers);
dd->buffers = NULL;
#ifdef REAL_TIME
if (dd->options->save_visu)
......@@ -813,6 +829,7 @@ void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data) {
mbs_delete_buffer(dd->buffer_visu[i]);
}
free(dd->buffer_visu);
dd->buffer_visu = NULL;
}
#endif
......@@ -828,11 +845,13 @@ void mbs_dirdyn_finish(MbsDirdyn* dd, MbsData* mbs_data) {
}
mbs_delete_growing_buffer(dd->user_buffer);
dd->user_buffer = NULL;
// release memory for the 'auto_output' structure
free_set_output();
reset_flag_output();
free(dd->savedArrays);
dd->savedArrays = NULL;
}
if (dd->options->verbose)
......
......@@ -94,13 +94,19 @@ int mbs_run_dirdyn(MbsDirdyn* dirdyn, MbsData* mbs_data);
int mbs_dirdyn_init(MbsDirdyn* dirdyn, MbsData* mbs_data);
/**
* Time loop of the run_dirdyn function:
* - call the time integrator
* - call the discrete state functions
* - call the storing function
*/
void mbs_dirdyn_loop(MbsDirdyn* dirdyn, MbsData* mbs_data);
/** \brief Time loop of the run_dirdyn function:
*
* The sequence of one iteration is:
* - call the time integrator
* - call the discrete state functions
* - call the storing function
*
* \param[in,out] dirdyn the MbsDirdyn to be run
* \param[in,out] mbs_data the MbsData structure of the model for which the direct dynamic is computed
*
* \return Error status, <0 in case of failure.
*/
int mbs_dirdyn_loop(MbsDirdyn* dirdyn, MbsData* mbs_data);
/**
......@@ -149,7 +155,7 @@ int mbs_dirdyn_write_buffers(MbsDirdyn* dd);
* \param[in,out] s Robotran main structure
* \param[in,out] dd direct dynamic main module structure
*
* \return Error status, <0 in case of failure.
* \return Error status, <0 in case of failure.
*/
int mbs_fct_dirdyn(double t, double y[], double dydt[], MbsData *s, MbsDirdyn *dd);
......
......@@ -27,16 +27,20 @@
#include "mbs_message.h"
void mbs_estim_jac_acc(double x, double htry, double y[], double dydx[], double dfdx[], double **dfdy, int n,
int mbs_estim_jac_acc(double x, double htry, double y[], double dydx[], double dfdx[], double **dfdy, int n,
int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), MbsData *s, MbsDirdyn *dd)
{
int i, j;
int i, j, err;
double perturb;
// troubleshooting value to avoid division by 0
double trsh = 1e-3;
// Compute f() with the initial values and store it in each row
user_dirdyn_loop(s, dd);
(*derivs)(x, y, dydx, s, dd);
err = (*derivs)(x, y, dydx, s, dd);
if (err < 0){
mbs_msg(">>integrator>> Error during Jacobian estimation.\n");
return err;
}
for (i = 0; i < n; i++) {
dfdx[i] = -dydx[i];
for (j = 0; j < n; j++) {
......@@ -45,7 +49,11 @@ void mbs_estim_jac_acc(double x, double htry, double y[], double dydx[], double
}
// Computing dfdx: time variation is htry/2.0
(*derivs)(x + htry / 2.0, y, dydx, s, dd);
err = (*derivs)(x + htry / 2.0, y, dydx, s, dd);
if (err < 0){
mbs_msg(">>integrator>> Error during Jacobian estimation.\n");
return err;
}
for (i = 0; i < n; i++) {
dfdx[i] = (dfdx[i] + dydx[i]) / (htry / 2.0);
}
......@@ -67,7 +75,11 @@ void mbs_estim_jac_acc(double x, double htry, double y[], double dydx[], double
y[i] += perturb;
// evaluate perturbed dydx
//user_dirdyn_loop(s, dd);
(*derivs)(x, y, dydx, s, dd);
err = (*derivs)(x, y, dydx, s, dd);
if (err < 0){
mbs_msg(">>integrator>> Error during Jacobian estimation.\n");
return err;
}
for (j = 0; j < n; j++) {
dfdy[j][i] = (dfdy[j][i] + dydx[j]) / perturb;
......@@ -76,20 +88,26 @@ void mbs_estim_jac_acc(double x, double htry, double y[], double dydx[], double
// Restore y vector
y[i] -= perturb;
}
return 0;
}
void mbs_freeze_jac(int freeze_index, int *next_freeze_index, double x, double h, double y[],
int mbs_freeze_jac(int freeze_index, int *next_freeze_index, double x, double h, double y[],
double dydx[], double dfdx[], double **dfdy, double dydx_freeze[],
double dfdx_freeze[], double **dfdy_freeze, int n,
int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *),
MbsData *s, MbsDirdyn *mbs_dd)
{
int err;
int nstate = mbs_dd->nState;
if (freeze_index == 0) // We want to compute the jacobian
{
// Compute Jacobian (dfdx and dfdy)
mbs_estim_jac_acc(x, h, y, dydx, dfdx, dfdy, n, derivs, s, mbs_dd);
err = mbs_estim_jac_acc(x, h, y, dydx, dfdx, dfdy, n, derivs, s, mbs_dd);
if (err < 0){
mbs_msg(">>integrator>> Error during Jacobian freezing.\n");
return err;
}
// copy the data to store
copy_dvec_0(dydx, dydx_freeze, nstate);
......@@ -114,6 +132,7 @@ void mbs_freeze_jac(int freeze_index, int *next_freeze_index, double x, double h
*next_freeze_index = 0;
}
return 0;
}
void set_integrator(MbsDirdyn *mbs_dd) {
......
......@@ -36,7 +36,7 @@ enum { RK4, Dopri5, Rosenbrock, EulerEx, Eulaire, EulerIm, Bader, WMethods, Cust
/** @brief initialization of the integrator structure(s) */
typedef void(*initialize_integrator_ptr)(MbsData *mbs_data, MbsDirdyn *mbs_dd);
/** @brief main loop of integration, in which the integrator is called */
typedef void(*loop_integrator_ptr)(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd);
typedef int(*loop_integrator_ptr)(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd);
/** @brief end of the integration, free the memory */
typedef void(*finish_integrator_ptr)(MbsData *mbs_data, MbsDirdyn *mbs_dd);
......@@ -69,8 +69,10 @@ void print_warnings_integrator(MbsDirdyn *mbs_dd, int type_of_integrator);
* @param derivs The function computing f'
* @param s the MbsData structure of the model on which dirdyn analysis is computed.
* @param dd the MbsDirdyn structure related to the integration.
*
* @return Error status, <0 in case of failure.
*/
void rk4(double y[], double dydx[], int n, double x, double h, double yout[],
int rk4(double y[], double dydx[], int n, double x, double h, double yout[],
int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *),
MbsData *s, MbsDirdyn *dd);
......@@ -103,12 +105,15 @@ int rosenbrock(int n, int(*derivs)(double, double[], double[], MbsData *, MbsDir
long nmax, double dydx[], double yscal[], double *hnext, MbsData *s,
MbsDirdyn *dd, double *hdid);
/* @brief Unknown integrator */
void ThetaSC(double y[], double dydx[], int n, double x, double h, double yout[],
/** @brief Unknown integrator
*
* @return Error status, <0 in case of failure.
*/
int ThetaSC(double y[], double dydx[], int n, double x, double h, double yout[],
int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *),
MbsData *s, MbsDirdyn *dd);
/* @brief Evaluate the Jacobian of a function accelerations using finite difference.
/** @brief Evaluate the Jacobian of a function accelerations using finite difference.
* Modify and do not restore dydx.
* WARNING: All vector/matrix indexes start at 0!
* Update the value of dfdx and dfdy.
......@@ -123,12 +128,14 @@ void ThetaSC(double y[], double dydx[], int n, double x, double h, double yout[]
* @param derivs the function computing f'
* @param s the MbsData structure of the model on which dirdyn analysis is computed.
* @param dd the MbsDirdyn structure related to the integration.
*
* @return Error status, <0 in case of failure.
*/
void mbs_estim_jac_acc(double x, double htry, double y[], double dydx[], double dfdx[], double **dfdy, int n,
int mbs_estim_jac_acc(double x, double htry, double y[], double dydx[], double dfdx[], double **dfdy, int n,
int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), MbsData *s, MbsDirdyn *dd);
/* @brief Freeze the Jacobian of an integrator structure
/** @brief Freeze the Jacobian of an integrator structure
* until mbs_dd->options->n_freeze
*
* WARNING: All vector/matrix indexes start at 0!
......@@ -148,8 +155,10 @@ void mbs_estim_jac_acc(double x, double htry, double y[], double dydx[], double
* @param derivs the function computing f'
* @param s the MbsData structure of the model on which dirdyn analysis is computed.
* @param mbs_dd the MbsDirdyn structure related to the integration.
*
* @return Error status, <0 in case of failure.
*/
void mbs_freeze_jac(int freeze_index, int *next_freeze_index, double x, double h, double y[], double dydx[],
int mbs_freeze_jac(int freeze_index, int *next_freeze_index, double x, double h, double y[], double dydx[],
double dfdx[], double **dfdy, double dydx_freeze[], double dfdx_freeze[], double **dfdy_freeze, int n,
int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), MbsData *s, MbsDirdyn *mbs_dd);
......
......@@ -57,7 +57,7 @@ void initialize_bader(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
mbs_warning_msg("The Bader semi-implicit integrator has not been test extensively ! \n ");
}
void loop_bader(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
int loop_bader(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
int i;
double *yscal;
......@@ -74,14 +74,32 @@ void loop_bader(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
// first call of f'
user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
mbs_fct_dirdyn(t, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
error_code = mbs_fct_dirdyn(t, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
if (error_code < 0)
{
free_dvec_0(yscal);
mbs_msg(">>DIRDYN>>\n");
mbs_msg(">>DIRDYN>> Error during integration in direct dynamics at time %g s !\n", mbs_dd->tsim);
mbs_dirdyn_finish(mbs_dd, mbs_data);
mbs_error_msg("[%d] in loop_bader !! \n", -413);
return -413;
}
while (t < tf)
{
if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
{
user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
error_code = mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
if (error_code < 0)
{
free_dvec_0(yscal);
mbs_msg(">>DIRDYN>>\n");
mbs_msg(">>DIRDYN>> Error during integration in direct dynamics at time %g s !\n", mbs_dd->tsim);
mbs_dirdyn_finish(mbs_dd, mbs_data);
mbs_error_msg("[%d] in loop_bader !! \n", -413);
return -413;
}
}
for (i = 0; i < mbs_dd->nState; i++) {
......@@ -91,21 +109,23 @@ void loop_bader(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
if (mbs_dd->nState != 0)
{
error_code = bader(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, &t, h_cur, mbs_dd->options->toler, yscal, &hdid, &hnext, h_max, mbs_fct_dirdyn, mbs_data, mbs_dd);
if (error_code < 0)
{
free_dvec_0(yscal);
mbs_msg(">>DIRDYN>>\n");
mbs_msg(">>DIRDYN>> Error during integration in direct dynamics at time %g s !\n", mbs_dd->tsim);
mbs_dirdyn_finish(mbs_dd, mbs_data);
mbs_error_msg("[%d] in loop_bader !! \n", -413);
return -413;
}
}
else
{
free_dvec_0(yscal);
mbs_msg(">>DIRDYN>> bader called without any independant joints \n");
mbs_error_msg("[%d] in loop_bader !! \n", -413);
exit(1);
}
if (error_code < 0) // Error management
{
mbs_msg(">>DIRDYN>>\n");
mbs_msg(">>DIRDYN>> Error during integration in direct dynamics at time %g s !\n", mbs_dd->tsim);
mbs_dirdyn_finish(mbs_dd, mbs_data);
mbs_error_msg("[%d] in loop_bader !! \n", -413);
exit(1);
return -413;
}
mbs_dd->tsim = t; // Update of tsim
......@@ -121,6 +141,7 @@ void loop_bader(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
}
free_dvec_0(yscal);
return 0;
}
void finish_bader(MbsData *mbs_data, MbsDirdyn *dd) {
......
......@@ -10,8 +10,8 @@
*
* (c) Universite catholique de Louvain
*/
#ifndef MBS_BADER_H_INCLUDED
#define MBS_BADER_H_INCLUDED
......@@ -57,8 +57,10 @@ void initialize_bader(MbsData *mbs_data, MbsDirdyn *mbs_dd);
* @param tf The final time
* @param mbs_data The computed MBS structure
* @param mbs_dd The associated MbsDirdyn structure
*
* @return Error status, <0 in case of failure.
*/
void loop_bader(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd);
int loop_bader(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd);
/** @brief Finalize and clear memory allocated for Bader integrator
*
......
......@@ -12,7 +12,7 @@
*
*/
#include "mbs_custom.h"
#include "mbs_custom.h"
#include "mbs_message.h"
#include "mbs_euler_explicit.h"
......@@ -34,10 +34,10 @@ void initialize_custom(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
// Loop of custom integrator
// This functions should call user_dirdyn_loop and mbs_fct_dirdyn when needed !
// This functions should call user_dirdyn_loop and mbs_fct_dirdyn when needed !
// Warning : only for advanced users
void loop_custom(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
int i;
int loop_custom(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
int i, err;
double h = tf - t0;
double dt0 = mbs_dd->options->dt0;
......@@ -46,7 +46,15 @@ void loop_custom(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
// first call of f'
user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
err = mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
if (err < 0)
{
mbs_msg(">>DIRDYN>>\n");
mbs_msg(">>DIRDYN>> Error during integration in direct dynamics at time %g s !\n", mbs_dd->tsim);
mbs_dirdyn_finish(mbs_dd, mbs_data);
mbs_error_msg("[%d] in loop_custom !! \n", -413);
return -413;
}
while (t < tf)
{
......@@ -54,13 +62,29 @@ void loop_custom(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
{
mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
err = mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
if (err < 0)
{
mbs_msg(">>DIRDYN>>\n");
mbs_msg(">>DIRDYN>> Error during integration in direct dynamics at time %g s !\n", mbs_dd->tsim);
mbs_dirdyn_finish(mbs_dd, mbs_data);
mbs_error_msg("[%d] in loop_custom !! \n", -413);
return -413;
}
}
// ###### start of custom unknown integrator #########
// Here we take ThetaSC which is not working
ThetaSC(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, mbs_dd->tsim, h, mbs_dd->yout, mbs_fct_dirdyn, mbs_data, mbs_dd); // not working, to check
err = ThetaSC(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, mbs_dd->tsim, h, mbs_dd->yout, mbs_fct_dirdyn, mbs_data, mbs_dd); // not working, to check
// ###### end of custom unknown integrator #########
if (err < 0)
{
mbs_msg(">>DIRDYN>>\n");
mbs_msg(">>DIRDYN>> Error during integration in direct dynamics at time %g s !\n", mbs_dd->tsim);
mbs_dirdyn_finish(mbs_dd, mbs_data);
mbs_error_msg("[%d] in loop_custom !! \n", -413);
return -413;
}
mbs_dd->tsim += h;
t = mbs_dd->tsim;
......@@ -69,7 +93,15 @@ void loop_custom(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
mbs_dd->y[i] = mbs_dd->yout[i];
}
mbs_fct_dirdyn(t, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd); // next f'
err = mbs_fct_dirdyn(t, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd); // next f'
if (err < 0)
{
mbs_msg(">>DIRDYN>>\n");
mbs_msg(">>DIRDYN>> Error during integration in direct dynamics at time %g s !\n", mbs_dd->tsim);
mbs_dirdyn_finish(mbs_dd, mbs_data);
mbs_error_msg("[%d] in loop_custom !! \n", -413);
return -413;
}
// save results if always asked or if waypoint reached
if ((!mbs_dd->options->flag_solout_wp) || (mbs_dd->options->flag_solout_wp && t == tf)) {
......@@ -77,9 +109,10 @@ void loop_custom(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
}
}
return 0;
}
// Free the elements in the structure of custom integrator
// Free the elements in the structure of custom integrator
// (See the allocations in initialize_custom() )
void finish_custom(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
if (mbs_dd->nState != 0)
......
......@@ -43,8 +43,10 @@ void initialize_custom(MbsData *mbs_data, MbsDirdyn *mbs_dd);
* @param tf The final time
* @param mbs_data The computed MBS structure
* @param mbs_dd The associated MbsDirdyn structure
*
* @return Error status, <0 in case of failure.
*/
void loop_custom(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd);
int loop_custom(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd);
/** @brief Finalize and clear memory allocated for Custom integrator
*
......
......@@ -40,7 +40,7 @@ void initialize_dopri5(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
}
}
void loop_dopri5(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
int loop_dopri5(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
double hout;
int err = 0;
......@@ -57,9 +57,10 @@ void loop_dopri5(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
mbs_msg(">>DIRDYN>> Error during integration in direct dynamics at time %g s !\n", mbs_dd->tsim);
mbs_dirdyn_finish(mbs_dd, mbs_data);
mbs_error_msg("[%d] in loop_dopri5 !! \n", -415);
exit(1);
return -415;
}
return 0;
}
void finish_dopri5(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
......
......@@ -53,8 +53,10 @@ void initialize_dopri5(MbsData *mbs_data, MbsDirdyn *mbs_dd);
* @param tf The final time
* @param mbs_data The computed MBS structure
* @param mbs_dd The associated MbsDirdyn structure
*
* @return Error status, <0 in case of failure.
*/
void loop_dopri5(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd);
int loop_dopri5(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd);
/** @brief Finalize and clear memory allocated for dopri5 integrator
*
......
......@@ -13,7 +13,7 @@
*
*/
#include "mbs_euler_explicit.h"
#include "mbs_euler_explicit.h"
#include "mbs_message.h"
void initialize_eulerEx(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
......@@ -37,8 +37,8 @@ void initialize_eulerEx(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
}
}
void loop_eulerEx(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
int loop_eulerEx(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
int err;
double h = tf - t0;
// Check remaining time
......@@ -48,7 +48,15 @@ void loop_eulerEx(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
// first call of f'
user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
err = mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
if (err < 0){
mbs_msg(">>Euler explicit>>\n");
mbs_msg(">>Euler explicit>> Error during integration in direct dynamics at time %g s !\n", mbs_dd->tsim);
mbs_dirdyn_finish(mbs_dd, mbs_data);
mbs_error_msg("[%d] in Euler explicit !! \n", -415);
return -415;
}
while (mbs_dd->tsim <= tf)
{
......@@ -56,14 +64,28 @@ void loop_eulerEx(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {