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
3249e824
Commit
3249e824
authored
Aug 06, 2020
by
Louis Beauloye
Browse files
Equil ok too
parent
9a4157a8
Changes
3
Hide whitespace changes
Inline
Side-by-side
MBsysC/mbs_interface/MBsysPy/mbsyspy/mbs_data.py
View file @
3249e824
...
...
@@ -797,7 +797,7 @@ class MbsData(object):
if
__DEBUG__
:
print
(
"DEBUG>> loading file '"
+
user_file
+
"' in folder '"
+
os
.
path
.
dirname
(
path
))
module
=
imp
.
load_source
(
user_file
[:
-
3
],
path
)
module
=
imp
.
load_source
(
user_file
[:
-
3
],
path
)
self
.
user_equil_init
=
module
.
user_equil_init
elif
fun
==
'equil_loop'
:
user_file
=
"user_equil.py"
...
...
@@ -809,7 +809,7 @@ class MbsData(object):
if
__DEBUG__
:
print
(
"DEBUG>> loading file '"
+
user_file
+
"' in folder '"
+
os
.
path
.
dirname
(
path
))
module
=
imp
.
load_source
(
user_file
[:
-
3
],
path
)
module
=
imp
.
load_source
(
user_file
[:
-
3
],
path
)
self
.
user_equil_loop
=
module
.
user_equil_loop
elif
fun
==
'equil_finish'
:
user_file
=
"user_equil.py"
...
...
@@ -821,7 +821,7 @@ class MbsData(object):
if
__DEBUG__
:
print
(
"DEBUG>> loading file '"
+
user_file
+
"' in folder '"
+
os
.
path
.
dirname
(
path
))
module
=
imp
.
load_source
(
user_file
[:
-
3
],
path
)
module
=
imp
.
load_source
(
user_file
[:
-
3
],
path
)
self
.
user_equil_finish
=
module
.
user_equil_finish
elif
fun
==
'equil_fxe'
:
...
...
@@ -834,12 +834,12 @@ class MbsData(object):
if
__DEBUG__
:
print
(
"DEBUG>> loading file '"
+
user_file
+
"' in folder '"
+
os
.
path
.
dirname
(
path
))
module
=
imp
.
load_source
(
user_file
[:
-
3
],
path
)
module
=
imp
.
load_source
(
user_file
[:
-
3
],
path
)
try
:
self
.
user_equil_fxe
=
module
.
user_equil_fxe
except
AttributeError
:
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
template_path
,
user_file
))
module
=
imp
.
load_source
(
user_file
[:
-
3
],
path
)
module
=
imp
.
load_source
(
user_file
[:
-
3
],
path
)
self
.
user_equil_fxe
=
module
.
user_equil_fxe
else
:
...
...
@@ -1033,37 +1033,72 @@ class MbsData(object):
self
.
mbs_data_ptr
.
contents
.
user_cons_jdqd
=
self
.
ptrs_to_user_fcts
[
"user_cons_jdqd"
]
else
:
print
(
' A module needs to be passed as mbs_module argument'
)
elif
fun
==
"
dirdyn_init
"
:
elif
fun
==
'
dirdyn_init
'
:
# user_dirdyn
if
mbs_module
is
not
None
:
self
.
ptrs_to_user_fcts
[
"user_dirdyn_init"
]
=
user_dirdyn_init_wrap
(
lambda
mbs
,
dd
:
self
.
user_dirdyn_init
(
self
,
mbs_module
))
self
.
mbs_data_ptr
.
contents
.
user_dirdyn_init
=
self
.
ptrs_to_user_fcts
[
"user_dirdyn_init"
]
if
isinstance
(
mbs_module
,
MbsDirdyn
):
self
.
ptrs_to_user_fcts
[
"user_dirdyn_init"
]
=
user_dirdyn_init_wrap
(
lambda
mbs
,
dd
:
self
.
user_dirdyn_init
(
self
,
mbs_module
))
self
.
mbs_data_ptr
.
contents
.
user_dirdyn_init
=
self
.
ptrs_to_user_fcts
[
"user_dirdyn_init"
]
else
:
print
(
'mbs_module must be MbsDirdyn to use dirdyn_init'
)
else
:
print
(
'Dirdyn module needs to be passed as mbs_module argument'
)
elif
fun
==
"
dirdyn_loop
"
:
elif
fun
==
'
dirdyn_loop
'
:
if
mbs_module
is
not
None
:
self
.
ptrs_to_user_fcts
[
"user_dirdyn_loop"
]
=
user_dirdyn_loop_wrap
(
lambda
mbs
,
dd
:
self
.
user_dirdyn_loop
(
self
,
mbs_module
))
self
.
mbs_data_ptr
.
contents
.
user_dirdyn_loop
=
self
.
ptrs_to_user_fcts
[
"user_dirdyn_loop"
]
if
isinstance
(
mbs_module
,
MbsDirdyn
):
self
.
ptrs_to_user_fcts
[
"user_dirdyn_loop"
]
=
user_dirdyn_loop_wrap
(
lambda
mbs
,
dd
:
self
.
user_dirdyn_loop
(
self
,
mbs_module
))
self
.
mbs_data_ptr
.
contents
.
user_dirdyn_loop
=
self
.
ptrs_to_user_fcts
[
"user_dirdyn_loop"
]
else
:
print
(
'mbs_module must be MbsDirdyn to use dirdyn_loop'
)
else
:
print
(
'Dirdyn module needs to be passed as mbs_module argument'
)
elif
fun
==
"
dirdyn_finish
"
:
elif
fun
==
'
dirdyn_finish
'
:
if
mbs_module
is
not
None
:
self
.
ptrs_to_user_fcts
[
"user_dirdyn_finish"
]
=
user_dirdyn_finish_wrap
(
lambda
mbs
,
dd
:
self
.
user_dirdyn_finish
(
self
,
mbs_module
))
self
.
mbs_data_ptr
.
contents
.
user_dirdyn_finish
=
self
.
ptrs_to_user_fcts
[
"user_dirdyn_finish"
]
if
isinstance
(
mbs_module
,
MbsDirdyn
):
self
.
ptrs_to_user_fcts
[
"user_dirdyn_finish"
]
=
user_dirdyn_finish_wrap
(
lambda
mbs
,
dd
:
self
.
user_dirdyn_finish
(
self
,
mbs_module
))
self
.
mbs_data_ptr
.
contents
.
user_dirdyn_finish
=
self
.
ptrs_to_user_fcts
[
"user_dirdyn_finish"
]
else
:
print
(
'mbs_module must be MbsDirdyn to use dirdyn_finish'
)
else
:
print
(
'Dirdyn module needs to be passed
ed
as mbs_module argument'
)
print
(
'Dirdyn module needs to be passed as mbs_module argument'
)
elif
fun
==
"equil_init"
:
# TODO
elif
fun
==
'equil_init'
:
# user_equil
self
.
ptrs_to_user_fcts
[
"user_equil_init"
]
=
user_equil_init_wrap
(
lambda
mbs
,
eq
:
self
.
user_equil_init
(
self
,
mbs_module
))
self
.
ptrs_to_user_fcts
[
"user_equil_loop"
]
=
user_equil_loop_wrap
(
lambda
mbs
,
eq
:
self
.
user_equil_loop
(
self
,
mbs_module
))
self
.
ptrs_to_user_fcts
[
"user_equil_finish"
]
=
user_equil_finish_wrap
(
lambda
mbs
,
eq
:
self
.
user_equil_finish
(
self
,
mbs_module
))
self
.
ptrs_to_symb_fcts
[
"user_equil_fxe"
]
=
user_equil_fxe_wrap
(
lambda
mbs
,
f
:
self
.
__callback_user_equil_fxe
(
mbs_module
,
self
.
user_equil_fxe
,
self
,
f
))
self
.
mbs_data_ptr
.
contents
.
user_equil_fxe
=
self
.
ptrs_to_symb_fcts
[
"user_equil_fxe"
]
self
.
mbs_data_ptr
.
contents
.
user_equil_init
=
self
.
ptrs_to_user_fcts
[
"user_equil_init"
]
self
.
mbs_data_ptr
.
contents
.
user_equil_loop
=
self
.
ptrs_to_user_fcts
[
"user_equil_loop"
]
self
.
mbs_data_ptr
.
contents
.
user_equil_finish
=
self
.
ptrs_to_user_fcts
[
"user_equil_finish"
]
if
mbs_module
is
not
None
:
if
isinstance
(
mbs_module
,
MbsEquil
):
self
.
ptrs_to_user_fcts
[
"user_equil_init"
]
=
user_equil_init_wrap
(
lambda
mbs
,
eq
:
self
.
user_equil_init
(
self
,
mbs_module
))
self
.
mbs_data_ptr
.
contents
.
user_equil_init
=
self
.
ptrs_to_user_fcts
[
"user_equil_init"
]
else
:
print
(
'mbs_module must be MbsEquil to use equil_init'
)
else
:
print
(
'Equil module needs to be passed as mbs_module argument'
)
elif
fun
==
'equil_loop'
:
if
mbs_module
is
not
None
:
if
isinstance
(
mbs_module
,
MbsEquil
):
self
.
ptrs_to_user_fcts
[
"user_equil_loop"
]
=
user_equil_loop_wrap
(
lambda
mbs
,
eq
:
self
.
user_equil_loop
(
self
,
mbs_module
))
self
.
mbs_data_ptr
.
contents
.
user_equil_loop
=
self
.
ptrs_to_user_fcts
[
"user_equil_loop"
]
else
:
print
(
'mbs_module must be MbsEquil to use equil_loop'
)
else
:
print
(
'Equil module needs to be passed as mbs_module argument'
)
elif
fun
==
'equil_finish'
:
if
mbs_module
is
not
None
:
if
isinstance
(
mbs_module
,
MbsEquil
):
self
.
ptrs_to_user_fcts
[
"user_equil_finish"
]
=
user_equil_finish_wrap
(
lambda
mbs
,
eq
:
self
.
user_equil_finish
(
self
,
mbs_module
))
self
.
mbs_data_ptr
.
contents
.
user_equil_finish
=
self
.
ptrs_to_user_fcts
[
"user_equil_finish"
]
else
:
print
(
'mbs_module must be MbsEquil to use equil_finish'
)
else
:
print
(
'Equil module needs to be passed as mbs_module argument'
)
elif
fun
==
'equil_fxe'
:
if
mbs_module
is
not
None
:
if
isinstance
(
mbs_module
,
MbsEquil
):
self
.
ptrs_to_symb_fcts
[
"user_equil_fxe"
]
=
user_equil_fxe_wrap
(
lambda
mbs
,
f
:
self
.
__callback_user_equil_fxe
(
mbs_module
,
self
.
user_equil_fxe
,
self
,
f
))
self
.
mbs_data_ptr
.
contents
.
user_equil_fxe
=
self
.
ptrs_to_symb_fcts
[
"user_equil_fxe"
]
else
:
print
(
'mbs_module must be MbsEquil to use equil_fxe'
)
else
:
print
(
'Equil module needs to be passed as mbs_module argument'
)
elif
fun
==
"derivative"
:
# derivative
self
.
ptrs_to_user_fcts
[
"user_Derivative"
]
=
user_Derivative_wrap
(
lambda
mbs
:
self
.
user_derivative
(
self
))
...
...
@@ -1181,9 +1216,14 @@ class MbsData(object):
# TODO use mbs_module as argument for phi
# invdyna
# not used in the MbsSolvekin module
self
.
ptrs_to_symb_fcts
[
"mbs_invdyna"
]
=
mbs_invdyna_wrap
(
lambda
phi
,
mbs
,
tsim
:
self
.
mbs_invdyna
(
self
.
_phi
,
self
,
tsim
))
self
.
mbs_invdyna
=
self
.
mbs_invdyna
self
.
mbs_data_ptr
.
contents
.
mbs_invdyna
=
self
.
ptrs_to_symb_fcts
[
"mbs_invdyna"
]
if
mbs_module
is
not
None
:
if
isinstance
(
mbs_module
,
MbsEquil
)
or
isinstance
(
mbs_module
,
MbsInvdyn
):
self
.
ptrs_to_symb_fcts
[
"mbs_invdyna"
]
=
mbs_invdyna_wrap
(
lambda
phi
,
mbs
,
tsim
:
self
.
mbs_invdyna
(
mbs_module
.
_phi
,
self
,
tsim
))
self
.
mbs_data_ptr
.
contents
.
mbs_invdyna
=
self
.
ptrs_to_symb_fcts
[
"mbs_invdyna"
]
else
:
print
(
'mbs_module must be MbsEquil or MbsInvdyn to use invdyna'
)
else
:
print
(
' A module needs to be passed as mbs_module argument'
)
else
:
print
(
fun
+
" is not an existing symbolic function"
)
...
...
MBsysC/mbs_interface/MBsysPy/mbsyspy/mbs_dirdyn.py
View file @
3249e824
...
...
@@ -279,11 +279,11 @@ class MbsDirdyn(object):
libmodules
.
mbs_dirdyn_finish
(
self
.
mbs_dirdyn_ptr
,
self
.
mbs
.
mbs_data_ptr
)
#
A
nassign user functions
#
U
nassign user functions
if
self
.
mbs
.
opt_load_c
<
2
:
self
.
mbs
.
__unassign_user_fct__
(
self
.
user_fun_list
)
#
A
nassing required symbolic functions
#
U
nassing required symbolic functions
if
self
.
mbs
.
opt_load_c
<
1
:
self
.
mbs
.
__unassign_symb_fct__
(
self
.
symb_fun_list
)
...
...
MBsysC/mbs_interface/MBsysPy/mbsyspy/mbs_equil.py
View file @
3249e824
...
...
@@ -14,7 +14,6 @@ setting the options, running an(or multiple) analysis and freeing the memory.
import
os
import
imp
import
ctypes
import
numpy
as
np
...
...
@@ -26,21 +25,6 @@ from ..mbs_utilities import str_to_bytes
# importing libraries
from
.._mbsysc_loader.loadlibs
import
libmodules
# importing utilities function
from
..mbs_utilities
import
callback_undefined
# importing wrapping function
from
.._mbsysc_loader.callback
import
user_cons_hJ_wrap
from
.._mbsysc_loader.callback
import
user_cons_jdqd_wrap
from
.._mbsysc_loader.callback
import
user_equil_init_wrap
from
.._mbsysc_loader.callback
import
user_equil_loop_wrap
from
.._mbsysc_loader.callback
import
user_equil_finish_wrap
from
.._mbsysc_loader.callback
import
user_equil_fxe_wrap
from
.._mbsysc_loader.callback
import
mbs_cons_hJ_wrap
from
.._mbsysc_loader.callback
import
mbs_cons_jdqd_wrap
from
.._mbsysc_loader.callback
import
mbs_invdyna_wrap
# =============================================================================
# Global parameter of the current module
...
...
@@ -114,15 +98,16 @@ class MbsEquil(object):
self
.
mbs
.
ptrs_to_user_fcts
=
dict
()
self
.
mbs
.
ptrs_to_symb_fcts
=
dict
()
# Storing project function pointer
self
.
user_cons_hJ
=
None
self
.
user_cons_jdqd
=
None
self
.
user_equil_init
=
None
self
.
user_equil_loop
=
None
self
.
user_equil_finish
=
None
self
.
mbs_cons_hJ
=
None
self
.
mbs_cons_jdqd
=
None
self
.
mbs_invdyna
=
None
self
.
user_fun_list
=
[
'cons_hJ'
,
'cons_jdqd'
,
'derivative'
,
'DrivenJoints'
,
'ExtForces'
,
'JointForces'
,
'LinkForces'
,
'Link3DForces'
,
'equil_init'
,
'equil_loop'
,
'equil_finish'
]
self
.
symb_fun_list
=
[
'accelred'
,
'cons_hJ'
,
'cons_jdqd'
,
'invdyna'
,
'extforces'
,
'gensensor'
,
'link'
,
'link3D'
,
'sensor'
]
self
.
mbs
.
__assign_user_fct_to_None__
(
self
.
user_fun_list
)
if
self
.
mbs
.
opt_load_c
<
2
:
self
.
mbs
.
__assign_user_to_undefined__
(
self
.
user_fun_list
)
self
.
mbs
.
__assign_symb_fct_to_None__
(
self
.
symb_fun_list
)
if
self
.
mbs
.
opt_load_c
<
1
:
self
.
mbs
.
__assign_symb_to_undefined__
(
self
.
symb_fun_list
)
# exchange of equilibrium variables
self
.
exchange_list
=
[]
...
...
@@ -160,13 +145,13 @@ class MbsEquil(object):
if
__DEBUG__
:
print
(
"DEBUG>> Loading user functions"
)
self
.
__load_user_fct__
(
self
.
user_path
)
# Loading symbolic function
if
self
.
mbs
.
opt_load_c
<
1
:
if
__DEBUG__
:
print
(
"DEBUG>> Loading symbolic functions"
)
self
.
mbs
.
__load_user_fct__
(
__MODULE_DIR__
,
self
.
user_fun_list
,
self
.
user_path
)
if
self
.
mbs
.
opt_load_c
<
1
:
# Loading symbolic function
if
__DEBUG__
:
print
(
"DEBUG>> Loading symbolic functions"
)
self
.
__load_symbolic_fct__
(
self
.
symbolic_path
)
self
.
mbs
.
__load_symbolic_fct__
(
__MODULE_DIR__
,
self
.
symb_fun_list
,
self
.
symbolic_path
)
def
__str__
(
self
):
"""Return str(self)."""
...
...
@@ -177,221 +162,6 @@ class MbsEquil(object):
'", loaded from the file "'
+
str
(
self
.
mbs_filename
)
+
'".'
return
msg
def
__load_user_fct__
(
self
,
user_path
):
"""
Load user function where some args depend on MbsEquil module.
Load the user functions in which some of the arguments are
dependent of MbsEquil module instance. The functions will be
assigned to the MbsData instance when the 'run' functions is called
and unassigned at the end.
The loader user functions are:
- user_cons_hJ(from user_cons_hJ.py)
- user_cons_jdqd(from user_cons_jdqd.py)
- user_equil_init(from user_equil.py)
- user_equil_loop(from user_equil.py)
- user_equil_finish(from user_equil.py)
"""
template_path
=
os
.
path
.
join
(
__MODULE_DIR__
,
'../templates/user'
)
# cons_hJ
user_file
=
"user_cons_hJ.py"
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
user_path
,
user_file
))
if
not
os
.
path
.
isfile
(
path
):
if
__DEBUG__
:
print
(
"DEBUG>> file '"
+
user_file
+
"' not found in folder '"
+
os
.
path
.
dirname
(
path
))
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
template_path
,
user_file
))
module
=
imp
.
load_source
(
user_file
[:
-
3
],
path
)
self
.
user_cons_hJ
=
module
.
user_cons_hJ
# cons_jdqd
user_file
=
"user_cons_jdqd.py"
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
user_path
,
user_file
))
if
not
os
.
path
.
isfile
(
path
):
if
__DEBUG__
:
print
(
"DEBUG>> file '"
+
user_file
+
"' not found in folder '"
+
os
.
path
.
dirname
(
path
))
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
template_path
,
user_file
))
module
=
imp
.
load_source
(
user_file
[:
-
3
],
path
)
self
.
user_cons_jdqd
=
module
.
user_cons_jdqd
# user_equil
user_file
=
"user_equil.py"
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
user_path
,
user_file
))
if
not
os
.
path
.
isfile
(
path
):
print
(
"file '"
+
user_file
+
"' not found in folder '"
+
os
.
path
.
dirname
(
path
))
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
template_path
,
user_file
))
else
:
if
__DEBUG__
:
print
(
"DEBUG>> loading file '"
+
user_file
+
"' in folder '"
+
os
.
path
.
dirname
(
path
))
module
=
imp
.
load_source
(
user_file
[:
-
3
],
path
)
self
.
user_equil_init
=
module
.
user_equil_init
self
.
user_equil_loop
=
module
.
user_equil_loop
self
.
user_equil_finish
=
module
.
user_equil_finish
try
:
self
.
user_equil_fxe
=
module
.
user_equil_fxe
except
AttributeError
:
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
template_path
,
user_file
))
module
=
imp
.
load_source
(
user_file
[:
-
3
],
path
)
self
.
user_equil_fxe
=
module
.
user_equil_fxe
return
def
__load_symbolic_fct__
(
self
,
symbolic_path
):
"""
Load symbolic function where some args depend on MbsEquil module.
Load the symb functions in which some of the arguments are
dependent of MbsEquil module instance. The functions will be
assigned to the MbsData instance when the 'run' functions is called
and unassigned at the end.
The loader user functions are:
- cons_hJ(from mbs_cons_hJ_MBSNAME.py)
- cons_jdqd(from mbs_cons_jdqd_MBSNAME.py)
- dirdyna(from mbs_dirdyna_MBSNAME.py)
- invdyna(from mbs_invdyna_MBSNAME.py)
"""
mbs_name
=
self
.
mbs
.
mbs_name
template_path
=
os
.
path
.
join
(
__MODULE_DIR__
,
'../templates/symbolic'
)
# mbs_cons_hJ
symb_file
=
"mbs_cons_hJ_"
+
mbs_name
+
".py"
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
symbolic_path
,
symb_file
))
if
not
os
.
path
.
isfile
(
path
):
if
__DEBUG__
:
print
(
"DEBUG>> file '"
+
symb_file
+
"' not found in folder '"
+
os
.
path
.
dirname
(
path
))
symb_file
=
"mbs_cons_hJ_PRJ.py"
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
template_path
,
symb_file
))
module
=
imp
.
load_source
(
symb_file
[:
-
3
],
path
)
self
.
mbs_cons_hJ
=
module
.
cons_hJ
# mbs_cons_jdqd
symb_file
=
"mbs_cons_jdqd_"
+
mbs_name
+
".py"
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
symbolic_path
,
symb_file
))
if
not
os
.
path
.
isfile
(
path
):
if
__DEBUG__
:
print
(
"DEBUG>> file '"
+
symb_file
+
"' not found in folder '"
+
os
.
path
.
dirname
(
path
))
symb_file
=
"mbs_cons_jdqd_PRJ.py"
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
template_path
,
symb_file
))
module
=
imp
.
load_source
(
symb_file
[:
-
3
],
path
)
self
.
mbs_cons_jdqd
=
module
.
cons_jdqd
# invdyna
symb_file
=
"mbs_invdyna_"
+
mbs_name
+
".py"
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
symbolic_path
,
symb_file
))
if
not
os
.
path
.
isfile
(
path
):
if
__DEBUG__
:
print
(
"DEBUG>> file '"
+
symb_file
+
"' not found in folder '"
+
os
.
path
.
dirname
(
path
))
symb_file
=
"mbs_invdyna_PRJ.py"
path
=
os
.
path
.
abspath
(
os
.
path
.
join
(
template_path
,
symb_file
))
module
=
imp
.
load_source
(
symb_file
[:
-
3
],
path
)
self
.
mbs_invdyna
=
module
.
invdyna
return
def
__assign_user_fct__
(
self
):
"""
Assign all user functions if some args are in MbsEquil instance.
Assign and wrap python user function where some args depend on MbsEquil
module. Store the functions in the MbsData instance and assign the
pointer of functions in the C structure.
"""
# cons_hJ
self
.
ptrs_to_user_fcts
[
"user_cons_hJ"
]
=
user_cons_hJ_wrap
(
lambda
h
,
Jac
,
mbs
,
tsim
:
self
.
user_cons_hJ
(
self
.
_huserc
,
self
.
_Juserc
,
self
.
mbs
,
tsim
))
self
.
mbs
.
user_cons_hJ
=
self
.
user_cons_hJ
self
.
mbs
.
mbs_data_ptr
.
contents
.
user_cons_hJ
=
self
.
ptrs_to_user_fcts
[
"user_cons_hJ"
]
# cons_jdqd
self
.
ptrs_to_user_fcts
[
"user_cons_jdqd"
]
=
user_cons_jdqd_wrap
(
lambda
jdqd
,
mbs
,
tsim
:
self
.
user_cons_jdqd
(
self
.
_jdqduserc
,
self
.
mbs
))
self
.
mbs
.
user_cons_jdqd
=
self
.
user_cons_jdqd
self
.
mbs
.
mbs_data_ptr
.
contents
.
user_cons_jdqd
=
self
.
ptrs_to_user_fcts
[
"user_cons_jdqd"
]
# user_equil
self
.
ptrs_to_user_fcts
[
"user_equil_init"
]
=
user_equil_init_wrap
(
lambda
mbs
,
eq
:
self
.
mbs
.
user_equil_init
(
self
.
mbs
,
self
))
self
.
ptrs_to_user_fcts
[
"user_equil_loop"
]
=
user_equil_loop_wrap
(
lambda
mbs
,
eq
:
self
.
mbs
.
user_equil_loop
(
self
.
mbs
,
self
))
self
.
ptrs_to_user_fcts
[
"user_equil_finish"
]
=
user_equil_finish_wrap
(
lambda
mbs
,
eq
:
self
.
mbs
.
user_equil_finish
(
self
.
mbs
,
self
))
self
.
ptrs_to_symb_fcts
[
"user_equil_fxe"
]
=
user_equil_fxe_wrap
(
lambda
mbs
,
f
:
self
.
__callback_user_equil_fxe
(
self
.
user_equil_fxe
,
self
.
mbs
,
f
))
self
.
mbs
.
mbs_data_ptr
.
contents
.
user_equil_fxe
=
self
.
ptrs_to_symb_fcts
[
"user_equil_fxe"
]
self
.
mbs
.
user_equil_fxe
=
self
.
user_equil_fxe
self
.
mbs
.
user_equil_init
=
self
.
user_equil_init
self
.
mbs
.
mbs_data_ptr
.
contents
.
user_equil_init
=
self
.
ptrs_to_user_fcts
[
"user_equil_init"
]
self
.
mbs
.
user_equil_loop
=
self
.
user_equil_loop
self
.
mbs
.
mbs_data_ptr
.
contents
.
user_equil_loop
=
self
.
ptrs_to_user_fcts
[
"user_equil_loop"
]
self
.
mbs
.
user_equil_finish
=
self
.
user_equil_finish
self
.
mbs
.
mbs_data_ptr
.
contents
.
user_equil_finish
=
self
.
ptrs_to_user_fcts
[
"user_equil_finish"
]
return
def
__assign_symbolic_fct__
(
self
):
"""
Assign all symbolic functions if some args are in MbsEquil instance.
Assign and wrap python symbolic function where some args depend on
MbsEquil module. Store the functions in the MbsData instance and assign
the pointer of functions in the C structure.
"""
# mbs_cons_hJ
self
.
ptrs_to_symb_fcts
[
"mbs_cons_hJ"
]
=
mbs_cons_hJ_wrap
(
lambda
h
,
Jac
,
mbs
,
tsim
:
self
.
mbs_cons_hJ
(
self
.
_h
,
self
.
_Jac
,
self
.
mbs
))
self
.
mbs
.
mbs_cons_hJ
=
self
.
mbs_cons_hJ
self
.
mbs
.
mbs_data_ptr
.
contents
.
mbs_cons_hJ
=
self
.
ptrs_to_symb_fcts
[
"mbs_cons_hJ"
]
# mbs_cons_jdqd
self
.
ptrs_to_symb_fcts
[
"mbs_cons_jdqd"
]
=
mbs_cons_jdqd_wrap
(
lambda
jdqd
,
mbs
,
tsim
:
self
.
mbs_cons_jdqd
(
self
.
_jdqd
,
self
.
mbs
))
self
.
mbs
.
mbs_cons_jdqd
=
self
.
mbs_cons_jdqd
self
.
mbs
.
mbs_data_ptr
.
contents
.
mbs_cons_jdqd
=
self
.
ptrs_to_symb_fcts
[
"mbs_cons_jdqd"
]
# invdyna
self
.
ptrs_to_symb_fcts
[
"mbs_invdyna"
]
=
mbs_invdyna_wrap
(
lambda
phi
,
mbs
,
tsim
:
self
.
mbs_invdyna
(
self
.
_phi
,
self
.
mbs
,
tsim
))
self
.
mbs
.
mbs_invdyna
=
self
.
mbs_invdyna
self
.
mbs
.
mbs_data_ptr
.
contents
.
mbs_invdyna
=
self
.
ptrs_to_symb_fcts
[
"mbs_invdyna"
]
return
# Callback function for function with advanced arguments
def
__callback_mbs_cons_hJ
(
self
,
fun
,
h
,
Jac
):
if
__DEBUG__
:
print
(
"DEBUG>> callback_mbs_cons_hJ"
)
__h
=
np
.
ctypeslib
.
as_array
(
h
,
(
self
.
mbs
.
Ncons
+
1
,
))
__Jac
=
np
.
ctypeslib
.
as_array
(
Jac
[
0
],
(
self
.
mbs
.
Ncons
+
1
,
self
.
mbs
.
njoint
+
1
))
fun
(
__h
,
__Jac
,
self
.
mbs
)
def
__callback_user_equil_fxe
(
self
,
fun
,
mbs
,
f
):
__f
=
np
.
ctypeslib
.
as_array
(
f
,
(
self
.
mbs_equil_ptr
.
contents
.
nx
+
1
,
))
fun
(
mbs
,
__f
)
def
__unassign_user_fct__
(
self
):
"""Unassign user function where some args depend on MbsEquil module."""
self
.
ptrs_to_user_fcts
=
{}
self
.
mbs
.
user_cons_hJ
=
None
self
.
mbs
.
mbs_data_ptr
.
contents
.
user_cons_hJ
=
user_cons_hJ_wrap
(
lambda
h
,
Jac
,
mbs
,
tsim
:
callback_undefined
(
"user_cons_hJ"
))
self
.
mbs
.
user_cons_jdqd
=
None
self
.
mbs
.
mbs_data_ptr
.
contents
.
user_cons_jdqd
=
user_cons_jdqd_wrap
(
lambda
h
,
Jac
,
mbs
,
tsim
:
callback_undefined
(
"user_cons_jdqd"
))
self
.
mbs
.
user_equil_init
=
None
self
.
mbs
.
mbs_data_ptr
.
contents
.
user_equil_init
=
user_equil_init_wrap
(
lambda
mbs
,
eq
:
callback_undefined
(
"user_equil_init"
))
self
.
mbs
.
user_equil_loop
=
None
self
.
mbs
.
mbs_data_ptr
.
contents
.
user_equil_loop
=
user_equil_loop_wrap
(
lambda
mbs
,
eq
:
callback_undefined
(
"user_equil_loop"
))
self
.
mbs
.
user_equil_finish
=
None
self
.
mbs
.
mbs_data_ptr
.
contents
.
user_equil_finish
=
user_equil_finish_wrap
(
lambda
mbs
,
eq
:
callback_undefined
(
"user_equil_finish"
))
self
.
mbs
.
user_equil_fxe
=
None
self
.
mbs
.
mbs_data_ptr
.
contents
.
user_equil_fxe
=
user_equil_fxe_wrap
(
lambda
mbs
,
f
:
callback_undefined
(
"user_equil_fxe"
))
def
__unassign_symbolic_fct__
(
self
):
"""Unassign symbolic function where some args depend on MbsEquil module."""
self
.
ptrs_to_symb_fcts
=
{}
self
.
mbs
.
mbs_cons_hJ
=
None
self
.
mbs
.
mbs_data_ptr
.
contents
.
mbs_cons_hJ
=
mbs_cons_hJ_wrap
(
lambda
h
,
Jac
,
mbs
,
tsim
:
callback_undefined
(
"mbs_cons_hJ"
))
self
.
mbs
.
mbs_cons_jdqd
=
None
self
.
mbs
.
mbs_data_ptr
.
contents
.
mbs_cons_jdqd
=
mbs_cons_jdqd_wrap
(
lambda
jdqd
,
mbs
,
tsim
:
callback_undefined
(
"mbs_cons_jdqd"
))
self
.
mbs
.
mbs_invdyna
=
None
self
.
mbs
.
mbs_data_ptr
.
contents
.
mbs_invdyna
=
mbs_invdyna_wrap
(
lambda
Q
,
mbs
,
tsim
:
callback_undefined
(
"mbs_invdyna"
))
def
__del__
(
self
):
"""Delete the object by freeing the C - related memory."""
libmodules
.
mbs_delete_equil
(
self
.
mbs_equil_ptr
,
self
.
mbs
.
mbs_data_ptr
)
...
...
@@ -410,33 +180,29 @@ class MbsEquil(object):
self.results: MbsResultEquil
The MbsResults containing the results of the analysis.
"""
# Assi
n
g required user functions
# Assig
n
required user functions
if
self
.
mbs
.
opt_load_c
<
2
:
self
.
__assign_user_fct__
()
self
.
mbs
.
__assign_user_fct__
()
self
.
mbs
.
__assign_user_fct__
(
self
.
user_fun_list
,
self
)
self
.
_compute_exchange
()
self
.
_compute_extra_variable
()
# Assi
n
g required symbolic functions
# Assig
n
required symbolic functions
if
self
.
mbs
.
opt_load_c
<
1
:
self
.
__assign_symbolic_fct__
()
self
.
mbs
.
__assign_symb_fct__
()
self
.
mbs
.
__assign_symb_fct__
(
self
.
symb_fun_list
,
self
)
libmodules
.
mbs_run_equil
(
self
.
mbs_equil_ptr
,
self
.
mbs
.
mbs_data_ptr
)
if
self
.
store_results
:
self
.
load_results
()
#
A
nassign user functions
#
U
nassign user functions
if
self
.
mbs
.
opt_load_c
<
2
:
self
.
__unassign_user_fct__
()
self
.
mbs
.
__unassign_user_fct__
()
self
.
mbs
.
__unassign_user_fct__
(
self
.
user_fun_list
)
#
A
nassing required symbolic functions
#
U
nassing required symbolic functions
if
self
.
mbs
.
opt_load_c
<
1
:
self
.
__unassign_symbolic_fct__
()
self
.
mbs
.
__unassign_symb_fct__
()
self
.
mbs
.
__unassign_symb_fct__
(
self
.
symb_fun_list
)
return
self
.
results
...
...
@@ -460,7 +226,7 @@ class MbsEquil(object):
# Create variable output name
self
.
results
.
x_name
=
[]
# independant variables
for
u
in
self
.
mbs
.
qu
[
1
:
self
.
mbs
.
nqu
+
1
]:
for
u
in
self
.
mbs
.
qu
[
1
:
(
self
.
mbs
.
nqu
+
1
)
]:
self
.
results
.
x_name
.
append
(
"q"
+
str
(
u
))
# User derivative variables
if
self
.
get_options
(
"compute_uxd"
):
...
...
@@ -523,7 +289,7 @@ class MbsEquil(object):
self
.
exchange_list
.
append
([
address
,
replaced_var_id
,
ptr_name
])
return
def
add_extra_variable
(
self
,
ptr
,
index
,
ptr_name
=
None
):
def
add_extra_variable
(
self
,
ptr
,
index
,
ptr_name
=
None
):
"""
Add a new equilibrium variable.
...
...
Write
Preview
Markdown
is supported
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