# -*- coding: utf-8 -*- """ Main class for Multibody systems. Summary ------- Define the class MbsData based on the MbsData structure of MBsysC. This class has the functions required to manipulate the Data (set the number of user constraints, set the independant or dependant variables...). """ # MBsysC version : 1.13.0 # Author : Robotran Team # (c) Universite catholique de Louvain, 2019 import os import importlib import ctypes import numpy as np # importing MbsysPy functions from ..mbs_utilities import mbs_msg from ..mbs_utilities import mbs_warning from ..mbs_utilities import mbs_error from ..mbs_utilities import bytes_to_str from ..mbs_utilities import str_to_bytes from ..mbs_utilities import convert_numpy_type_to_int from ..mbs_utilities import convert_numpy_type_to_float # importing MbsysPy classes from .mbs_sensor import MbsSensor from .mbs_part import MbsPart from .mbs_equil import MbsEquil from .mbs_dirdyn import MbsDirdyn from .mbs_modal import MbsModal from .mbs_solvekin import MbsSolvekin from .mbs_invdyn import MbsInvdyn # importing libraries from .._mbsysc_loader.loadlibs import libutilities from .._mbsysc_loader.loadlibs import libloadXML # 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_Derivative_wrap from .._mbsysc_loader.callback import user_DrivenJoints_wrap from .._mbsysc_loader.callback import user_ExtForces_wrap from .._mbsysc_loader.callback import user_JointForces_wrap from .._mbsysc_loader.callback import user_LinkForces_wrap from .._mbsysc_loader.callback import user_Link3DForces_wrap from .._mbsysc_loader.callback import user_dirdyn_init_wrap from .._mbsysc_loader.callback import user_dirdyn_loop_wrap from .._mbsysc_loader.callback import user_dirdyn_finish_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 user_invdyn_init_wrap from .._mbsysc_loader.callback import user_invdyn_loop_wrap from .._mbsysc_loader.callback import user_invdyn_finish_wrap from .._mbsysc_loader.callback import mbs_accelred_wrap from .._mbsysc_loader.callback import mbs_extforces_wrap from .._mbsysc_loader.callback import mbs_gensensor_wrap from .._mbsysc_loader.callback import mbs_link_wrap from .._mbsysc_loader.callback import mbs_link3D_wrap from .._mbsysc_loader.callback import mbs_sensor_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 from .._mbsysc_loader.callback import mbs_dirdyna_wrap # ============================================================================= # Global parameter of the current module # ============================================================================= __DEBUG__ = False __MODULE_DIR__ = os.path.dirname(os.path.abspath(__file__)) # ============================================================================= # Defining Python MbsData class # ============================================================================= class MbsData(object): """ Class containing geometrical and dynamical information of robotran Mbs. Attributes ---------- body_id : dict Dictionary containing the names of the bodies as keys and their ids as values. DonePart : int Flag that indicates if the coordinate partitioning module has been executed (default: 0=not done; 1=done). DoneEquil : int Flag that indicates if the equilibrium module has been executed (default: 0=not done; 1=done). DoneModal : int Flag that indicates if the modal module has been executed (default: 0=not done; 1=done). dpt : numpy.ndarray Numpy array containing the coordinates of all anchor points in body fixed frame: [X_i,Y_i,Z_i] = dpt[1:4,i] extforce_id : dict Dictionary containing the names of the extforces as keys and their ids as values. Fl : numpy.ndarray Numpy array containing the current values of the forces on each link. frc: numpy.ndarray Numpy array containing the components of the resultant external forces (in the body fixed frame) applied to the center of mass of each body. g : numpy.ndarray The 3 gravity components in the inertial frame: g[1:4]=[g_x, g_y, g_z] In : numpy.ndarray Numpy array containing the inertia tensor component of each body (in the body fixed frame, relative to the center of mass). For body k: In[1:10, k]=[I_11,I_12,I_13,I_21,I_22,I_23,I_31,I_32,I_33] joint_id : dict Dictionary containing the names of the joints as keys and their ids as values. l : numpy.ndarray Numpy array containing the center of mass coordinates in the body fixed frame. For body k: l[1:4, k]=[gc_x, gc_y, gc_z] lambda_ : numpy.ndarray Numpy array containing the values of the Lagrange Multipliers related to the constraints. The sequence is the same sequence as the constraint definition. link_id : dict Dictionary containing the names of the links as keys and their ids as values. lrod : numpy.ndarray Numpy array containing the length of each rod. m : numpy.ndarray Numpy array containing the mass of each body. mbs_filename : str Path to mbs file including the file with the extension (.mbs). mbs_name : str Filename of the mbs file (excluding path and ".mbs" extension). nbody : int Number of bodies in the system. The fictious bodies are not taken into account. Ncons : int Number of algebraic constraints. nhu : int Number of independent constraints. njoint : int Number of joints in the system. Nlink : int Number of forces acting between two points of the system. Nloopc : int Number of loop constraints. npt : int Number of anchor points. nqa : int Number of actuated articulations. nqc : int Number of driven articulations, it includes qlocked and qdriven. nqdriven : int Number of driven articulations. nqlocked : int Number of locked articulations. nqu : int Number of independent articulations. nqv : int Number of dependent articulations. NRerr : float Maximal error on constraint allowed in Newton-Raphson algorithm, default 1.0e-9. nrod : int Number of rod constraints defined on the system. Nsensor : int Number of kinematic sensors. Nuserc : int Number of user constraints. Nux : int Number of user variable. Nxfrc : int Number of points where an external force is applied into a body. points_id : dict Dictionary containing the ids of the anchor points of each bodies. The first key is the body name and the second key is the anchor point name. The retrieve value is the id of the requested anchor point. Example: my_id = mbs_data.points_id["body_1"]["point_0"] process : int Flag that indicate which module is currently running: 1=partitioning; 2=equilibrium; 3=direct dynamic; 4=modal project_path : str Path to the mbs project folder. q : numpy.ndarray Numpy array containing the current values of the generalized coordinates. q0 : numpy.ndarray Numpy array containing the initial values of the generalized coordinates. qa : numpy.ndarray of int Numpy array of integers containing the indices of actuated articulations (only for inverse dynamic). Those articulations are controlled by an actuator. Qa : numpy.ndarray Numpy array containing the value of actuation forces. qc : numpy.ndarray of int Numpy array of integers containing the indices of driven (locked and driven) articulations. Qc : numpy.ndarray Numpy array containing the value of joint force introduced in driven joints to respect the user function. The driven forces/torques are saved in the entries given by index vector 'qc'. qd : numpy.ndarray Numpy array containing the current values of the generalized velocities. qd0 : numpy.ndarray Numpy array containing the initial values of the generalized velocities. qdd : numpy.ndarray Numpy array containing the current values of the generalized accelerations. qdd0 : numpy.ndarray Numpy array containing the initial values of the generalized accelerations. qdriven : numpy.ndarray of int Numpy array of integers containing the indices of driven articulations. Those articulations are controlled by a user function: q[qdriven]=f(tsim) qlocked : numpy.ndarray of int Numpy array of integers containing the indices of locked articulations. Those articulations have a constant position defined by the user: q[qlocked]=cte Qq : numpy.ndarray Numpy array containing the values of the joint forces. qu : numpy.ndarray of int Numpy array of integers containing the indices of the independent articulations. qv : numpy.ndarray of int Numpy array of integers containing the indices of the dependent articulations. sensor_id : dict Dictionary containing the names of the sensors as keys and their ids as values. sensors : list of MbsSensor List containing one instance of MbsSensor (without defined id). User can append as many as needed sensor in the list. Sensors defined in this list can be used by the user for various computation. SWr : numpy.ndarray Numpy array containing the Swr vector for each external forces. for force k, Swr[k,1:10] = [Fx, Fy, Fz, Mx, My, Mz, dxF] with - Force components (expressed in the inertial frame) : Fx,Fy,Fz - Pure torque components (expressed in the inertial frame) : Mx,My,Mz - Application point local coordinates vector (expressed in the body fixed frame): dxF[1:4] t0 : scalar Initial time of the simulation [s]. For dirdyn and invdyn only. This parameter is set from dirdyn/invyn option at start of time simulation. tf : scalar Final time of the simulation [s]. For dirdyn and invdyn only. This parameter is set from dirdyn/invyn option at start of time simulation. trq : numpy.ndarray Numpy array containing the components of the resultant external torques (pure torque and couple produced by forces displacement in the body fixed frame) applied to each body. tsim : scalar The time value. user_model : dict Dictionary containing the names of the user models as keys and a second dictionary with their parameters as values. The second dictionary contains the names of the parameters as keys and their values as values ux : numpy.ndarray Numpy array containing the values of the user variables. ux0 : numpy.ndarray Numpy array containing the initial values of the user variables. uxd : numpy.ndarray Numpy array containing the values of the time derivative of the user variables. xfidpt : numpy.ndarray of int Numpy array of integers containing the indices of the points defined as force application points. Z : numpy.ndarray Numpy array containing the current values of the distances between of the points of a link. Zd : numpy.ndarray Numpy array containing the current values of the speed (spreading) between of the points of a link. Examples -------- >>> mbs_data = MBsysPy.MbsData("../dataR/ExampleProject.mbs") >>> mbs_data.user_model["MyUserModel"]["MyScalar"] = 3.0 >>> mbs_msg(mbs_data.user_model["MyUserModel"]["MyScalar"]) 3.0 >>> #get a copy of the scalar >>> a = mbs_data.user_model["MyUserModel"]["MyScalar"] >>> a = 2 >>> mbs_msg(mbs_data.user_model["MyUserModel"]["MyScalar"]) 3.0 >>> print(mbs_data.user_model["MyUserModel"]["MyVector"]) array([1.0, 3.0]) >>> mbs_data.user_model["MyUserModel"]["MyVector"] = [1.0, 2.0] >>> print(mbs_data.user_model["MyUserModel"]["MyVector"]) array([1.0, 2.0]) >>> b = mbs_data.user_model["MyUserModel"]["MyVector"] >>> b[1] = 10.0 >>> print(mbs_data.user_model["MyUserModel"]["MyVector"]) array([1.0, 10.0]) >>> b = np.array([2.0, 4.0]) >>> print(mbs_data.user_model["MyUserModel"]["MyVector"]) array([1.0, 10.0]) """ def __init__(self, name, user_path=None, symbolic_path=None, prj_functions_c=None, prj_lib_path=None): """ Load a specific *.mbs file into a MbsData class instance. Parameters ---------- name : str The path to the .mbs file to load, including the file extension. user_path : str, optionnal The path to the folder containing the user functions if the default project structure are not used. symbolic_path : str, optionnal The path to the folder containing the symbolic functions if the default project structure are not used. prj_functions_c : None or str, optionnal Option to load some of the project functions from the project C library. Accepted values are: - None : All project function are defined in python - "symbolic_only" or "S" : Symbolic function comes from C library - "symbolic_and_user" or "SU" : Symbolic and user functions come from C library default is None prj_lib_path : None or str, optionnal Required if 'prj_functions_c' is not None - In Linux, it gives the location of the folder containing the "symbolicR/" and, or "userfctR/" folders. Each one of the subfolder have to contain the corresponding library (ie: libProject_symbolic.so). - In Windows, it gives the location of the folder containing the project libraries (Project_symbolic.dll and/or Project_userfct.dll). - In MacOs the expected behavior is the same as Linux. It must be tested. default is None Returns ------- out : MBsysPy.MbsData A loaded mbs file in a MbsData instance. """ # Assing C pointer to NULL self.mbs_infos_ptr = None self.mbs_data_ptr = None # Retrieve prj_functions_c value if prj_functions_c is None: self.opt_load_c = 0 if not (prj_lib_path is None): mbs_msg(">>MBsData>> Argument 'prj_lib_path' is ignored as option 'prj_functions_c' is 'None'\n.") prj_lib_path = None elif (type(prj_functions_c) is str) or (type(prj_functions_c) is bytes): if (type(prj_functions_c) is bytes): prj_functions_c = bytes_to_str(prj_functions_c) if prj_functions_c == "symbolic_only" or prj_functions_c == "S": self.opt_load_c = 1 elif prj_functions_c == "symbolic_and_user" or prj_functions_c == "SU": self.opt_load_c = 2 else: raise ValueError("prj_functions_c is not valid:'" + prj_functions_c + "'.") if (prj_lib_path is None): raise ValueError("prj_lib_path must be given if 'prj_functions_c' is set.") if not (type(prj_lib_path) is str): raise TypeError("prj_lib_path type must be str but it is '" + str(type(prj_lib_path)) + "'.") else: prj_lib_path = str_to_bytes(os.path.abspath(prj_lib_path)) else: raise TypeError("prj_functions_c type is not valid:'" + str(type(prj_functions_c)) + "'.") # Create a byte object from the string name_c = str_to_bytes(name) # Load the MbsInfos and keep a copy if __DEBUG__: mbs_msg("DEBUG>> Loading: '" + bytes_to_str(name) + "'\n" + " from:'" + os.getcwd() + "'.\n") self.mbs_infos_ptr = libloadXML.mbs_info_reader(name_c) if not self.mbs_infos_ptr: mbs_msg("\n--------------------------------------------------\n" "READ CAREFULLY !!!\n" "--------------------------------------------------\n\n" "MBsysC was unable to create MbsInfos structure.\n" "The messages above give deeper informations on what went wrong.\n" "The messages below gives classic error and error backtrace.\n\n" "Usual errors are:\n" " - Incorrect file path and name;\n" " - Corrupted project file;\n" "\n--------------------------------------------------\n" "\n--------------------------------------------------\n") raise RuntimeError("MbsData loading failed, read previous messages.") if __DEBUG__: mbs_msg("DEBUG>> mbs_infos structure loaded") # Preparing loader and its options for loading loader = libloadXML.mbs_new_loader() if __DEBUG__: mbs_msg("DEBUG>> Loader structure created") loader.contents.opts.contents.no_project_fct = self.opt_load_c if __DEBUG__: mbs_msg("DEBUG>> flag ignoring project function set") loader.contents.mbs_infos = self.mbs_infos_ptr if __DEBUG__: mbs_msg("DEBUG>> mbs_infos assigned to loader") # Load and retrieve MbsData structure self.mbs_data_ptr = libloadXML.mbs_load_with_loader(name_c, prj_lib_path, loader) if not self.mbs_data_ptr: mbs_msg("\n--------------------------------------------------\n" "READ CAREFULLY !!!\n" "--------------------------------------------------\n\n" "MBsysC was unable to create MbsData structure.\n" "The messages above give deeper informations on what went wrong.\n" "The messages below gives classic error and error backtrace.\n\n" "Usual errors are:\n" " - Invalid project file;\n" " - Invalid path for project C libraries;\n" "\n--------------------------------------------------\n" "\n--------------------------------------------------\n") libloadXML.mbs_delete_loader(loader) raise RuntimeError("MbsData loading failed, read previous messages.") if __DEBUG__: mbs_msg("DEBUG>> MbsData loaded") # Project with link3D are not (yet) compatible if self.mbs_data_ptr.contents.Nlink3D: print("The Project contains 3Dlinks which are an advanced feature not" " yet supported in MBsysPy.\nUncheck the 'Link 3D' box in " "MBsysPAD, or migrate to MBsysC/MBsysLab.") raise RuntimeError("ERROR: 3Dlinks are not yet supported in MBsysPy, modify your project.") libloadXML.mbs_delete_loader(loader) # symbolic and user function folder self.user_path = None self.symbolic_path = None # pointers dict to avoid garbage collecting self.ptrs_to_user_fcts = dict() self.ptrs_to_symb_fcts = dict() # Creating field for all project function. user_fun_list = ['cons_hJ', 'cons_jdqd', 'derivative', 'DrivenJoints', 'ExtForces', 'JointForces', 'LinkForces', 'Link3DForces', 'dirdyn_init', 'dirdyn_loop', 'dirdyn_finish', 'equil_init', 'equil_loop', 'equil_finish', 'equil_fxe', 'invdyn_init', 'invdyn_loop', 'invdyn_finish', ] symb_fun_list = ['accelred', 'cons_hJ', 'cons_jdqd', 'invdyna', 'dirdyna', 'extforces', 'gensensor', 'link', 'link3D', 'sensor' ] self.__assign_user_fct_to_none__(user_fun_list) if self.opt_load_c < 2: self.__assign_user_to_undefined__(user_fun_list) self.__assign_symb_fct_to_none__(symb_fun_list) if self.opt_load_c < 1: self.__assign_symb_to_undefined__(symb_fun_list) # Exposing some memory if __DEBUG__: mbs_msg("DEBUG>> Exposing MbsData fields") # Geometric and dynamic datas self.__dpt = None if(self.npt): self.__dpt = np.ctypeslib.as_array(self.mbs_data_ptr.contents.dpt[0], (3 + 1, self.npt + 1)) self.__l = np.ctypeslib.as_array(self.mbs_data_ptr.contents.l[0], (3 + 1, self.njoint + 1)) self.__m = np.ctypeslib.as_array(self.mbs_data_ptr.contents.m, (self.njoint + 1, )) self.__In = np.ctypeslib.as_array(self.mbs_data_ptr.contents.In[0], (9 + 1, self.njoint + 1)) self.__g = np.ctypeslib.as_array(self.mbs_data_ptr.contents.g, (4, )) # Partitioning informations self.__qu = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qu, (self.njoint + 1, )) self.__qu.flags["WRITEABLE"] = False self.__qc = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qc, (self.njoint + 1, )) self.__qc.flags["WRITEABLE"] = False self.__qlocked = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qlocked, (self.njoint + 1, )) self.__qlocked.flags["WRITEABLE"] = False self.__qdriven = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qdriven, (self.njoint + 1, )) self.__qdriven.flags["WRITEABLE"] = False self.__qa = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qa, (self.njoint + 1, )) self.__qa.flags["WRITEABLE"] = False self.__qv = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qv, (self.njoint + 1, )) self.__qv.flags["WRITEABLE"] = False # Generalized coordinates velocities and acceleration self.__q = np.ctypeslib.as_array(self.mbs_data_ptr.contents.q, (self.njoint + 1, )) self.__qd = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qd, (self.njoint + 1, )) self.__qdd = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qdd, (self.njoint + 1, )) self.__q0 = np.ctypeslib.as_array(self.mbs_data_ptr.contents.q0, (self.njoint + 1, )) self.__qd0 = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qd0, (self.njoint + 1, )) self.__qdd0 = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qdd0, (self.njoint + 1, )) # Forces array self.__frc = np.ctypeslib.as_array(self.mbs_data_ptr.contents.frc[0], (3 + 1, self.njoint + 1)) self.__trq = np.ctypeslib.as_array(self.mbs_data_ptr.contents.trq[0], (3 + 1, self.njoint + 1)) self.__Qq = np.ctypeslib.as_array(self.mbs_data_ptr.contents.Qq, (self.njoint + 1, )) self.__Qa = np.ctypeslib.as_array(self.mbs_data_ptr.contents.Qa, (self.njoint + 1, )) # Constraints datas self.nrod = 0 self.__lrod = None if (self.mbs_data_ptr.contents.lrod): self.nrod = int(round(self.mbs_data_ptr.contents.lrod[0])) self.__lrod = np.ctypeslib.as_array(self.mbs_data_ptr.contents.lrod, (self.nrod + 1, )) self.__lambda_ = np.ctypeslib.as_array(self.mbs_data_ptr.contents.lambda_, (self.njoint + 1, )) self.__Qc = np.ctypeslib.as_array(self.mbs_data_ptr.contents.Qc, (self.njoint + 1, )) # LinksForces self.__Z = self.__Zd = self.__Fl = None if self.Nlink: self.__Z = np.ctypeslib.as_array(self.mbs_data_ptr.contents.Z, (self.Nlink + 1, )) self.__Zd = np.ctypeslib.as_array(self.mbs_data_ptr.contents.Zd, (self.Nlink + 1, )) self.__Fl = np.ctypeslib.as_array(self.mbs_data_ptr.contents.Fl, (self.Nlink + 1, )) # ExtForces self.__xfidpt = self.__SWr = None if self.Nxfrc: self.__xfidpt = np.ctypeslib.as_array(self.mbs_data_ptr.contents.xfidpt, (self.Nxfrc + 1, )) self.__xfidpt.flags["WRITEABLE"] = False self.__SWr = np.ctypeslib.as_array(self.mbs_data_ptr.contents.SWr[0], (self.Nxfrc + 1, 9 + 1)) # User State self.__ux = self.__ux0 = self.__uxd = None if self.Nux: self.__ux = np.ctypeslib.as_array(self.mbs_data_ptr.contents.ux, (self.Nux + 1, )) self.__ux0 = np.ctypeslib.as_array(self.mbs_data_ptr.contents.ux0, (self.Nux + 1, )) self.__uxd = np.ctypeslib.as_array(self.mbs_data_ptr.contents.uxd, (self.Nux + 1, )) # Adding internal sensor self.sensors = [MbsSensor(self, None)] # ID self.body_id = {} self.joint_id = {} self.link_id = {} self.extforce_id = {} self.sensor_id = {} self.points_id = {} self.__generate_id__() # UserModel self.user_model = _UserModelDict() self.__load_user_model__() # Loading user function if self.opt_load_c < 2: if __DEBUG__: mbs_msg("DEBUG>> Loading user functions") project_path = self.project_path # Creating user path if user_path is None: user_path = os.path.join(project_path, "userfctR") else: user_path = os.path.join(project_path, user_path) # Error handeling if not os.path.isdir(user_path): mbs_msg('The user function directory does not exist: "' + user_path + '"') mbs_msg('The current root folder is: "' + os.getcwd() + '"') self.user_path = user_path # Loading symbolic function if self.opt_load_c < 1: if __DEBUG__: mbs_msg("DEBUG>> Loading symbolic functions") project_path = self.project_path # Creating user path if symbolic_path is None: symbolic_path = os.path.join(project_path, "symbolicR") else: symbolic_path = os.path.join(project_path, symbolic_path) # Error handeling if not os.path.isdir(symbolic_path): mbs_msg('The symbolic function directory does not exist: "' + symbolic_path + '"') mbs_msg('The current root folder is: "' + os.getcwd() + '"') self.symbolic_path = symbolic_path def __str__(self): """Return str(self).""" if __DEBUG__: mbs_msg("DEBUG>> start of __str") libutilities.mbs_print_data(self.mbs_data_ptr) if __DEBUG__: mbs_msg("DEBUG>> MbsData printed") return '' def __assign_user_fct_to_none__(self, functions): """Create fields for user functions fields and set them to None. Parameters ---------- functions : List List of strings containing the names of the user functions to assign. Notes ----- Only the following user functions names can be passed as parameter: - 'cons_hJ' - 'cons_jdqd' - 'derivative' - 'DrivenJoints' - 'ExtForces' - 'JointForces' - 'LinkForces' - 'Link3DForces' - 'dirdyn_init' - 'dirdyn_loop' - 'dirdyn_finish' - 'equil_init' - 'equil_loop' - 'equil_finish' - 'equil_fxe' Returns ------- None. """ if isinstance(functions, list): for fun in functions: if fun == "cons_hJ": self.user_cons_hJ = None elif fun == "cons_jdqd": self.user_cons_jdqd = None elif fun == "derivative": self.user_derivative = None elif fun == "DrivenJoints": self.user_DrivenJoints = None elif fun == "ExtForces": self.user_ExtForces = None elif fun == "JointForces": self.user_JointForces = None elif fun == "LinkForces": self.user_LinkForces = None elif fun == "Link3DForces": self.user_Link3DForces = None elif fun == "dirdyn_init": self.user_dirdyn_init = None elif fun == "dirdyn_loop": self.user_dirdyn_loop = None elif fun == "dirdyn_finish": self.user_dirdyn_finish = None elif fun == "equil_init": self.user_equil_init = None elif fun == "equil_loop": self.user_equil_loop = None elif fun == "equil_finish": self.user_equil_finish = None elif fun == "equil_fxe": self.user_equil_fxe = None elif fun == "invdyn_init": self.user_invdyn_init = None elif fun == "invdyn_loop": self.user_invdyn_loop = None elif fun == "invdyn_finish": self.user_invdyn_finish = None else: raise TypeError(fun + " is not an existing user function") else: raise TypeError('The "functions" parameter must be a list, got a {:}.'.format(type(functions))) def __assign_symb_fct_to_none__(self, functions): """Create fields for symbolic functions fields and set them to None. Parameters ---------- functions : list List of strings containing the names of the symbolic functions to assign Notes ----- Only the following symbolic functions names can be passed as parameter: - 'accelred' - 'cons_hJ' - 'cons_jdqd' - 'invdyna' - 'dirdyna' - 'extforces' - 'gensensor' - 'link' - 'link3D' - 'sensor' Returns ------- None. """ if isinstance(functions, list): for fun in functions: if fun == "accelred": self.mbs_accelred = None elif fun == "cons_hJ": self.mbs_cons_hJ = None elif fun == "cons_jdqd": self.mbs_cons_jdqd = None elif fun == "dirdyna": self.mbs_dirdyna = None elif fun == "extforces": self.mbs_extforces = None elif fun == "gensensor": self.mbs_gensensor = None elif fun == "invdyna": self.mbs_invdyna = None elif fun == "link": self.mbs_link = None elif fun == "link3D": self.mbs_link3D = None elif fun == "sensor": self.mbs_sensor = None else: raise TypeError(fun + " is not an existing symbolic function") else: raise TypeError('The "functions" parameter must be a list, got a {:}.'.format(type(functions))) def __load_user_fct__(self, module_dir, functions, user_path): """Load specified user functions in MbsData instance. Load only if not yet loaded by the user. The functions will be assigned to the MbsData instance when the 'run' functions is called and unassigned at the end. Parameters ---------- module_dir : str The path to the module directory functions : List List of strings containing the names of the user functions to assign user_path: str The path to the folder containing the user functions. Notes ----- Only the following user functions names can be passed as parameter: - 'cons_hJ' - 'cons_jdqd' - 'derivative' - 'DrivenJoints' - 'ExtForces' - 'JointForces' - 'LinkForces' - 'Link3DForces' - 'dirdyn_init' - 'dirdyn_loop' - 'dirdyn_finish' - 'equil_init' - 'equil_loop' - 'equil_finish' - 'equil_fxe' - 'invdyn_init' - 'invdyn_loop' - 'invdyn_finish' """ template_path = os.path.join(module_dir, '../templates/user') if isinstance(functions, list): for fun in functions: if fun == "cons_hJ": if self.user_cons_hJ is None: # 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__: mbs_msg("DEBUG>> file '" + user_file + "' not found in folder '" + os.path.dirname(path)) path = os.path.abspath(os.path.join(template_path, user_file)) spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_cons_hJ = module.user_cons_hJ elif fun == "cons_jdqd": if self.user_cons_jdqd is None: # 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__: mbs_msg("DEBUG>> file '" + user_file + "' not found in folder '" + os.path.dirname(path)) path = os.path.abspath(os.path.join(template_path, user_file)) spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_cons_jdqd = module.user_cons_jdqd elif fun == "derivative": if self.user_derivative is None: # derivative user_file = "user_Derivative.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): if __DEBUG__: mbs_msg("DEBUG>> file '" + user_file + "' not found in folder '" + os.path.dirname(path)) path = os.path.abspath(os.path.join(template_path, user_file)) spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_derivative = module.user_derivatives elif fun == "DrivenJoints": if self.user_DrivenJoints is None: # drivenJoints user_file = "user_DrivenJoints.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): if __DEBUG__: mbs_msg("DEBUG>> file '" + user_file + "' not found in folder '" + os.path.dirname(path)) path = os.path.abspath(os.path.join(template_path, user_file)) spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_DrivenJoints = module.user_DrivenJoints elif fun == "ExtForces": if self.user_ExtForces is None: # ext_forces user_file = "user_ExtForces.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): if __DEBUG__: mbs_msg("DEBUG>> file '" + user_file + "' not found in folder '" + os.path.dirname(path)) path = os.path.abspath(os.path.join(template_path, user_file)) spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_ExtForces = module.user_ExtForces elif fun == "JointForces": if self.user_JointForces is None: # joint_forces user_file = "user_JointForces.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): if __DEBUG__: mbs_msg("DEBUG>> file '" + user_file + "' not found in folder '" + os.path.dirname(path)) path = os.path.abspath(os.path.join(template_path, user_file)) spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_JointForces = module.user_JointForces elif fun == "LinkForces": if self.user_LinkForces is None: # link_forces user_file = "user_LinkForces.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): if __DEBUG__: mbs_msg("DEBUG>> file '" + user_file + "' not found in folder '" + os.path.dirname(path)) path = os.path.abspath(os.path.join(template_path, user_file)) spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_LinkForces = module.user_LinkForces elif fun == "Link3DForces": if self.user_Link3DForces is None: # link3D_forces user_file = "user_Link3DForces.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): if __DEBUG__: mbs_msg("DEBUG>> file '" + user_file + "' not found in folder '" + os.path.dirname(path)) path = os.path.abspath(os.path.join(template_path, user_file)) spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_Link3DForces = module.user_Link3DForces elif fun == 'dirdyn_init': if self.user_dirdyn_init is None: # user_dirdyn user_file = "user_dirdyn.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg("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__: mbs_msg("DEBUG>> loading file '" + user_file + "' in folder '" + os.path.dirname(path)) spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_dirdyn_init = module.user_dirdyn_init elif fun == 'dirdyn_loop': if self.user_dirdyn_loop is None: # user_dirdyn user_file = "user_dirdyn.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg("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__: mbs_msg("DEBUG>> loading file '" + user_file + "' in folder '" + os.path.dirname(path)) spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_dirdyn_loop = module.user_dirdyn_loop elif fun == 'dirdyn_finish': if self.user_dirdyn_finish is None: # user_dirdyn user_file = "user_dirdyn.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg("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__: mbs_msg("DEBUG>> loading file '" + user_file + "' in folder '" + os.path.dirname(path)) spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_dirdyn_finish = module.user_dirdyn_finish elif fun == 'equil_init': if self.user_equil_init is None: user_file = "user_equil.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg("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__: mbs_msg("DEBUG>> loading file '" + user_file + "' in folder '" + os.path.dirname(path)) spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_equil_init = module.user_equil_init elif fun == 'equil_loop': if self.user_equil_loop is None: user_file = "user_equil.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg("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__: mbs_msg("DEBUG>> loading file '" + user_file + "' in folder '" + os.path.dirname(path)) spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_equil_loop = module.user_equil_loop elif fun == 'equil_finish': if self.user_equil_finish is None: user_file = "user_equil.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg("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__: mbs_msg("DEBUG>> loading file '" + user_file + "' in folder '" + os.path.dirname(path)) spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_equil_finish = module.user_equil_finish elif fun == 'equil_fxe': if self.user_equil_fxe is None: user_file = "user_equil.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg("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__: mbs_msg("DEBUG>> loading file '" + user_file + "' in folder '" + os.path.dirname(path)) spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) try: self.user_equil_fxe = module.user_equil_fxe # Attribute user_equil_fxe is only added on MbsEquil if the user defined a function. except AttributeError: path = os.path.abspath(os.path.join(template_path, user_file)) spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_equil_fxe = module.user_equil_fxe elif fun == 'invdyn_init': if self.user_invdyn_init is None: # user_invdyn user_file = "user_invdyn.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg("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__: mbs_msg("DEBUG>> loading file '" + user_file + "' in folder '" + os.path.dirname(path)) spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_invdyn_init = module.user_invdyn_init elif fun == 'invdyn_loop': if self.user_invdyn_loop is None: # user_invdyn user_file = "user_invdyn.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg("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__: mbs_msg("DEBUG>> loading file '" + user_file + "' in folder '" + os.path.dirname(path)) spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_invdyn_loop = module.user_invdyn_loop elif fun == 'invdyn_finish': if self.user_invdyn_finish is None: # user_invdyn user_file = "user_invdyn.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg("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__: mbs_msg("DEBUG>> loading file '" + user_file + "' in folder '" + os.path.dirname(path)) spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_invdyn_finish = module.user_invdyn_finish else: raise TypeError(fun + " is not an existing user function") else: raise TypeError('The "functions" parameter must be a list, got a {:}.'.format(type(functions))) def __load_symbolic_fct__(self, module_dir, functions, symbolic_path): """Load specified symbolic functions in MbsData instance. Load only if not yet loaded by the user. The functions will be assigned to the MbsData instance when the 'run' functions is called and unassigned at the end. Parameters ---------- module_dir : str The path to the module directory functions : List List of strings containing the names of the symbolic functions to assign symbolic_path: str The path to the folder containing the symbolic functions. Notes ----- Only the following symbolic functions names can be passed as parameter: - 'accelred' - 'cons_hJ' - 'cons_jdqd' - 'invdyna' - 'dirdyna' - 'extforces' - 'gensensor' - 'link' - 'link3D' - 'sensor' """ mbs_name = self.mbs_name template_path = os.path.join(module_dir, '../templates/symbolic') if isinstance(functions, list): for fun in functions: if fun == "accelred": symb_file = "mbs_accelred_" + mbs_name + ".py" path = os.path.abspath(os.path.join(symbolic_path, symb_file)) if not os.path.isfile(path): if __DEBUG__: mbs_msg("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)) spec = importlib.util.spec_from_file_location(symb_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.mbs_accelred = module.mbs_accelred elif fun == "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__: mbs_msg("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)) spec = importlib.util.spec_from_file_location(symb_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.mbs_cons_hJ = module.cons_hJ elif fun == "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__: mbs_msg("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)) spec = importlib.util.spec_from_file_location(symb_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.mbs_cons_jdqd = module.cons_jdqd elif fun == "dirdyna": symb_file = "mbs_dirdyna_" + mbs_name + ".py" path = os.path.abspath(os.path.join(symbolic_path, symb_file)) if not os.path.isfile(path): if __DEBUG__: mbs_msg("DEBUG>> file '" + symb_file + "' not found in folder '" + os.path.dirname(path)) symb_file = "mbs_dirdyna_PRJ.py" path = os.path.abspath(os.path.join(template_path, symb_file)) spec = importlib.util.spec_from_file_location(symb_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.mbs_dirdyna = module.dirdyna elif fun == "extforces": symb_file = "mbs_extforces_" + mbs_name + ".py" path = os.path.abspath(os.path.join(symbolic_path, symb_file)) if not os.path.isfile(path): if __DEBUG__: mbs_msg("DEBUG>> file '" + symb_file + "' not found in folder '" + os.path.dirname(path)) symb_file = "mbs_extforces_PRJ.py" path = os.path.abspath(os.path.join(template_path, symb_file)) spec = importlib.util.spec_from_file_location(symb_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.mbs_extforces = module.extforces elif fun == "gensensor": symb_file = "mbs_gensensor_" + mbs_name + ".py" path = os.path.abspath(os.path.join(symbolic_path, symb_file)) if not os.path.isfile(path): if __DEBUG__: mbs_msg("DEBUG>> file '" + symb_file + "' not found in folder '" + os.path.dirname(path)) symb_file = "mbs_gensensor_PRJ.py" path = os.path.abspath(os.path.join(template_path, symb_file)) spec = importlib.util.spec_from_file_location(symb_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.mbs_gensensor = module.sensor elif fun == "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__: mbs_msg("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)) spec = importlib.util.spec_from_file_location(symb_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.mbs_invdyna = module.invdyna elif fun == "link": symb_file = "mbs_link_" + mbs_name + ".py" path = os.path.abspath(os.path.join(symbolic_path, symb_file)) if not os.path.isfile(path): if __DEBUG__: mbs_msg("DEBUG>> file '" + symb_file + "' not found in folder '" + os.path.dirname(path)) symb_file = "mbs_link_PRJ.py" path = os.path.abspath(os.path.join(template_path, symb_file)) spec = importlib.util.spec_from_file_location(symb_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.mbs_link = module.link elif fun == "link3D": symb_file = "mbs_link3D_" + mbs_name + ".py" path = os.path.abspath(os.path.join(symbolic_path, symb_file)) if not os.path.isfile(path): if __DEBUG__: mbs_msg("DEBUG>> file '" + symb_file + "' not found in folder '" + os.path.dirname(path)) symb_file = "mbs_link3D_PRJ.py" path = os.path.abspath(os.path.join(template_path, symb_file)) spec = importlib.util.spec_from_file_location(symb_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.mbs_link3D = module.link3D elif fun == "sensor": symb_file = "mbs_sensor_" + mbs_name + ".py" path = os.path.abspath(os.path.join(symbolic_path, symb_file)) if not os.path.isfile(path): if __DEBUG__: mbs_msg("DEBUG>> file '" + symb_file + "' not found in folder '" + os.path.dirname(path)) symb_file = "mbs_sensor_PRJ.py" path = os.path.abspath(os.path.join(template_path, symb_file)) spec = importlib.util.spec_from_file_location(symb_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.mbs_sensor = module.sensor else: raise TypeError(fun + " is not an existing symbolic function") else: raise TypeError('The "functions" parameter must be a list, got a {:}.'.format(type(functions))) def __assign_user_fct__(self, functions, mbs_module=None): """Assign specified user functions pointers in MBsysC structure. Assign and wrap python user functions. If the argments of a function are dependent of another module instance (ie. MbsPart, MbsDirdyn...), then mbs_module must be given. Store the functions in the MbsData instance and assign the pointer of functions in the C structure. Parameters ---------- functions : List List of strings containing the names of the user functions to assign mbs_module : mbs_module : MbsPart or MbsEquil or MbsModal or MbsDirdyn or MbsInvdyn or MbsSolvekin, optional The module in which the functions are called. Present if some arguments depend on this module. The default is None. Notes ----- Only the following user functions names can be passed as parameter: - 'cons_hJ' - 'cons_jdqd' - 'derivative' - 'DrivenJoints' - 'ExtForces' - 'JointForces' - 'LinkForces' - 'Link3DForces' - 'dirdyn_init' - 'dirdyn_loop' - 'dirdyn_finish' - 'equil_init' - 'equil_loop' - 'equil_finish' - 'equil_fxe' - 'invdyn_init' - 'invdyn_loop' - 'invdyn_finish' Returns ------- None. """ data_c_ptr = self.mbs_data_ptr.contents if isinstance(functions, list): for fun in functions: if fun == "cons_hJ": # cons_hJ if mbs_module is not None: if isinstance(mbs_module, MbsPart): self.ptrs_to_user_fcts["user_cons_hJ"] = user_cons_hJ_wrap(lambda h, Jac, mbs, tsim: self.__callback_user_cons_hJ_part(self.user_cons_hJ, h, Jac, tsim)) self.mbs_data_ptr.contents.user_cons_hJ = self.ptrs_to_user_fcts["user_cons_hJ"] elif isinstance(mbs_module, (MbsDirdyn, MbsEquil, MbsModal, MbsSolvekin, MbsInvdyn)): self.ptrs_to_user_fcts["user_cons_hJ"] = user_cons_hJ_wrap(lambda h, Jac, mbs, tsim: self.user_cons_hJ(mbs_module._huserc, mbs_module._Juserc, self, tsim)) self.mbs_data_ptr.contents.user_cons_hJ = self.ptrs_to_user_fcts["user_cons_hJ"] else: raise TypeError('mbs_module must belong to {:}'.format((MbsDirdyn, MbsEquil, MbsModal, MbsSolvekin, MbsInvdyn, MbsPart))) else: raise ValueError(' A module needs to be passed as mbs_module argument') elif fun == "cons_jdqd": # cons_jdqd if mbs_module is not None: if isinstance(mbs_module, (MbsDirdyn, MbsEquil, MbsModal, MbsSolvekin, MbsInvdyn, MbsPart)): self.ptrs_to_user_fcts["user_cons_jdqd"] = user_cons_jdqd_wrap(lambda jdqd, mbs, tsim: self.user_cons_jdqd(mbs_module._jdqduserc, self)) self.mbs_data_ptr.contents.user_cons_jdqd = self.ptrs_to_user_fcts["user_cons_jdqd"] else: raise TypeError('mbs_module must belong to {:}'.format((MbsDirdyn, MbsEquil, MbsModal, MbsSolvekin, MbsInvdyn, MbsPart))) else: raise ValueError(' A module needs to be passed as mbs_module argument') elif fun == 'dirdyn_init': # user_dirdyn if mbs_module is not None: 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: raise TypeError('mbs_module must be MbsDirdyn to use dirdyn_init') else: raise ValueError('Dirdyn module needs to be passed as mbs_module argument') elif fun == 'dirdyn_loop': if mbs_module is not None: 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: raise TypeError('mbs_module must be MbsDirdyn to use dirdyn_loop') else: raise ValueError('Dirdyn module needs to be passed as mbs_module argument') elif fun == 'dirdyn_finish': if mbs_module is not None: 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: raise TypeError('mbs_module must be MbsDirdyn to use dirdyn_finish') else: raise ValueError('Dirdyn module needs to be passed as mbs_module argument') elif fun == 'equil_init': # user_equil 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: raise TypeError('mbs_module must be MbsEquil to use equil_init') else: raise ValueError('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: raise TypeError('mbs_module must be MbsEquil to use equil_loop') else: raise ValueError('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: raise TypeError('mbs_module must be MbsEquil to use equil_finish') else: raise ValueError('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, f)) self.mbs_data_ptr.contents.user_equil_fxe = self.ptrs_to_symb_fcts["user_equil_fxe"] else: raise TypeError('mbs_module must be MbsEquil to use equil_fxe') else: raise ValueError('Equil module needs to be passed as mbs_module argument') elif fun == 'invdyn_init': # user_invdyn if mbs_module is not None: if isinstance(mbs_module, MbsInvdyn): self.ptrs_to_user_fcts["user_invdyn_init"] = user_invdyn_init_wrap(lambda mbs, dd: self.user_invdyn_init(self, mbs_module)) self.mbs_data_ptr.contents.user_invdyn_init = self.ptrs_to_user_fcts["user_invdyn_init"] else: raise TypeError('mbs_module must be MbsInvdyn to use invdyn_init') else: raise ValueError('Dirdyn module needs to be passed as mbs_module argument') elif fun == 'invdyn_loop': if mbs_module is not None: if isinstance(mbs_module, MbsInvdyn): self.ptrs_to_user_fcts["user_invdyn_loop"] = user_invdyn_loop_wrap(lambda mbs, dd: self.user_invdyn_loop(self, mbs_module)) self.mbs_data_ptr.contents.user_invdyn_loop = self.ptrs_to_user_fcts["user_invdyn_loop"] else: raise TypeError('mbs_module must be MbsInvdyn to use invdyn_loop') else: raise ValueError('Dirdyn module needs to be passed as mbs_module argument') elif fun == 'invdyn_finish': if mbs_module is not None: if isinstance(mbs_module, MbsInvdyn): self.ptrs_to_user_fcts["user_invdyn_finish"] = user_invdyn_finish_wrap(lambda mbs, dd: self.user_invdyn_finish(self, mbs_module)) self.mbs_data_ptr.contents.user_invdyn_finish = self.ptrs_to_user_fcts["user_invdyn_finish"] else: raise TypeError('mbs_module must be MbsInvdyn to use invdyn_finish') else: raise ValueError('Dirdyn 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)) data_c_ptr.user_Derivative = self.ptrs_to_user_fcts["user_Derivative"] elif fun == "DrivenJoints": # 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"] elif fun == "ExtForces": # ext_forces 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"] elif fun == "JointForces": # joint_forces 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"] elif fun == "LinkForces": # link_forces 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"] elif fun == "Link3DForces": # link3D_forces 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"] else: raise ValueError(fun + " is not an existing user function") else: raise TypeError('The "functions" parameter must be a list, got a {:}.'.format(type(functions))) return def __assign_symb_fct__(self, functions, mbs_module=None): """ Assign specified symbolic functions in MbsData instance. Assign and wrap python symbolic functions. If the argments of a function are dependent of another module instance (ie. MbsPart, MbsDirdyn...), then mbs_module must be given. Store the functions in the MbsData instance and assign the pointer of functions in the C structure. Parameters ---------- functions : List List of strings containing the names of the symbolic functions to assign mbs_module : MbsModule, optional The module in which the functions are called. Present if some arguments depend on this module. The default is None. Notes ----- Only the following symbolic functions names can be passed as parameter: - 'accelred' - 'cons_hJ' - 'cons_jdqd' - 'invdyna' - 'dirdyna' - 'extforces' - 'gensensor' - 'link' - 'link3D' - 'sensor' Returns ------- None. """ data_c_ptr = self.mbs_data_ptr.contents if isinstance(functions, list): for fun in functions: if fun == "accelred": # 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"] elif fun == "cons_hJ": # mbs_cons_hJ if mbs_module is not None: if isinstance(mbs_module, MbsPart): self.ptrs_to_symb_fcts["mbs_cons_hJ"] = mbs_cons_hJ_wrap(lambda h, Jac, mbs, tsim: self.__callback_mbs_cons_hJ_part(self.mbs_cons_hJ, h, Jac)) self.mbs_data_ptr.contents.mbs_cons_hJ = self.ptrs_to_symb_fcts["mbs_cons_hJ"] elif isinstance(mbs_module, (MbsDirdyn, MbsEquil, MbsModal, MbsSolvekin, MbsInvdyn)): self.ptrs_to_symb_fcts["mbs_cons_hJ"] = mbs_cons_hJ_wrap(lambda h, Jac, mbs, tsim: self.mbs_cons_hJ(mbs_module._h, mbs_module._Jac, self)) self.mbs_data_ptr.contents.mbs_cons_hJ = self.ptrs_to_symb_fcts["mbs_cons_hJ"] else: raise TypeError('mbs_module must belong to {:}'.format((MbsDirdyn, MbsEquil, MbsModal, MbsSolvekin, MbsInvdyn, MbsPart))) else: raise ValueError(' A module needs to be passed as mbs_module argument') elif fun == "cons_jdqd": # mbs_cons_jdqd if mbs_module is not None: if isinstance(mbs_module, (MbsDirdyn, MbsEquil, MbsModal, MbsSolvekin, MbsInvdyn, MbsPart)): self.ptrs_to_symb_fcts["mbs_cons_jdqd"] = mbs_cons_jdqd_wrap(lambda jdqd, mbs, tsim: self.mbs_cons_jdqd(mbs_module._jdqd, self)) self.mbs_data_ptr.contents.mbs_cons_jdqd = self.ptrs_to_symb_fcts["mbs_cons_jdqd"] else: raise TypeError('mbs_module must belong to {:}'.format((MbsDirdyn, MbsEquil, MbsModal, MbsSolvekin, MbsInvdyn, MbsPart))) else: raise ValueError(' A module needs to be passed as mbs_module argument') elif fun == "extforces": # mbs_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"] elif fun == "gensensor": # mbs_gensensor 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"] elif fun == "link": # mbs_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"] elif fun == "link3D": # mbs_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"] elif fun == "sensor": # mbs_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"] elif fun == "dirdyna": # dirdyna if mbs_module is not None: if isinstance(mbs_module, (MbsDirdyn, MbsEquil, MbsModal, MbsSolvekin, MbsInvdyn, MbsPart)): self.ptrs_to_symb_fcts["mbs_dirdyna"] = mbs_dirdyna_wrap(lambda M, c, mbs, tsim: self.mbs_dirdyna(mbs_module._M, mbs_module._c, self, tsim)) self.mbs_data_ptr.contents.mbs_dirdyna = self.ptrs_to_symb_fcts["mbs_dirdyna"] else: raise TypeError('mbs_module must belong to {:}'.format((MbsDirdyn, MbsEquil, MbsModal, MbsSolvekin, MbsInvdyn, MbsPart))) else: raise ValueError(' A module needs to be passed as mbs_module argument for dirdyna') elif fun == "invdyna": # invdyna # not used in the MbsSolvekin module if mbs_module is not None: if isinstance(mbs_module, (MbsDirdyn, MbsEquil, MbsModal, MbsSolvekin, MbsInvdyn, MbsPart)): 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: raise TypeError('mbs_module must belong to {:}'.format((MbsDirdyn, MbsEquil, MbsModal, MbsSolvekin, MbsInvdyn, MbsPart))) else: raise ValueError(' A module needs to be passed as mbs_module argument for invdyna') else: raise TypeError(fun + " is not an existing symbolic function") else: raise TypeError('The "functions" parameter must be a list, got a {:}.'.format(type(functions))) def __callback_user_equil_fxe(self, mbs_module, fun, f): if not isinstance(mbs_module, MbsEquil): raise TypeError('This function must be called with the Equil module') __f = np.ctypeslib.as_array(f, (mbs_module.mbs_equil_ptr.contents.nx + 1, )) fun(self, __f) def __callback_user_ExtForces(self, 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, self, tsim, ixF) self.SWr[ixF, :] = SWr[:] return self.SWr[ixF].ctypes.data_as(ctypes.c_void_p).value def __callback_user_Link3DForces(self, 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, self, tsim, ixF) self.SWr[ixF, 1:7] = SWr[1:7] def __callback_mbs_sensor(self, fun, sens, isens): """Do the callback for gensensor, forcesensor and sensor. Notes ----- This callback is used when a C-library (MBsysC, symbolicR, userfctR) calls ask for the computation of a sensor if the symbolic functions are in Python. Currently the MBsysC core library never calls for a sensor computation. Currently MBsysPy does not allows to load a userfctR C-library with symbolic functions in Python. So this function should never be called and has not been tested. This function is defined for the future (change in MBsysPy, addons...). """ mbs_warning("Warning: Computing MBsysPy sensor from C-library has not been tested! Check your results with care.") # Check if the sensor is MBsysPy.MbsSensor instance or C pointer if isinstance(sens, MbsSensor): # Then use the current sensor __sens = sens else: # Then create a MbsSensor arout the C pointer __sens = MbsSensor(self, id_=isens, sensor_ptr=sens) # Note: The C-memory will not be freed by object destructor. fun(__sens, self, isens) def __callback_user_cons_hJ_part(self, fun, h, Jac, tsim): __h = np.ctypeslib.as_array(h, (self.Nuserc + 1,)) __Jac = np.ctypeslib.as_array(Jac[0], (self.Nuserc + 1, self.njoint + 1)) fun(__h, __Jac, self, tsim) def __callback_mbs_cons_hJ_part(self, fun, h, Jac): if __DEBUG__: mbs_msg("DEBUG>> callback_mbs_cons_hJ") __h = np.ctypeslib.as_array(h, (self.Ncons + 1,)) __Jac = np.ctypeslib.as_array(Jac[0], (self.Ncons + 1, self.njoint + 1)) fun(__h, __Jac, self) def __unassign_user_fct__(self, functions): """ Unassign specified user functions in MbsData instance. Parameters ---------- functions : List List of strings containing the names of the user functions to unassign Notes ----- Only the following user functions names can be passed as parameter: - 'cons_hJ' - 'cons_jdqd' - 'derivative' - 'DrivenJoints' - 'ExtForces' - 'JointForces' - 'LinkForces' - 'Link3DForces' - 'dirdyn_init' - 'dirdyn_loop' - 'dirdyn_finish' - 'equil_init' - 'equil_loop' - 'equil_finish' - 'equil_fxe' Returns ------- None. """ self.ptrs_to_user_fcts.clear() self.__assign_user_to_undefined__(functions) self.__assign_user_fct_to_none__(functions) def __assign_user_to_undefined__(self, functions): """Assign user function to dummy functions. Assignment to dummy function is required to avoid calling a function being NULL, wich has an 'undefined behavior'. Parameters ---------- functions : List List of strings containing the names of the user functions to assign Notes ----- Only the following user functions names can be passed as parameter: - 'cons_hJ' - 'cons_jdqd' - 'derivative' - 'DrivenJoints' - 'ExtForces' - 'JointForces' - 'LinkForces' - 'Link3DForces' - 'dirdyn_init' - 'dirdyn_loop' - 'dirdyn_finish' - 'equil_init' - 'equil_loop' - 'equil_finish' - 'equil_fxe' - 'invdyn_init' - 'invdyn_loop' - 'invdyn_finish' Returns ------- None. """ if isinstance(functions, list): for fun in functions: if fun == "cons_hJ": self.mbs_data_ptr.contents.user_cons_hJ = user_cons_hJ_wrap(lambda h, Jac, mbs, tsim: callback_undefined("user_cons_hJ")) elif fun == "cons_jdqd": self.mbs_data_ptr.contents.user_cons_jdqd = user_cons_jdqd_wrap(lambda h, Jac, mbs, tsim: callback_undefined("user_cons_jdqd")) elif fun == "derivative": self.mbs_data_ptr.contents.user_Derivative = user_Derivative_wrap(lambda mbs: callback_undefined("user_Derivative")) elif fun == "DrivenJoints": self.mbs_data_ptr.contents.user_DrivenJoints = user_DrivenJoints_wrap(lambda mbs, t: callback_undefined("user_DrivenJoints")) elif fun == "ExtForces": self.mbs_data_ptr.contents.user_ExtForces = user_ExtForces_wrap(lambda PxF, RxF, VxF, OMxF, AxF, OMPxF, mbs, tsim, ixF: callback_undefined("user_ExtForces")) elif fun == "JointForces": self.mbs_data_ptr.contents.user_JointForces = user_JointForces_wrap(lambda mbs, t: callback_undefined("user_JointForces")) elif fun == "LinkForces": self.mbs_data_ptr.contents.user_LinkForces = user_LinkForces_wrap(lambda Z, Zd, mbs, tsim, ilnk: callback_undefined("user_LinkForces")) elif fun == "Link3DForces": self.mbs_data_ptr.contents.user_Link3DForces = user_Link3DForces_wrap(lambda PxF, RxF, VxF, OMxF, AxF, OMPxF, mbs, tsim, ixF: callback_undefined("user_Link3DForces")) elif fun == "dirdyn_init": self.mbs_data_ptr.contents.user_dirdyn_init = user_dirdyn_init_wrap(lambda mbs, dd: callback_undefined("user_dirdyn_init")) elif fun == "dirdyn_loop": self.mbs_data_ptr.contents.user_dirdyn_loop = user_dirdyn_loop_wrap(lambda mbs, dd: callback_undefined("user_dirdyn_loop")) elif fun == "dirdyn_finish": self.mbs_data_ptr.contents.user_dirdyn_finish = user_dirdyn_finish_wrap(lambda mbs, dd: callback_undefined("user_dirdyn_finish")) elif fun == "equil_init": self.mbs_data_ptr.contents.user_equil_init = user_equil_init_wrap(lambda mbs, eq: callback_undefined("user_equil_init")) elif fun == "equil_loop": self.mbs_data_ptr.contents.user_equil_loop = user_equil_loop_wrap(lambda mbs, eq: callback_undefined("user_equil_loop")) elif fun == "equil_finish": self.mbs_data_ptr.contents.user_equil_finish = user_equil_finish_wrap(lambda mbs, eq: callback_undefined("user_equil_finish")) elif fun == "equil_fxe": self.mbs_data_ptr.contents.user_equil_fxe = user_equil_fxe_wrap(lambda mbs, f: callback_undefined("user_equil_fxe")) elif fun == "invdyn_init": self.mbs_data_ptr.contents.user_invdyn_init = user_invdyn_init_wrap(lambda mbs, dd: callback_undefined("user_invdyn_init")) elif fun == "invdyn_loop": self.mbs_data_ptr.contents.user_invdyn_loop = user_invdyn_loop_wrap(lambda mbs, dd: callback_undefined("user_invdyn_loop")) elif fun == "invdyn_finish": self.mbs_data_ptr.contents.user_invdyn_finish = user_invdyn_finish_wrap(lambda mbs, dd: callback_undefined("user_invdyn_finish")) else: raise TypeError(fun + " is not an existing user function") else: raise TypeError('The "functions" parameter must be a list, got a {:}.'.format(type(functions))) def __unassign_symb_fct__(self, functions): """ Unassign specified symbolic functions in MbsData instance. Parameters ---------- functions : List List of strings containing the names of the symbolic functions to unassign Notes ----- Only the following symbolic functions names can be passed as parameter: - 'accelred' - 'cons_hJ' - 'cons_jdqd' - 'invdyna' - 'dirdyna' - 'extforces' - 'gensensor' - 'link' - 'link3D' - 'sensor' Returns ------- None. """ # Not necessary but if not present, dictionary must be filled in the # load function and not the assign function. self.ptrs_to_symb_fcts.clear() self.__assign_symb_to_undefined__(functions) self.__assign_symb_fct_to_none__(functions) def __assign_symb_to_undefined__(self, functions): """Assign symbolic function to dummy functions. Assignment to dummy function is required to avoid calling a function being NULL, wich has an 'undefined behavior'. Parameters ---------- functions : List List of strings containing the names of the symbolic functions to assign Notes ----- Only the following symbolic functions names can be passed as parameter: - 'accelred' - 'cons_hJ' - 'cons_jdqd' - 'invdyna' - 'dirdyna' - 'extforces' - 'gensensor' - 'link' - 'link3D' - 'sensor' Returns ------- None. """ if isinstance(functions, list): for fun in functions: if fun == "accelred": self.mbs_data_ptr.contents.mbs_accelred = mbs_accelred_wrap(lambda mbs, tsim: callback_undefined("mbs_accelred")) elif fun == "cons_hJ": self.mbs_data_ptr.contents.mbs_cons_hJ = mbs_cons_hJ_wrap(lambda h, Jac, mbs, tsim: callback_undefined("mbs_cons_hJ")) elif fun == "cons_jdqd": self.mbs_data_ptr.contents.mbs_cons_jdqd = mbs_cons_jdqd_wrap(lambda jdqd, mbs, tsim: callback_undefined("mbs_cons_jdqd")) elif fun == "dirdyna": self.mbs_data_ptr.contents.mbs_dirdyna = mbs_dirdyna_wrap(lambda M, c, mbs, tsim: callback_undefined("mbs_dirdyna")) elif fun == "extforces": self.mbs_data_ptr.contents.mbs_extforces = mbs_extforces_wrap(lambda frc, trq, mbs, tsim: callback_undefined("mbs_extforces")) elif fun == "gensensor": self.mbs_data_ptr.contents.mbs_gensensor = mbs_gensensor_wrap(lambda sens, mbs, isens: callback_undefined("mbs_gensensor")) elif fun == "invdyna": self.mbs_data_ptr.contents.mbs_invdyna = mbs_invdyna_wrap(lambda Q, mbs, tsim: callback_undefined("mbs_invdyna")) elif fun == "link": self.mbs_data_ptr.contents.mbs_link = mbs_link_wrap(lambda frc, trq, Flnk, Z, Zd, mbs, tsim: callback_undefined("mbs_link")) elif fun == "link3D": self.mbs_data_ptr.contents.mbs_link3D = mbs_link3D_wrap(lambda frc, trq, mbs, tsim: callback_undefined("mbs_link3D")) elif fun == "sensor": self.mbs_data_ptr.contents.mbs_sensor = mbs_sensor_wrap(lambda sens, mbs, isens: callback_undefined("mbs_sensor")) else: raise TypeError(fun + " is not an existing symbolic function") else: raise TypeError('The "functions" parameter must be a list, got a {:}.'.format(type(functions))) 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. If the user function is not used by the following modules, it has to be unassigned by the user. Parameters ---------- function_name : str name of the user function to replace. user_path : str path to the new user function file. user_file : str name of the new user function file. """ # Creating user path user_path = os.path.join(self.project_path, user_path) # Error handeling if not os.path.isdir(user_path): mbs_msg('The function directory does not exist: "' + user_path + '"') mbs_msg('The current root folder is: "' + os.getcwd() + '"') else: if function_name == "cons_hJ": # cons_hJ path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg(" file '" + user_file + "' not found in folder '" + os.path.dirname(path)) else: spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_cons_hJ = module.user_cons_hJ elif function_name == "cons_jdqd": # cons_jdqd path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg(" file '" + user_file + "' not found in folder '" + os.path.dirname(path)) else: spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_cons_jdqd = module.user_cons_jdqd elif function_name == "derivative": # derivative path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg(" file '" + user_file + "' not found in folder '" + os.path.dirname(path)) else: spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_derivative = module.user_derivatives elif function_name == "DrivenJoints": # drivenJoints path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg(" file '" + user_file + "' not found in folder '" + os.path.dirname(path)) else: spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_DrivenJoints = module.user_DrivenJoints elif function_name == "ExtForces": # ext_forces path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg(" file '" + user_file + "' not found in folder '" + os.path.dirname(path)) else: spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_ExtForces = module.user_ExtForces elif function_name == "JointForces": # joint_forces path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg(" file '" + user_file + "' not found in folder '" + os.path.dirname(path)) else: spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_JointForces = module.user_JointForces elif function_name == "LinkForces": # link_forces path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg(" file '" + user_file + "' not found in folder '" + os.path.dirname(path)) else: spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_LinkForces = module.user_LinkForces elif function_name == "Link3DForces": # link3D_forces path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg(" file '" + user_file + "' not found in folder '" + os.path.dirname(path)) else: spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_Link3DForces = module.user_Link3DForces elif function_name == 'dirdyn_init': # user_dirdyn path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg(" file '" + user_file + "' not found in folder '" + os.path.dirname(path)) else: spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_dirdyn_init = module.user_dirdyn_init elif function_name == 'dirdyn_loop': # user_dirdyn path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg(" file '" + user_file + "' not found in folder '" + os.path.dirname(path)) else: spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_dirdyn_loop = module.user_dirdyn_loop elif function_name == 'dirdyn_finish': # user_dirdyn path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg(" file '" + user_file + "' not found in folder '" + os.path.dirname(path)) else: spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_dirdyn_finish = module.user_dirdyn_finish elif function_name == 'equil_init': path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg(" file '" + user_file + "' not found in folder '" + os.path.dirname(path)) else: spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_equil_init = module.user_equil_init elif function_name == 'equil_loop': path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg(" file '" + user_file + "' not found in folder '" + os.path.dirname(path)) else: spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_equil_loop = module.user_equil_loop elif function_name == 'equil_finish': path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg(" file '" + user_file + "' not found in folder '" + os.path.dirname(path)) else: spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_equil_finish = module.user_equil_finish elif function_name == 'equil_fxe': path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg(" file '" + user_file + "' not found in folder '" + os.path.dirname(path)) else: spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_equil_fxe = module.user_equil_fxe elif function_name == 'invdyn_init': # user_invdyn path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg(" file '" + user_file + "' not found in folder '" + os.path.dirname(path)) else: spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_invdyn_init = module.user_invdyn_init elif function_name == 'invdyn_loop': # user_invdyn path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg(" file '" + user_file + "' not found in folder '" + os.path.dirname(path)) else: spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_invdyn_loop = module.user_invdyn_loop elif function_name == 'invdyn_finish': # user_invdyn path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): mbs_msg(" file '" + user_file + "' not found in folder '" + os.path.dirname(path)) else: spec = importlib.util.spec_from_file_location(user_file[:-3], path) module = importlib.util.module_from_spec(spec) spec.loader.exec_module(module) self.user_invdyn_finish = module.user_invdyn_finish else: raise TypeError(function_name + " is not an existing user function") def __set_user_fct_from_ptr__(self, function_name, user_fct_ptr): """ Load a user function chosen by the user instead of the default one in the userfctR folder. If the user function is not used by the following modules, it has to be unassigned by the user. Parameters ---------- function_name : str name of the user function to replace. user_fct_ptr : ptr new user function pointer. """ if hasattr(self, 'user_' + function_name): setattr(self, 'user_' + function_name, user_fct_ptr) else: raise TypeError(function_name + " is not an existing user function") def __generate_id__(self): """ Fill the dictionnaries containing the relevant MBS ids. Read the id's of the joints, links, sensors, extforces (ect.) from the MbsInfos structure and store them into dictionaries in the MbsData instance. """ for i in range(self.njoint): name = bytes_to_str(self.mbs_infos_ptr.contents.bodytree.contents.joint_list[i].contents.name) self.joint_id[name] = i + 1 for i in range(self.nbody): name = bytes_to_str(self.mbs_infos_ptr.contents.bodytree.contents.body_list[i].contents.name) local_id_parent_joint = self.mbs_infos_ptr.contents.bodytree.contents.body_list[i].contents.n_joint - 1 parent_joint_name = bytes_to_str(self.mbs_infos_ptr.contents.bodytree.contents.body_list[i].contents.joint_list[local_id_parent_joint].contents.name) self.body_id[name] = self.joint_id[parent_joint_name] for i in range(self.npt): bodyname = bytes_to_str(self.mbs_infos_ptr.contents.point_list[i].contents.body_name) if bodyname not in self.points_id: self.points_id[bodyname] = {} pointname = bytes_to_str(self.mbs_infos_ptr.contents.point_list[i].contents.name) self.points_id[bodyname][pointname] = i + 1 if self.Nlink: for i in range(self.Nlink): name = bytes_to_str(self.mbs_infos_ptr.contents.links.contents.link_list[i].contents.name) self.link_id[name] = i + 1 if self.Nsensor: for i in range(self.Nsensor): index = self.mbs_infos_ptr.contents.sensor_point_list[i] name = bytes_to_str(self.mbs_infos_ptr.contents.point_list[index].contents.sensor.contents.name) self.sensor_id[name] = i + 1 if self.Nxfrc: for i in range(self.Nxfrc): index = self.mbs_infos_ptr.contents.extforce_point_list[i] name = bytes_to_str(self.mbs_infos_ptr.contents.point_list[index].contents.extforce.contents.name) self.extforce_id[name] = i + 1 def __load_user_model__(self): """ Load user models in 2 nested dictionaries. The first one contains the names of the user models as keys and a second dictionary for the parameters as values. The second one contains the names of the parameters as keys and numpy arrays that point toward C memory as values """ infos_c_ptr = self.mbs_infos_ptr.contents # Unlock modification in dictionary. self.user_model._locked = False # Counter for generation of user state id, used if user functions are in Python. next_user_state = 1 # Shortcut to know if we are using Python user function UmPy = self.opt_load_c < 2 # Loop on each user model for i in range(infos_c_ptr.user_models.contents.n_user_model): # Retrieve the user model c structure (MbsInfoUserModel) user_model_list = infos_c_ptr.user_models.contents.user_model_list[i].contents # Retrieve its name and create subdictionary for the parameters name = bytes_to_str(user_model_list.name) self.user_model[name] = _UserModelDict() self.user_model[name]._parent_key = name self.user_model[name]._locked = False self.user_model[name]._type = dict() # Loop on each parameter of the current user model for j in range(user_model_list.n_parameter): # Retrieve the parameter c structure (MbsInfoParameter) parameter_list = user_model_list.parameter_list[j].contents # And parameter name name2 = bytes_to_str(parameter_list.name) # Retrieve pointer to values, as ctypes.POINTER(ctypes.c_double) val_ptr = parameter_list.val_ptr # Currently set in memory # Retrieve number of value in the parameter size = parameter_list.n_value # Type of the parameter self.user_model[name]._type[name2] = parameter_list.type um_type = parameter_list.type # shortcut for comparison if um_type == 1: # scalar self.user_model[name][name2] = np.ctypeslib.as_array(val_ptr, (1, 1)) elif um_type == 2: # vector, using index starting at 1. self.user_model[name][name2] = np.ctypeslib.as_array(val_ptr, (1, size + 1)) elif um_type == 5: # user state if UmPy: # The pointer is set to the initial value of the user states. # The indices of the user state must be created. state = np.array([np.arange(next_user_state - 1, size + next_user_state)]) state[0][0] = size next_user_state += size else: # The indices of the user state have been created by the c UserModel. state = np.ctypeslib.as_array(ctypes.cast(val_ptr, ctypes.POINTER(ctypes.c_int)), (1, size + 1)) # Preventing the user to modify the user state ids. state.flags["WRITEABLE"] = False self.user_model[name][name2] = state elif um_type == 7: if UmPy: # The values loaded from files are stored as double. The conversion # to integer will be done for the user when asking for the parameter. self.user_model[name][name2] = np.ctypeslib.as_array(val_ptr, (1, 1)) else: # At UserModel creation the val_ptr is redirected toward an int. # Casting the pointer to a pointer to int is required self.user_model[name][name2] = np.ctypeslib.as_array(ctypes.cast(val_ptr, ctypes.POINTER(ctypes.c_int)), (1, 1)) else: # Other parameter types are either non accessible or unknown. type_convertor = ['Invalid Value', 'a scalar', 'a vector', 'a 1D Look-Up-Table', 'a 2D Look-Up-Table', 'an user state', 'a structure', 'an integer'] type_str = 'not an known type' if len(type_convertor) <= um_type else type_convertor[um_type] message = "The user model '{:}->{:}', which is {:}, is not supported in MBsysPy.\n"\ .format(name, name2, type_str) message += " " # Spacing for alignment if UmPy: message += "This user model will not be used at all." else: message += "This user model will not be accessible from Python side." mbs_warning(message) # Lock the current user model (additional parameters not possible) self.user_model[name]._locked = True # Lock the user models (additional user model not possible) self.user_model._locked = True def set_nb_userc(self, nb): """ Set the number of user constraints. Parameters ---------- nb : int The number of user constraints. """ libutilities.mbs_set_nb_userc(self.mbs_data_ptr, nb) def set_qu(self, indices): """ Set the specified joints as independent. Parameters ---------- indices : int, list or numpy.ndarray of int The ids of the joints to set as independent. """ if isinstance(indices, (list, np.ndarray)): # Local list to avoid self-modification (ie. set all driven joints as independent) # Removing index 0 from list (default value in MbsData::qu and other). local_indices = list(filter(lambda q_i: q_i != 0, indices)) for index in local_indices: libutilities.mbs_set_qu(self.mbs_data_ptr, index) else: libutilities.mbs_set_qu(self.mbs_data_ptr, indices) def set_qv(self, indices): """ Set the specified joints as dependent. Parameters ---------- indices : int, list or numpy.ndarray of int The ids of the joints to set as dependent. """ if isinstance(indices, (list, np.ndarray)): # Local list to avoid self-modification (ie. set all independent to dependent) # Removing index 0 from list (default value in MbsData::qu and other). local_indices = list(filter(lambda q_i: q_i != 0, indices)) for index in local_indices: libutilities.mbs_set_qv(self.mbs_data_ptr, index) else: libutilities.mbs_set_qv(self.mbs_data_ptr, indices) def set_qdriven(self, indices): """ Set the specified joints as driven. Parameters ---------- indices : int, list or numpy.ndarray of int The ids of the joints to set as driven. """ if isinstance(indices, (list, np.ndarray)): # Local list to avoid self-modification (ie. set all independent to driven) # Removing index 0 from list (default value in MbsData::qu and other). local_indices = list(filter(lambda q_i: q_i != 0, indices)) for index in local_indices: libutilities.mbs_set_qdriven(self.mbs_data_ptr, index) else: libutilities.mbs_set_qdriven(self.mbs_data_ptr, indices) def set_qa(self, indices): """ Set the specified joints as actuated. Actuated joints are only used by equilibrium analysis (in some cases) and inverse dynamics. Actuated joint are also member of one of the other partition (independent or dependent). Parameters ---------- indices : int, list or numpy.ndarray of int The ids of the joints to set as actuated. """ if isinstance(indices, (list, np.ndarray)): # Removing index 0 from list (default value in MbsData::qu and other). local_indices = list(filter(lambda q_i: q_i != 0, indices)) for index in local_indices: libutilities.mbs_set_qa(self.mbs_data_ptr, index) else: libutilities.mbs_set_qa(self.mbs_data_ptr, indices) def unset_qa(self, indices): """ Remove the specified joints from the actuated joints. Parameters ---------- indices : int, list or numpy.ndarray of int The ids of the joints to be removed from the actuated list. """ if isinstance(indices, (list, np.ndarray)): # local list to avoid self-modification (ie. unset all qa) # Removing index 0 from list (default value in MbsData::qu and other). local_indices = list(filter(lambda q_i: q_i != 0, indices)) for index in local_indices: libutilities.mbs_unset_qa(self.mbs_data_ptr, index) else: libutilities.mbs_unset_qa(self.mbs_data_ptr, indices) def empty_qa(self): """Remove all joints from the actuated partition.""" libutilities.mbs_empty_qa(self.mbs_data_ptr) def __del__(self): """Delete the object by freeing the C-related memory.""" libloadXML.mbs_delete_infos(self.mbs_infos_ptr) libloadXML.mbs_delete_data(self.mbs_data_ptr) if __DEBUG__: mbs_msg("DEBUG>> MbsData pointer deleted.") def reset(self, verbose=False): """ Reset mbs_data fields to its inital value. The following fields are resetted according to the values storied in the field in parentheses: - MbsData.q (MbsData.q0) - MbsData.qd (MbsData.qd0) - MbsData.qdd (MbsData.qdd0) - MbsData.ux (MbsData.ux0) - MbsData.Qq (filled with 0) - MbsData.Qa (filled with 0) - MbsData.Qc (filled with 0) - MbsData.tsim (MbsData.t0) Parameters ---------- verbose : bool, optional Set to 'True' to mbs_msg resetting validation message. default is False """ libutilities.mbs_reset_data(self.mbs_data_ptr) if verbose: mbs_msg("mbs_data reseted to initial value.") def load_state(self, baseFileName="dirdyn", result_path="resultsR", time=-1, files_to_load=["all"]): """ Load a state to MbsData instance from files for specific time. If the time specified is not found in the file, the last time step lower than the specification is taken. The fieds tsim, q, qd, qdd, ux, uxd (and q0, qd0, qdd0, ux0) are set. If a file is not found a warning is generated and the value in memory are kept back. Parameters ---------- baseFileName : str The path and resfilename containing the state to load. The suffix _q, _qd, _ux.res will be added. default is "dirdyn" result_path : str The relative path of the result folder from the project folder. default is "resultsR" time : float The time corresponding to the state to load. defaut is "-1" (last state of the files). files_to_load : list of str List of the results files to load amongs: - 'all': load all results files - 'q': load the generalized coordinates - 'qd': load the generalized velocities - 'qdd': load the generalized acceleration - 'user': load the user state variables - 'userd': load the derivative of the user state variables. default is "['all']". """ def find_index(temp, t): if (t == -1): index = -1 else: # Look the specified time index = np.nonzero(temp[:, 0] >= t) if (np.size(index)): index = index[0][0] if not temp[index, 0] == t: mbs_warning("Time specified ({:.3f}s) not found, take the" "last occurence where t