Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
robotran
mbsysc
Commits
39ff3931
Commit
39ff3931
authored
Dec 18, 2020
by
Sébastien Timmermans
Browse files
[ErrMsg] solve/inv kin mbs_error call
parent
e2029e62
Changes
2
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
(
err
1
);
}
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)
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment