mbs_modal.py

# -*- coding: utf-8 -*-
"""
Module to handle modal analysis on Multibody systems.

Summary
-------
Define the class MbsModal based on the MbsModal structure of MBsysC. This
class has the functions required to manipulate the direct modal analysis module.
This includes setting the options, running an(or multiple) analysis and freeing
the memory.
"""

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_msg

# importing libraries
from .._mbsysc_loader.loadlibs import libmodules


# =============================================================================
# Global parameter of the current module
# =============================================================================
__DEBUG__ = False
__MODULE_DIR__ = os.path.dirname(os.path.abspath(__file__))


# =============================================================================
# Defining Python MbsModal class
# =============================================================================

class MbsModal(object):
    """
    Class of the modal analysis module.

    Attributes
    ----------
    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.
    user_path: str
        Path to the folder containing the user functions(python modules) to be loaded.
    """

    def __init__(self, mbs, user_path=None, symbolic_path=None):
        """
        Create an instance of the MbsModal 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
        -------
        MbsModal: self
            A MbsModal instance.
        """
        self.module_name = type(self).__name__

        if __DEBUG__:
            mbs_msg("DEBUG>>  Creating MbsModal struct for " + mbs.mbs_name + "' MBS.")

        self.mbs_modal_ptr = libmodules.mbs_new_modal(mbs.mbs_data_ptr)
        if __DEBUG__:
            mbs_msg("DEBUG>>  MbsModal structure loaded")

        self.mbs = mbs

        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('Thesymbolic 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

        self.user_fun_list = ['cons_hJ', 'cons_jdqd', 'derivative', 'DrivenJoints',
                              'ExtForces', 'JointForces', 'LinkForces', 'Link3DForces'
                              ]
        self.symb_fun_list = ['accelred', 'cons_hJ', 'cons_jdqd', 'invdyna',
                              'dirdyna', 'extforces', 'gensensor',
                              'link', 'link3D', 'sensor'
                              ]

        # Storing Results
        self.results = MbsResultModal()
        self.store_results = False

        # Exposing some memory
        if __DEBUG__:
            mbs_msg("DEBUG>>  Exposing " + self.module_name + " fields")

        # Constraints
        self._h = self._Jac = self._jdqd = None
        if self.mbs.Ncons > 0:
            self._h = np.ctypeslib.as_array(self.mbs_modal_ptr.contents.aux.contents.h, (self.mbs.Ncons + 1, ))
            self._Jac = np.ctypeslib.as_array(self.mbs_modal_ptr.contents.aux.contents.Jac[0], (self.mbs.Ncons + 1, self.mbs.njoint + 1))
            self._jdqd = np.ctypeslib.as_array(self.mbs_modal_ptr.contents.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_modal_ptr.contents.aux.contents.huserc, (self.mbs.Nuserc + 1, ))
            self._Juserc = np.ctypeslib.as_array(self.mbs_modal_ptr.contents.aux.contents.Juserc[0], (self.mbs.Nuserc + 1, self.mbs.njoint + 1))
            self._jdqduserc = np.ctypeslib.as_array(self.mbs_modal_ptr.contents.aux.contents.jdqduserc, (self.mbs.Nuserc + 1, ))
        # Fields from equation of motion
        self._M = np.ctypeslib.as_array(self.mbs_modal_ptr.contents.aux.contents.M[0], (self.mbs.njoint + 1, self.mbs.njoint + 1))
        self._c = np.ctypeslib.as_array(self.mbs_modal_ptr.contents.aux.contents.c, (self.mbs.njoint + 1, ))
        self._F = np.ctypeslib.as_array(self.mbs_modal_ptr.contents.aux.contents.F, (self.mbs.njoint + 1, ))
        self._phi = np.ctypeslib.as_array(self.mbs_modal_ptr.contents.aux.contents.phi, (self.mbs.njoint + 1, ))

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

        msg = 'MbsModal instance of the project "' + str(self.mbs_name) +\
              '", loaded from the file "' + str(self.mbs_filename) + '".'

        return msg

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

    def run(self, **kwargs):
        """
        Run a modal 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: MbsResultModal
            The instance 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)

        error = libmodules.mbs_run_modal(self.mbs_modal_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)

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

        if error < 0:
            mbs_msg("\n--------------------------------------------------------------------------------\n"  # There is 80 characters printed
                    "READ CAREFULLY !!!\n"
                    "--------------------------------------------------------------------------------\n\n"
                    "An error occurs during the modal analysis.\n"
                    "The messages above give deeper informations on what went wrong.\n"
                    "The messages below gives classic error and error backtrace.\n\n"

                    "Usual errors are:\n"
                    "    - Some options are not correct;\n"
                    "    - Some mass or Inertia component is missing, leading to a invalid mass\n"
                    "      matrix during the computation;\n"
                    "    - The system is in a singular configuration;\n"
                    "    - An error in user function (joint force, external forces) return wrong\n"
                    "       value. Excessive force can generate infinte acceleration.\n"
                    "\n"
                    "If the simulation runs a little you should:\n"
                    "    - In all cases, open the results/animation file and check the motion of the \n"
                    "      system until it fails.\n"
                    "    - In case of loop closure problem, you can alos check the dedicated animation\n"
                    "      under the name 'Failed_loop_closing_procedure_q.res' if the related option\n"
                    "      has been enable.\n"
                    "\n--------------------------------------------------------------------------------\n"
                    "\n--------------------------------------------------------------------------------\n")
            raise RuntimeError("MbsModal.run() failed, read previous messages.")

        if self.store_results:
            self.load_results()

        return self.results

    def load_results(self):
        """Load the results of an modal analysis."""
        self.results.eval_a = np.ctypeslib.as_array(self.mbs_modal_ptr.contents.eval_a, (self.nx, ))
        self.results.eval_b = np.ctypeslib.as_array(self.mbs_modal_ptr.contents.eval_b, (self.nx, ))

        self.results.evec_a = np.ctypeslib.as_array(self.mbs_modal_ptr.contents.evec_a[0], (self.nx, self.nx))
        self.results.evec_b = np.ctypeslib.as_array(self.mbs_modal_ptr.contents.evec_b[0], (self.nx, self.nx))

        self.results.evec_r = np.ctypeslib.as_array(self.mbs_modal_ptr.contents.evec_r[0], (self.nx, self.nx))
        self.results.evec_phi = np.ctypeslib.as_array(self.mbs_modal_ptr.contents.evec_phi[0], (self.nx, self.nx))

    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 Modal module.

        Parameters
        ----------
        save2file: int
            Determine whether results are written to files on disk(in MbsModalOptions::respath folder):
            1: results are saved
            0: results are not saved
            default = 1
        resfilename: str
            The keyword used for determining the name of result files
        respath: str
            Path in which result file are saved.
            Default: the resultsR folder of the project
        animpath: str
            Path in which anim file is saved.
            Default: the animationR folder of the project
        save_anim: int
            1 to save the anim file
            0 otherwise(only valid if 'save2file' is set to 1)
            default = 1
        save_mat: int
            no = 0, yes = 1
            flag to save the linearized matrix A, Mr, Gr, Kr
            default = 0
        save_eval: int
            no = 0, yes = 1
            flag to save the eigen values in two separate files: "_a" for Real part and "_b" for Imag part
            default = 0
        save_evec: int
            no = 0, yes = 1
            flag to save the eigen vector in two separate files:
            "_a" for Real part, "_b" for Imag part, "_r" for magnitude and "_phi" for phase
            default = 0
        save_result: int
            no = 0, yes = 1
            flag to save the modal results summary
            default = 1
        save_anim: int
            no = 0, yes = 1
            flag to save and generate the animation for the modes
            default = 0
        mode_ampl: float
            amplification factor for the animation of the modes
            application dependent
            default = 0.2
        compute_uxd: int
            no = 0, yes = 1
            flag to compute the modes associated with the extra constitutive differential equations
            default = 1
        compute_JS: int
            no = 0, yes = 1
            flag to compute the non diagonal terms
            default = 0
            TO IMPLEMENT !
        norm_evec: int
            no = 0, yes = 1
            flag to normalize the eigenvectors
            default = 1
        lpk_itermax: int
            options for the lpk gradient
            default = 10
        lpk_relincr: float
            options for the lpk gradient
            default = 1e-2
        lpk_absincr: float
            options for the lpk gradient
            default = 1e-3
        lpk_equitol: float
            options for the lpk gradient
            default = 1e-6
        lpk_lintol: float
            options for the lpk gradient
            default = 1e-3
        anim_speed: int
            flag to decide whether the independent speeds have to be integrated while generating the animation files
            default = 0
        anim_dt: int
            delta t for the animation generation
            default = 1e-2
        anim_t: int
            time for the animation generation
            default = 2
        verbose: int
            Gives informations during the process.
            default: 1, to disable set it to 0.
        """
        options = {'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_mat': {'convert': int, 'c_name': 'save_mat'},
                   'save_eval': {'convert': int, 'c_name': 'save_eval'},
                   'save_evec': {'convert': int, 'c_name': 'save_evec'},
                   'save_result': {'convert': int, 'c_name': 'save_result'},
                   'mode_ampl': {'convert': float, 'c_name': 'mode_ampl'},
                   'compute_uxd': {'convert': int, 'c_name': 'compute_uxd'},
                   'compute_JS': {'convert': int, 'c_name': 'compute_JS'},
                   'norm_evec': {'convert': int, 'c_name': 'norm_evec'},
                   'lpk_itermax': {'convert': int, 'c_name': 'lpk_itermax'},
                   'lpk_relincr': {'convert': float, 'c_name': 'lpk_relincr'},
                   'lpk_absincr': {'convert': float, 'c_name': 'lpk_absincr'},
                   'lpk_equitol': {'convert': float, 'c_name': 'lpk_equitol'},
                   'lpk_lintol': {'convert': float, 'c_name': 'lpk_lintol'},
                   'anim_speed': {'convert': int, 'c_name': 'anim_speed'},
                   'anim_dt': {'convert': int, 'c_name': 'anim_dt'},
                   'anim_t': {'convert': int, 'c_name': 'anim_t'},
                   'verbose': {'convert': int, 'c_name': 'verbose'},
                   }
        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']
                setattr(self.mbs_modal_ptr.contents.options.contents, c_name, options[key]['convert'](value))
            except ValueError as err:
                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

    def get_options(self, *args):
        """
        Get the specified options for Modal 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 == "save2file":
                options.append(self.mbs_modal_ptr.contents.options.contents.save2file)
            elif key == "resfilename":
                options.append(bytes_to_str(ctypes.string_at(self.mbs_modal_ptr.contents.options.contents.resfilename)))
            elif key == "respath":
                options.append(bytes_to_str(ctypes.string_at(self.mbs_modal_ptr.contents.options.contents.respath)))
            elif key == "animpath":
                options.append(bytes_to_str(ctypes.string_at(self.mbs_modal_ptr.contents.options.contents.animpath)))
            elif key == "save_anim":
                options.append(self.mbs_modal_ptr.contents.options.contents.save_anim)
            elif key == "save_mat":
                options.append(self.mbs_modal_ptr.contents.options.contents.save_mat)
            elif key == "save_eval":
                options.append(self.mbs_modal_ptr.contents.options.contents.save_eval)
            elif key == "save_evec":
                options.append(self.mbs_modal_ptr.contents.options.contents.save_evec)
            elif key == "save_result":
                options.append(self.mbs_modal_ptr.contents.options.contents.save_result)
            elif key == "mode_ampl":
                options.append(self.mbs_modal_ptr.contents.options.contents.mode_ampl)
            elif key == "compute_uxd":
                options.append(self.mbs_modal_ptr.contents.options.contents.compute_uxd)
            elif key == "compute_JS":
                options.append(self.mbs_modal_ptr.contents.options.contents.compute_JS)
            elif key == "norm_evec":
                options.append(self.mbs_modal_ptr.contents.options.contents.norm_evec)
            elif key == "lpk_itermax":
                options.append(self.mbs_modal_ptr.contents.options.contents.lpk_itermax)
            elif key == "lpk_relincr":
                options.append(self.mbs_modal_ptr.contents.options.contents.lpk_relincr)
            elif key == "lpk_absincr":
                options.append(self.mbs_modal_ptr.contents.options.contents.lpk_absincr)
            elif key == "lpk_equitol":
                options.append(self.mbs_modal_ptr.contents.options.contents.lpk_equitol)
            elif key == "lpk_lintol":
                options.append(self.mbs_modal_ptr.contents.options.contents.lpk_lintol)
            elif key == "anim_speed":
                options.append(self.mbs_modal_ptr.contents.options.contents.anim_speed)
            elif key == "anim_dt":
                options.append(self.mbs_modal_ptr.contents.options.contents.anim_dt)
            elif key == "anim_t":
                options.append(self.mbs_modal_ptr.contents.options.contents.anim_t)
            elif key == "verbose":
                options.append(self.mbs_modal_ptr.contents.options.contents.verbose)
            else:
                mbs_msg(">>" + self.module_name + ">> 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
    # =========================================================================
    # No property yo be defined.


class MbsResultModal(object):
    """Class dedicated to the results of a modal analysis.

    Attributes
    ----------
    eval_a: numpy.ndarray
        Result of the Eigenvalue problem: real part of the nx eigen values.
    eval_b: numpy.ndarray
        Result of the Eigenvalue problem: imaginary part of the nx eigen values.
    evec_a: numpy.ndarray
        Result of the Eigenvalue problem: real part of the eigen vectors (each
        column is a eigenvector).
    evec_b: numpy.ndarray
        Result of the Eigenvalue problem: imaginary part of the eigen vectors (each
        column is a eigenvector).
    evec_r: numpy.ndarray
        Result of the Eigenvalue problem: norm of the eigen vectors (each column
        is a eigenvector).
    evec_phi: numpy.ndarray
        Result of the Eigenvalue problem: phase of the eigen vectors (each column
        is a eigenvector).
    """

    def __init__(self):
        self.eval_a = None
        self.eval_b = None
        self.evec_a = None
        self.evec_b = None
        self.evec_r = None
        self.evec_phi = None