# -*- coding: utf-8 -*- """ ------------------------------- (c) Universite catholique de Louvain, 2019 Creation : 2019 by O. Lantsoght Last update : 2019 version MBsysC v1.11.2 ------------------------------- Portable Python interface to MBsysC using Ctypes. 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...). """ import os import imp import ctypes import numpy as np # importing MbsysPy classes from .mbs_sensor import MbsSensor from .mbs_dirdyn import MbsDirdyn from .mbs_part import MbsPart # 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_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 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 #============================================================================== # Global parameter of the current module #============================================================================== __DEBUG__ = True __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 ---------- DonePart : int Flag that indicates if the coordinate partitioning module has been executed (default: 0=not done; 1=done). dpt : 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 : ndarray Numpy array containing the current values of the forces on each link. frc: 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 : ndarray The 3 gravity components in the inertial frame: g[1:4]=[g_x, g_y, g_z] In : 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 : 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_ : ndarray Numpy array containing the values of the Lagrange Multipliers related to the constraints. link_id : dict Dictionary containing the names of the links as keys and their ids as values. lrod : ndarray Numpy array containing the length of each rod. m : 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. 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 : ndarray Numpy array containing the current values of the generalized coordinates. q0 : ndarray Numpy array containing the initial values of the generalized coordinates. qa : ndarray of int Numpy array of integers containing the indices of actuated articulations (only for inverse dynamic). Those articulations are controlled by an actuator. qc : ndarray of int Numpy array of integers containing the indices of driven (locked and driven) articulations. Qc : 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 : ndarray Numpy array containing the current values of the generalized velocities. qd0 : ndarray Numpy array containing the initial values of the generalized velocities. qdd : ndarray Numpy array containing the current values of the generalized accelerations. qdd0 : ndarray Numpy array containing the initial values of the generalized accelerations. qdriven : 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 : 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 : ndarray Numpy array containing the values of the joint forces. qu : ndarray of int Numpy array of integers containing the indices of the independent articulations. qv : 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 : 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 : 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 : ndarray Numpy array containing the values of the user variables. ux0 : ndarray Numpy array containing the initial values of the user variables. uxd : ndarray Numpy array containing the values of the time derivative of the user variables. xfidpt : ndarray of int Numpy array of integers containing the indices of the points defined as force application points. Z : ndarray Numpy array containing the current values of the distances between of the points of a link. Zd : ndarray Numpy array containing the current values of the speed (spreading) between of the points of a link. Example ------- >>> mbs_data = robotran.MbsData("../dataR/UExampleProject.mbs") >>> mbs_data.user_model["MyUserModel"]["MyScalar"] = 3.0 >>> print(mbs_data.user_model["MyUserModel"]["MyScalar"]) 3.0 >>> #get a copy of the scalar >>> a = mbs_data.user_model["MyUserModel"]["MyScalar"] >>> a = 2 >>> print(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): """ 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. """ # Create a byte object from the string name_c = name.encode('utf-8') # Load the MbsInfos and keep a copy if __DEBUG__ : print( "DEBUG>> Loading: '"+name+"' from:'"+os.getcwd()+"'.") self.mbs_infos_ptr = libloadXML.mbs_info_reader(name_c) if __DEBUG__ : print( "DEBUG>> mbs_infos structure loaded") # Preparing loader and its options for loading loader = libloadXML.mbs_new_loader() if __DEBUG__ : print( "DEBUG>> Loader structure created") loader.contents.opts.contents.no_project_fct = 1 if __DEBUG__ : print( "DEBUG>> flag ignoring project function set") loader.contents.mbs_infos = self.mbs_infos_ptr if __DEBUG__ : print( "DEBUG>> mbs_infos assigned to loader") # Load and retrieve MbsData structure self.mbs_data_ptr = libloadXML.mbs_load_with_loader(name_c, None, loader) if __DEBUG__ : print( "DEBUG>> MbsData loaded") libloadXML.mbs_delete_loader(loader) # symbolic and user function folder self.user_path = None self.symbolic_path = None # Storing project function pointer # User function that only relies on MbsData structure self.user_cons_hJ = None self.user_cons_jdqd = None self.user_derivative = None self.user_DrivenJoints = None self.user_ExtForces = None self.user_JointForces = None self.user_LinkForces = None self.user_Link3DForces = None # User function also related to dirdyn, equil... self.user_dirdyn_init = None self.user_dirdyn_loop = None self.user_dirdyn_finish = None # Symbolic functions self.mbs_accelred = None self.mbs_cons_hJ = None self.mbs_cons_jdqd = None self.mbs_dirdyna = None self.mbs_extforces = None self.mbs_gensensor = None self.mbs_invdyna = None self.mbs_link = None self.mbs_link3D = None self.mbs_sensor = None # 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") # Geometric and dynamic datas 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.__qc = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qc, (self.njoint+1,)) self.__qlocked = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qlocked, (self.njoint+1,)) self.__qdriven = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qdriven, (self.njoint+1,)) self.__qa = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qa, (self.njoint+1,)) self.__qv = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qv, (self.njoint+1,)) # 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): print(self.mbs_data_ptr.contents.lrod) print(self.mbs_data_ptr.contents.lrod is None) 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.__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.joint_id = {} self.link_id = {} self.extforce_id = {} self.sensor_id = {} self.__generate_id__() #UserModel self.user_model = _UserModelDict() self.__load_user_model__() # Loading user function if __DEBUG__ : print( "DEBUG>> Loading user functions") self.__load_user_fct__(user_path) # Loading symbolic function if __DEBUG__ : print( "DEBUG>> Loading symbolic functions") self.__load_symb_fct__(symbolic_path) def __str__(self): if __DEBUG__ : print( "DEBUG>> start of __str") libutilities.mbs_print_data(self.mbs_data_ptr) if __DEBUG__ : print( "DEBUG>> MbsData printed") return "MbsData instance printed from C library!" def __load_user_fct__(self, user_path=None): """ Load all user functions where arguments are in MbsData instance. Only load the user functions in which the arguments are not dependent of another module instance (ie. MbsPart, MbsDirdyn...). The other user functions will be loaded when a module is created. The functions will be assigned to the MbsData instance when the 'run' function in other modules is called and unassigned at the end. The loader user functions are : - user_derivatives (from derivatives.py) - user_DrivenJoints (from driven_joints.py) - user_ExtForces (from ext_forces.py) - user_JointForces (from joint_forces.py) - user_LinkForces (from link_forces.py) - user_Link3DForces (from link3D_forces.py) """ template_path = os.path.join(__MODULE_DIR__,'../templates/user') project_path = self.project_path # Creating user path if user_path == 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): print ('The user function directory does not exist: "'+ user_path+'"') print ('The current root folder is: "'+os.getcwd()+'"') self.user_path = user_path # derivative user_file = "derivatives.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): if __DEBUG__ : print( "DEBUG>> file '"+user_file+"' not found in folder '"+os.path.dirname(path)) path = os.path.abspath(os.path.join(template_path, user_file)) module = imp.load_source(user_file[:-3], path) self.user_derivative = module.user_derivatives # drivenJoints user_file = "driven_joints.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): if __DEBUG__ : print( "DEBUG>> file '"+user_file+"' not found in folder '"+os.path.dirname(path)) path = os.path.abspath(os.path.join(template_path, user_file)) module = imp.load_source(user_file[:-3], path) self.user_DrivenJoints = module.user_DrivenJoints # ext_forces user_file = "ext_forces.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): if __DEBUG__ : print( "DEBUG>> file '"+user_file+"' not found in folder '"+os.path.dirname(path)) path = os.path.abspath(os.path.join(template_path, user_file)) module = imp.load_source(user_file[:-3], path) self.user_ExtForces = module.user_ExtForces # joint_forces user_file = "joint_forces.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): if __DEBUG__ : print( "DEBUG>> file '"+user_file+"' not found in folder '"+os.path.dirname(path)) path = os.path.abspath(os.path.join(template_path, user_file)) module = imp.load_source(user_file[:-3], path) self.user_JointForces = module.user_JointForces # link_forces user_file = "link_forces.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): if __DEBUG__ : print( "DEBUG>> file '"+user_file+"' not found in folder '"+os.path.dirname(path)) path = os.path.abspath(os.path.join(template_path, user_file)) module = imp.load_source(user_file[:-3], path) self.user_LinkForces = module.user_LinkForces # link3D_forces user_file = "link3D_forces.py" path = os.path.abspath(os.path.join(user_path, user_file)) if not os.path.isfile(path): if __DEBUG__ : print( "DEBUG>> file '"+user_file+"' not found in folder '"+os.path.dirname(path)) path = os.path.abspath(os.path.join(template_path, user_file)) module = imp.load_source(user_file[:-3], path) self.user_Link3DForces = module.user_Link3DForces return def __load_symb_fct__(self, symb_path=None): """ Load all symbolic functions if all args are in MbsData instance. Only load the symbolic functions in which the arguments are not dependent of another module instance (ie. MbsPart, MbsDirdyn...). The other symbolic functions will be loaded when a module is instancied. The functions will be assigned to the MbsData instance when the 'run' function in other modules is called and unassigned at the end. The loader symbolic functions are : - mbs_accelred (from mbs_accelred_MBSNAME.py) - extforces (from mbs_extforces_MBSNAME.py) - sensor (from mbs_gensensor_MBSNAME.py) - sensor (from mbs_sensor_MBSNAME.py) - link (from mbs_link_MBSNAME.py) - link3D (from mbs_link3D_MBSNAME.py) """ mbs_name = self.mbs_name template_path = os.path.join(__MODULE_DIR__,'../templates/symbolic') project_path = self.project_path # Creating user path if symb_path == None: symb_path = os.path.join(project_path, "symbolicR") else: symb_path = os.path.join(project_path, symb_path) # Error handeling if not os.path.isdir(symb_path): print ('The symbolic function directory does not exist: "'+ symb_path+'"') print ('The current root folder is: "'+os.getcwd()+'"') self.symbolic_path = symb_path # mbs_accelred symb_file = "mbs_accelred_"+mbs_name+".py" path = os.path.abspath(os.path.join(symb_path, symb_file)) if not os.path.isfile(path): if __DEBUG__ : print( "DEBUG>> file '"+symb_file+"' not found in folder '"+os.path.dirname(path)) symb_file = "mbs_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 # mbs_extforces symb_file = "mbs_extforces_"+mbs_name+".py" path = os.path.abspath(os.path.join(symb_path, symb_file)) if not os.path.isfile(path): if __DEBUG__ : print( "DEBUG>> file '"+symb_file+"' not found in folder '"+os.path.dirname(path)) symb_file = "mbs_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 # mbs_gensensor symb_file = "mbs_gensensor_"+mbs_name+".py" path = os.path.abspath(os.path.join(symb_path, symb_file)) if not os.path.isfile(path): if __DEBUG__ : print( "DEBUG>> file '"+symb_file+"' not found in folder '"+os.path.dirname(path)) symb_file = "mbs_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 # mbs_link symb_file = "mbs_link_"+mbs_name+".py" path = os.path.abspath(os.path.join(symb_path, symb_file)) if not os.path.isfile(path): if __DEBUG__ : print( "DEBUG>> file '"+symb_file+"' not found in folder '"+os.path.dirname(path)) symb_file = "mbs_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 # mbs_link3D symb_file = "mbs_link3D_"+mbs_name+".py" path = os.path.abspath(os.path.join(symb_path, symb_file)) if not os.path.isfile(path): if __DEBUG__ : print( "DEBUG>> file '"+symb_file+"' not found in folder '"+os.path.dirname(path)) symb_file = "mbs_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 # mbs_sensor symb_file = "mbs_sensor_"+mbs_name+".py" path = os.path.abspath(os.path.join(symb_path, symb_file)) if not os.path.isfile(path): if __DEBUG__ : print( "DEBUG>> file '"+symb_file+"' not found in folder '"+os.path.dirname(path)) symb_file = "mbs_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 def __assign_user_fct__(self): """ Assign all user functions if all args are in MbsData instance. Assign and wrap python user functions in which the arguments are not dependent of another module instance (ie. MbsPart, MbsDirdyn...). Store the functions in the MbsData instance and assign the pointer of functions in the C structure. The other user functions will be assigned when a module is instancied. """ data_c_ptr=self.mbs_data_ptr.contents # 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"] # 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 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 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 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 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 def __assign_symb_fct__(self): """ Assign all symbolic functions if all args are in MbsData instance. Assign and wrap python symbolic functions in which the arguments are not dependent of another module instance (ie. MbsPart, MbsDirdyn...). Store the functions in the MbsData instance and assign the pointer of functions in the C structure. The other user functions will be assigned when a module is instancied. """ data_c_ptr=self.mbs_data_ptr.contents # 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 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 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_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 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_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"] 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[:] 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): """ callback for gensensor, forcesensor and sensor """ self.__sens.__reassign_memory__(sens) fun(self.__sens,self, isens) def __callback_mbs_invdyna(self, fun,Q, tsim): __Q = np.ctypeslib.as_array(Q, (self.njoint+1,)) fun(self, tsim, __Q) def __unassign_user_fct__(self): """ Unassign all user function if all args are in MbsData instance. Only unassign the user functions in which the arguments are not dependent of another module instance (ie. MbsPart, MbsDirdyn...). """ #not necessary but if not present, dictionary must be filled in the load #function and not the assign function self.ptrs_to_user_fcts = {} self.mbs_data_ptr.contents.user_Derivative = user_Derivative_wrap(lambda mbs : callback_undefined("user_Derivative")) self.mbs_data_ptr.contents.user_DrivenJoints = user_DrivenJoints_wrap(lambda mbs,t : callback_undefined("user_DrivenJoints")) self.mbs_data_ptr.contents.user_ExtForces = user_ExtForces_wrap(lambda PxF,RxF,VxF,OMxF,AxF,OMPxF,mbs,tsim,ixF : callback_undefined("user_ExtForces")) self.mbs_data_ptr.contents.user_JointForces = user_JointForces_wrap(lambda mbs,t : callback_undefined("user_JointForces")) self.mbs_data_ptr.contents.user_LinkForces = user_LinkForces_wrap(lambda Z, Zd, mbs, tsim, ilnk : callback_undefined("user_LinkForces")) self.mbs_data_ptr.contents.user_Link3DForces = user_Link3DForces_wrap(lambda PxF,RxF,VxF,OMxF,AxF,OMPxF,mbs,tsim,ixF : callback_undefined("user_Link3DForces")) def __unassign_symb_fct__(self): """ Unassign all symbolic function if all args are in MbsData instance. Only unassign the symbolic functions in which the arguments are not dependent of another module instance (ie. MbsPart, MbsDirdyn...). """ #not necessary but if not present, dictionary must be filled in the load #function and not the assign function self.ptrs_to_symb_fcts = {} self.mbs_data_ptr.contents.mbs_accelred = mbs_accelred_wrap(lambda mbs,tsim : callback_undefined("mbs_accelred")) self.mbs_data_ptr.contents.mbs_extforces= mbs_extforces_wrap(lambda frc,trq,mbs,tsim : callback_undefined("mbs_extforces")) self.mbs_data_ptr.contents.mbs_gensensor = mbs_gensensor_wrap(lambda sens,mbs,isens : callback_undefined("mbs_gensensor")) self.mbs_data_ptr.contents.mbs_link = mbs_link_wrap(lambda frc,trq,Flnk,Z,Zd,mbs,tsim : callback_undefined("mbs_link")) self.mbs_data_ptr.contents.mbs_link3D = mbs_link3D_wrap(lambda frc,trq,mbs,tsim : callback_undefined("mbs_link3D")) self.mbs_data_ptr.contents.mbs_sensor = mbs_sensor_wrap(lambda sens,mbs,isens : callback_undefined("mbs_sensor")) def __generate_id__(self): """ Read the id's of the joints, links, sensors and extforces from MbsInfos structure and store them into dictionaries in the MbsData instance. """ for i in range(self.njoint): name = self.mbs_infos_ptr.contents.bodytree.contents.joint_list[i].contents.name.decode('utf-8') self.joint_id[name] = i+1 if self.Nlink: for i in range(self.Nlink): name = self.mbs_infos_ptr.contents.links.contents.link_list[i].contents.name.decode('utf-8') 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 = self.mbs_infos_ptr.contents.point_list[index].contents.sensor.contents.name.decode('utf-8') 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 = self.mbs_infos_ptr.contents.point_list[index].contents.extforce.contents.name.decode('utf-8') 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 """ self.user_model._locked = False infos_c_ptr=self.mbs_infos_ptr.contents for i in range(infos_c_ptr.user_models.contents.n_user_model): user_model_list = infos_c_ptr.user_models.contents.user_model_list[i].contents name = user_model_list.name.decode('utf-8') self.user_model[name] = _UserModelDict() self.user_model[name]._parent_key = name self.user_model[name]._locked = False self.user_model[name]._type = dict() for j in range(user_model_list.n_parameter): parameter_list = user_model_list.parameter_list[j].contents name2 = parameter_list.name.decode('utf-8') val_ptr = parameter_list.val_ptr value_list = parameter_list.value_list self.user_model[name]._type[name2] = parameter_list.type size = parameter_list.n_value UmPy = (ctypes.cast(val_ptr, ctypes.c_void_p).value==ctypes.cast(value_list, ctypes.c_void_p).value) if UmPy : if self.user_model[name]._type[name2] == 5: self.user_model[name][name2] = np.array(np.ctypeslib.as_array(val_ptr, (1,size)), dtype = np.int) else: self.user_model[name][name2] = np.ctypeslib.as_array(val_ptr, (1,size)) #in full python project, the C memory is always a double else : #not yet tested, it requires user and symbolic fonctions in C if self.user_model[name]._type[name2] == 5 or self.user_model[name]._type[name2] == 7: self.user_model[name][name2] = np.ctypeslib.as_array(ctypes.cast(val_ptr, ctypes.POINTER(ctypes.c_int)), (1,size)) else : self.user_model[name][name2] = np.ctypeslib.as_array(val_ptr, (1,size)) self.user_model[name]._locked = True self.user_model._locked = True def set_nb_userc(self, nb): """ Set 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. """ if isinstance(indices,(list,np.ndarray)): for index in 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. """ if isinstance(indices,(list,np.ndarray)): for index in indices: libutilities.mbs_set_qv(self.mbs_data_ptr, index) else: libutilities.mbs_set_qv(self.mbs_data_ptr, indices) def set_qa(self, indices): """ Set a variable to actuated partition """ if isinstance(indices,(list,np.ndarray)): for index in 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): """ Unset a variable from the actuated partition """ if isinstance(indices,(list,np.ndarray)): for index in 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): libloadXML.mbs_delete_infos(self.mbs_infos_ptr) libloadXML.mbs_delete_data(self.mbs_data_ptr) if __DEBUG__ : print("DEBUG>> MbsData pointer deleted") def mbs_reset_data(self,verbose=True): """ Reset mbs_data to its inital value """ libutilities.mbs_reset_data(self.mbs_data_ptr) if verbose : print("mbs_data reseted to initial value") def mbs_load_from_file(self, baseFileName="dirdyn", result_path="resultsR", time = -1): """ 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 q, qd, qdd, ux, uxd, q0, qd0, qdd0, ux0 ant tsim are set. :param baseFileName The path and resfilename containing the state to load. The suffix _q, _qd, _ux.res will be added. default = "dirdyn" if not specified :param result_path The relative path of the result folder from the project folder default = "resultsR" if not specified :param time The time corresponding to the state to load defaut value is the last state. """ 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: if __DEBUG__ : print ("Time specified not found, take the last occurence where t> The result directory does not exist: "'+ result_path+'"') index = -2 # Generalized coordinates CurFile = baseFileName+'_q.res' path = os.path.abspath(os.path.join(result_path, CurFile)) if (os.path.isfile(path) ): temp = np.loadtxt(path) index = find_index(temp, time) self.q[1:] = temp[index, 1:] self.q0[1:] = self.q[1:] self.tsim = temp[index, 0] else: if __DEBUG__ : print( "DEBUG>> file '"+CurFile+"' not found in folder '"+os.path.dirname(path)) # Generalized velocities # Assumption: index is still valid CurFile = baseFileName+'_qd.res' path = os.path.abspath(os.path.join(result_path, CurFile)) if (os.path.isfile(path)): temp = np.loadtxt(path) if index == -2: index = find_index(temp, time) self.qd[1:] = temp[index, 1:] self.qd0[1:] = self.qd[1:] else: if __DEBUG__ : print( "DEBUG>> file '"+CurFile+"' not found in folder '"+os.path.dirname(path)) # Generalized accelerations # Assumption: index is still valid CurFile = baseFileName+'_qdd.res' path = os.path.abspath(os.path.join(result_path, CurFile)) if (os.path.isfile(path)): temp = np.loadtxt(path) if index == -2: index = find_index(temp, time) self.qdd[1:] = temp[index, 1:] self.qdd0[1:] = self.qdd[1:] else: if __DEBUG__ : print( "DEBUG>> file '"+CurFile+"' not found in folder '"+os.path.dirname(path)) # Generalized user state # Assumption: index is still valid if self.Nux: CurFile = baseFileName+'_ux.res' path = os.path.abspath(os.path.join(result_path, CurFile)) if (os.path.isfile(path) ): temp = np.loadtxt(path) if index == -2: index = find_index(temp, time) self.ux[1:] = temp[index, 1:] self.ux0[1:] = self.ux[1:] else: if __DEBUG__ : print( "DEBUG>> file '"+CurFile+"' not found in folder '"+os.path.dirname(path)) CurFile = baseFileName+'_uxd.res' path = os.path.abspath(os.path.join(result_path, CurFile)) if (os.path.isfile(path)): temp = np.loadtxt(path) if index == -2: index = find_index(temp, time) self.uxd[1:] = temp[index, 1:] else: if __DEBUG__ : print( "DEBUG>> file '"+CurFile+"' not found in folder '"+os.path.dirname(path)) def exe_dirdyn(self, **kwargs): """ Run a dirdyn module with a specified set of options Also run a partitioning module if needed. Partioning options are setted to default values expect rowperm = 1 param : options of the dirdyn module (see set_options in dirdyn module) out : MbsResults instance """ if (not self.DonePart) and self.Ncons: mbs_part = MbsPart(self) mbs_part.set_options(verbose=False, rowperm = True) mbs_part.run() mbs_dirdyn = MbsDirdyn(self) mbs_dirdyn.set_options(**kwargs) results = mbs_dirdyn.run() else: mbs_dirdyn = MbsDirdyn(self) mbs_dirdyn.set_options(**kwargs) results = mbs_dirdyn.run() return results def exe_part(self, **kwargs): """ Run a partitioning module with the specified options param : options of the partitioning module (see set_options in part module) """ mbs_part = MbsPart(self) mbs_part.set_options(**kwargs) mbs_part.run() #========================================================================== # Defining properties #========================================================================== @property def npt(self): return self.mbs_data_ptr.contents.npt @property def dpt(self): return self.__dpt @property def l(self): return self.__l @property def m(self): return self.__m @property def In(self): return self.__In @property def g(self): return self.__g @property def nbody(self): return self.mbs_data_ptr.contents.nbody @property def njoint(self): return self.mbs_data_ptr.contents.njoint @property def nqu(self): return self.mbs_data_ptr.contents.nqu @property def nqc(self): return self.mbs_data_ptr.contents.nqc @property def nqlocked(self): return self.mbs_data_ptr.contents.nqlocked @property def nqdriven(self): return self.mbs_data_ptr.contents.nqdriven @property def nqa(self): return self.mbs_data_ptr.contents.nqa @property def nqv(self): return self.mbs_data_ptr.contents.nqv @property def nhu(self): return self.mbs_data_ptr.contents.nhu @property def qc(self): return self.__qc @property def qa(self): return self.__qa @property def q(self): return self.__q @property def qd(self): return self.__qd @property def qdd(self): return self.__qdd @property def q0(self): return self.__q0 @property def qd0(self): return self.__qd0 @property def qdd0(self): return self.__qdd0 @property def frc(self): return self.__frc @property def trq(self): return self.__trq @property def Qa(self): return self.__Qa @property def Qq(self): return self.__Qq @property def tsim(self): return self.mbs_data_ptr.contents.tsim @tsim.setter def tsim(self, value): self.mbs_data_ptr.contents.tsim = value @property def t0(self): return self.mbs_data_ptr.contents.t0 @t0.setter def t0(self, value): self.mbs_data_ptr.contents.t0 = value @property def tf(self): return self.mbs_data_ptr.contents.tf @tf.setter def tf(self, value): self.mbs_data_ptr.contents.tf = value @property def lrod(self): return self.__lrod @property def Nloopc(self): return self.mbs_data_ptr.contents.Nloopc @property def Ncons(self): return self.mbs_data_ptr.contents.Ncons @property def Nuserc(self): return self.mbs_data_ptr.contents.Nuserc @property def lambda_(self): return self.__lambda_ @property def Qc(self): return self.__Qc @property def Nlink(self): return self.mbs_data_ptr.contents.Nlink @property def Z(self): return self.__Z @property def Zd(self): return self.__Zd @property def Fl(self): return self.__Fl # @property # def Nlink3D(self): # return self.mbs_data_ptr.contents.Nlink3D @property def Nsensor(self): return self.mbs_data_ptr.contents.Nsensor @property def Nxfrc(self): return self.mbs_data_ptr.contents.Nxfrc @property def xfidpt(self): return self.__xfidpt @property def SWr(self): return self.__SWr @property def ux(self): return self.__ux @property def uxd(self): return self.__uxd @property def ux0(self): return self.__ux0 @property def Nux(self): return self.mbs_data_ptr.contents.Nux @property def DonePart(self): return self.mbs_data_ptr.contents.DonePart @property def process(self): return self.mbs_data_ptr.contents.process @process.setter def process(self, value): self.mbs_data_ptr.contents.process = value @property def mbs_filename(self): return ctypes.string_at(self.mbs_data_ptr.contents.mbs_filename).decode("utf-8") @property def project_path(self): return ctypes.string_at(self.mbs_data_ptr.contents.project_path).decode("utf-8") @property def mbs_name(self): return ctypes.string_at(self.mbs_data_ptr.contents.mbs_name).decode("utf-8") class _UserModelDict(dict): """ Class defined for user models, inherites from dict setitem and getitem function were updated """ def __init__(self, *args, **kwargs): dict.__init__(self, *args, **kwargs) self._locked = True "flag that locks the addition of a new item" self._parent_key = None "key of the parent dict, None if not" self._type = None "type of the parameter, None if not" def __setitem__(self, key, value): if self._locked == True: # if it is a parameter and not a parent if not (self._type is None): if key in self.keys(): if self._type[key] == 1: if type(value) is float: tab = self.__getarray__(key) tab[0][0] = value elif type(value) is int: tab = self.__getarray__(key) tab[0][0] = int(value) else : print("UserModel "+self._parent_key+"."+key+" is a float, "+str(type(value))+" was given") elif self._type[key] == 7: if type(value) is int: tab = self.__getarray__(key) tab[0][0] = value else : print("UserModel "+self._parent_key+"."+key+" is an int, "+str(type(value))+" was given") elif self._type[key] == 2 or self._type[key] == 5: if type(value) is np.ndarray or type(value) is list: tab = self.__getarray__(key) if np.size(value) == np.size(tab): tab[:] = value[:] else: print("UserModel "+self._parent_key+"."+key+" is an array of size "+str(np.size(tab))+", array of size "+str(np.size(value))+" was given") else : print("UserModel "+self._parent_key+"."+key+" is an array, "+str(type(value))+" was given") elif self._type[key] == 3: print("Type lut1D for a user mode is not available in python") elif self._type[key] == 4: print("Type lut2D for a user mode is not available in python") elif self._type[key] == 6: print("Type structure for a user model is not available in python") else : print("Invalid type for the user model") else: print("Unknown user model "+str(self._parent_key)+"."+str(key)) else : dict.__setitem__(self, key, value) def __getarray__(self, key): """ Get the full array stored in the dict """ return dict.__getitem__(self, key) def __getitem__(self, key): """ Get the item in the array in the dict if scalar, return a scalar if vector, return a vector """ if self._type is None: return dict.__getitem__(self, key) else : if key in self.keys(): if self._type[key] == 1 or self._type[key] == 7 : return dict.__getitem__(self, key)[0][0] else: return dict.__getitem__(self, key)[0] else: print("Unknown user model "+str(self._parent_key)+"."+str(key))