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
33e98f7f
Commit
33e98f7f
authored
Jun 17, 2021
by
Olivier Lantsoght
Browse files
[MBsysPy][MbsResults] Implement specific class into dirdyn, invdyn and solvekin
parent
c78f7350
Changes
3
Show whitespace changes
Inline
Side-by-side
MBsysC/mbs_interface/MBsysPy/mbsyspy/mbs_dirdyn.py
View file @
33e98f7f
...
...
@@ -29,6 +29,9 @@ from ..mbs_utilities import mbs_msg
from
.._mbsysc_loader.loadlibs
import
libmodules
from
.._mbsysc_loader.loadlibs
import
libutilities
# Import MBsysPy class
from
.mbs_results
import
MbsResult
# =============================================================================
# Global parameter of the current module
...
...
@@ -51,7 +54,7 @@ class MbsDirdyn(object):
Current integration step size.
mbs: MbsData
Instance of MbsData related to the analysis.
results: MbsResult
results:
MBsysPy.
MbsResult
Instance of MbsResult containing the results of the direct dynamics analysis.
symbolic_path: str
Path to the folder containing the symbolic functions(python modules)
...
...
@@ -212,6 +215,7 @@ class MbsDirdyn(object):
self
.
set_options
(
**
kwargs
)
error2
=
0
if
not
self
.
store_results
:
error
=
libmodules
.
mbs_run_dirdyn
(
self
.
mbs_dirdyn_ptr
,
self
.
mbs
.
mbs_data_ptr
)
else
:
...
...
@@ -230,28 +234,15 @@ class MbsDirdyn(object):
self
.
mbs_dirdyn_ptr
)
if
(
error
>=
0
and
self
.
get_options
(
"save2file"
)):
# Results(buffer) memory is kept BUT FILES WILL BE WRITTEN LATER
# Arrays are initialized to the time pointer so as to start index of joints at 1.
nb_joint
=
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
0
].
contents
.
nx
+
1
nb_steps
=
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
0
].
contents
.
index
if
nb_steps
==
0
:
# nb_steps is the number of steps not (yet) save to disk.
# If the value is 0 it means that all has been saved to disk.
# But all is also still in memory.
nb_steps
=
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
0
].
contents
.
size
# Load the joint coordinates to test if the whole integration is in memory
self
.
results
.
q
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
0
].
contents
.
tx
,
(
nb_steps
,
nb_joint
)))
# get time array from the q buffer
self
.
results
.
t
=
self
.
results
.
q
[:,
0
]
if
self
.
results
.
t
[
0
]
!=
self
.
get_options
(
"t0"
):
results_loaded
=
False
# Clear loaded data
self
.
results
.
q
=
[]
self
.
results
.
t
=
[]
# Results have to be loaded from file, extract files names.
if
error
>=
0
:
if
self
.
get_options
(
"save2file"
):
# Results(buffer) memory is kept BUT FILES WILL BE WRITTEN LATER.
results_loaded
=
self
.
results
.
load_results_from_buffer
(
self
.
mbs_dirdyn_ptr
,
self
.
get_options
(
"t0"
),
self
.
get_options
(
"resfilename"
))
# If failed to load from buffer, save the user output (and vector) name
if
not
results_loaded
:
results_filename
=
bytes_to_str
(
ctypes
.
string_at
(
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
0
].
contents
.
filename
))
# Check if user vector output have been defined
user_output_vector_filenames
=
[]
...
...
@@ -260,6 +251,7 @@ class MbsDirdyn(object):
for
i
in
range
(
nb_user_output_vector
):
vector_name
=
bytes_to_str
(
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
first_buffer_id
+
i
].
contents
.
filename
)
user_output_vector_filenames
.
append
(
os
.
path
.
basename
(
vector_name
))
# Check if user auto output have been used
user_output_filenames
=
[]
nbOutput
=
self
.
mbs_dirdyn_ptr
.
contents
.
user_buffer
.
contents
.
nx
...
...
@@ -267,63 +259,14 @@ class MbsDirdyn(object):
name
=
bytes_to_str
(
self
.
mbs_dirdyn_ptr
.
contents
.
user_buffer
.
contents
.
names
[
i
])
user_output_filenames
.
append
(
self
.
get_options
(
"resfilename"
)
+
'_'
+
name
+
'.res'
)
else
:
results_loaded
=
True
# The whole integration is in the memory, load all results.
self
.
results
.
qd
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
1
].
contents
.
tx
,
(
nb_steps
,
nb_joint
)))
self
.
results
.
qdd
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
2
].
contents
.
tx
,
(
nb_steps
,
nb_joint
)))
self
.
results
.
Qq
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
3
].
contents
.
tx
,
(
nb_steps
,
nb_joint
)))
buffer_id
=
4
if
self
.
mbs
.
Nux
and
self
.
get_options
(
"compute_all_uxd"
):
size2
=
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
buffer_id
].
contents
.
nx
+
1
self
.
results
.
ux
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
buffer_id
].
contents
.
tx
,
(
nb_steps
,
size2
)))
self
.
results
.
uxd
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
buffer_id
+
1
].
contents
.
tx
,
(
nb_steps
,
size2
)))
buffer_id
=
buffer_id
+
2
if
self
.
mbs
.
Nlink
:
size2
=
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
buffer_id
].
contents
.
nx
+
1
self
.
results
.
Z
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
buffer_id
].
contents
.
tx
,
(
nb_steps
,
size2
)))
self
.
results
.
Zd
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
buffer_id
+
1
].
contents
.
tx
,
(
nb_steps
,
size2
)))
self
.
results
.
Fl
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
buffer_id
+
2
].
contents
.
tx
,
(
nb_steps
,
size2
)))
buffer_id
=
buffer_id
+
3
if
self
.
mbs
.
nqc
:
self
.
results
.
Qc
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
buffer_id
].
contents
.
tx
,
(
nb_steps
,
nb_joint
)))
buffer_id
=
buffer_id
+
1
if
self
.
mbs
.
nhu
:
self
.
results
.
Lambda
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
buffer_id
].
contents
.
tx
,
(
nb_steps
,
nb_joint
)))
buffer_id
=
buffer_id
+
1
nb_user_output_vector
=
libutilities
.
get_output_vector_nb
()
for
i
in
range
(
nb_user_output_vector
):
vector_size
=
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
buffer_id
+
i
].
contents
.
nx
+
1
user_out
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
buffer_id
+
i
].
contents
.
tx
,
(
nb_steps
,
vector_size
)))
# Name of the output vector should be recoverable with function
# 'libutilities.get_output_vector_label(i)' returning char*.
# However on MacOs (see comments of merge request !383) this
# does not work (pointer seems to be random).
name
=
bytes_to_str
(
os
.
path
.
basename
(
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
buffer_id
+
i
].
contents
.
filename
)[:
-
4
])
len_prefix
=
len
(
self
.
get_options
(
"resfilename"
))
name
=
name
[
len_prefix
+
1
:]
self
.
results
.
outputs
[
name
]
=
user_out
if
self
.
mbs_dirdyn_ptr
.
contents
.
user_buffer
.
contents
.
nx
:
size
=
self
.
mbs_dirdyn_ptr
.
contents
.
user_buffer
.
contents
.
index
if
not
size
:
size
=
self
.
mbs_dirdyn_ptr
.
contents
.
user_buffer
.
contents
.
size
nbOutput
=
self
.
mbs_dirdyn_ptr
.
contents
.
user_buffer
.
contents
.
nx
for
i
in
range
(
nbOutput
):
user_out
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_dirdyn_ptr
.
contents
.
user_buffer
.
contents
.
X
[
i
],
(
1
,
size
)))
name
=
bytes_to_str
(
self
.
mbs_dirdyn_ptr
.
contents
.
user_buffer
.
contents
.
names
[
i
])
self
.
results
.
outputs
[
name
]
=
user_out
[
0
]
if
(
error
>=
0
):
libmodules
.
mbs_dirdyn_finish
(
self
.
mbs_dirdyn_ptr
,
self
.
mbs
.
mbs_data_ptr
)
# Finalize the simulation (includes writing results files to disk)
error2
=
libmodules
.
mbs_dirdyn_finish
(
self
.
mbs_dirdyn_ptr
,
self
.
mbs
.
mbs_data_ptr
)
if
self
.
get_options
(
"save2file"
)
and
not
results_loaded
:
# Load results from file if required
if
error2
>=
0
and
self
.
get_options
(
"save2file"
)
and
not
results_loaded
:
mbs_msg
(
"The beginning of the integration is not available in the buffer.
\n
"
"The complete results are loaded from files.
\n
"
)
self
.
results
.
load_results_from_file
(
results_filename
,
module
=
6
,
self
.
results
.
load_results_from_file
(
results_filename
,
user_output
=
user_output_filenames
,
user_vector
=
user_output_vector_filenames
)
...
...
@@ -352,6 +295,7 @@ class MbsDirdyn(object):
" Excessive force can generate infinte acceleration.
\n
"
" - The parameters of the integrator are not compatible with the system dynamics.
\n
"
" - The integrator is not adapted to the current system dynamics;
\n
"
" - The outputs folders does not exists (by default 'resultsR' and 'animationR');
\n
"
"
\n
"
"If the simulation runs a little you should:
\n
"
" - In all cases, open the results/animation file and check the motion of the
\n
"
...
...
@@ -361,6 +305,15 @@ class MbsDirdyn(object):
"
\n
--------------------------------------------------
\n
"
"
\n
--------------------------------------------------
\n
"
)
raise
RuntimeError
(
"MbsDirdyn.run() failed, read previous messages."
)
if
error2
<
0
:
mbs_msg
(
"
\n
--------------------------------------------------
\n
"
"READ CAREFULLY !!!
\n
"
"--------------------------------------------------
\n\n
"
"An error occurs during direct dynamic module finalization process.
\n
"
"It means that one or more results files (or animation file) was not saved to disk.
\n
"
"The messages above give deeper informations on what went wrong.
\n
"
"Check the existence of outputs folders, writing wrigth and free space.
\n
"
)
raise
RuntimeError
(
"MbsDirdyn.run() failed during mbs_dirdyn_finish, read previous messages."
)
return
self
.
results
...
...
@@ -911,229 +864,3 @@ class MbsDirdyn(object):
def
buffer_nb
(
self
):
"""Access to `bufferNb` attribute (read-only)."""
return
self
.
mbs_dirdyn_ptr
.
contents
.
bufferNb
class
MbsResult
(
object
):
"""
Class containing results.
The user-specified vector are not available.
Attributes
----------
q: ndarray
Numpy array containing the current values of the generalized
coordinates.
qd: ndarray
Numpy array containing the current values of the generalized
velocities.
qdd: ndarray
Numpy array containing the current values of the generalized
accelerations.
Qq: ndarray
Numpy array containing the values of the joint forces.
Qc: ndarray
Numpy array containing the value of joint force introduced in driven
joints to respect the user function
qa: ndarray of int
Numpy array of integers containing the indices of actuated
articulations(only for inverse dynamic). Those articulations are
controlled by an actuator.
t: ndarray
Numpy array containing the values of the time vector
ux: ndarray
Numpy array containing the values of the user variables.
uxd: ndarray
Numpy array containing the values of the time derivative of the user
variables.
Fl: ndarray
Numpy array containing the current values of the forces between of
the points of a link.
Z: ndarray
Numpy array containing the current values of the distances between of
the points of a link.
Zd: ndarray
Numpy array containing the current values of the speed(spreading)
between of the points of a link.
outputs: dict
Dict containing the names and the values of the user outputs
mbs: MbsData
instance of MbsData from where results come from
"""
def
__init__
(
self
,
mbs
):
"""Initialize the fields to empty list or dict."""
self
.
q
=
[]
self
.
qd
=
[]
self
.
qdd
=
[]
self
.
Fl
=
[]
self
.
Z
=
[]
self
.
Zd
=
[]
self
.
Qq
=
[]
self
.
Qa
=
[]
self
.
t
=
[]
self
.
ux
=
[]
self
.
uxd
=
[]
self
.
Qc
=
[]
self
.
outputs
=
{}
self
.
mbs
=
mbs
def
load_results_from_file
(
self
,
filename
,
result_path
=
"resultsR"
,
module
=
0
,
user_output
=
None
,
user_vector
=
None
):
"""
Load the results from the files into a MbsResult instance.
This function is called if the buffers don not contain the full simulation.
Parameters
----------
filename : str
The resfilename containing the results to load. The required suffix
(ie. "_q.res") will be added automatically
result_path : str, optional
The relative path of the result folder from the project folder.
default is "resultsR"
module : int
The module in which this function is called. In some modules, some
results files don't exist. Module ids corresponds to the
'MbsData.process' value. If set to 0 we try to load all files
default is 0
user_output : list
List containing the user output files to be loaded if not None.
default is None.
user_vector : list
List containing the user vector output files to be loaded if not None.
default is None.
"""
project_path
=
self
.
mbs
.
project_path
baseFileName
=
os
.
path
.
basename
(
filename
)
baseFileName
=
baseFileName
[:
-
6
]
result_path
=
os
.
path
.
join
(
project_path
,
result_path
)
# Error handeling
if
not
os
.
path
.
isdir
(
result_path
):
if
__DEBUG__
:
mbs_msg
(
'DEBUG>> The result directory does not exist: "'
+
result_path
+
'"'
)
# Generalized coordinates
CurFile
=
baseFileName
+
'_q.res'
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
result_path
,
CurFile
))
if
(
os
.
path
.
isfile
(
path
)):
self
.
q
=
np
.
loadtxt
(
path
)
if
np
.
array
(
self
.
q
).
ndim
>
1
:
self
.
t
=
self
.
q
[:,
0
]
else
:
self
.
t
=
self
.
q
[
0
]
else
:
if
__DEBUG__
:
mbs_msg
(
"DEBUG>> file '"
+
CurFile
+
"' not found in folder '"
+
os
.
path
.
dirname
(
path
))
# Generalized velocities
CurFile
=
baseFileName
+
'_qd.res'
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
result_path
,
CurFile
))
if
(
os
.
path
.
isfile
(
path
)):
self
.
qd
=
np
.
loadtxt
(
path
)
else
:
if
__DEBUG__
:
mbs_msg
(
"DEBUG>> file '"
+
CurFile
+
"' not found in folder '"
+
os
.
path
.
dirname
(
path
))
# Generalized accelerations
CurFile
=
baseFileName
+
'_qdd.res'
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
result_path
,
CurFile
))
if
(
os
.
path
.
isfile
(
path
)):
self
.
qdd
=
np
.
loadtxt
(
path
)
else
:
if
__DEBUG__
:
mbs_msg
(
"DEBUG>> file '"
+
CurFile
+
"' not found in folder '"
+
os
.
path
.
dirname
(
path
))
if
module
!=
5
:
CurFile
=
baseFileName
+
'_Qq.res'
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
result_path
,
CurFile
))
if
(
os
.
path
.
isfile
(
path
)):
self
.
Qq
=
np
.
loadtxt
(
path
)
else
:
if
__DEBUG__
:
mbs_msg
(
"DEBUG>> file '"
+
CurFile
+
"' not found in folder '"
+
os
.
path
.
dirname
(
path
))
# Generalized user state
if
self
.
mbs
.
Nux
and
(
module
!=
5
and
module
!=
6
):
CurFile
=
baseFileName
+
'_ux.res'
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
result_path
,
CurFile
))
if
(
os
.
path
.
isfile
(
path
)):
self
.
ux
=
np
.
loadtxt
(
path
)
else
:
if
__DEBUG__
:
mbs_msg
(
"DEBUG>> file '"
+
CurFile
+
"' not found in folder '"
+
os
.
path
.
dirname
(
path
))
CurFile
=
baseFileName
+
'_uxd.res'
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
result_path
,
CurFile
))
if
(
os
.
path
.
isfile
(
path
)):
self
.
uxd
=
np
.
loadtxt
(
path
)
else
:
if
__DEBUG__
:
mbs_msg
(
"DEBUG>> file '"
+
CurFile
+
"' not found in folder '"
+
os
.
path
.
dirname
(
path
))
# Link
if
self
.
mbs
.
Nlink
and
module
!=
5
:
CurFile
=
baseFileName
+
'_linkF.res'
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
result_path
,
CurFile
))
if
(
os
.
path
.
isfile
(
path
)):
self
.
Fl
=
np
.
loadtxt
(
path
)
else
:
if
__DEBUG__
:
mbs_msg
(
"DEBUG>> file '"
+
CurFile
+
"' not found in folder '"
+
os
.
path
.
dirname
(
path
))
CurFile
=
baseFileName
+
'_linkZ.res'
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
result_path
,
CurFile
))
if
(
os
.
path
.
isfile
(
path
)):
self
.
Z
=
np
.
loadtxt
(
path
)
else
:
if
__DEBUG__
:
mbs_msg
(
"DEBUG>> file '"
+
CurFile
+
"' not found in folder '"
+
os
.
path
.
dirname
(
path
))
CurFile
=
baseFileName
+
'_linkZd.res'
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
result_path
,
CurFile
))
if
(
os
.
path
.
isfile
(
path
)):
self
.
Zd
=
np
.
loadtxt
(
path
)
else
:
if
__DEBUG__
:
mbs_msg
(
"DEBUG>> file '"
+
CurFile
+
"' not found in folder '"
+
os
.
path
.
dirname
(
path
))
# Qc
if
self
.
mbs
.
nqc
and
module
!=
5
:
CurFile
=
baseFileName
+
'_Qc.res'
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
result_path
,
CurFile
))
if
(
os
.
path
.
isfile
(
path
)):
self
.
Qc
=
np
.
loadtxt
(
path
)
else
:
if
__DEBUG__
:
mbs_msg
(
"DEBUG>> file '"
+
CurFile
+
"' not found in folder '"
+
os
.
path
.
dirname
(
path
))
# Qa
if
self
.
mbs
.
nqa
and
(
module
!=
5
and
module
!=
3
):
CurFile
=
baseFileName
+
'_Qa.res'
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
result_path
,
CurFile
))
if
(
os
.
path
.
isfile
(
path
)):
self
.
Qa
=
np
.
loadtxt
(
path
)
else
:
if
__DEBUG__
:
mbs_msg
(
"DEBUG>> file '"
+
CurFile
+
"' not found in folder '"
+
os
.
path
.
dirname
(
path
))
if
user_output
:
for
CurFile
in
user_output
:
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
result_path
,
CurFile
))
name
=
CurFile
[
len
(
baseFileName
)
+
1
:
-
4
]
if
(
os
.
path
.
isfile
(
path
)):
self
.
outputs
[
name
]
=
np
.
loadtxt
(
path
)[:,
1
:]
else
:
if
__DEBUG__
:
mbs_msg
(
"DEBUG>> file '"
+
CurFile
+
"' not found in folder '"
+
os
.
path
.
dirname
(
path
))
if
user_vector
:
for
CurFile
in
user_vector
:
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
result_path
,
CurFile
))
name
=
CurFile
[
len
(
baseFileName
)
+
1
:
-
4
]
if
(
os
.
path
.
isfile
(
path
)):
self
.
outputs
[
name
]
=
np
.
loadtxt
(
path
)
else
:
if
__DEBUG__
:
mbs_msg
(
"DEBUG>> file '"
+
CurFile
+
"' not found in folder '"
+
os
.
path
.
dirname
(
path
))
MBsysC/mbs_interface/MBsysPy/mbsyspy/mbs_invdyn.py
View file @
33e98f7f
...
...
@@ -25,10 +25,11 @@ from ..mbs_utilities import str_to_bytes
from
..mbs_utilities
import
mbs_msg
# importing MbsysPy classes
from
.mbs_
dirdyn
import
MbsResult
from
.mbs_
results
import
MbsResult
# importing libraries
from
.._mbsysc_loader.loadlibs
import
libmodules
from
.._mbsysc_loader.loadlibs
import
libutilities
# =============================================================================
# Global parameter of the current module
...
...
@@ -197,9 +198,10 @@ class MbsInvdyn(object):
self
.
set_options
(
**
kwargs
)
error2
=
0
# Default value
if
not
self
.
store_results
:
error
=
libmodules
.
mbs_run_invdyn
(
self
.
mbs_invdyn_ptr
,
self
.
mbs
.
mbs_data_ptr
)
error2
=
0
# Unused for this module
else
:
# save2file forced to 1 because if buffers don't have the complete
# results, results are loaded from files.
...
...
@@ -210,17 +212,39 @@ class MbsInvdyn(object):
results_loaded
=
False
# Results(buffer) memory is kept BUT FILES WILL BE WRITTEN LATER
if
error
>=
0
and
self
.
get_options
(
"save2file"
):
results_loaded
=
self
.
_load_results
()
if
error
>=
0
:
if
self
.
get_options
(
"save2file"
):
results_loaded
=
self
.
results
.
load_results_from_buffer
(
self
.
mbs_invdyn_ptr
,
self
.
get_options
(
"t0"
),
self
.
get_options
(
"resfilename"
))
# If failed to load from buffer, save the user output (and vector) name
if
not
results_loaded
:
results_filename
=
bytes_to_str
(
ctypes
.
string_at
(
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
0
].
contents
.
filename
))
# Check if user vector output have been defined
user_output_vector_filenames
=
[]
nb_user_output_vector
=
libutilities
.
get_output_vector_nb
()
first_buffer_id
=
self
.
buffer_nb
-
nb_user_output_vector
for
i
in
range
(
nb_user_output_vector
):
vector_name
=
bytes_to_str
(
self
.
mbs_dirdyn_ptr
.
contents
.
buffers
[
first_buffer_id
+
i
].
contents
.
filename
)
user_output_vector_filenames
.
append
(
os
.
path
.
basename
(
vector_name
))
# Check if user auto output have been used
user_output_filenames
=
[]
nbOutput
=
self
.
mbs_dirdyn_ptr
.
contents
.
user_buffer
.
contents
.
nx
for
i
in
range
(
nbOutput
):
name
=
bytes_to_str
(
self
.
mbs_dirdyn_ptr
.
contents
.
user_buffer
.
contents
.
names
[
i
])
user_output_filenames
.
append
(
self
.
get_options
(
"resfilename"
)
+
'_'
+
name
+
'.res'
)
# finish function is required to close the module and write the results to disk.
error2
=
libmodules
.
mbs_invdyn_finish
(
self
.
mbs_invdyn_ptr
,
self
.
mbs
.
mbs_data_ptr
)
if
error2
>=
0
and
error
>=
0
and
not
results_loaded
:
if
error2
>=
0
and
self
.
get_options
(
"save2file"
)
and
not
results_loaded
:
mbs_msg
(
"The beginning of the integration is not available in the buffer.
\n
"
"The complete results are loaded from files.
\n
"
)
filename
=
bytes_to_str
(
ctypes
.
string_at
(
self
.
mbs_invdyn_ptr
.
contents
.
buffers
[
0
].
contents
.
filename
))
self
.
results
.
load_results_from_file
(
filename
,
module
=
6
)
self
.
results
.
load_results_from_file
(
results_filename
,
user_output
=
user_output_filenames
,
user_vector
=
user_output_vector_filenames
)
# Unassign user functions
if
self
.
mbs
.
opt_load_c
<
2
:
...
...
@@ -252,69 +276,6 @@ class MbsInvdyn(object):
return
self
.
results
def
_load_results
(
self
):
"""
Load the results from the buffers.
If the beginning of the integration is not available in the buffers,
the complete results are loaded from files.
Returns
-------
bool
True if the results have been fully loaded from memory.
False if the results must be loaded from files.
"""
# c_mbs_invdyn_write_buffers(self.mbs_invdyn_ptr)
size1
=
self
.
mbs_invdyn_ptr
.
contents
.
buffers
[
0
].
contents
.
index
if
size1
==
0
:
size1
=
self
.
mbs_invdyn_ptr
.
contents
.
buffers
[
0
].
contents
.
size
size2
=
self
.
mbs_invdyn_ptr
.
contents
.
buffers
[
0
].
contents
.
nx
+
1
# array are initialized to the time pointer so as to start index of joints at 1(we have to ensure contiguity between t and x in buffers ! ! !)
self
.
results
.
q
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_invdyn_ptr
.
contents
.
buffers
[
0
].
contents
.
tx
,
(
size1
,
size2
)))
# get time array from the q buffer
self
.
results
.
t
=
self
.
results
.
q
[:,
0
]
if
not
self
.
results
.
t
[
0
]
==
self
.
get_options
(
"t0"
):
self
.
results
.
q
=
[]
self
.
results
.
t
=
[]
return
False
# get qd and qdd buffer
self
.
results
.
qd
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_invdyn_ptr
.
contents
.
buffers
[
1
].
contents
.
tx
,
(
size1
,
size2
)))
self
.
results
.
qdd
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_invdyn_ptr
.
contents
.
buffers
[
2
].
contents
.
tx
,
(
size1
,
size2
)))
size2
=
self
.
mbs_invdyn_ptr
.
contents
.
buffers
[
3
].
contents
.
nx
+
1
self
.
results
.
Qq
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_invdyn_ptr
.
contents
.
buffers
[
3
].
contents
.
tx
,
(
size1
,
size2
)))
if
self
.
module_name
==
"MbsInvdyn"
:
size2
=
self
.
mbs_invdyn_ptr
.
contents
.
buffers
[
4
].
contents
.
nx
+
1
self
.
results
.
Qa
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_invdyn_ptr
.
contents
.
buffers
[
4
].
contents
.
tx
,
(
size1
,
size2
)))
buffer_id
=
5
else
:
buffer_id
=
4
if
self
.
mbs
.
Nlink
:
size2
=
self
.
mbs_invdyn_ptr
.
contents
.
buffers
[
buffer_id
].
contents
.
nx
+
1
self
.
results
.
Z
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_invdyn_ptr
.
contents
.
buffers
[
buffer_id
].
contents
.
tx
,
(
size1
,
size2
)))
self
.
results
.
Zd
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_invdyn_ptr
.
contents
.
buffers
[
buffer_id
+
1
].
contents
.
tx
,
(
size1
,
size2
)))
self
.
results
.
Fl
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_invdyn_ptr
.
contents
.
buffers
[
buffer_id
+
2
].
contents
.
tx
,
(
size1
,
size2
)))
buffer_id
=
buffer_id
+
3
if
self
.
mbs
.
nqc
:
size2
=
self
.
mbs_invdyn_ptr
.
contents
.
buffers
[
buffer_id
].
contents
.
nx
+
1
self
.
results
.
Qc
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_invdyn_ptr
.
contents
.
buffers
[
buffer_id
].
contents
.
tx
,
(
size1
,
size2
)))
buffer_id
=
buffer_id
+
1
if
self
.
mbs
.
nhu
:
size2
=
self
.
mbs_invdyn_ptr
.
contents
.
buffers
[
buffer_id
].
contents
.
nx
+
1
self
.
results
.
Lambda
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_invdyn_ptr
.
contents
.
buffers
[
buffer_id
].
contents
.
tx
,
(
size1
,
size2
)))
if
self
.
mbs_invdyn_ptr
.
contents
.
user_buffer
.
contents
.
nx
:
size
=
self
.
mbs_invdyn_ptr
.
contents
.
user_buffer
.
contents
.
index
nbOutput
=
self
.
mbs_invdyn_ptr
.
contents
.
user_buffer
.
contents
.
nx
user_out
=
np
.
copy
(
np
.
ctypeslib
.
as_array
(
self
.
mbs_invdyn_ptr
.
contents
.
user_buffer
.
contents
.
X
[
0
],
(
nbOutput
,
size
)))
for
i
in
range
(
nbOutput
):
name
=
bytes_to_str
(
self
.
mbs_invdyn_ptr
.
contents
.
user_buffer
.
contents
.
names
[
i
])
self
.
results
.
outputs
[
name
]
=
user_out
[
i
,
:]
return
True
def
set_user_fct_from_file
(
self
,
function_name
,
user_path
,
user_file
):
"""
Load a user function from a file chosen by the user instead of the default one in the userfctR folder.
...
...
@@ -329,8 +290,6 @@ class MbsInvdyn(object):
path to the new user function file.
user_file : str
name of the new user function file.
"""
self
.
mbs
.
__set_user_fct_from_file__
(
function_name
,
user_path
,
user_file
)
...
...
@@ -346,7 +305,6 @@ class MbsInvdyn(object):
name of the user function to replace.
user_fct_ptr : ptr
new user function pointer.
"""
self
.
mbs
.
__set_user_fct_from_ptr__
(
function_name
,
user_fct_ptr
)
...
...
MBsysC/mbs_interface/MBsysPy/mbsyspy/mbs_solvekin.py
View file @
33e98f7f
...
...
@@ -25,12 +25,13 @@ from ..mbs_utilities import str_to_bytes
from
..mbs_utilities
import
mbs_msg
# importing MbsysPy classes
from
.mbs_
dirdyn
import
MbsResult
from
.mbs_
results
import
MbsResult
from
.mbs_invdyn
import
MbsInvdyn
# importing libraries
from
.._mbsysc_loader.loadlibs
import
libmodules
from
.._mbsysc_loader.loadlibs
import
libutilities
# =============================================================================
# Global parameter of the current module
...
...
@@ -162,9 +163,9 @@ class MbsSolvekin(MbsInvdyn):
self
.
set_options
(
**
kwargs
)
error_2
=
0
# Default value
if
not
self
.
store_results
:
error_1
=
libmodules
.
mbs_run_solvekin
(
self
.
mbs_solvekin_ptr
,
self
.
mbs
.
mbs_data_ptr
)
error_2
=
0
# Unused for this path
else
:
# save2file forced to 1 because if buffers don't have the complete
# results, results are loaded from files.
...
...
@@ -175,16 +176,38 @@ class MbsSolvekin(MbsInvdyn):
# Results (buffer) memory is kept BUT FILES WILL BE WRITTEN LATER
results_loaded
=
False