mbs_dirdyn.py

# -*- coding: utf-8 -*-
"""
Module to handle direct dynamics simulations on Multibody systems.

Summary
-------
Define the class MbsDirdyn based on the MbsDirdyn structure of MBsysC. This
class has the functions required to manipulate the direct dynamic module. This
includes setting the options, running an(or multiple) analysis and freeing the
memory.
"""
# MBsysC version: 1.13.0
# Author: Robotran Team
# (c) Universite catholique de Louvain, 2019

import os
import ctypes

import numpy as np

# importing MbsysPy functions
from ..mbs_utilities import bytes_to_str
from ..mbs_utilities import str_to_bytes
from ..mbs_utilities import mbs_warning
from ..mbs_utilities import mbs_error
from ..mbs_utilities import mbs_msg

# importing libraries
from .._mbsysc_loader.loadlibs import libmodules
from .._mbsysc_loader.loadlibs import libutilities


# =============================================================================
# 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
        -------
        MbsDirdyn: self
            A MbsDirdyn instance.
        """
        if __DEBUG__:
            mbs_msg("DEBUG>>  Creating MbsDirdyn struct for " + mbs.mbs_name + "' MBS.")

        self.mbs_dirdyn_ptr = libmodules.mbs_new_dirdyn(mbs.mbs_data_ptr)
        if __DEBUG__:
            mbs_msg("DEBUG>>  MbsDirdyn structure loaded")

        self.mbs = mbs

        if __DEBUG__:
            mbs_msg("DEBUG>>  MbsDirdyn created.")

        # Path to user function used by partitionning module
        self.user_path = self.mbs.user_path
        if user_path is not None:
            project_path = bytes_to_str(self.mbs.project_path)
            user_path = os.path.join(project_path, user_path)
            # Error handeling
            if not os.path.isdir(user_path):
                mbs_msg('The user function directory for direct dynamic module does not exist: "' + user_path + '"')
                mbs_msg('The current root folder is: "' + os.getcwd() + '"')
                mbs_msg('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 symbolic_path is not None:
            project_path = bytes_to_str(self.mbs.project_path)
            symbolic_path = os.path.join(project_path, symbolic_path)
            # Error handeling
            if not os.path.isdir(symbolic_path):
                mbs_msg('The symbolic function directory for direct dynamic module does not exist: "' + symbolic_path + '"')
                mbs_msg('The current root folder is: "' + os.getcwd() + '"')
                mbs_msg('The following directory is used instead: "' + self.symbolic_path + '".')
            else:
                self.symbolic_path = symbolic_path

        # Storing project function pointer
        self.user_fun_list = ['cons_hJ', 'cons_jdqd', 'derivative', 'DrivenJoints',
                              'ExtForces', 'JointForces', 'LinkForces', 'Link3DForces',
                              'dirdyn_init', 'dirdyn_loop', 'dirdyn_finish'
                              ]
        self.symb_fun_list = ['accelred', 'cons_hJ', 'cons_jdqd', 'invdyna',
                              'dirdyna', 'extforces', 'gensensor',
                              'link', 'link3D', 'sensor'
                              ]

        # Storing Results
        self.results = MbsResult(self.mbs)
        self.store_results = True

        # Exposing some memory
        if __DEBUG__:
            mbs_msg("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,))

    def __str__(self):
        """Return str(self)."""
        if __DEBUG__:
            mbs_msg("DEBUG>>  start of __str")

        return "MbsDirdyn instance has nothing to be printed from C library!"

    def __del__(self):
        """Delete the object by freeing the C-related memory."""
        libmodules.mbs_delete_dirdyn(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
        if __DEBUG__:
            mbs_msg("DEBUG>>  MbsDirdyn pointer deleted.")

    def run(self, **kwargs):
        """
        Run a direct dynamics analysis.

        Options can be setted with the function 'set_options()' before calling
        this function. 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
        -------
        self.results: MbsResult
            The MbsResults containing the results of the analysis.
        """
        # Assign required user functions
        if self.mbs.opt_load_c < 2:
            if __DEBUG__:
                mbs_msg("DEBUG>>  Loading user functions")

            self.mbs.__load_user_fct__(__MODULE_DIR__, self.user_fun_list, self.user_path)
            self.mbs.__assign_user_fct__(self.user_fun_list, self)

        # Assign required symbolic functions
        if self.mbs.opt_load_c < 1:
            # Loading symbolic function
            if __DEBUG__:
                mbs_msg("DEBUG>>  Loading symbolic functions")

            self.mbs.__load_symbolic_fct__(__MODULE_DIR__, self.symb_fun_list, self.symbolic_path)
            self.mbs.__assign_symb_fct__(self.symb_fun_list, self)

        self.set_options(**kwargs)

        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,
            # they 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"):
                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"):
                    mbs_msg("The beginning of the integration is not available in the buffer.\n The complete results have to be loaded from files.")
                    filename = bytes_to_str(ctypes.string_at(self.mbs_dirdyn_ptr.contents.buffers[0].contents.filename))
                    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)))
                    buffer_id = buffer_id + 1
                if self.mbs.nhu:
                    size2 = self.mbs_dirdyn_ptr.contents.buffers[buffer_id].contents.nx + 1
                    self.results.Lambda = np.copy(np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.buffers[buffer_id].contents.tx, (size1, size2)))
                    buffer_id = buffer_id + 1

                nb_user_output_vector = libutilities.get_output_vector_nb()
                for i in range(nb_user_output_vector):
                    size2 = self.mbs_dirdyn_ptr.contents.buffers[buffer_id + i].contents.nx + 1
                    user_out = np.copy(np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.buffers[buffer_id + i].contents.tx, (size1, size2)))
                    name = os.path.basename(bytes_to_str(ctypes.string_at(libutilities.get_output_vector_label(i))))
                    self.results.outputs[name] = user_out

                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 = bytes_to_str(self.mbs_dirdyn_ptr.contents.user_buffer.contents.names[i])
                        self.results.outputs[name] = user_out[0]

            libmodules.mbs_dirdyn_finish(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)

        # Unassign user functions
        if self.mbs.opt_load_c < 2:
            self.mbs.__unassign_user_fct__(self.user_fun_list)

        # Unassing required symbolic functions
        if self.mbs.opt_load_c < 1:
            self.mbs.__unassign_symb_fct__(self.symb_fun_list)

        return self.results

    def set_user_fct_from_file(self, function_name, user_path, user_file):
        """Load a user function from a file chosen by the user instead of the default one in the userfctR folder.

        The function is then unassigned by the module at the end of the run().

        Parameters
        ----------
        function_name : str
            name of the user function to replace.
        user_path : str
            path to the new user function file.
        user_file : str
            name of the new user function file.


        """
        self.mbs.__set_user_fct_from_file__(function_name, user_path, user_file)

    def set_user_fct_from_ptr(self, function_name, user_fct_ptr):
        """Load a user function chosen by the user instead of the default one in the userfctR folder.

        The function is then unassigned by the module at the end of the run().

        Parameters
        ----------
        function_name : str
            name of the user function to replace.
        user_fct_ptr : ptr
            new user function pointer.

        """
        self.mbs.__set_user_fct_from_ptr__(function_name, user_fct_ptr)

    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 values are:

            - "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
            - "AlphaM": Explicit integrator simplification of alpha-method.
                - fixed stepsize
                - multistep method
            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: int
            If set to 1, a copy of the results is done.
            default is 1
        """

        def __realtime__(mbs_dirdyn, value):
            """Cast value into integer and set it to the option 'realtime' in C-memory.

            Parameters
            ----------
            mbs_dirdyn : MbsDirdyn
                Instance of MbsDirdyn.
            value : int
                Value of the realtime option.
                See doc of set_options() for more information about realtime

            Returns
            -------
            None.

            """
            if value != 0:
                mbs_warning('Realtime option is not working with pre-compiled '
                            'libraries(as included in Pip package)!')
                if mbs_dirdyn.mbs.opt_load_c < 2:
                    mbs_error('Realtime simulation requires to provide a C-compiled '
                              'libraries of the user functions!')
            mbs_dirdyn.mbs_dirdyn_ptr.contents.options.contents.realtime = int(value)

        def __int_2_integrator__(value):
            """Set value into the C-memory as integrator option.

            Parameters
            ----------
            value : int or str
                Value corresponding to an integrator type.
                See doc of set_options() for more information about integrator

            Raises
            ------
            ValueError
                If value does not correspond to a valid integrator.
            TypeError
                If value does not have the right type.

            Returns
            -------
            int
                Integer from 0 to 8 corresponding to an integrator.

            """
            if(type(value) is int):
                if (value >= 0 and value <= 8):
                    return value
                else:
                    raise ValueError
            elif type(value) is str:
                if value == "RK4":
                    return 0
                elif value == "Dopri5":
                    return 1
                elif value == "Rosenbrock":
                    return 2
                elif value == "EulerEx":
                    return 3
                elif value == "Eulaire":
                    return 4
                elif value == "EulerIm":
                    return 5
                elif value == "Bader":
                    return 6
                elif value == "WMethods":
                    return 7
                elif value == "AlphaM":
                    return 8
                elif value == "Custom":
                    return 9
                else:
                    raise ValueError
            else:
                raise TypeError

        def __compute_Qc__(mbs_dirdyn, value):
            """Set value into the C-memory as compute_Qc option.

            Parameters
            ----------
            mbs_dirdyn : MbsDirdyn
                Instance of MbsDirdyn.
            value : list or numpy.ndarray
                Value of compute_Qc option.
                See doc of set_options() for more information about compute_Qc

            Raises
            ------
            ValueError
                If value does not have the right size.
            TypeError
                If value is not a list or a numpy.ndarray.

            Returns
            -------
            None.

            """
            if(type(value) is np.ndarray) or (type(value) is list):
                if np.size(value) == mbs_dirdyn.mbs.mbs_data_ptr.contents.njoint + 1:
                    for i, val in enumerate(value[1:]):
                        mbs_dirdyn.mbs_dirdyn_ptr.contents.options.contents.compute_Qc[i + 1] = val
                else:
                    raise ValueError
            else:
                raise TypeError

        options = {'t0': {'convert': float, 'c_name': 't0'},
                   'tf': {'convert': float, 'c_name': 'tf'},
                   'dt0': {'convert': float, 'c_name': 'dt0'},
                   'save2file': {'convert': int, 'c_name': 'save2file'},
                   'resfilename': {'convert': str_to_bytes, 'c_name': 'resfilename'},
                   'respath': {'convert': str_to_bytes, 'c_name': 'respath'},
                   'animpath': {'convert': str_to_bytes, 'c_name': 'animpath'},
                   'save_anim': {'convert': int, 'c_name': 'save_anim'},
                   'save_visu': {'convert': int, 'c_name': 'save_visu'},
                   'framerate': {'convert': int, 'c_name': 'framerate'},
                   'saveperiod': {'convert': int, 'c_name': 'saveperiod'},
                   'max_save_user': {'convert': int, 'c_name': 'max_save_user'},
                   'buffersize': {'convert': int, 'c_name': 'buffersize'},
                   'realtime': {'convert': int, 'c_name': 'realtime'},
                   'accelred': {'convert': int, 'c_name': 'accelred'},
                   'flag_compute_Qc': {'convert': int, 'c_name': 'flag_compute_Qc'},
                   'compute_all_uxd': {'convert': int, 'c_name': 'compute_all_uxd'},
                   'compute_Qc': {'convert': 'list or numpy.ndarray', 'c_name': 'compute_Qc'},
                   'integrator': {'convert': __int_2_integrator__, 'c_name': 'integrator'},
                   'verbose': {'convert': int, 'c_name': 'verbose'},
                   'flag_stop_stiff': {'convert': int, 'c_name': 'flag_stop_stiff'},
                   'flag_precise_dynamics': {'convert': int, 'c_name': 'flag_precise_dynamics'},
                   'flag_waypoint': {'convert': int, 'c_name': 'flag_waypoint'},
                   'flag_solout_wp': {'convert': int, 'c_name': 'flag_solout_wp'},
                   'delta_t_wp': {'convert': float, 'c_name': 'delta_t_wp'},
                   'nmax': {'convert': int, 'c_name': 'nmax'},
                   'toler': {'convert': float, 'c_name': 'toler'},
                   'rtoler': {'convert': float, 'c_name': 'rtoler'},
                   'atoler': {'convert': float, 'c_name': 'atoler'},
                   'dt_max': {'convert': float, 'c_name': 'dt_max'},
                   'n_freeze': {'convert': int, 'c_name': 'n_freeze'},
                   'show_failed_closure': {'convert': int, 'c_name': 'show_failed_closure'},
                   'store_results': {'convert': int, 'c_name': 'store_results'},
                   }

        for key, value in kwargs.items():
            if key not in options:
                raise TypeError("{:} is an invalid option name.".format(key))
            try:
                c_name = options[key]['c_name']
                if key == 'realtime':
                    __realtime__(self, value)
                elif key == 'compute_Qc':
                    __compute_Qc__(self, value)
                elif key == 'store_results':
                    self.store_results = int(value)
                else:
                    setattr(self.mbs_dirdyn_ptr.contents.options.contents, c_name, options[key]['convert'](value))
            except ValueError as err:
                # if wrong value in integrator or compute_QC
                if key == 'integrator':
                    raise ValueError('>>DIRDYN>>  {:} is not a valid integrator'.format(value)).with_traceback(err.__traceback__) from None
                elif key == 'compute_Qc':
                    raise ValueError('>>DIRDYN>>  The size of compute_Qc is not consistent with njoint').with_traceback(err.__traceback__) from None
                # if error during the cast
                else:
                    raise TypeError("{:} is {:}, can not be casted from {:}.".format(key, options[key]['convert'], type(value))).with_traceback(err.__traceback__) from None

            except AttributeError as err:
                # if error during cast of strings (i.e in str_to_bytes)
                raise TypeError("{:} is str, can not be casted from {:}".format(key, type(value))).with_traceback(err.__traceback__) from None

            except TypeError as err:
                # if wrong type in integrator or compute_QC
                if key == 'integrator':
                    raise TypeError("{:} is int or str, can not be casted from {:}.".format(key, type(value))).with_traceback(err.__traceback__) from None
                elif key == 'compute_Qc':
                    raise TypeError("{:} is list or numpy.ndarray, can not be casted from {:}.".format(key, type(value))).with_traceback(err.__traceback__) from None

    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(bytes_to_str(ctypes.string_at(self.mbs_dirdyn_ptr.contents.options.contents.resfilename)))
            elif key == "respath":
                options.append(bytes_to_str(ctypes.string_at(self.mbs_dirdyn_ptr.contents.options.contents.respath)))
            elif key == "animpath":
                options.append(bytes_to_str(ctypes.string_at(self.mbs_dirdyn_ptr.contents.options.contents.animpath)))
            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("AlphaM")
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 9:
                    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:
                mbs_msg(">>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):
        """Access to `tsim` attribute (read-only)."""
        return self.mbs_dirdyn_ptr.contents.tsim

    @property
    def dt(self):
        """Access to `dt` attribute (read-only)."""
        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):
        """Initialize the fields to empty list or dict."""
        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__:
                mbs_msg('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__:
                mbs_msg("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__:
                mbs_msg("DEBUG>>  file '" + CurFile + "' not found in folder '" + os.path.dirname(path))

        # Generalized accelerations
        CurFile = baseFileName + '_qdd.res'
        path = os.path.abspath(os.path.join(result_path, CurFile))
        if(os.path.isfile(path)):
            self.qdd = np.loadtxt(path)