Commit 8ac33049 authored by Louis Beauloye's avatar Louis Beauloye
Browse files

dirdyn and part ok with new function handler

parent 73675cfc
......@@ -9,7 +9,7 @@ __author__ = "Robotran team"
import os
import imp
import numpy as np
# importing utilities function
from .mbs_utilities import callback_undefined
......@@ -113,6 +113,8 @@ def assign_symb_fct_to_None(module, functions):
----------
module : TYPE
DESCRIPTION.
functions : TYPE
DESCRIPTION.
Returns
-------
......@@ -146,6 +148,7 @@ def assign_symb_fct_to_None(module, functions):
else:
print('The list of symbolic functions to load can not be None')
def __load_user_fct_2__(module, module_dir, functions, user_path):
"""
Load user function where some args depend on MbsDirdyn module.
......@@ -294,7 +297,7 @@ def __load_user_fct_2__(module, module_dir, functions, user_path):
print("DEBUG>> loading file '" + user_file + "' in folder '" + os.path.dirname(path))
module_base = imp.load_source(user_file[:-3], path)
module.user_dirdyn_init = module_base.user_dirdyn_loop
module.user_dirdyn_loop = module_base.user_dirdyn_loop
elif fun == 'dirdyn_finish':
# user_dirdyn
......@@ -489,6 +492,8 @@ def __unassign_user_fct_2__(module, functions):
# load function and not the assign function.
# module.ptrs_to_user_fcts.clear()
module.ptrs_to_user_fcts.clear()
__assign_user_to_undefined__(module, functions)
# if from data, to not suppress it
......@@ -536,6 +541,228 @@ def __assign_user_to_undefined__(module, functions):
print('The list of user functions to load can not be None')
def __assign_user_fct_2__(module, functions, module_add=None):
# to use with module
"""
Assign all user functions if all args are in MbsData instance.
Assign and wrap python user functions in which the arguments are
not dependent of another module instance (ie. MbsPart, MbsDirdyn...).
Store the functions in the MbsData instance and assign the pointer
of functions in the C structure.
The other user functions will be assigned when a module is
instancied.
"""
data_c_ptr = module.mbs_data_ptr.contents
if functions is not None:
for fun in functions:
if fun == "cons_hJ":
# cons_hJ
if module_add is not None:
if module_add.name == "MbsDirdyn":
module.ptrs_to_user_fcts["user_cons_hJ"] = user_cons_hJ_wrap(lambda h, Jac, mbs, tsim: module.user_cons_hJ(module_add._huserc, module_add._Juserc, module, tsim))
module.mbs_data_ptr.contents.user_cons_hJ = module.ptrs_to_user_fcts["user_cons_hJ"]
elif module_add.name == "MbsPart":
module.ptrs_to_user_fcts["user_cons_hJ"] = user_cons_hJ_wrap(lambda h, Jac, mbs, tsim: __callback_user_cons_hJ_part(module, module.user_cons_hJ, h, Jac, tsim))
module.mbs_data_ptr.contents.user_cons_hJ = module.ptrs_to_user_fcts["user_cons_hJ"]
else:
print(module_add.name + ' is not a valid module name')
else:
print(' A module needs to be pass as module_add argument')
elif fun == "cons_jdqd":
# cons_jdqd
if module_add is not None:
module.ptrs_to_user_fcts["user_cons_jdqd"] = user_cons_jdqd_wrap(lambda jdqd, mbs, tsim: module.user_cons_jdqd(module_add._jdqduserc, module))
module.mbs_data_ptr.contents.user_cons_jdqd = module.ptrs_to_user_fcts["user_cons_jdqd"]
else:
print(' A module needs to be pass as module_add argument')
elif fun == "dirdyn_init":
# user_dirdyn
if module_add is not None:
module.ptrs_to_user_fcts["user_dirdyn_init"] = user_dirdyn_init_wrap(lambda mbs, dd: module.user_dirdyn_init(module, module_add))
module.mbs_data_ptr.contents.user_dirdyn_init = module.ptrs_to_user_fcts["user_dirdyn_init"]
else:
print('Dirdyn module needs to be pass as module_add argument')
elif fun == "dirdyn_loop":
if module_add is not None:
module.ptrs_to_user_fcts["user_dirdyn_loop"] = user_dirdyn_loop_wrap(lambda mbs, dd: module.user_dirdyn_loop(module, module_add))
module.mbs_data_ptr.contents.user_dirdyn_loop = module.ptrs_to_user_fcts["user_dirdyn_loop"]
else:
print('Dirdyn module needs to be pass as module_add argument')
elif fun == "dirdyn_finish":
if module_add is not None:
module.ptrs_to_user_fcts["user_dirdyn_finish"] = user_dirdyn_finish_wrap(lambda mbs, dd: module.user_dirdyn_finish(module, module_add))
module.mbs_data_ptr.contents.user_dirdyn_finish = module.ptrs_to_user_fcts["user_dirdyn_finish"]
else:
print('Dirdyn module needs to be pass as module_add argument')
elif fun == "derivative":
# derivative
module.ptrs_to_user_fcts["user_Derivative"] = user_Derivative_wrap(lambda mbs: module.user_derivative(module))
data_c_ptr.user_Derivative = module.ptrs_to_user_fcts["user_Derivative"]
elif fun == "DrivenJoints":
# drivenJoints
module.ptrs_to_user_fcts["user_DrivenJoints"] = user_DrivenJoints_wrap(lambda mbs, t: module.user_DrivenJoints(module, t))
data_c_ptr.user_DrivenJoints = module.ptrs_to_user_fcts["user_DrivenJoints"]
elif fun == "ExtForces":
# ext_forces
module.ptrs_to_user_fcts["user_ExtForces"] = user_ExtForces_wrap(lambda PxF, RxF, VxF, OMxF, AxF, OMPxF, mbs, tsim, ixF: __callback_user_ExtForces(module, module.user_ExtForces, PxF, RxF, VxF, OMxF, AxF, OMPxF, tsim, ixF))
data_c_ptr.user_ExtForces = module.ptrs_to_user_fcts["user_ExtForces"]
elif fun == "JointForces":
# joint_forces
module.ptrs_to_user_fcts["user_JointForces"] = user_JointForces_wrap(lambda mbs, t: module.user_JointForces(module, t))
data_c_ptr.user_JointForces = module.ptrs_to_user_fcts["user_JointForces"]
elif fun == "LinkForces":
# link_forces
module.ptrs_to_user_fcts["user_LinkForces"] = user_LinkForces_wrap(lambda Z, Zd, mbs, tsim, ilnk: module.user_LinkForces(Z, Zd, module, tsim, ilnk))
data_c_ptr.user_LinkForces = module.ptrs_to_user_fcts["user_LinkForces"]
elif fun == "Link3DForces":
# link3D_forces
module.ptrs_to_user_fcts["user_Link3DForces"] = user_Link3DForces_wrap(lambda PxF, RxF, VxF, OMxF, AxF, OMPxF, mbs, tsim, ixF: __callback_user_Link3DForces(module, module.user_Link3DForces, PxF, RxF, VxF, OMxF, AxF, OMPxF, tsim, ixF))
data_c_ptr.user_Link3DForces = module.ptrs_to_user_fcts["user_Link3DForces"]
else:
print(fun + " is not an existing user function")
else:
print('The list of user functions to load can not be None')
return
def __assign_symb_fct_2__(module, functions, module_add=None):
"""
Assign all symbolic functions if all args are in MbsData instance.
Assign and wrap python symbolic functions in which the arguments are
not dependent of another module instance (ie. MbsPart, MbsDirdyn...).
Store the functions in the MbsData instance and assign the pointer
of functions in the C structure.
The other user functions will be assigned when a module is
instancied.
"""
data_c_ptr = module.mbs_data_ptr.contents
if functions is not None:
for fun in functions:
if fun == "accelred":
# mbs_accelred
module.ptrs_to_symb_fcts["mbs_accelred"] = mbs_accelred_wrap(lambda mbs, tsim: module.mbs_accelred(module, tsim))
data_c_ptr.mbs_accelred = module.ptrs_to_symb_fcts["mbs_accelred"]
elif fun == "cons_hJ":
# mbs_cons_hJ
if module_add is not None:
if module_add.name == "MbsDirdyn":
module.ptrs_to_symb_fcts["mbs_cons_hJ"] = mbs_cons_hJ_wrap(lambda h, Jac, mbs, tsim: module.mbs_cons_hJ(module_add._h, module_add._Jac, module))
module.mbs_data_ptr.contents.mbs_cons_hJ = module.ptrs_to_symb_fcts["mbs_cons_hJ"]
elif module_add.name == "MbsPart":
module.ptrs_to_symb_fcts["mbs_cons_hJ"] = mbs_cons_hJ_wrap(lambda h, Jac, mbs, tsim: __callback_mbs_cons_hJ_part(module, module.mbs_cons_hJ, h, Jac))
module.mbs_data_ptr.contents.mbs_cons_hJ = module.ptrs_to_symb_fcts["mbs_cons_hJ"]
else:
print(module_add.name + ' is not a valid module name')
else:
print(' A module needs to be pass as module_add argument')
elif fun == "cons_jdqd":
# mbs_cons_jdqd
if module_add is not None:
if module_add.name == "MbsDirdyn":
module.ptrs_to_symb_fcts["mbs_cons_jdqd"] = mbs_cons_jdqd_wrap(lambda jdqd, mbs, tsim: module.mbs_cons_jdqd(module_add._jdqd, module))
module.mbs_data_ptr.contents.mbs_cons_jdqd = module.ptrs_to_symb_fcts["mbs_cons_jdqd"]
else:
print(module_add.name + ' is not a valid module name')
else:
print(' A module needs to be pass as module_add argument')
elif fun == "extforces":
# mbs_extforces
module.ptrs_to_symb_fcts["mbs_extforces"] = mbs_extforces_wrap(lambda frc, trq, mbs, tsim: module.mbs_extforces(module.frc, module.trq, module, tsim))
data_c_ptr.mbs_extforces = module.ptrs_to_symb_fcts["mbs_extforces"]
elif fun == "gensensor":
# mbs_gensensor
module.ptrs_to_symb_fcts["mbs_gensensor"] = mbs_gensensor_wrap(lambda sens, mbs, isens: module.__callback_mbs_sensor(module.mbs_gensensor, sens, isens))
data_c_ptr.mbs_gensensor = module.ptrs_to_symb_fcts["mbs_gensensor"]
elif fun == "link":
# mbs_link
module.ptrs_to_symb_fcts["mbs_link"] = mbs_link_wrap(lambda frc, trq, Flnk, Z, Zd, mbs, tsim: module.mbs_link(module.frc, module.trq, module.Fl, module.Z, module.Zd, module, tsim))
data_c_ptr.mbs_link = module.ptrs_to_symb_fcts["mbs_link"]
elif fun == "link3D":
# mbs_link3D
module.ptrs_to_symb_fcts["mbs_link3D"] = mbs_link3D_wrap(lambda frc, trq, mbs, tsim: module.mbs_link3D(module.frc, module.trq, module, tsim))
data_c_ptr.mbs_link3D = module.ptrs_to_symb_fcts["mbs_link3D"]
elif fun == "sensor":
# mbs_sensor
module.ptrs_to_symb_fcts["mbs_sensor"] = mbs_sensor_wrap(lambda sens, mbs, isens: module.__callback_mbs_sensor(module.mbs_sensor, sens, isens))
data_c_ptr.mbs_sensor = module.ptrs_to_symb_fcts["mbs_sensor"]
elif fun == "dirdyna":
# dirdyna
if module_add is not None:
if module_add.name == "MbsDirdyn":
module.ptrs_to_symb_fcts["mbs_dirdyna"] = mbs_dirdyna_wrap(lambda M, c, mbs, tsim: module.mbs_dirdyna(module_add._M, module_add._c, module, tsim))
module.mbs_dirdyna = module.mbs_dirdyna
module.mbs_data_ptr.contents.mbs_dirdyna = module.ptrs_to_symb_fcts["mbs_dirdyna"]
else:
print(module_add.name + ' is not a valid module name')
else:
print(' A module needs to be pass as module_add argument')
elif fun == "invdyna":
# TODO
# invdyna
# not used in the MbsSolvekin module
module.ptrs_to_symb_fcts["mbs_invdyna"] = mbs_invdyna_wrap(lambda phi, mbs, tsim: module.mbs_invdyna(module._phi, module.mbs, tsim))
module.mbs_invdyna = module.mbs_invdyna
module.mbs_data_ptr.contents.mbs_invdyna = module.ptrs_to_symb_fcts["mbs_invdyna"]
else:
print(fun + " is not an existing symbolic function")
else:
print('The list of symbolic functions to load can not be None')
def __callback_user_ExtForces(module, fun, PxF, RxF, VxF, OMxF, AxF, OMPxF, tsim, ixF):
__PxF = np.ctypeslib.as_array(PxF, (4, ))
__RxF = np.ctypeslib.as_array(RxF, (4, 4))
__VxF = np.ctypeslib.as_array(VxF, (4, ))
__OMxF = np.ctypeslib.as_array(OMxF, (4, ))
__AxF = np.ctypeslib.as_array(AxF, (4, ))
__OMPxF = np.ctypeslib.as_array(OMPxF, (4, ))
SWr = fun(__PxF, __RxF, __VxF, __OMxF, __AxF, __OMPxF, module, tsim, ixF)
module.SWr[ixF, :] = SWr[:]
def __callback_user_Link3DForces(module, fun, PxF, RxF, VxF, OMxF, AxF, OMPxF, tsim, ixF):
__PxF = np.ctypeslib.as_array(PxF, (4, ))
__RxF = np.ctypeslib.as_array(RxF, (4, 4))
__VxF = np.ctypeslib.as_array(VxF, (4, ))
__OMxF = np.ctypeslib.as_array(OMxF, (4, ))
__AxF = np.ctypeslib.as_array(AxF, (4, ))
__OMPxF = np.ctypeslib.as_array(OMPxF, (4, ))
SWr = fun(__PxF, __RxF, __VxF, __OMxF, __AxF, __OMPxF, module, tsim, ixF)
module.SWr[ixF, 1:7] = SWr[1:7]
def __callback_mbs_sensor(module, fun, sens, isens):
"""Do the callback for gensensor, forcesensor and sensor."""
module.__sens.__reassign_memory__(sens)
fun(module.__sens, module, isens)
def __callback_mbs_invdyna(module, fun, Q, tsim):
__Q = np.ctypeslib.as_array(Q, (module.njoint + 1, ))
fun(module, tsim, __Q)
def __callback_user_cons_hJ_part(mbs, fun, h, Jac, tsim):
__h = np.ctypeslib.as_array(h, (mbs.Nuserc + 1,))
__Jac = np.ctypeslib.as_array(Jac[0], (mbs.Nuserc + 1, mbs.njoint + 1))
fun(__h, __Jac, mbs, tsim)
def __callback_mbs_cons_hJ_part(mbs, fun, h, Jac):
if __DEBUG__:
print("DEBUG>> callback_mbs_cons_hJ")
__h = np.ctypeslib.as_array(h, (mbs.Ncons + 1,))
__Jac = np.ctypeslib.as_array(Jac[0], (mbs.Ncons + 1, mbs.njoint + 1))
fun(__h, __Jac, mbs)
def __unassign_symb_fct_2__(module, functions):
"""
Unassign all user function if all args are in MbsData instance.
......@@ -548,6 +775,8 @@ def __unassign_symb_fct_2__(module, functions):
# load function and not the assign function.
# module.ptrs_to_user_fcts.clear()
module.ptrs_to_symb_fcts.clear()
__assign_symb_to_undefined__(module, functions)
# if from data, to not suppress it
......
......@@ -13,14 +13,14 @@ Copyright 2019 Universite Catholique de Louvain
# Packages loading
#==============================================================================
import sys, time
sys.path.insert(1,"./../../../..") # Relative path for this example script
sys.path.insert(1,"./../../../../../build/python") # Relative path for this example script
import MBsysPy as robotran
#==============================================================================
# Project loading
#==============================================================================
print("\n>>PYTHON>> Loading Project!")
mbs_data = robotran.MbsData("../dataR/Cuts.mbs",
mbs_data = robotran.MbsData("../dataR/Cuts.mbs",
prj_functions_c=None, prj_lib_path="..")
print(mbs_data)
......
......@@ -40,6 +40,8 @@ from .._mbs_function_handler import __assign_user_to_undefined__
from .._mbs_function_handler import __assign_symb_to_undefined__
from .._mbs_function_handler import __load_symbolic_fct_2__
from .._mbs_function_handler import __load_user_fct_2__
from .._mbs_function_handler import __assign_symb_fct_2__
from .._mbs_function_handler import __assign_user_fct_2__
# importing wrapping function
from .._mbsysc_loader.callback import user_cons_hJ_wrap
......@@ -126,6 +128,8 @@ class MbsDirdyn(object):
self.mbs = mbs
self.name = "MbsDirdyn"
if __DEBUG__:
print("DEBUG>> MbsDirdyn created.")
......@@ -155,18 +159,18 @@ class MbsDirdyn(object):
self.symbolic_path = symbolic_path
# dictionary containing the pointers to avoid garbage collecting
self.ptrs_to_user_fcts = dict()
self.ptrs_to_symb_fcts = dict()
self.mbs.ptrs_to_user_fcts = dict()
self.mbs.ptrs_to_symb_fcts = dict()
# Storing project function pointer
self.user_fun_list = ['cons_hJ', 'cons_jdqd', 'derivative', 'DrivenJoints', 'ExtForces', 'JointForces',
'LinkForces', 'Link3DForces', 'dirdyn_init', 'dirdyn_loop', 'dirdyn_finish']
self.symb_fun_list = ['accelred', 'cons_hJ', 'cons_jdqd', 'invdyna', 'dirdyna', 'extforces', 'gensensor',
'link', 'link3D', 'sensor']
assign_user_fct_to_None(self, self.user_fun_list)
assign_user_fct_to_None(self.mbs, self.user_fun_list)
if self.mbs.opt_load_c < 2:
__assign_user_to_undefined__(self.mbs, self.user_fun_list)
assign_symb_fct_to_None(self, self.symb_fun_list)
assign_symb_fct_to_None(self.mbs, self.symb_fun_list)
if self.mbs.opt_load_c < 1:
__assign_symb_to_undefined__(self.mbs, self.symb_fun_list)
# self.user_cons_hJ = None
......@@ -355,26 +359,27 @@ class MbsDirdyn(object):
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_dirdyn
self.ptrs_to_user_fcts["user_dirdyn_init"] = user_dirdyn_init_wrap(lambda mbs, dd: self.mbs.user_dirdyn_init(self.mbs, self))
self.ptrs_to_user_fcts["user_dirdyn_loop"] = user_dirdyn_loop_wrap(lambda mbs, dd: self.mbs.user_dirdyn_loop(self.mbs, self))
self.ptrs_to_user_fcts["user_dirdyn_finish"] = user_dirdyn_finish_wrap(lambda mbs, dd: self.mbs.user_dirdyn_finish(self.mbs, self))
self.mbs.user_dirdyn_init = self.user_dirdyn_init
self.mbs.mbs_data_ptr.contents.user_dirdyn_init = self.ptrs_to_user_fcts["user_dirdyn_init"]
self.mbs.user_dirdyn_loop = self.user_dirdyn_loop
self.mbs.mbs_data_ptr.contents.user_dirdyn_loop = self.ptrs_to_user_fcts["user_dirdyn_loop"]
self.mbs.user_dirdyn_finish = self.user_dirdyn_finish
self.mbs.mbs_data_ptr.contents.user_dirdyn_finish = self.ptrs_to_user_fcts["user_dirdyn_finish"]
__assign_user_fct_2__(self.mbs, self.user_fun_list, self)
# # 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_dirdyn
# self.ptrs_to_user_fcts["user_dirdyn_init"] = user_dirdyn_init_wrap(lambda mbs, dd: self.mbs.user_dirdyn_init(self.mbs, self))
# self.ptrs_to_user_fcts["user_dirdyn_loop"] = user_dirdyn_loop_wrap(lambda mbs, dd: self.mbs.user_dirdyn_loop(self.mbs, self))
# self.ptrs_to_user_fcts["user_dirdyn_finish"] = user_dirdyn_finish_wrap(lambda mbs, dd: self.mbs.user_dirdyn_finish(self.mbs, self))
# self.mbs.user_dirdyn_init = self.user_dirdyn_init
# self.mbs.mbs_data_ptr.contents.user_dirdyn_init = self.ptrs_to_user_fcts["user_dirdyn_init"]
# self.mbs.user_dirdyn_loop = self.user_dirdyn_loop
# self.mbs.mbs_data_ptr.contents.user_dirdyn_loop = self.ptrs_to_user_fcts["user_dirdyn_loop"]
# self.mbs.user_dirdyn_finish = self.user_dirdyn_finish
# self.mbs.mbs_data_ptr.contents.user_dirdyn_finish = self.ptrs_to_user_fcts["user_dirdyn_finish"]
return
......@@ -386,20 +391,21 @@ class MbsDirdyn(object):
on MbsDirdyn 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"]
__assign_symb_fct_2__(self.mbs, self.symb_fun_list, self)
# # 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"]
# # 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"]
# dirdyna
self.ptrs_to_symb_fcts["mbs_dirdyna"] = mbs_dirdyna_wrap(lambda M, c, mbs, tsim: self.mbs_dirdyna(self._M, self._c, self.mbs, tsim))
self.mbs.mbs_dirdyna = self.mbs_dirdyna
self.mbs.mbs_data_ptr.contents.mbs_dirdyna = self.ptrs_to_symb_fcts["mbs_dirdyna"]
# # dirdyna
# self.ptrs_to_symb_fcts["mbs_dirdyna"] = mbs_dirdyna_wrap(lambda M, c, mbs, tsim: self.mbs_dirdyna(self._M, self._c, self.mbs, tsim))
# self.mbs.mbs_dirdyna = self.mbs_dirdyna
# self.mbs.mbs_data_ptr.contents.mbs_dirdyna = self.ptrs_to_symb_fcts["mbs_dirdyna"]
return
......@@ -414,7 +420,7 @@ class MbsDirdyn(object):
def __unassign_user_fct__(self):
"""Unassign user function where some args depend on MbsDirdyn module."""
self.ptrs_to_user_fcts.clear()
# self.ptrs_to_user_fcts.clear()
__unassign_user_fct_2__(self.mbs, self.user_fun_list)
# self.mbs.user_cons_hJ = None
......@@ -430,7 +436,7 @@ class MbsDirdyn(object):
def __unassign_symbolic_fct__(self):
"""Unassign symbolic function where some args depend on MbsDirdyn module."""
self.ptrs_to_symb_fcts.clear()
# self.ptrs_to_symb_fcts.clear()
__unassign_symb_fct_2__(self.mbs, self.symb_fun_list)
# self.mbs.mbs_cons_hJ = None
......@@ -464,12 +470,12 @@ class MbsDirdyn(object):
# Assing required user functions
if self.mbs.opt_load_c < 2:
self.__assign_user_fct__()
self.mbs.__assign_user_fct__()
#self.mbs.__assign_user_fct__()
# Assing required symbolic functions
if self.mbs.opt_load_c < 1:
self.__assign_symbolic_fct__()
self.mbs.__assign_symb_fct__()
#self.mbs.__assign_symb_fct__()
if not self.store_results:
libmodules.mbs_run_dirdyn(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
......@@ -533,12 +539,12 @@ class MbsDirdyn(object):
# Anassign user functions
if self.mbs.opt_load_c < 2:
self.__unassign_user_fct__()
self.mbs.__unassign_user_fct__()
#self.mbs.__unassign_user_fct__()
# Anassing required symbolic functions
if self.mbs.opt_load_c < 1:
self.__unassign_symbolic_fct__()
self.mbs.__unassign_symb_fct__()
#self.mbs.__unassign_symb_fct__()
return self.results
......@@ -753,7 +759,7 @@ class MbsDirdyn(object):
elif key == "compute_all_uxd":
self.mbs_dirdyn_ptr.contents.options.contents.compute_all_uxd = value
elif key == "compute_Qc":
if(type(value) is np.ndarray) or(type(value) is list):
if(type(value) is np.ndarray) or (type(value) is list):
if np.size(value) == self.mbs.mbs_data_ptr.contents.njoint + 1:
for i, val in enumerate(value[1:]):
self.mbs_dirdyn_ptr.contents.options.contents.compute_Qc[i + 1] = val
......@@ -762,7 +768,7 @@ class MbsDirdyn(object):
else:
print(">>DIRDYN>> The specified value for compute_Qc is not a list or a np.array")
elif key == "integrator":
if(type(value) is int) and(value >= 0 and value <= 8):
if(type(value) is int) and (value >= 0 and value <= 8):
self.mbs_dirdyn_ptr.contents.options.contents.integrator = value
elif type(value) is str:
if value == "RK4":
......@@ -1075,7 +1081,7 @@ class MbsResult(object):
print("DEBUG>> file '" + CurFile + "' not found in folder '" + os.path.dirname(path))
# Generalized user state
if self.mbs.Nux and(module != 5 and module != 6):
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)):
......@@ -1129,7 +1135,7 @@ class MbsResult(object):
print("DEBUG>> file '" + CurFile + "' not found in folder '" + os.path.dirname(path))
# Qa
if self.mbs.nqa and(module != 5 and module != 3):
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)):
......
......@@ -29,6 +29,10 @@ from .._mbs_function_handler import __unassign_user_fct_2__
from .._mbs_function_handler import __unassign_symb_fct_2__
from .._mbs_function_handler import __assign_user_to_undefined__
from .._mbs_function_handler import __assign_symb_to_undefined__
from .._mbs_function_handler import __load_symbolic_fct_2__
from .._mbs_function_handler import __load_user_fct_2__
from .._mbs_function_handler import __assign_symb_fct_2__
from .._mbs_function_handler import __assign_user_fct_2__
# importing wrapping function
from .._mbsysc_loader.callback import user_cons_hJ_wrap
......@@ -108,6 +112,8 @@ class MbsPart(object):
self.mbs = mbs
self.name = "MbsPart"
# Setting options with different default values in MBsysPy
self.set_options(visualise=1)
......@@ -137,18 +143,22 @@ class MbsPart(object):
self.symbolic_path = symbolic_path
# pointers list to avoid garbage collecting
self.ptrs_to_user_fcts = dict()
self.ptrs_to_symb_fcts = dict()
self.mbs.ptrs_to_user_fcts = dict()
self.mbs.ptrs_to_symb_fcts = dict()
# Storing project function pointer
# self.user_cons_hJ = None
# self.mbs_cons_hJ = None
self.user_fun_list = ['cons_hJ']
self.symb_fun_list = ['cons_hJ']
assign_user_fct_to_None(self, self.user_fun_list)
__assign_user_to_undefined__(self.mbs, self.user_fun_list)
assign_symb_fct_to_None(self, self.symb_fun_list)
__assign_symb_to_undefined__(self.mbs, self.symb_fun_list)
self.user_fun_list = ['cons_hJ', 'derivative', 'DrivenJoints', 'ExtForces', 'JointForces',
'LinkForces', 'Link3DForces']
self.symb_fun_list = ['accelred', 'cons_hJ', 'extforces', 'gensensor',
'link', 'link3D', 'sensor']
assign_user_fct_to_None(self.mbs, self.user_fun_list)
if self.mbs.opt_load_c < 2:
__assign_user_to_undefined__(self.mbs, self.user_fun_list)
assign_symb_fct_to_None(self.mbs, self.symb_fun_list)
if self.mbs.opt_load_c < 1:
__assign_symb_to_undefined__(self.mbs, self.symb_fun_list)
# Exposing some memory
if __DEBUG__:
......@@ -161,13 +171,15 @@ class MbsPart(object):
if __DEBUG__:
print("DEBUG>> Loading user functions")
self.__load_user_fct__(self.user_path)
# self.__load_user_fct__(self.user_path)
__load_user_fct_2__(self.mbs, __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.__load_symbolic_fct__(self.symbolic_path)
__load_symbolic_fct_2__(self.mbs, __MODULE_DIR__, self.symb_fun_list, self.symbolic_path)
def __str__(self):
"""Return str(self)."""
......@@ -193,18 +205,19 @@ class MbsPart(object):
user_path: str
The path to the folder containing the user functions.
"""
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))