Skip to content

GitLab

Commit 39ff3931 authored by Sebastien Timmermans's avatar Sebastien Timmermans 🎹
Browse files

[ErrMsg] solve/inv kin mbs_error call

parent e2029e62
Hide whitespace changes
Inline Side-by-side
MBsysC/mbs_common/mbs_module/mbs_solvekin.c
View file @ 39ff3931
......@@ -105,7 +105,8 @@ int mbs_run_solvekin(MbsSolvekin* sk, MbsData* mbs_data)
err = mbs_solvekin_init(sk, mbs_data);
if (err < 0){
mbs_solvekin_finish(sk, mbs_data);
return err;
mbs_error_msg("[%d] in mbs_solvekin_loop !! \n", _MBS_ERR_MOD_INV_KIN + err);
return _MBS_ERR_MOD_INV_KIN + err;
}
......@@ -114,13 +115,19 @@ int mbs_run_solvekin(MbsSolvekin* sk, MbsData* mbs_data)
err = mbs_solvekin_loop(sk, mbs_data);
if (err < 0){
mbs_solvekin_finish(sk, mbs_data);
return err;
mbs_error_msg("[%d] in mbs_solvekin_loop !! \n", _MBS_ERR_MOD_INV_KIN + err);
return _MBS_ERR_MOD_INV_KIN + err;
}
// 3. Finish the simulation
// - - - - - - - - - - - -
err = mbs_solvekin_finish(sk, mbs_data);
if (err < 0) {
mbs_error_msg("[%d] in mbs_solvekin_finish !! \n", _MBS_ERR_MOD_INV_KIN + err);
return _MBS_ERR_MOD_INV_KIN + err;
}
return err;
}
......@@ -151,28 +158,18 @@ int mbs_solvekin_save(MbsSolvekin* sk, MbsData *mbs_data, double t) {
if (err1 < 0 && err2 <0){ // Error management
err1 += _MBS_ERR_MOD_INV_KIN;
err2 += _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>> mbs_solvekin_save: error [%d] while buffering the modules results.\n", err1);
mbs_msg(">>SOLVEKIN>> mbs_solvekin_save: error [%d] while buffering the user results.\n", err2);
return err1; // arbitrary choice
}
else if (err1 < 0)
{
err1 += _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>> mbs_solvekin_save: error [%d] while buffering the modules results.\n", err);
return err1;
}
else if (err2 < 0) // Error management
{
err2 += _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>> mbs_solvekin_save: error [%d] while buffering the user results.\n", err2);
return err2;
}
......@@ -194,34 +191,23 @@ int mbs_solvekin_init(MbsSolvekin* sk, MbsData* mbs_data)
// Checking mbs_data coherence
err = mbs_check_mbs_data_values(sk->mbs_aux, mbs_data);
if (err < 0) {
err += _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>> Incoherences detected during module initialization! (See message above) \n");
return err;
}
// Relevance of the analysis
if (!(mbs_data->Ncons > 0)) { // Error management
err += _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>>> This analysis is irrelevant in absence of constraints.\n");
return err;
return _MBS_ERR_MOD_SPEC_11;
}
if (mbs_data->nqv == 0) { // Error management
err += _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>>> This analysis is irrelevant in absence of dependant variable.\n");
return err;
return _MBS_ERR_MOD_SPEC_12;
}
// Checking constraints and dependant variable coherence
err = mbs_check_nhu_nqv(mbs_data);
if (err < 0) {
err += _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>> 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);
......@@ -244,11 +230,8 @@ int mbs_solvekin_init(MbsSolvekin* sk, MbsData* mbs_data)
if (sk->options->motion == trajectory) { // Error management
if (sk->options->trajectoryqname == NULL || sk->options->trajectoryqdname == NULL || sk->options->trajectoryqddname == NULL) {
if (mbs_data->nqu != 0) {
err = _MBS_ERR_INIT + _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>> No trajectory (q, qd or qdd) provided for independant joints.\n");
return err;
return _MBS_ERR_INIT;
}
sk->trajectorytype = 1;
sk->q = NULL;
......@@ -261,55 +244,37 @@ int mbs_solvekin_init(MbsSolvekin* sk, MbsData* mbs_data)
mbs_data->dt0 = sk->options->dt;
sk->tsim = sk->options->t0;
if (mbs_data->dt0 <= 0.0) { // Error management
err = _MBS_ERR_INIT + _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>> Time step has to be stricly positive (dt0>0) "
"for a fully driven kinematic 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_KIN;
mbs_msg(">>SOLVEKIN>> initial time has to be stricly lower than final time (t0<tf).\n");
return err;
return _MBS_ERR_INIT;
}
}
else {
sk->q = mbs_lut_res_load_file(sk->options->trajectoryqname);
if (!sk->q){
err = _MBS_ERR_LOW_FILES + _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>> Unable to load coordinates trajectory file.\n");
return err;
return _MBS_ERR_LOW_FILES;
}
sk->qd = mbs_lut_res_load_file(sk->options->trajectoryqdname);
if (!sk->qd){
err = _MBS_ERR_LOW_FILES + _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>> Unable to load velocities trajectory file.\n");
return err;
return _MBS_ERR_LOW_FILES;
}
sk->qdd = mbs_lut_res_load_file(sk->options->trajectoryqddname);
if (!sk->qdd){
err = _MBS_ERR_LOW_FILES + _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>> Unable to load accelerations trajectory file.\n");
return err;
return _MBS_ERR_LOW_FILES;
}
if (sk->q->nq != sk->qd->nq || sk->q->nq != sk->qdd->nq) { // Error management
err = _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>> Number of coordinates is not consistent between trajectories files.\n");
return err;
return _MBS_ERR_MOD_SPEC_12;
}
if (sk->q->nq == mbs_data->njoint) {
......@@ -323,13 +288,11 @@ int mbs_solvekin_init(MbsSolvekin* sk, MbsData* mbs_data)
sk->trajectorytype = 3;
}
else { // Error management
err = _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>> Provided trajectories number of variable (%d) does not match neither:\n", sk->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 (sk->options->dt <= 0) {
......@@ -347,29 +310,20 @@ int mbs_solvekin_init(MbsSolvekin* sk, MbsData* mbs_data)
mbs_data->dt0 = sk->options->dt;
sk->tsim = sk->options->t0;
if (mbs_data->t0 < sk->q->tq[0][0]) { // Error management
err = _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>> 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_INIT;
}
if (mbs_data->tf > sk->q->tq[sk->q->nt - 1][0]) { // Error management
err = _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>> 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_INIT;
}
if (mbs_data->t0 >= mbs_data->tf) { // Error management
err = _MBS_ERR_INIT + _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>> initial time has to be stricly lower than final time (t0<tf).\n");
return err;
return _MBS_ERR_INIT;
}
}
}
......@@ -549,7 +503,6 @@ int mbs_solvekin_loop(MbsSolvekin* sk, MbsData* mbs_data)
{
mbs_msg(">>SOLVEKIN>> Error during inverse kinematic analysis at time %g for oneshot case !\n", mbs_data->tsim);
mbs_msg(" >> [%d] in mbs_solvekin_loop !! \n", err);
return err;
}
......@@ -558,36 +511,33 @@ int mbs_solvekin_loop(MbsSolvekin* sk, MbsData* mbs_data)
// to allow MBsysPAD visualization.
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_KIN;
err = _MBS_ERR_LOW_NAN ;
mbs_msg(">>SOLVEKIN>> q[%d] is Nan! \n", i);
}
if (isnan(mbs_data->qd[i])) {
err = _MBS_ERR_LOW_NAN + _MBS_ERR_MOD_INV_KIN;
err = _MBS_ERR_LOW_NAN;
mbs_msg(">>SOLVEKIN>> qd[%d] is Nan! \n", i);
}
if (isnan(mbs_data->qdd[i])) {
err = _MBS_ERR_LOW_NAN + _MBS_ERR_MOD_INV_KIN;
err = _MBS_ERR_LOW_NAN;
mbs_msg(">>SOLVEKIN>> qdd[%d] is Nan! \n", i);
}
}
if (err < 0){
err2 = mbs_solvekin_write_buffers(sk);
if (err2 < 0) {
err2 = err2 + _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>> Error [%d] while saving results to disk !\n", err2);
}
mbs_error_msg("[%d] in mbs_solvekin_loop !! \n", err);
mbs_msg("[%d] in mbs_solvekin_loop !! \n", err);
return err;
}
// adding to buffer, continue the loop if more than one step.
if (sk->options->motion == oneshot) {
mbs_solvekin_save(sk, mbs_data, mbs_data->tsim);
return err;
err = mbs_solvekin_save(sk, mbs_data, mbs_data->tsim);
}
else {
mbs_solvekin_save(sk, mbs_data, iter);
err = mbs_solvekin_save(sk, mbs_data, iter);
}
}
}
......@@ -608,8 +558,6 @@ int mbs_solvekin_loop(MbsSolvekin* sk, MbsData* mbs_data)
err2 = mbs_solvekin_write_buffers(sk);
if (err2 < 0) {
err2 += _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>> Error [%d] while saving results to disk !\n", err2);
}
......@@ -622,23 +570,21 @@ int mbs_solvekin_loop(MbsSolvekin* sk, MbsData* mbs_data)
// to allow MBsysPAD visualization.
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_KIN;
err = _MBS_ERR_LOW_NAN;
mbs_msg(">>SOLVEKIN>> q[%d] is Nan! \n", i);
}
if (isnan(mbs_data->qd[i])) {
err = _MBS_ERR_LOW_NAN + _MBS_ERR_MOD_INV_KIN;
err = _MBS_ERR_LOW_NAN;
mbs_msg(">>SOLVEKIN>> qd[%d] is Nan! \n", i);
}
if (isnan(mbs_data->qdd[i])) {
err = _MBS_ERR_LOW_NAN + _MBS_ERR_MOD_INV_KIN;
err = _MBS_ERR_LOW_NAN;
mbs_msg(">>SOLVEKIN>> qdd[%d] is Nan! \n", i);
}
}
if (err < 0){
err2 = mbs_solvekin_write_buffers(sk);
if (err2 < 0) {
err2 = err2 + _MBS_ERR_MOD_INV_KIN;
mbs_msg(">>SOLVEKIN>> Error [%d] while saving results to disk !\n", err2);
}
mbs_msg("[%d] in mbs_solvekin_loop !! \n", err);
......@@ -689,8 +635,6 @@ int mbs_solvekin_finish(MbsSolvekin* sk, MbsData* mbs_data) {
err1 = mbs_solvekin_write_buffers(sk);
if (err1 < 0) {
err1 += _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>SOLVEKIN>> Error [%d] while saving results (module buffers) to disk !\n", err1);
}
if (sk->buffers){
......@@ -707,8 +651,6 @@ int mbs_solvekin_finish(MbsSolvekin* sk, MbsData* mbs_data) {
err2 = mbs_growing_buffer_write(sk->user_buffer);
if (err2 < 0) {
err2 += _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>SOLVEKIN>> Error [%d] while saving results (user buffers) to disk !\n", err2);
}
......@@ -724,22 +666,16 @@ int mbs_solvekin_finish(MbsSolvekin* sk, MbsData* mbs_data) {
// memory has been released, raise error
if (err1 < 0 && err2 < 0)
{
err = err1; // arbitrary choice
mbs_msg(">>SOLVEKIN>> Errors [%d] and [%d] in mbs_solvekin_finish!\n", err1, err2);
return(err);
return(err1);
}
else if (err1 < 0) {
err = err1;
mbs_msg(">>INVDYN>> Errors[%d] in mbs_solvekin_finish!\n", err);
return(err);
mbs_msg(">>INVDYN>> Errors[%d] in mbs_solvekin_finish!\n", err1);
return(err1);
}
else if (err2 < 0) {
err = err2;
mbs_msg(">>INVDYN>> Errors[%d] in mbs_solvekin_finish!\n", err);
return(err);
mbs_msg(">>INVDYN>> Errors[%d] in mbs_solvekin_finish!\n", err2);
return(err2);
}
}
......@@ -839,22 +775,16 @@ int mbs_fct_solvekin(MbsData *s, MbsSolvekin *sk)
}
if (err1 < 0) { // Error management
err = _MBS_ERR_MOD_SPEC_11 + _MBS_ERR_MOD_INV_KIN;
mbs_msg("\t >SOLVEKIN> in mbs_fct_solvekin : Impossible to interpolate trajectory : positions \n");
return err;
return _MBS_ERR_MOD_SPEC_15;
}
else if (err2 < 0) { // Error management
err = _MBS_ERR_MOD_SPEC_11 + _MBS_ERR_MOD_INV_KIN;
mbs_msg("\t >SOLVEKIN> in mbs_fct_solvekin : Impossible to interpolate trajectory : velocities \n");
return err;
return _MBS_ERR_MOD_SPEC_15;
}
else if (err3 < 0) { // Error management
err = _MBS_ERR_MOD_SPEC_11 + _MBS_ERR_MOD_INV_KIN;
mbs_msg("\t >SOLVEKIN> in mbs_fct_solvekin : Impossible to interpolate trajectory : accelerations \n");
return err;
return _MBS_ERR_MOD_SPEC_15;
}
// Solving constraints, Ncons checked in initialization
......@@ -864,17 +794,18 @@ int mbs_fct_solvekin(MbsData *s, MbsSolvekin *sk)
sk->mbs_aux->n_iter_close_loop = err;
if (err < 0) // Error management
{
err += _MBS_ERR_MOD_INV_KIN;
mbs_msg("\t >SOLVEKIN> error while closing geometry in mbs_fct_solvekin \n");
return err;
}
}
else {
// We only do one iteration of N-R
err = mbs_step_close_geo(s, sk->mbs_aux);
return err;
if (err < 0)
{
mbs_msg("\t >SOLVEKIN> error while closing geometry in mbs_fct_solvekin for one step \n");
return err;
}
}
// Kinematic resolution
......
MBsysC/mbs_common/mbs_struct/mbs_errors_names.h
View file @ 39ff3931
......@@ -256,6 +256,17 @@ extern "C" {
* -18 : ThetaSC (unused)
* -19 :
*
* Inverse kinematics (solvekin):
* -11 : This analysis is irrelevant in absence of constraints
* -12 : This analysis is irrelevant in absence of dependant variable
* -13 : Number of coordinates is not consistent between trajectories files
* -14 : Provided trajectories number of variable does not match : total joint or independant joint
* -15 : Impossible to interpolate trajectory
* -16 :
* -17 :
* -18 :
* -19 :
*
* Inverse dynamics:
* -11 : Interpolation of trajectories
* -12 : Not compatible options detected during the run (should not occur)
......
Markdown is supported
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