mbs_dirdyn.py

# -*- 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 MbsDirdyn based on the MbsDirdyn structure of MBsysC.
This class has the functions required to manipulate the direct dynamic module.
This include setting the options, running an (or multiple) analysis and freeing
the memory.
"""

import os
import imp
import ctypes

import numpy as np

# importing MbsysPy classes

# importing libraries
from ..mbsysc_loader.loadlibs import libmodules

# importing utilities function
from ..mbs_utilities import callback_undefined

# importing wrapping function
from ..mbsysc_loader.callback import user_cons_hJ_wrap
from ..mbsysc_loader.callback import user_cons_jdqd_wrap
from ..mbsysc_loader.callback import user_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 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 MbsDirdyn class
#==============================================================================

class MbsDirdyn(object):
    """
    Class of the direct dynamic module
    
    Attributes
    ----------
    dt : double
        Current integration step size.
    mbs : MbsData
        Instance of MbsData related to the analysis.
    results : MbsResult
        Instance of MbsResult containing the results of the direct dynamics analysis.
    symbolic_path : str
        Path to the folder containing the symbolic functions (python modules) 
        to be loaded.
    tsim : double
        Current simulation time.
    user_path : str
        Path to the folder containing the user functions (python modules) to be loaded.
    
    Examples
    --------
    >>> mbs_data = MBsysPy.MbsData("../dataR/ExampleProject.mbs")
    >>> mbs_dirdyn = MBsysPy.MbsDirdyn(mbs_data)
    >>> mbs_dirdyn.set_options(t0 = 5, tf = 10)
    >>> mbs_dirdyn.get_options("t0", "tf")
        (5.0, 10.0)
    >>> results = mbs_dirdyn.run()
    
    """
    def __init__(self, mbs, user_path=None, symbolic_path=None):
        """
        Create an instance of the MbsDirdyn class for the provided MbsData instance.

        Parameters
        ----------
        mbs : MbsData
            Instance of MbsData related to the analysis.
        user_path : str or None, optionnal
            The path to the folder containing the user functions. If not provided 
            ('None') the path is retrieved from the MbsData instance 'mbs'.
            default is None
        symbolic_path : str or None, optionnal
            The path to the folder containing the symbolic functions. If not 
            provided ('None') the path is retrieved from the MbsData instance 'mbs'.
            default is None
            
        Returns
        -------
            A MbsDirdyn instance.
        """
            
        if __DEBUG__ : print( "DEBUG>>  Creating MbsDirdyn struct for "+mbs.mbs_name+"' MBS.")
        self.mbs_dirdyn_ptr = libmodules.mbs_new_dirdyn(mbs.mbs_data_ptr)
        if __DEBUG__ : print( "DEBUG>>  MbsDirdyn structure loaded")
        self.mbs = mbs

        #Test
        if __DEBUG__ :
            print( "DEBUG>>  MbsDirdyn created.")
        # Path to user function used by partitionning module
        self.user_path = self.mbs.user_path
        if not (user_path==None):
            project_path  = (self.mbs.project_path).decode("utf-8")
            user_path = os.path.join(project_path, user_path)
            # Error handeling
            if not os.path.isdir(user_path):
                print ('The user function directory for direct dynamic module does not exist: "'+ user_path+'"')
                print ('The current root folder is: "'+os.getcwd()+'"')
                print ('The following directory is used instead: "'+self.user_path+'".')
            else:
                self.user_path = user_path
        # Path to user function used by partitionning modue
        self.symbolic_path = self.mbs.symbolic_path
        if not (user_path==None):
            project_path  = (self.mbs.project_path).decode("utf-8")
            symbolic_path = os.path.join(project_path, symbolic_path)
            # Error handeling
            if not os.path.isdir(symbolic_path):
                print ('Thesymbolic function directory for direct dynamic module does not exist: "'+ symbolic_path+'"')
                print ('The current root folder is: "'+os.getcwd()+'"')
                print ('The following directory is used instead: "'+self.symbolic_path+'".')
            else:
                self.symbolic_path = symbolic_path
        
        # dictionary containing the pointers to avoid garbage collecting
        self.ptrs_to_user_fcts = dict()
        self.ptrs_to_symb_fcts = dict()
        
        # Storing project function pointer
        self.user_cons_hJ = None
        self.user_cons_jdqd = None
        self.user_dirdyn_init = None
        self.user_dirdyn_loop = None
        self.user_dirdyn_finish = None
        self.mbs_cons_hJ = None
        self.mbs_cons_jdqd = None
        self.mbs_invdyna = None
        self.mbs_dirdyna = None
        
        # Storing Results
        self.results = MbsResult(self.mbs)
        self.store_results = True
        
        # Exposing some memory
        if __DEBUG__ : print( "DEBUG>>  Exposing MbsDirdyn fields")
        # Constraints
        self._h = self._Jac = self._jdqd = None
        if self.mbs.Ncons>0:
            self._h = np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.mbs_aux.contents.h, (self.mbs.Ncons+1,))
            self._Jac = np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.mbs_aux.contents.Jac[0], (self.mbs.Ncons+1,self.mbs.njoint+1))
            self._jdqd = np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.mbs_aux.contents.jdqd, (self.mbs.Ncons+1,))
        self._huserc = self._Juserc = self._jdqduserc = None
        if self.mbs.Nuserc>0:
            self._huserc = np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.mbs_aux.contents.huserc, (self.mbs.Nuserc+1,))
            self._Juserc = np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.mbs_aux.contents.Juserc[0], (self.mbs.Nuserc+1,self.mbs.njoint+1))
            self._jdqduserc = np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.mbs_aux.contents.jdqduserc, (self.mbs.Nuserc+1,))
        # Fields from equation of motion
        self._M = np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.mbs_aux.contents.M[0], (self.mbs.njoint+1,self.mbs.njoint+1))
        self._c = np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.mbs_aux.contents.c, (self.mbs.njoint+1,))
        self._F = np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.mbs_aux.contents.F, (self.mbs.njoint+1,))
        
        # Loading user function
        if self.mbs.opt_load_c < 2:
            if __DEBUG__ : print( "DEBUG>>  Loading user functions")
            self.__load_user_fct__(self.user_path)
        # Loading symbolic function
        if self.mbs.opt_load_c < 1:
            if __DEBUG__ : print( "DEBUG>>  Loading symbolic functions")
            self.__load_symbolic_fct__(self.symbolic_path)
        

    def __str__(self):
        if __DEBUG__ : print( "DEBUG>>  start of __str")
        
        return "MbsDirdyn instance has nothing to be printed from C library!"
    
    def __load_user_fct__(self, user_path):
        """ 
        Load user function where some args depend on MbsDirdyn module.
        
        Load the user functions in which some of the arguments are  
        dependent of MbsDirdyn module instance. The functions will be 
        assigned to the MbsData instance when the 'run' functions is called 
        and unassigned at the end.
        
        The loader user functions are :
         - user_cons_hJ (from cons_hJ.py)
         - user_cons_jdqd (from cons_hJ.py)
         - user_dirdyn_init (from user_dirdyn.py)
         - user_dirdyn_loop (from user_dirdyn.py)
         - user_dirdyn_finish (from user_dirdyn.py)

        Parameters
        ----------
        user_path : str
            The path to the folder containing the user functions.
        """
        
        template_path = os.path.join(__MODULE_DIR__,'../templates/user')
        
        
        # cons_hJ
        user_file = "cons_hJ.py"
        path = os.path.abspath(os.path.join(user_path, user_file))
        if not os.path.isfile(path):
            if __DEBUG__ : print( "DEBUG>>  file '"+user_file+"' not found in folder '"+os.path.dirname(path))
            path = os.path.abspath(os.path.join(template_path, user_file))
        module = imp.load_source(user_file[:-3], path)
        self.user_cons_hJ = module.user_cons_hJ
        
        # cons_jdqd
        user_file = "cons_jdqd.py"
        path = os.path.abspath(os.path.join(user_path, user_file))
        if not os.path.isfile(path):
            if __DEBUG__ : print( "DEBUG>>  file '"+user_file+"' not found in folder '"+os.path.dirname(path))
            path = os.path.abspath(os.path.join(template_path, user_file))
        module = imp.load_source(user_file[:-3], path)
        self.user_cons_jdqd = module.user_cons_jdqd
        
        # user_dirdyn
        user_file = "user_dirdyn.py"
        path      = os.path.abspath(os.path.join(user_path, user_file))
        if not os.path.isfile(path):
            print ("file '"+user_file+"' not found in folder '"+os.path.dirname(path))
            path = os.path.abspath(os.path.join(template_path, user_file))
        else:
            if __DEBUG__ : print( "DEBUG>>  loading file '"+user_file+"' in folder '"+os.path.dirname(path))
        module = imp.load_source(user_file[:-3], path)
        self.user_dirdyn_init   = module.user_dirdyn_init
        self.user_dirdyn_loop   = module.user_dirdyn_loop
        self.user_dirdyn_finish = module.user_dirdyn_finish
        return
        
    def __load_symbolic_fct__(self, symbolic_path):
        """ 
        Load symbolic function where some args depend on MbsDirdyn module.
        
        Load the symb functions in which some of the arguments are  
        dependent of MbsDirdyn module instance. The functions will be 
        assigned to the MbsData instance when the 'run' functions is called 
        and unassigned at the end.
        
        The loader user functions are :
         - cons_hJ (from mbs_cons_hJ_MBSNAME.py)
         - cons_jdqd (from mbs_cons_jdqd_MBSNAME.py)
         - dirdyna (from mbs_dirdyna_MBSNAME.py)
         - invdyna (from mbs_invdyna_MBSNAME.py)

        Parameters
        ----------
        symbolic_path : str
            The path to the folder containing the symbolic functions.
        """
        mbs_name = self.mbs.mbs_name
        template_path = os.path.join(__MODULE_DIR__,'../templates/symbolic')
        # mbs_cons_hJ
        symb_file = "mbs_cons_hJ_"+mbs_name+".py"
        path = os.path.abspath(os.path.join(symbolic_path, symb_file))
        if not os.path.isfile(path):
            if __DEBUG__ : print( "DEBUG>>  file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
            symb_file = "mbs_cons_hJ_PRJ.py"
            path = os.path.abspath(os.path.join(template_path, symb_file))
        module = imp.load_source(symb_file[:-3], path)
        self.mbs_cons_hJ = module.cons_hJ
        
        
        # mbs_cons_jdqd
        symb_file = "mbs_cons_jdqd_"+mbs_name+".py"
        path = os.path.abspath(os.path.join(symbolic_path, symb_file))
        if not os.path.isfile(path):
            if __DEBUG__ : print( "DEBUG>>  file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
            symb_file = "mbs_cons_jdqd_PRJ.py"
            path = os.path.abspath(os.path.join(template_path, symb_file))
        module = imp.load_source(symb_file[:-3], path)
        self.mbs_cons_jdqd = module.cons_jdqd
        
        # 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__ : print( "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))
        module = imp.load_source(symb_file[:-3], path)
        self.mbs_dirdyna = module.dirdyna
       
        return
    
    def __assign_user_fct__(self):
        """ 
        Assign and wrap python user function where some args depend 
        on MbsDirdyn module. Store the functions in the MbsData instance 
        and assign the pointer of functions in the C structure
        """
        # cons_hJ
        self.ptrs_to_user_fcts["user_cons_hJ"] = user_cons_hJ_wrap(lambda h,Jac,mbs,tsim : self.user_cons_hJ(self._huserc, self._Juserc, self.mbs, tsim))
        self.mbs.user_cons_hJ = self.user_cons_hJ
        self.mbs.mbs_data_ptr.contents.user_cons_hJ = self.ptrs_to_user_fcts["user_cons_hJ"]
        
        # cons_jdqd
        self.ptrs_to_user_fcts["user_cons_jdqd"] = user_cons_jdqd_wrap(lambda jdqd,mbs,tsim : self.user_cons_jdqd(self._jdqduserc, self.mbs))
        self.mbs.user_cons_jdqd = self.user_cons_jdqd
        self.mbs.mbs_data_ptr.contents.user_cons_jdqd = self.ptrs_to_user_fcts["user_cons_jdqd"]

        # user_dirdyn
        self.ptrs_to_user_fcts["user_dirdyn_init"] = user_dirdyn_init_wrap(lambda mbs, dd : self.mbs.user_dirdyn_init(self.mbs, self))
        self.ptrs_to_user_fcts["user_dirdyn_loop"] = user_dirdyn_loop_wrap(lambda mbs, dd : self.mbs.user_dirdyn_loop(self.mbs, self))
        self.ptrs_to_user_fcts["user_dirdyn_finish"] = user_dirdyn_finish_wrap(lambda mbs, dd : self.mbs.user_dirdyn_finish(self.mbs, self))
        self.mbs.user_dirdyn_init = self.user_dirdyn_init
        self.mbs.mbs_data_ptr.contents.user_dirdyn_init = self.ptrs_to_user_fcts["user_dirdyn_init"]
        self.mbs.user_dirdyn_loop = self.user_dirdyn_loop
        self.mbs.mbs_data_ptr.contents.user_dirdyn_loop = self.ptrs_to_user_fcts["user_dirdyn_loop"]
        self.mbs.user_dirdyn_finish = self.user_dirdyn_finish
        self.mbs.mbs_data_ptr.contents.user_dirdyn_finish = self.ptrs_to_user_fcts["user_dirdyn_finish"]
        return
        
    def __assign_symbolic_fct__(self):
        """ 
        Assign and wrap python symbolic function where some args depend 
        on MbsDirdyn module. Store the functions in the MbsData instance 
        and assign the pointer of functions in the C structure
        """
        
        # mbs_cons_hJ
        self.ptrs_to_symb_fcts["mbs_cons_hJ"] = mbs_cons_hJ_wrap(lambda h,Jac,mbs,tsim : self.mbs_cons_hJ(self._h,self._Jac, self.mbs))
        self.mbs.mbs_cons_hJ = self.mbs_cons_hJ
        self.mbs.mbs_data_ptr.contents.mbs_cons_hJ = self.ptrs_to_symb_fcts["mbs_cons_hJ"]
        
        # mbs_cons_jdqd
        self.ptrs_to_symb_fcts["mbs_cons_jdqd"] = mbs_cons_jdqd_wrap(lambda jdqd,mbs,tsim : self.mbs_cons_jdqd(self._jdqd, self.mbs))
        self.mbs.mbs_cons_jdqd = self.mbs_cons_jdqd
        self.mbs.mbs_data_ptr.contents.mbs_cons_jdqd = self.ptrs_to_symb_fcts["mbs_cons_jdqd"]
        
        # dirdyna
        self.ptrs_to_symb_fcts["mbs_dirdyna"] = mbs_dirdyna_wrap(lambda M,c,mbs,tsim : self.mbs_dirdyna(self._M, self._c, self.mbs, tsim))
        self.mbs.mbs_dirdyna = self.mbs_dirdyna
        self.mbs.mbs_data_ptr.contents.mbs_dirdyna = self.ptrs_to_symb_fcts["mbs_dirdyna"]
         
        return
    
     # Callback function for function with advanced arguments
    def __callback_mbs_cons_hJ(self, fun, h, Jac):
        if __DEBUG__ : print( "DEBUG>>  callback_mbs_cons_hJ")
        __h = np.ctypeslib.as_array(h, (self.mbs.Ncons+1,))
        __Jac = np.ctypeslib.as_array(Jac[0], (self.mbs.Ncons+1,self.mbs.njoint+1))
        fun(__h,__Jac,self.mbs)
    
    
    def __unassign_user_fct__(self):
        """ Unassign user function where some args depend on MbsDirdyn module.  """
        
        self.ptrs_to_user_fcts = {}
        
        self.mbs.user_cons_hJ = None
        self.mbs.mbs_data_ptr.contents.user_cons_hJ = user_cons_hJ_wrap(lambda h,Jac,mbs,tsim : callback_undefined("user_cons_hJ"))
        self.mbs.user_cons_jdqd = None
        self.mbs.mbs_data_ptr.contents.user_cons_jdqd = user_cons_jdqd_wrap(lambda h,Jac,mbs,tsim : callback_undefined("user_cons_jdqd"))
        self.mbs.user_dirdyn_init = None
        self.mbs.mbs_data_ptr.contents.user_dirdyn_init = user_dirdyn_init_wrap(lambda mbs, dd : callback_undefined("user_dirdyn_init"))
        self.mbs.user_dirdyn_loop = None
        self.mbs.mbs_data_ptr.contents.user_dirdyn_loop = user_dirdyn_loop_wrap(lambda mbs, dd : callback_undefined("user_dirdyn_loop"))
        self.mbs.user_dirdyn_finish = None
        self.mbs.mbs_data_ptr.contents.user_dirdyn_finish = user_dirdyn_finish_wrap(lambda mbs, dd : callback_undefined("user_dirdyn_finish"))
        
    def __unassign_symbolic_fct__(self):
        
        """ Unassign symbolic function where some args depend on MbsDirdyn module. """
        
        self.ptrs_to_symb_fcts = {}
        
        self.mbs.mbs_cons_hJ = None
        self.mbs.mbs_data_ptr.contents.mbs_cons_hJ = mbs_cons_hJ_wrap(lambda h,Jac,mbs,tsim : callback_undefined("mbs_cons_hJ"))
        self.mbs.mbs_cons_jdqd = None
        self.mbs.mbs_data_ptr.contents.mbs_cons_jdqd = mbs_cons_jdqd_wrap(lambda jdqd,mbs,tsim : callback_undefined("mbs_cons_jdqd"))
        self.mbs.mbs_dirdyna = None
        self.mbs.mbs_data_ptr.contents.mbs_dirdyna = mbs_dirdyna_wrap(lambda M,c,mbs,tsim : callback_undefined("mbs_dirdyna"))
        
    def __del__(self):
        libmodules.mbs_delete_dirdyn(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
        if __DEBUG__ : print("DEBUG>>  MbsDirdyn pointer deleted")
        
    def run(self):
        """
        Run a direct dynamics analysis
        
        Options can be setted with the function 'set_options()' 
        Options can be retrieved with the function 'get_options()'
        
        Results are stored in the field 'results' if the options 'store_results'
        is set to True.
        
        Returns
        -------
            The MbsResults containing the results of the analysis.
        """     
        # Assing required user functions
        if self.mbs.opt_load_c < 2:
            self.__assign_user_fct__()
            self.mbs.__assign_user_fct__()
        
        # Assing required symbolic functions
        if self.mbs.opt_load_c < 1:
            self.__assign_symbolic_fct__()
            self.mbs.__assign_symb_fct__()
        
        
        if not self.store_results:   
            libmodules.mbs_run_dirdyn(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
        else:
            #save2file forced to 1 because if buffers don't have the complete results, results are loaded from files
            self.set_options(save2file = 1)
            libmodules.mbs_dirdyn_init(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
            libmodules.mbs_dirdyn_loop(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
       
            # Results (buffer) memory is kept BUT FILES WILL BE WRITTEN LATER
            if self.get_options("save2file"):
                #c_mbs_dirdyn_write_buffers(self.mbs_dirdyn_ptr)
                size1 = self.mbs_dirdyn_ptr.contents.buffers[0].contents.index
                if size1 == 0:
                    size1 = self.mbs_dirdyn_ptr.contents.buffers[0].contents.size
                size2 = self.mbs_dirdyn_ptr.contents.buffers[0].contents.nx+1
                
                # array are initialized to the time pointer so as to start index of joints at 1 (we have to ensure contiguity between t and x in buffers ! ! !)    
                self.results.q = np.copy(np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.buffers[0].contents.tx ,(size1, size2)))
                # get time array from the q buffer
                self.results.t = self.results.q[:,0]
                if not self.results.t[0] == self.get_options("t0"):
                    print("The beginning of the integration is not available in the buffer.\n The complete results have to be loaded from files.")
                    filename = ctypes.string_at(self.mbs_dirdyn_ptr.contents.buffers[0].contents.filename).decode('utf-8')
                    self.results.load_results_from_file(filename, module = 6)
                # get qd and qdd buffer
                self.results.qd = np.copy(np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.buffers[1].contents.tx ,(size1, size2)))
                self.results.qdd = np.copy(np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.buffers[2].contents.tx ,(size1, size2)))
                size2 = self.mbs_dirdyn_ptr.contents.buffers[3].contents.nx+1
                self.results.Qq = np.copy(np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.buffers[3].contents.tx ,(size1, size2)))
                buffer_id = 4
                if self.mbs.Nux:
                    size2 = self.mbs_dirdyn_ptr.contents.buffers[buffer_id].contents.nx+1
                    self.results.ux = np.copy(np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.buffers[buffer_id].contents.tx ,(size1, size2)))
                    self.results.uxd = np.copy(np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.buffers[buffer_id+1].contents.tx ,(size1, size2)))
                    buffer_id = buffer_id + 2
                if self.mbs.Nlink:
                    size2 = self.mbs_dirdyn_ptr.contents.buffers[buffer_id].contents.nx+1
                    self.results.Z = np.copy(np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.buffers[buffer_id].contents.tx ,(size1, size2)))
                    self.results.Zd = np.copy(np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.buffers[buffer_id+1].contents.tx ,(size1, size2)))
                    self.results.Fl = np.copy(np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.buffers[buffer_id+2].contents.tx ,(size1, size2)))
                    buffer_id = buffer_id + 3
                if self.mbs.nqc:
                    size2 = self.mbs_dirdyn_ptr.contents.buffers[buffer_id].contents.nx+1
                    self.results.Qc = np.copy(np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.buffers[buffer_id].contents.tx ,(size1, size2)))
                    
                if self.mbs_dirdyn_ptr.contents.user_buffer.contents.nx:
                    size = self.mbs_dirdyn_ptr.contents.user_buffer.contents.index
                    nbOutput = self.mbs_dirdyn_ptr.contents.user_buffer.contents.nx
                    for i in range(nbOutput):
                        user_out = np.copy(np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.user_buffer.contents.X[i],(1, size)))
                        name = self.mbs_dirdyn_ptr.contents.user_buffer.contents.names[i]
                        name = name.decode('utf-8')
                        self.results.outputs[name] = user_out[0]
            
            #libmodules.mbs_dirdyn_finish(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
        
        # Anassign user functions
        if self.mbs.opt_load_c < 2:
            self.__unassign_user_fct__()
            self.mbs.__unassign_user_fct__()
        
        # Anassing required symbolic functions
        if self.mbs.opt_load_c < 1:
            self.__unassign_symbolic_fct__()
            self.mbs.__unassign_symb_fct__()
        
        return self.results
    
    def set_options(self, **kwargs):
        """
        Set the specified options for Dirdyn module.
        
        Parameters
        ----------
        t0 : float
            Initial time of the simulation.
            default is 0.0
        tf : float
            Final time of the simulation
            default is 5.0
        dt0 : float
            Initial value of the integration step size. For fixed-step integrator
            (ie. RK4, Euler explicit...) this is the time step for the whole simulation.
            default is 0.001
        save2file : int
            Determine whether results are written to files on disk (in resultsR folder):  
             - 1: results are saved
             - 0: results are not saved 
            default is 1
        resfilename : str
            The keyword used for determining the name of result files.
            default is 'dirdyn'
        respath : str
            Path in which result file are saved. This is the full path or the
            relative path. The folder must exist. 
            default is the 'resultsR/' folder of the project
        animpath : str
            Path in which animation file is saved. This is the full path or the
            relative path. The folder must exist. 
            default is the 'animationR/' folder of the project
        save_anim : int
            1 to save the animation file, if 'save2file' is set to 1. Set to 0 
            to not save the animation file.
            default is 1
        save_visu : int 
            Unused options as realtime is deactivated is MBsysPy.
            1 to save the visualizazion file (as it appears in 'user_realtime_visu.c'),
            if 'save2file' is set to 1. Set to O to not save the visualization.
            default is 0
        framerate : int
            number of frame per second for the animation file.
            default is 1000
        saveperiod : int
            The number of time steps between two buffer records.
            default is 1 (every time step are recorded)
        max_save_user : int
            The maximal number of user variables saved.
            default is 12
        buffersize : int 
            The number of time step that can be recorded in the buffer. Results 
            are written to disk when the buffer is full. Writing to disk is slow.
            If set to -1, it computes the buffer size for saving results only once
            at the end according to dt0, t0 and tf.
            default is -1
        realtime : int
            Unused options as realtime is deactivated is MBsysPy.
            1 to activate to real-time features, 0 to deactivate them.
            default = 0
        accelred : int
            1 to use accelred, 0 otherwise.
            default is 0
        flag_compute_Qc : int
            If 1 it computes the forces/torques applied in each driven joints in
            order to follow the specified trajectory. Otherwhise the forces/torques
            are only computed in the joints specified in the option 'compute_Qc'.
            Setting the option to 0 speeds up the computation.
            default is 1
        compute_all_uxd : int
            If 1 the user derivative are computed during the analysis. Otherwhise
            If set to 0, they are not computed.
            default is 1
        compute_Qc : numpy.ndarray
            If option "flag_compute_Qc' is set to 0, the vector allows to select
            the (driven) joints on which the force/torque required to follow the 
            trajectory will be computed.
            The shape of the array is '(mbs.njoint+1)', for example with 
            'compute_Qc = numpy.array([mbs.njoint 1 0 0 1 0])', we will compute
            Qc(1) and Qc(4) only, on a mbs made of 5 joints.
            compute_Qc[0] is always mbs.njoint
            default is an array full of zero (no forces computation)
        integrator : str
            Set integrator to use, available value: 
             - "RK4": explicit Runge–Kutta method order 4
                      fixed stepsize
             - "Dopri5": explicit Runge–Kutta method of order (4)5
                         adaptative stepsize
             - "Rosenbrock": implicit fourth-order Rosenbrock
                             adaptative stepsize
                             for stiff problems
             - "EulerEx": Euler explicit method
                          fixed stepsize
             - "Eulaire": Modified Euler explicit method
                          fixed stepsize
             - "EulerIm": Euler implicit method
                          fixed stepsize
                          for stiff problems
             - "WMethods":  Implicit Euler extented to two stages
                            for stiff problems
             - "Bader": Semi-implicit midpoint rule method
                        adaptative stepsize
                        for stiff problems
            default is "RK4"
        verbose : int 
            Set to 1 to get messages related to adaptive stepsize integrator. To
            disable message set to 0.
            default is 1
        flag_stop_stiff : int 
            For adaptive stepsize integrator, set at 1 to stop integration if the
            systems become too stiff. Set it at 0 to continue integration.
            default is 0
        flag_precise_dynamics : int
            Flag to set which values are saved in the output files. This only
            changes the value save to the output files not the simulation results.
            
            If set at 1, the direct dynamics (constraint solving, computation of
            the generalized acceleration) will be done at the beginning of each
            time step of integration. The output files will be exact, but the 
            computation will be a little slower.
            If set at 0, the output file will contains the value (constraint 
            solution anf the generalized acceleration) of the last computation
            of the integrator. The values may be the one used for an internal step
            of the integrator, but the computation is a little faster.

            default is 1
        flag_waypoint : int
            If set to 1, the integrator will be forced to give a solution at the
            specified time interval ('delta_t_wp'). If set to 0, the solution
            will be given at every computed time-step.
            default is 0
        flag_solout_wp : int
            Only used if 'flag_waypoint' is 1. In that case if set to 1, the 
            integration results will only contains the value at the specified 
            time interval ('delta_t_wp'). Otherwhise the solution will contain 
            all the timesteps and the waypoints.
            default is 0
        delta_t_wp : float
            Time interval between two waypoints [s], unused if 'flag_waypoint' 
            is set to 0.
            default is 1.0e-3
        nmax : int
            maximal number of steps for adaptative stepsize integrators.
            default is 1e9
        toler : float 
            mixed error tolerances for some adaptative stepsize integrators.
            default is 1.0e-3
        rtoler : float
            relative error tolerances for some adaptative stepsize integrators.
            default is 1.0e-3
        atoler : float
            absolute error tolerances for some adaptative stepsize integrators.
            default is 1.0e-6
        dt_max : float
            maximal time step [s] for some adaptative stepsize integrators.
            default is 1.0e-3
        n_freeze : int 
            number of time step when the jacobian is freezed (computed once at 
            the start of the n_freeze time steps) for implicit integrators.
            default is 0
        show_failed_closure : int 
            If set to 1, an animation of the Newton-Raphson procedure is generated 
            if the closure fails.
            default is 0
        store_results : boolean
            If set to 1, a copy of the results is done.
            default is 1
        """
        
        for key, value in kwargs.items():
            if key == "t0":
                self.mbs_dirdyn_ptr.contents.options.contents.t0 = value
            elif key == "tf":
                self.mbs_dirdyn_ptr.contents.options.contents.tf = value
            elif key == "dt0":
                self.mbs_dirdyn_ptr.contents.options.contents.dt0 = value
            elif key == "save2file":
                self.mbs_dirdyn_ptr.contents.options.contents.save2file = value
            elif key == "resfilename":
                self.mbs_dirdyn_ptr.contents.options.contents.resfilename = value.encode('utf-8')
            elif key == "respath":
                self.mbs_dirdyn_ptr.contents.options.contents.respath = value.encode('utf-8')
            elif key == "animpath":
                self.mbs_dirdyn_ptr.contents.options.contents.animpath = value.encode('utf-8')
            elif key == "save_anim":
                self.mbs_dirdyn_ptr.contents.options.contents.save_anim = value
            elif key == "save_visu":
                self.mbs_dirdyn_ptr.contents.options.contents.save_visu = value
            elif key == "framerate":
                self.mbs_dirdyn_ptr.contents.options.contents.framerate = value
            elif key == "saveperiod":
                self.mbs_dirdyn_ptr.contents.options.contents.saveperiod= value
            elif key == "max_save_user":
                self.mbs_dirdyn_ptr.contents.options.contents.max_save_user = value
            elif key == "buffersize":
                self.mbs_dirdyn_ptr.contents.options.contents.buffersize = value
            elif key == "realtime":
                self.mbs_dirdyn_ptr.contents.options.contents.realtime = value
            elif key == "accelred":
                self.mbs_dirdyn_ptr.contents.options.contents.accelred = value
            elif key == "flag_compute_Qc":
                self.mbs_dirdyn_ptr.contents.options.contents.flag_compute_Qc = value
            elif key == "compute_all_uxd":
                self.mbs_dirdyn_ptr.contents.options.contents.compute_all_uxd = value
            elif key == "compute_Qc":
                if (type(value) is np.ndarray) or (type(value) is list):
                    if  np.size(value) == self.mbs.mbs_data_ptr.contents.njoint + 1:
                        for i, val in enumerate(value[1:]):
                            self.mbs_dirdyn_ptr.contents.options.contents.compute_Qc[i+1] = val
                    else:
                        print(">>DIRDYN>> The size of compute_Qc is not consistent with njoint")
                else:
                    print(">>DIRDYN>> The specified value for compute_Qc is not a list or a np.array")
            elif key == "integrator":
                if (type(value) is int) and (value>=0 and value<=8):
                    self.mbs_dirdyn_ptr.contents.options.contents.integrator = value
                elif type(value) is str:
                    if value == "RK4":
                        self.mbs_dirdyn_ptr.contents.options.contents.integrator = 0
                    elif value == "Dopri5":
                        self.mbs_dirdyn_ptr.contents.options.contents.integrator = 1
                    elif value == "Rosenbrock":
                        self.mbs_dirdyn_ptr.contents.options.contents.integrator = 2
                    elif value == "EulerEx":
                        self.mbs_dirdyn_ptr.contents.options.contents.integrator = 3
                    elif value == "Eulaire":
                        self.mbs_dirdyn_ptr.contents.options.contents.integrator = 4
                    elif value == "EulerIm":
                        self.mbs_dirdyn_ptr.contents.options.contents.integrator = 5
                    elif value == "Bader":
                        self.mbs_dirdyn_ptr.contents.options.contents.integrator = 6
                    elif value == "WMethods":
                        self.mbs_dirdyn_ptr.contents.options.contents.integrator = 7
                    elif value == "Custom":
                        self.mbs_dirdyn_ptr.contents.options.contents.integrator = 8
                    else:
                        print(">>DIRDYN>>  " + value + " is not an integrator")
                else:
                    print(">>DIRDYN>>  " + value + " is not an integrator")
            elif key == "verbose":
                self.mbs_dirdyn_ptr.contents.options.contents.verbose = value
            elif key == "flag_stop_stiff":
                self.mbs_dirdyn_ptr.contents.options.contents.flag_stop_stiff = value
            elif key == "flag_precise_dynamics":
                self.mbs_dirdyn_ptr.contents.options.contents.flag_precise_dynamics = value
            elif key == "flag_waypoint":
                self.mbs_dirdyn_ptr.contents.options.contents.flag_waypoint = value
            elif key == "flag_solout_wp":
                self.mbs_dirdyn_ptr.contents.options.contents.flag_solout_wp= value
            elif key == "delta_t_wp":
                self.mbs_dirdyn_ptr.contents.options.contents.delta_t_wp = value
            elif key == "nmax":
                self.mbs_dirdyn_ptr.contents.options.contents.nmax = value
            elif key == "toler":
                self.mbs_dirdyn_ptr.contents.options.contents.toler = value
            elif key == "rtoler":
                self.mbs_dirdyn_ptr.contents.options.contents.rtoler = value
            elif key == "atoler":
                self.mbs_dirdyn_ptr.contents.options.contents.atoler = value
            elif key == "dt_max":
                self.mbs_dirdyn_ptr.contents.options.contents.dt_max = value
            elif key == "n_freeze":
                self.mbs_dirdyn_ptr.contents.options.contents.n_freeze = value
            elif key == "show_failed_closure":
                self.mbs_dirdyn_ptr.contents.options.contents.show_failed_closure = value
            elif key == "store_results":
                self.store_results = value
            else:
                print(">>DIRDYN>> The option "+key+ " is not defined in this module")
                
        
    def get_options(self, *args):
        """
        Get the specified options for Dirdyn module
        
        Parameters
        ----------
        The different options specifed in the documentation of 'set_options()'
        
        Returns
        -------
        The value of the different options specifed in the documentation of 'set_options()'
        """
        options = []
        for key in args:
            if key == "t0":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.t0)
            elif key == "tf":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.tf)
            elif key == "dt0":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.dt0)
            elif key == "save2file":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.save2file)
            elif key == "resfilename":
                options.append(ctypes.string_at(self.mbs_dirdyn_ptr.contents.options.contents.resfilename).decode("utf-8"))
            elif key == "respath":
                options.append(ctypes.string_at(self.mbs_dirdyn_ptr.contents.options.contents.respath).decode("utf-8"))
            elif key == "animpath":
                options.append(ctypes.string_at(self.mbs_dirdyn_ptr.contents.options.contents.animpath).decode("utf-8"))
            elif key == "save_anim":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.save_anim)
            elif key == "save_visu":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.save_visu)
            elif key == "framerate":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.framerate)
            elif key == "saveperiod":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.saveperiod)
            elif key == "max_save_user":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.max_save_user)
            elif key == "buffersize":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.buffersize)
            elif key == "realtime":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.realtime)
            elif key == "accelred":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.accelred)
            elif key == "flag_compute_Qc":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.flag_compute_Qc)
            elif key == "compute_all_uxd":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.compute_all_uxd)
            elif key == "compute_Qc":
                compute_Qc_py = np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.options.contents.compute_Qc, (self.mbs.mbs_data_ptr.contents.njoint+1,))
                options.append(compute_Qc_py)
            elif key == "integrator":
                if self.mbs_dirdyn_ptr.contents.options.contents.integrator == 0:
                    options.append("RK4")
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 1:
                    options.append("Dopri5")
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 2:
                    options.append("Rosenbrock")
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 3:
                    options.append("EulerEx")    
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 4:
                    options.append("Eulaire") 
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 5:
                    options.append("EulerIm")  
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 6:
                    options.append("Bader")
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 7:
                    options.append("WMethods")   
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 8:
                    options.append("Custom")   
            elif key == "verbose":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.verbose)
            elif key == "flag_stop_stiff":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.flag_stop_stiff)
            elif key == "flag_precise_dynamics":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.flag_precise_dynamics)
            elif key == "flag_waypoint":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.flag_waypoint)
            elif key == "flag_solout_wp":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.flag_solout_wp)
            elif key == "delta_t_wp":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.delta_t_wp)
            elif key == "nmax":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.nmax)
            elif key == "toler":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.toler)
            elif key == "rtoler":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.rtoler)
            elif key == "atoler":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.atoler)
            elif key == "dt_max":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.dt_max)
            elif key == "n_freeze":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.n_freeze)
            elif key == "show_failed_closure":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.show_failed_closure)
            elif key == "store_results":
                options.append(self.store_results)
            else:
                print(">>DIRDYN>> The option "+key+ " is not defined in this module")
                
        if len(options) == 0:
            return
        if len(options) == 1:
            return options[0]
            
        return tuple(options)
    
                
    #==========================================================================
    # Defining properties
    #==========================================================================
    @property
    def tsim(self):
        return self.mbs_dirdyn_ptr.contents.tsim
    
    @property
    def dt(self):
        return self.mbs_dirdyn_ptr.contents.dt
    
    
class MbsResult(object):
    """
    Class containing results.
    
    The user-specified vector are not available.
    
    Attributes
    ----------
    q : ndarray
        Numpy array containing the current values of the generalized 
        coordinates.
    qd : ndarray
        Numpy array containing the current values of the generalized 
        velocities.
    qdd : ndarray
        Numpy array containing the current values of the generalized 
        accelerations.
    Qq : ndarray
        Numpy array containing the values of the joint forces.
    Qc : ndarray
        Numpy array containing the value of joint force introduced in driven 
        joints to respect the user function
    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.
    t : ndarray
        Numpy array containing the values of the time vector
    ux : ndarray
        Numpy array containing the values of the user variables.
    uxd : ndarray
        Numpy array containing the values of the time derivative of the user 
        variables.
    Fl : ndarray
        Numpy array containing the current values of the forces between of 
        the points of a link.
    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.
    outputs : dict
        Dict containing the names and the values of the user outputs
    mbs : MbsData
        instance of MbsData from where results come from
    """
        
    def __init__(self,mbs):
        """
            
        """
        self.q=[]
        self.qd=[]
        self.qdd=[]
        self.Fl=[]
        self.Z=[]
        self.Zd=[]
        self.Qq=[]
        self.Qa=[]
        self.t=[]
        self.ux=[]
        self.uxd=[]
        self.Qc=[]
        self.outputs={}
        self.mbs=mbs
    
    def load_results_from_file(self, filename, result_path="resultsR", module=0):
        """
        Load the results from the files into a MbsResult instance.
        
        This function is called if the buffers don not contain the full simulation.
        
        Parameters
        ----------
        filename : str
            The resfilename containing the results to load. The required suffix
            (ie. "_q.res") will be added automatically
        result_path : str, optional
            The relative path of the result folder from the project folder.
            default is "resultsR"
        module : int
            The module in which this function is called. In some modules, some 
            results files don't exist. Module ids corresponds to the 
            'MbsData.process' value. If set to 0 we try to load all files
            default is 0
        """
        
        project_path = self.mbs.project_path
        baseFileName = os.path.basename(filename)    
        baseFileName = baseFileName[:-6]
        result_path = os.path.join(project_path, result_path)
        # Error handeling
        if not os.path.isdir(result_path):
            if __DEBUG__ : print ('DEBUG>> The result directory does not exist: "'+ result_path+'"')
            
        # Generalized coordinates
        CurFile = baseFileName+'_q.res'
        path = os.path.abspath(os.path.join(result_path, CurFile))
        if (os.path.isfile(path) ):
            self.q = np.loadtxt(path)
            if np.array(self.q).ndim > 1:
                self.t = self.q[:,0]
            else:
                self.t = self.q[0]
        else:
            if __DEBUG__ : print( "DEBUG>>  file '"+CurFile+"' not found in folder '"+os.path.dirname(path))
        
        # Generalized velocities
        CurFile = baseFileName+'_qd.res'
        path = os.path.abspath(os.path.join(result_path, CurFile))
        if (os.path.isfile(path)):
            self.qd = np.loadtxt(path)
        else:
             if __DEBUG__ : print( "DEBUG>>  file '"+CurFile+"' not found in folder '"+os.path.dirname(path))