Skip to content
GitLab
Commit 23e67722 authored by Louis Beauloye's avatar Louis Beauloye
Browse files

[Ctypes] Remplace list of pointers by dictionnary and free pointers

parent ec71bb36
Hide whitespace changes
Inline Side-by-side
MBsysC/mbs_interface/MBsysPy/mbsyspy/mbs_data.py
View file @ 23e67722
......@@ -291,8 +291,9 @@ class MbsData(object):
self.mbs_link3D = None
self.mbs_sensor = None
# pointers list to avoid garbage collecting
self.ptrs_to_fcts = []
# pointers dict to avoid garbage collecting
self.ptrs_to_user_fcts = dict()
self.ptrs_to_symb_fcts = dict()
# Exposing some memory
if __DEBUG__ : print( "DEBUG>> Exposing MbsData fields")
......@@ -393,6 +394,7 @@ class MbsData(object):
- user_Link3DForces (from link3D_forces.py)
"""
data_c_ptr=self.mbs_data_ptr.contents
template_path = os.path.join(__MODULE_DIR__,'../templates/user')
project_path = self.project_path
# Creating user path
......@@ -433,74 +435,68 @@ class MbsData(object):
# derivative
user_file = "derivatives.py"
path = os.path.abspath(os.path.join(user_path, user_file))
if os.path.isfile(path):
module = imp.load_source(user_file[:-3], path)
self.user_derivative = module.user_derivatives
self.ptrs_to_fcts.append(user_Derivative_wrap(lambda mbs : self.user_derivative(self)))
data_c_ptr.user_Derivative = self.ptrs_to_fcts[-1]
else:
if not os.path.isfile(path):
if __DEBUG__ : print( "DEBUG>> file '"+user_file+"' not found in folder '"+os.path.dirname(path))
self.user_derivatives = None
path = os.path.abspath(os.path.join(template_path, user_file))
module = imp.load_source(user_file[:-3], path)
self.user_derivative = module.user_derivatives
self.ptrs_to_user_fcts["user_Derivative"] = user_Derivative_wrap(lambda mbs : self.user_derivative(self))
data_c_ptr.user_Derivative = self.ptrs_to_user_fcts["user_Derivative"]
# drivenJoints
user_file = "driven_joints.py"
path = os.path.abspath(os.path.join(user_path, user_file))
if os.path.isfile(path):
module = imp.load_source(user_file[:-3], path)
self.user_DrivenJoints = module.user_DrivenJoints
self.ptrs_to_fcts.append(user_DrivenJoints_wrap(lambda mbs,t : self.user_DrivenJoints(self, t)))
data_c_ptr.user_DrivenJoints = self.ptrs_to_fcts[-1]
else:
if not os.path.isfile(path):
if __DEBUG__ : print( "DEBUG>> file '"+user_file+"' not found in folder '"+os.path.dirname(path))
self.user_DrivenJoints = None
path = os.path.abspath(os.path.join(template_path, user_file))
module = imp.load_source(user_file[:-3], path)
self.user_DrivenJoints = module.user_DrivenJoints
self.ptrs_to_user_fcts["user_DrivenJoints"] = user_DrivenJoints_wrap(lambda mbs,t : self.user_DrivenJoints(self, t))
data_c_ptr.user_DrivenJoints = self.ptrs_to_user_fcts["user_DrivenJoints"]
# ext_forces
user_file = "ext_forces.py"
path = os.path.abspath(os.path.join(user_path, user_file))
if os.path.isfile(path):
module = imp.load_source(user_file[:-3], path)
self.user_ExtForces = module.user_ExtForces
self.ptrs_to_fcts.append(user_ExtForces_wrap(lambda PxF,RxF,VxF,OMxF,AxF,OMPxF,mbs,tsim,ixF : self.__callback_user_ExtForces(self.user_ExtForces,PxF,RxF,VxF,OMxF,AxF,OMPxF,tsim,ixF)))
data_c_ptr.user_ExtForces = self.ptrs_to_fcts[-1]
else:
if not os.path.isfile(path):
if __DEBUG__ : print( "DEBUG>> file '"+user_file+"' not found in folder '"+os.path.dirname(path))
self.user_ExtForces = None
path = os.path.abspath(os.path.join(template_path, user_file))
module = imp.load_source(user_file[:-3], path)
self.user_ExtForces = module.user_ExtForces
self.ptrs_to_user_fcts["user_ExtForces"] = user_ExtForces_wrap(lambda PxF,RxF,VxF,OMxF,AxF,OMPxF,mbs,tsim,ixF : self.__callback_user_ExtForces(self.user_ExtForces,PxF,RxF,VxF,OMxF,AxF,OMPxF,tsim,ixF))
data_c_ptr.user_ExtForces = self.ptrs_to_user_fcts["user_ExtForces"]
# joint_forces
user_file = "joint_forces.py"
path = os.path.abspath(os.path.join(user_path, user_file))
if os.path.isfile(path):
module = imp.load_source(user_file[:-3], path)
self.user_JointForces = module.user_JointForces
self.ptrs_to_fcts.append(user_JointForces_wrap(lambda mbs,t : self.user_JointForces(self, t)))
data_c_ptr.user_JointForces = self.ptrs_to_fcts[-1]
else:
if not os.path.isfile(path):
if __DEBUG__ : print( "DEBUG>> file '"+user_file+"' not found in folder '"+os.path.dirname(path))
self.user_JointForces = None
path = os.path.abspath(os.path.join(template_path, user_file))
module = imp.load_source(user_file[:-3], path)
self.user_JointForces = module.user_JointForces
self.ptrs_to_user_fcts["user_JointForces"] = user_JointForces_wrap(lambda mbs,t : self.user_JointForces(self, t))
data_c_ptr.user_JointForces = self.ptrs_to_user_fcts["user_JointForces"]
# link_forces
user_file = "link_forces.py"
path = os.path.abspath(os.path.join(user_path, user_file))
if os.path.isfile(path):
module = imp.load_source(user_file[:-3], path)
self.user_LinkForces = module.user_LinkForces
self.ptrs_to_fcts.append(user_LinkForces_wrap(lambda Z, Zd, mbs, tsim, ilnk : self.user_LinkForces(Z, Zd, self, tsim, ilnk)))
data_c_ptr.user_LinkForces = self.ptrs_to_fcts[-1]
else:
if not os.path.isfile(path):
if __DEBUG__ : print( "DEBUG>> file '"+user_file+"' not found in folder '"+os.path.dirname(path))
self.user_LinkForces = None
path = os.path.abspath(os.path.join(template_path, user_file))
module = imp.load_source(user_file[:-3], path)
self.user_LinkForces = module.user_LinkForces
self.ptrs_to_user_fcts["user_LinkForces"] = user_LinkForces_wrap(lambda Z, Zd, mbs, tsim, ilnk : self.user_LinkForces(Z, Zd, self, tsim, ilnk))
data_c_ptr.user_LinkForces = self.ptrs_to_user_fcts["user_LinkForces"]
# link3D_forces
user_file = "link3D_forces.py"
path = os.path.abspath(os.path.join(user_path, user_file))
if os.path.isfile(path):
module = imp.load_source(user_file[:-3], path)
self.user_Link3DForces = module.user_Link3DForces
self.ptrs_to_fcts.append(user_Link3DForces_wrap(lambda PxF,RxF,VxF,OMxF,AxF,OMPxF,mbs,tsim,ixF : self.__callback_user_Link3DForces(self.user_Link3DForces,PxF,RxF,VxF,OMxF,AxF,OMPxF,tsim,ixF)))
data_c_ptr.user_Link3DForces = self.ptrs_to_fcts[-1]
else:
if not os.path.isfile(path):
if __DEBUG__ : print( "DEBUG>> file '"+user_file+"' not found in folder '"+os.path.dirname(path))
self.user_Link3DForces = None
path = os.path.abspath(os.path.join(template_path, user_file))
module = imp.load_source(user_file[:-3], path)
self.user_Link3DForces = module.user_Link3DForces
self.ptrs_to_user_fcts["user_Link3DForces"] = user_Link3DForces_wrap(lambda PxF,RxF,VxF,OMxF,AxF,OMPxF,mbs,tsim,ixF : self.__callback_user_Link3DForces(self.user_Link3DForces,PxF,RxF,VxF,OMxF,AxF,OMPxF,tsim,ixF))
data_c_ptr.user_Link3DForces = self.ptrs_to_user_fcts["user_Link3DForces"]
return
......@@ -541,79 +537,39 @@ class MbsData(object):
# mbs_accelred
symb_file = "mbs_accelred_"+mbs_name+".py"
path = os.path.abspath(os.path.join(symb_path, symb_file))
if os.path.isfile(path):
module = imp.load_source(symb_file[:-3], path)
self.mbs_accelred = module.mbs_accelred
self.ptrs_to_fcts.append(mbs_accelred_wrap(lambda mbs,tsim : self.mbs_accelred(self, tsim)))
data_c_ptr.mbs_accelred = self.ptrs_to_fcts[-1]
else:
if not os.path.isfile(path):
if __DEBUG__ : print( "DEBUG>> file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
symb_file = "mbs_accelred_PRJ.py"
path = os.path.abspath(os.path.join(template_path, symb_file))
module = imp.load_source(symb_file[:-3], path)
self.mbs_accelred = module.mbs_accelred
self.ptrs_to_fcts.append(mbs_accelred_wrap(lambda mbs,tsim : self.mbs_accelred(self, tsim)))
data_c_ptr.mbs_accelred = self.ptrs_to_fcts[-1]
# # mbs_cons_hJ
# symb_file = "mbs_cons_hJ_"+mbs_name+".py"
# path = os.path.abspath(os.path.join(symb_path, symb_file))
# if os.path.isfile(path):
# module = imp.load_source(symb_file[:-3], path)
# self.mbs_cons_hJ = module.cons_hJ
# self.ptrs_to_fcts.append(mbs_cons_hJ_wrap(lambda h,Jac,mbs,tsim : self.__callback_mbs_cons_hJ(self.mbs_cons_hJ,h,Jac)))
# data_c_ptr.mbs_cons_hJ = self.ptrs_to_fcts[-1]
# else:
# if __DEBUG__ : print( "DEBUG>> file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
# self.mbs_cons_hJ = None
#
# # mbs_cons_jdqd
# symb_file = "mbs_cons_jdqd_"+mbs_name+".py"
# path = os.path.abspath(os.path.join(symb_path, symb_file))
# if os.path.isfile(path):
# module = imp.load_source(symb_file[:-3], path)
# self.mbs_cons_jdqd = module.cons_jdqd
# self.ptrs_to_fcts.append(mbs_cons_jdqd_wrap(lambda jdqd,mbs,tsim : self.__callback_mbs_cons_jdqd(self.mbs_cons_jdqd,jdqd,tsim)))
# data_c_ptr.mbs_cons_jdqd = self.ptrs_to_fcts[-1]
# else:
# if __DEBUG__ : print( "DEBUG>> file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
# self.mbs_cons_jdqd = None
#
# # mbs_dirdyna
# symb_file = "mbs_dirdyna_"+mbs_name+".py"
# path = os.path.abspath(os.path.join(symb_path, symb_file))
# if os.path.isfile(path):
# module = imp.load_source(symb_file[:-3], path)
# self.mbs_dirdyna = module.dirdyna
# self.ptrs_to_fcts.append(mbs_dirdyna_wrap(lambda M,c,mbs,tsim : self.__callback_mbs_dirdyna(self.mbs_dirdyna,M,c,tsim)))
# data_c_ptr.mbs_dirdyna = self.ptrs_to_fcts[-1]
# else:
# if __DEBUG__ : print( "DEBUG>> file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
# self.mbs_dirdyna = None
path = os.path.abspath(os.path.join(template_path, symb_file))
module = imp.load_source(symb_file[:-3], path)
self.mbs_accelred = module.mbs_accelred
self.ptrs_to_symb_fcts["mbs_accelred"] = mbs_accelred_wrap(lambda mbs,tsim : self.mbs_accelred(self, tsim))
data_c_ptr.mbs_accelred = self.ptrs_to_symb_fcts["mbs_accelred"]
# mbs_extforces
symb_file = "mbs_extforces_"+mbs_name+".py"
path = os.path.abspath(os.path.join(symb_path, symb_file))
if os.path.isfile(path):
module = imp.load_source(symb_file[:-3], path)
self.mbs_extforces = module.extforces
self.ptrs_to_fcts.append(mbs_extforces_wrap(lambda frc,trq,mbs,tsim : self.mbs_extforces(self.frc,self.trq, self, tsim)))
data_c_ptr.mbs_extforces = self.ptrs_to_fcts[-1]
else:
if not os.path.isfile(path):
if __DEBUG__ : print( "DEBUG>> file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
self.mbs_extforces = None
symb_file = "mbs_extforces_PRJ.py"
path = os.path.abspath(os.path.join(template_path, symb_file))
module = imp.load_source(symb_file[:-3], path)
self.mbs_extforces = module.extforces
self.ptrs_to_symb_fcts["mbs_extforces"] = mbs_extforces_wrap(lambda frc,trq,mbs,tsim : self.mbs_extforces(self.frc,self.trq, self, tsim))
data_c_ptr.mbs_extforces = self.ptrs_to_symb_fcts["mbs_extforces"]
# mbs_gensensor
symb_file = "mbs_gensensor_"+mbs_name+".py"
path = os.path.abspath(os.path.join(symb_path, symb_file))
if os.path.isfile(path):
module = imp.load_source(symb_file[:-3], path)
self.mbs_gensensor = module.sensor
self.ptrs_to_fcts.append(mbs_gensensor_wrap(lambda sens,mbs,isens : self.__callback_mbs_sensor(self.mbs_gensensor, sens, isens)))
data_c_ptr.mbs_gensensor = self.ptrs_to_fcts[-1]
else:
if not os.path.isfile(path):
if __DEBUG__ : print( "DEBUG>> file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
self.mbs_gensensor = None
symb_file = "mbs_gensensor_PRJ.py"
path = os.path.abspath(os.path.join(template_path, symb_file))
module = imp.load_source(symb_file[:-3], path)
self.mbs_gensensor = module.sensor
self.ptrs_to_symb_fcts["mbs_gensensor"] = mbs_gensensor_wrap(lambda sens,mbs,isens : self.__callback_mbs_sensor(self.mbs_gensensor, sens, isens))
data_c_ptr.mbs_gensensor = self.ptrs_to_symb_fcts["mbs_gensensor"]
# # mbs_invdyna
# symb_file = "mbs_invdyna_"+mbs_name+".py"
......@@ -630,38 +586,38 @@ class MbsData(object):
# mbs_link
symb_file = "mbs_link_"+mbs_name+".py"
path = os.path.abspath(os.path.join(symb_path, symb_file))
if os.path.isfile(path):
module = imp.load_source(symb_file[:-3], path)
self.mbs_link = module.link
self.ptrs_to_fcts.append(mbs_link_wrap(lambda frc,trq,Flnk,Z,Zd,mbs,tsim : self.mbs_link(self.frc,self.trq,self.Fl,self.Z,self.Zd, self, tsim)))
data_c_ptr.mbs_link = self.ptrs_to_fcts[-1]
else:
if not os.path.isfile(path):
if __DEBUG__ : print( "DEBUG>> file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
self.mbs_link = None
symb_file = "mbs_link_PRJ.py"
path = os.path.abspath(os.path.join(template_path, symb_file))
module = imp.load_source(symb_file[:-3], path)
self.mbs_link = module.link
self.ptrs_to_symb_fcts["mbs_link"] = mbs_link_wrap(lambda frc,trq,Flnk,Z,Zd,mbs,tsim : self.mbs_link(self.frc,self.trq,self.Fl,self.Z,self.Zd, self, tsim))
data_c_ptr.mbs_link = self.ptrs_to_symb_fcts["mbs_link"]
# mbs_link3D
symb_file = "mbs_link3D_"+mbs_name+".py"
path = os.path.abspath(os.path.join(symb_path, symb_file))
if os.path.isfile(path):
module = imp.load_source(symb_file[:-3], path)
self.mbs_link3D = module.link3D
self.ptrs_to_fcts.append(mbs_link3D_wrap(lambda frc,trq,mbs,tsim : self.mbs_link3D(self.frc,self.trq,self, tsim)))
data_c_ptr.mbs_link3D = self.ptrs_to_fcts[-1]
else:
if not os.path.isfile(path):
if __DEBUG__ : print( "DEBUG>> file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
self.mbs_link3D = None
symb_file = "mbs_link3D_PRJ.py"
path = os.path.abspath(os.path.join(template_path, symb_file))
module = imp.load_source(symb_file[:-3], path)
self.mbs_link3D = module.link3D
self.ptrs_to_symb_fcts["mbs_link3D"] = mbs_link3D_wrap(lambda frc,trq,mbs,tsim : self.mbs_link3D(self.frc,self.trq,self, tsim))
data_c_ptr.mbs_link3D = self.ptrs_to_symb_fcts["mbs_link3D"]
# mbs_gensensor
# mbs_sensor
symb_file = "mbs_sensor_"+mbs_name+".py"
path = os.path.abspath(os.path.join(symb_path, symb_file))
if os.path.isfile(path):
module = imp.load_source(symb_file[:-3], path)
self.mbs_sensor = module.sensor
self.ptrs_to_fcts.append(mbs_sensor_wrap(lambda sens,mbs,isens : self.__callback_mbs_sensor(self.mbs_sensor, sens, isens)))
data_c_ptr.mbs_sensor = self.ptrs_to_fcts[-1]
else:
if not os.path.isfile(path):
if __DEBUG__ : print( "DEBUG>> file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
self.mbs_sensor = None
symb_file = "mbs_sensor_PRJ.py"
path = os.path.abspath(os.path.join(template_path, symb_file))
module = imp.load_source(symb_file[:-3], path)
self.mbs_sensor = module.sensor
self.ptrs_to_symb_fcts["mbs_sensor"] = mbs_sensor_wrap(lambda sens,mbs,isens : self.__callback_mbs_sensor(self.mbs_sensor, sens, isens))
data_c_ptr.mbs_sensor = self.ptrs_to_symb_fcts["mbs_sensor"]
# Callback function for function with advanced arguments
......@@ -717,6 +673,10 @@ class MbsData(object):
def __callback_mbs_invdyna(self, fun,Q, tsim):
__Q = np.ctypeslib.as_array(Q, (self.njoint+1,))
fun(self, tsim, __Q)
def callback_undefined(self,function_name=None):
print("undefined functions: "+function_name)
return
def set_nb_userc(self, nb):
""" Set the number of user constraints.
......@@ -972,4 +932,4 @@ class MbsData(object):
@property
def mbs_name(self):
return ctypes.string_at(self.mbs_data_ptr.contents.mbs_name).decode("utf-8")
\ No newline at end of file
return ctypes.string_at(self.mbs_data_ptr.contents.mbs_name).decode("utf-8")
MBsysC/mbs_interface/MBsysPy/mbsyspy/mbs_dirdyn.py
View file @ 23e67722
......@@ -44,7 +44,7 @@ from ..mbsysc_loader.callback import mbs_dirdyna_wrap
#==============================================================================
# Global parameter of the current module
#==============================================================================
__DEBUG__ = True
__DEBUG__ = False
__MODULE_DIR__ = os.path.dirname(os.path.abspath(__file__))
......@@ -98,7 +98,8 @@ class MbsDirdyn(object):
self.symbolic_path = symbolic_path
# pointers list to avoid garbage collecting
self.ptrs_to_fcts = []
self.ptrs_to_user_fcts = dict()
self.ptrs_to_symb_fcts = dict()
# [0]: user_cons_hJ
# [1]: user_cons_jdqd
# [2]: user_dirdyn_init
......@@ -167,50 +168,45 @@ class MbsDirdyn(object):
- user_dirdyn_loop (from user_dirdyn.py)
- user_dirdyn_finish (from user_dirdyn.py)
"""
template_path = os.path.join(__MODULE_DIR__,'../templates/user')
# cons_hJ
user_file = "cons_hJ.py"
path = os.path.abspath(os.path.join(user_path, user_file))
if os.path.isfile(path):
module = imp.load_source(user_file[:-3], path)
self.user_cons_hJ = module.user_cons_hJ
self.ptrs_to_fcts.append(user_cons_hJ_wrap(lambda h,Jac,mbs,tsim : self.user_cons_hJ(self._huserc, self._Juserc, self.mbs, tsim)))
else:
if not os.path.isfile(path):
if __DEBUG__ : print( "DEBUG>> file '"+user_file+"' not found in folder '"+os.path.dirname(path))
self.ptrs_to_fcts.append(user_cons_hJ_wrap(lambda h,Jac,mbs,tsim : None))
self.user_cons_hJ = lambda *args: None
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
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))
# cons_jdqd
user_file = "cons_jdqd.py"
path = os.path.abspath(os.path.join(user_path, user_file))
if os.path.isfile(path):
module = imp.load_source(user_file[:-3], path)
self.user_cons_jdqd = module.user_cons_jdqd
self.ptrs_to_fcts.append(user_cons_jdqd_wrap(lambda h,Jac,mbs,tsim : self.user_cons_jdqd(self._jdqduserc, self.mbs, tsim)))
else:
if not os.path.isfile(path):
if __DEBUG__ : print( "DEBUG>> file '"+user_file+"' not found in folder '"+os.path.dirname(path))
self.ptrs_to_fcts.append(user_cons_jdqd_wrap(lambda h,Jac,mbs,tsim : None))
self.user_cons_jdqd = lambda *args: None
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
self.ptrs_to_user_fcts["user_cons_jdqd"] = user_cons_jdqd_wrap(lambda h,Jac,mbs,tsim : self.user_cons_jdqd(self._jdqduserc, self.mbs, tsim))
# user_dirdyn
user_file = "user_dirdyn.py"
path = os.path.abspath(os.path.join(user_path, user_file))
if os.path.isfile(path):
if __DEBUG__ : print( "DEBUG>> loading file '"+user_file+"' in folder '"+os.path.dirname(path))
module = imp.load_source(user_file[:-3], path)
self.user_dirdyn_init = module.user_dirdyn_init
self.user_dirdyn_loop = module.user_dirdyn_loop
self.user_dirdyn_finish = module.user_dirdyn_finish
self.ptrs_to_fcts.append(user_dirdyn_init_wrap(lambda mbs, dd : self.mbs.user_dirdyn_init(self.mbs, self)))
self.ptrs_to_fcts.append(user_dirdyn_loop_wrap(lambda mbs, dd : self.mbs.user_dirdyn_loop(self.mbs, self)))
self.ptrs_to_fcts.append(user_dirdyn_finish_wrap(lambda mbs, dd : self.mbs.user_dirdyn_finish(self.mbs, self)))
else:
if not os.path.isfile(path):
print ("file '"+user_file+"' not found in folder '"+os.path.dirname(path))
self.ptrs_to_fcts.append(user_dirdyn_init_wrap(lambda mbs, dd : None))
self.ptrs_to_fcts.append(user_dirdyn_loop_wrap(lambda mbs, dd : None))
self.ptrs_to_fcts.append(user_dirdyn_finish_wrap(lambda mbs, dd : None))
self.mbs.user_dirdyn_init = lambda *args: None
self.mbs.user_dirdyn_loop = lambda *args: None
self.mbs.user_dirdyn_finish = lambda *args: None
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_dirdyn_init = module.user_dirdyn_init
self.user_dirdyn_loop = module.user_dirdyn_loop
self.user_dirdyn_finish = module.user_dirdyn_finish
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))
return
def __load_symbolic_fct__(self, symbolic_path):
......@@ -229,55 +225,66 @@ class MbsDirdyn(object):
- 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 os.path.isfile(path):
module = imp.load_source(symb_file[:-3], path)
self.mbs_cons_hJ = module.cons_hJ
self.ptrs_to_fcts.append(mbs_cons_hJ_wrap(lambda h,Jac,mbs,tsim : self.mbs_cons_hJ(self._h, self._Jac, self.mbs)))
else:
if not os.path.isfile(path):
if __DEBUG__ : print( "DEBUG>> file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
self.ptrs_to_fcts.append(mbs_cons_hJ_wrap(lambda h,Jac,mbs,tsim : None))
self.mbs_cons_hJ = lambda *args: None
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
self.ptrs_to_symb_fcts["mbs_cons_hJ"] = mbs_cons_hJ_wrap(lambda h,Jac,mbs,tsim : self.__callback_mbs_cons_hJ(self.mbs_cons_hJ,h,Jac))
# mbs_cons_jdqd
symb_file = "mbs_cons_jdqd_"+mbs_name+".py"
path = os.path.abspath(os.path.join(symbolic_path, symb_file))
if os.path.isfile(path):
module = imp.load_source(symb_file[:-3], path)
self.mbs_cons_jdqd = module.cons_jdqd
self.ptrs_to_fcts.append(mbs_cons_jdqd_wrap(lambda jdqd,mbs,tsim : self.mbs_cons_jdqd(self._jdqd, self.mbs)))
else:
if not os.path.isfile(path):
if __DEBUG__ : print( "DEBUG>> file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
self.ptrs_to_fcts.append(mbs_cons_jdqd_wrap(lambda jdqd,mbs,tsim : None))
self.mbs_cons_jdqd = lambda *args: None
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
self.ptrs_to_symb_fcts["mbs_cons_jdqd"] = mbs_cons_jdqd_wrap(lambda jdqd,mbs,tsim : self.mbs_cons_jdqd(self._jdqd, self.mbs))
# invdyna
symb_file = "mbs_invdyna_"+mbs_name+".py"
path = os.path.abspath(os.path.join(symbolic_path, symb_file))
if os.path.isfile(path):
module = imp.load_source(symb_file[:-3], path)
self.mbs_invdyna = module.invdyna
self.ptrs_to_fcts.append(mbs_invdyna_wrap(lambda Q,mbs,tsim : self.mbs_invdyna(self.mbs, tsim, None)))
else:
if not os.path.isfile(path):
if __DEBUG__ : print( "DEBUG>> file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
self.ptrs_to_fcts.append(mbs_invdyna_wrap(lambda Q,mbs,tsim : None))
self.mbs_invdyna = lambda *args: None
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
self.ptrs_to_symb_fcts["mbs_invdyna"] = mbs_invdyna_wrap(lambda Q,mbs,tsim : self.mbs_invdyna(self.mbs, tsim, None))
# dirdyna
symb_file = "mbs_dirdyna_"+mbs_name+".py"
path = os.path.abspath(os.path.join(symbolic_path, symb_file))
if os.path.isfile(path):
module = imp.load_source(symb_file[:-3], path)
self.mbs_dirdyna = module.dirdyna
self.ptrs_to_fcts.append(mbs_dirdyna_wrap(lambda M,c,mbs,tsim : self.mbs_dirdyna(self._M, self._c, self.mbs, tsim)))
else:
if not os.path.isfile(path):
if __DEBUG__ : print( "DEBUG>> file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
self.ptrs_to_fcts.append(mbs_dirdyna_wrap(lambda M,c,mbs,tsim : None))
self.mbs_dirdyna = lambda *args: None
symb_file = "mbs_dirdyna_PRJ.py"
path = os.path.abspath(os.path.join(template_path, symb_file))
module = imp.load_source(symb_file[:-3], path)
self.mbs_dirdyna = module.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))
return
# Callback function for function with advanced arguments
def __callback_user_cons_hJ(self, fun, h, Jac, tsim):
__h = np.ctypeslib.as_array(h, (self.mbs.Nuserc+1,))
__Jac = np.ctypeslib.as_array(Jac[0], (self.mbs.Nuserc+1,self.mbs.njoint+1))
fun(__h,__Jac,self.mbs,tsim)
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 run(self):
# [0]: user_cons_hJ
# [1]: user_cons_jdqd
......@@ -290,26 +297,49 @@ class MbsDirdyn(object):
# [6]: mbs_dirdyna
# Assing required user functions
self.mbs.user_cons_hJ = self.user_cons_hJ
self.mbs.mbs_data_ptr.contents.user_cons_hJ = self.ptrs_to_fcts[0]
self.mbs.mbs_data_ptr.contents.user_cons_hJ = self.ptrs_to_user_fcts["user_cons_hJ"]
self.mbs.user_cons_jdqd = self.user_cons_jdqd
self.mbs.mbs_data_ptr.contents.user_cons_jdqd = self.ptrs_to_fcts[1]
self.mbs.mbs_data_ptr.contents.user_cons_jdqd = self.ptrs_to_user_fcts["user_cons_jdqd"]
self.mbs.user_dirdyn_init = self.user_dirdyn_init
self.mbs.mbs_data_ptr.contents.user_dirdyn_init = self.ptrs_to_fcts[2]
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_fcts[3]
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_fcts[4]
self.mbs.mbs_data_ptr.contents.user_dirdyn_finish = self.ptrs_to_user_fcts["user_dirdyn_finish"]
# Assing required symbolic functions
self.mbs.mbs_cons_hJ = self.mbs_cons_hJ
self.mbs.mbs_data_ptr.contents.mbs_cons_hJ = self.ptrs_to_fcts[5]
self.mbs.mbs_data_ptr.contents.mbs_cons_hJ = self.ptrs_to_symb_fcts["mbs_cons_hJ"]
self.mbs.mbs_cons_jdqd = self.mbs_cons_jdqd
self.mbs.mbs_data_ptr.contents.mbs_cons_jdqd = self.ptrs_to_fcts[6]
self.mbs.mbs_data_ptr.contents.mbs_cons_jdqd = self.ptrs_to_symb_fcts["mbs_cons_jdqd"]
self.mbs.mbs_invdyna = self.mbs_invdyna
self.mbs.mbs_data_ptr.contents.mbs_invdyna = self.ptrs_to_fcts[7]
self.mbs.mbs_data_ptr.contents.mbs_invdyna = self.ptrs_to_symb_fcts["mbs_invdyna"]
self.mbs.mbs_dirdyna = self.mbs_dirdyna
self.mbs.mbs_data_ptr.contents.mbs_dirdyna = self.ptrs_to_fcts[8]
self.mbs.mbs_data_ptr.contents.mbs_dirdyna = self.ptrs_to_symb_fcts["mbs_dirdyna"]
libmodules.mbs_run_dirdyn(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
# Anassign user functions
self.mbs.user_cons_hJ = None
self.mbs.mbs_data_ptr.contents.user_cons_hJ = user_cons_hJ_wrap(lambda h,Jac,mbs,tsim : self.mbs.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 : self.mbs.callback_undefined("user_cons_jdqd"))
self.mbs.user_dirdyn_init = None
self.mbs.mbs_data_ptr.contents.user_dirdyn_init = user_dirdyn_init_wrap(lambda mbs, dd : self.mbs.callback_undefined("user_dirdyn_init"))
self.mbs.user_dirdyn_loop = None
self.mbs.mbs_data_ptr.contents.user_dirdyn_loop = user_dirdyn_loop_wrap(lambda mbs, dd : self.mbs.callback_undefined("user_dirdyn_loop"))
self.mbs.user_dirdyn_finish = None
self.mbs.mbs_data_ptr.contents.user_dirdyn_finish = user_dirdyn_finish_wrap(lambda mbs, dd : self.mbs.callback_undefined("user_dirdyn_finish"))
# Anassing required symbolic functions
self.mbs.mbs_cons_hJ = None
self.mbs.mbs_data_ptr.contents.mbs_cons_hJ = mbs_cons_hJ_wrap(lambda h,Jac,mbs,tsim : self.mbs.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 : self.mbs.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 : self.mbs.callback_undefined("mbs_invdyna"))