Commit 196e450c authored by Sébastien Timmermans's avatar Sébastien Timmermans
Browse files

[ErrMsg] InvDyn err_msg call

parent 39ff3931
......@@ -104,7 +104,8 @@ int mbs_run_invdyn(MbsInvdyn* invd, MbsData* mbs_data)
err = mbs_invdyn_init(invd, mbs_data);
if (err < 0){
mbs_invdyn_finish(invd, mbs_data);
return err;
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", _MBS_ERR_MOD_INV_DYN + err);
return _MBS_ERR_MOD_INV_DYN + err;
}
......@@ -113,14 +114,18 @@ int mbs_run_invdyn(MbsInvdyn* invd, MbsData* mbs_data)
err = mbs_invdyn_loop(invd, mbs_data);
if (err < 0){
mbs_invdyn_finish(invd, mbs_data);
return err;
mbs_error_msg("[%d] in mbs_invdyn_loop !! \n", _MBS_ERR_MOD_INV_DYN + err);
return _MBS_ERR_MOD_INV_DYN + err;
}
// 3. Finish the simulation
// - - - - - - - - - - - -
err = mbs_invdyn_finish(invd, mbs_data);
if (err < 0) {
mbs_error_msg("[%d] in mbs_invdyn_finish !! \n", _MBS_ERR_MOD_INV_DYN + err);
return _MBS_ERR_MOD_INV_DYN + err;
}
return err;
}
......@@ -153,28 +158,18 @@ int mbs_invdyn_save(MbsInvdyn* invd, MbsData *mbs_data, double t) {
if (err1 < 0 && err2 <0){ // Error management
err1 += _MBS_ERR_MOD_INV_DYN;
err2 += _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> mbs_invdyn_save: error [%d] while buffering the modules results.\n", err1);
mbs_msg(">>INVDYN>> mbs_invdyn_save: error [%d] while buffering the user results.\n", err2);
return err1; // arbitrary choice
}
else if (err1 < 0)
{
err1 += _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> mbs_invdyn_save: error [%d] while buffering the modules results.\n", err);
return err1;
}
else if (err2 < 0) // Error management
{
err2 += _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> mbs_invdyn_save: error [%d] while buffering the user results.\n", err2);
return err2;
}
......@@ -196,23 +191,17 @@ int mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
// Checking mbs_data coherence
err = mbs_check_mbs_data_values(invd->mbs_aux, mbs_data);
if (err < 0) {
err += _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> Incoherences detected during module initialization! (See message above) \n");
return err;
}
// Checking constraints and dependant variable coherence
err = mbs_check_nhu_nqv(mbs_data);
if (err < 0) {
err += _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> Inchoherence detected during module initialization! \n");
mbs_msg(" >> -Number of independent constraints (%d)\n", mbs_data->nhu);
mbs_msg(" >> -Number of dependent coordinates (%d)\n", mbs_data->nqv);
mbs_msg(" >> must be equal!\n");
return err;
}
......@@ -220,13 +209,11 @@ int mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
if (mbs_data->nqlocked && mbs_data->nqa) {
for (i = 1; i <= mbs_data->nqlocked; i++) {
if (find_ivec_1(mbs_data->qa, mbs_data->qlocked[i], mbs_data->nqa) != -1) {
err = _MBS_ERR_INIT + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> Inchoherence detected during module initialization!\n");
mbs_msg(" >> The actuated joint %d is also locked.\n", mbs_data->qlocked[i]);
mbs_msg(" >> This is not allowed!\n");
return err;
return _MBS_ERR_INIT;
}
}
}
......@@ -238,13 +225,10 @@ int mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
// Locked joint cannont be actuated
for (i = 1; i <= mbs_data->nqc; i++) {
if (find_ivec_1(mbs_data->qa, mbs_data->nqa, mbs_data->qc[i]) != -1) {
err = _MBS_ERR_INIT + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> Inchoherence detected during module initialization!\n");
mbs_msg(" >> The actuated joint %d is also driven.\n", mbs_data->qc[i]);
mbs_msg(" >> This is not allowed when some joints are independent.\n");
return err;
return _MBS_ERR_INIT;
}
}
......@@ -308,20 +292,14 @@ int mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
invd->tindex = 0;
// Motion identification
if (invd->options->motion == closeloop) {
err = _MBS_ERR_INIT + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> Inchoherence detected during module initialization!\n");
mbs_msg(" >> 'MbsInvdyn->options->motion' cannot be set as 'closeloop' for inverse dynamic analysis.\n");
return err;
return _MBS_ERR_INIT;
}
else if (invd->options->motion != oneshot && invd->options->motion != trajectory) {
err = _MBS_ERR_INIT + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> Inchoherence detected during module initialization!\n");
mbs_msg(" >> 'MbsInvdyn->options->motion' has an unsupported value.\n");
return err;
return _MBS_ERR_INIT;
}
// Oneshot : Setting values.
......@@ -338,11 +316,8 @@ int mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
if (invd->options->trajectoryqname == NULL || invd->options->trajectoryqdname == NULL || invd->options->trajectoryqddname == NULL) {
// At least one trajectory file is not provided, we should have a driven inverse dynamic.
if (mbs_data->nqu != 0) {
err = _MBS_ERR_INIT + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> Trajectory (q, qd or qdd) files are mandatory when some joints are independent.\n");
return err;
return _MBS_ERR_INIT;
}
invd->trajectorytype = 1;
invd->q = NULL;
......@@ -355,55 +330,38 @@ int mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
mbs_data->dt0 = invd->options->dt;
invd->tsim = invd->options->t0;
if (mbs_data->dt0 <= 0.0) { // Error management
err = _MBS_ERR_INIT + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> Time step has to be stricly positive (dt0>0) for a fully driven inverse dynamic analysis.\n");
return err;
return _MBS_ERR_INIT;
}
if (mbs_data->t0 >= mbs_data->tf) { // Error management
err = _MBS_ERR_INIT + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> Tnitial time has to be stricly lower than final time (t0<tf).\n");
return err;
return _MBS_ERR_INIT;
}
}
else {
// All trajectory files are provided.
invd->q = mbs_lut_res_load_file(invd->options->trajectoryqname);
if (!invd->q){
err = _MBS_ERR_LOW_FILES + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> Unable to load coordinates trajectory file.\n");
return err;
return _MBS_ERR_LOW_FILES;
}
invd->qd = mbs_lut_res_load_file(invd->options->trajectoryqdname);
if (!invd->qd){
err = _MBS_ERR_LOW_FILES + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> Unable to load velocities trajectory file.\n");
return err;
return _MBS_ERR_LOW_FILES;
}
invd->qdd = mbs_lut_res_load_file(invd->options->trajectoryqddname);
if (!invd->qdd){
err = _MBS_ERR_LOW_FILES + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> Unable to load accelerations trajectory file.\n");
return err;
return _MBS_ERR_LOW_FILES;
}
if (invd->q->nq != invd->qd->nq || invd->q->nq != invd->qdd->nq) { // Error management
err = _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> Number of coordinates is not consistent between trajectories file.\n");
return err;
return _MBS_ERR_MOD_SPEC_11;
}
if (invd->q->nq == mbs_data->njoint) {
......@@ -417,13 +375,11 @@ int mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
invd->trajectorytype = 3;
}
else { // Error management
err = _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> Provided trajectories number of variable (%d) does not match neither:\n", invd->q->nq);
mbs_msg(" >>> - the number of total joint (%d);\n", mbs_data->njoint);
mbs_msg(" >>> - the number of independant joint (%d);\n", mbs_data->nqu);
return err;
return _MBS_ERR_MOD_SPEC_14;
}
if (invd->options->dt <= 0) {
......@@ -441,27 +397,18 @@ int mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
mbs_data->dt0 = invd->options->dt;
invd->tsim = invd->options->t0;
if (mbs_data->t0 < invd->q->tq[0][0]) { // Error management
err = _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> The initial time is below the minimal time of the provided trajectory files.\n");
mbs_msg(" >>> Trajectory extrapolation is not allowed.\n");
return err;
return _MBS_ERR_MOD_SPEC_15;
}
if (mbs_data->tf > invd->q->tq[invd->q->nt - 1][0]) { // Error management
err = _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> The final time is over the maximal time of the provided trajectory files.\n");
mbs_msg(" >>> Trajectory extrapolation is not allowed.\n");
return err;
return _MBS_ERR_MOD_SPEC_14;
}
if (mbs_data->t0 >= mbs_data->tf) { // Error management
err = _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> initial time has to be stricly lower (t0<tf) than final time.\n");
return err;
return _MBS_ERR_MOD_SPEC_14;
}
}
......@@ -691,21 +638,20 @@ int mbs_invdyn_loop(MbsInvdyn* invd, MbsData* mbs_data)
// Check contents of vectors, buffers will be written by finish function
for (i = 1; i <= mbs_data->njoint; i++) {
if (isnan(mbs_data->q[i])) { // Error management
err = _MBS_ERR_LOW_NAN + _MBS_ERR_MOD_INV_DYN;
err = _MBS_ERR_LOW_NAN;
mbs_msg(">>INVDYN>> q[%d] is Nan at time %g s ! \n", i, mbs_data->tsim);
}
if (isnan(mbs_data->qd[i])) {
err = _MBS_ERR_LOW_NAN + _MBS_ERR_MOD_INV_DYN;
err = _MBS_ERR_LOW_NAN;
mbs_msg(">>INVDYN>> qd[%d] is Nan at time %g s ! \n", i, mbs_data->tsim);
}
if (isnan(mbs_data->qdd[i])) {
err = _MBS_ERR_LOW_NAN + _MBS_ERR_MOD_INV_DYN;
err = _MBS_ERR_LOW_NAN;
mbs_msg(">>INVDYN>> qdd[%d] is Nan at time %g s ! \n", i, mbs_data->tsim);
}
}
if (err < 0){
mbs_msg(">>INVDYN>> [%d] in mbs_invdyn_loop !! \n", err);
return err;
}
}
......@@ -724,7 +670,6 @@ int mbs_invdyn_loop(MbsInvdyn* invd, MbsData* mbs_data)
{
mbs_msg(">>INVDYN>> Error during inverse dynamics analysis at time %g for trajectory case !\n", err, mbs_data->tsim);
mbs_msg(" >> [%d] in mbs_invdyn_loop !! \n", err);
return err;
}
......@@ -733,15 +678,15 @@ int mbs_invdyn_loop(MbsInvdyn* invd, MbsData* mbs_data)
for (i = 1; i <= mbs_data->njoint; i++) {
if (isnan(mbs_data->q[i])) { // Error management
err = _MBS_ERR_LOW_NAN + _MBS_ERR_MOD_INV_DYN;
err = _MBS_ERR_LOW_NAN;
mbs_msg(">>INVDYN>> q[%d] is Nan at time %g s ! \n", i, mbs_data->tsim);
}
if (isnan(mbs_data->qd[i])) {
err = _MBS_ERR_LOW_NAN + _MBS_ERR_MOD_INV_DYN;
err = _MBS_ERR_LOW_NAN;
mbs_msg(">>INVDYN>> qd[%d] is Nan at time %g s ! \n", i, mbs_data->tsim);
}
if (isnan(mbs_data->qdd[i])) {
err = _MBS_ERR_LOW_NAN + _MBS_ERR_MOD_INV_DYN;
err = _MBS_ERR_LOW_NAN;
mbs_msg(">>INVDYN>> qdd[%d] is Nan at time %g s ! \n", i, mbs_data->tsim);
}
}
......@@ -792,8 +737,6 @@ int mbs_invdyn_finish(MbsInvdyn* invd, MbsData* mbs_data) {
err1 = mbs_invdyn_write_buffers(invd);
if (err1 < 0) {
err1 += _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> Error [%d] while saving results (module buffers) to disk !\n", err1);
}
if (invd->buffers){
......@@ -810,8 +753,6 @@ int mbs_invdyn_finish(MbsInvdyn* invd, MbsData* mbs_data) {
err2 = mbs_growing_buffer_write(invd->user_buffer);
if (err2 < 0) {
err2 += _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> Error [%d] while saving results (user buffers) to disk !\n", err2);
}
......@@ -827,22 +768,16 @@ int mbs_invdyn_finish(MbsInvdyn* invd, MbsData* mbs_data) {
// memory has been released, raise error
if (err1 < 0 && err2 < 0)
{
err = err1; // arbitrary choice
mbs_msg(">>INVDYN>> Errors [%d] and [%d] in mbs_invdyn_finish!\n", err1, err2);
return(err);
return(err1);
}
else if (err1 < 0) {
err = err1;
mbs_msg(">>INVDYN>> Errors[%d] in mbs_invdyn_finish!\n", err);
return(err);
mbs_msg(">>INVDYN>> Errors[%d] in mbs_invdyn_finish!\n", err1);
return(err1);
}
else if (err2 < 0) {
err = err2;
mbs_msg(">>INVDYN>> Errors[%d] in mbs_invdyn_finish!\n", err);
return(err);
mbs_msg(">>INVDYN>> Errors[%d] in mbs_invdyn_finish!\n", err2);
return(err2);
}
}
......@@ -941,20 +876,17 @@ int mbs_fct_invdyn(MbsData *s, MbsInvdyn *invd)
}
if (err1 < 0) { // Error management
err = _MBS_ERR_MOD_SPEC_11 + _MBS_ERR_MOD_INV_DYN;
err = _MBS_ERR_MOD_SPEC_11;
mbs_msg("\t >INVDYN> in mbs_fct_invdyn : Impossible to interpolate trajectory : positions \n");
return err;
}
else if (err2 < 0) { // Error management
err = _MBS_ERR_MOD_SPEC_11 + _MBS_ERR_MOD_INV_DYN;
err = _MBS_ERR_MOD_SPEC_11;
mbs_msg("\t >INVDYN> in mbs_fct_invdyn : Impossible to interpolate trajectory : velocities \n");
return err;
}
else if (err3 < 0) { // Error management
err = _MBS_ERR_MOD_SPEC_11 + _MBS_ERR_MOD_INV_DYN;
err = _MBS_ERR_MOD_SPEC_11;
mbs_msg("\t >INVDYN> in mbs_fct_invdyn : Impossible to interpolate trajectory : accelerations \n");
return err;
}
......@@ -964,10 +896,7 @@ int mbs_fct_invdyn(MbsData *s, MbsInvdyn *invd)
if (err < 0) // Error management
{
err += _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> Error during inverse dynamics at time %g s !\n", s->tsim);
return err;
}
return err;
......
......@@ -213,7 +213,7 @@ extern "C" {
* Error specific to the modules:
*
* Part :
* -11 :
* -11 : Number of coordinates is not consistent between trajectories file
* -12 : LU tot = wrong choice of qu and qv
* -13 :
* -14 :
......@@ -271,8 +271,8 @@ extern "C" {
* -11 : Interpolation of trajectories
* -12 : Not compatible options detected during the run (should not occur)
* -13 :
* -14 :
* -15 :
* -14 : Provided trajectories number of variable does not match : total joint or independant joint
* -15 : Impossible to interpolate trajectory
* -16 :
* -17 :
* -18 :
......
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