mbs_equil.py 40.7 KB
Newer Older
1
# - * - coding: utf - 8 - * -
2
"""
3
Module to handle equilibrium analysis on Multibody systems.
4

5
6
7
8
9
Summary
 -------
Define the class MbsEquil based on the MbsEquil structure of MBsysC.This class
has the functions required to manipulate the equilibrium module. This includes
setting the options, running an(or multiple) analysis and freeing the memory.
10
"""
11
12
# Author: Robotran Team
# (c) Universite catholique de Louvain, 2019
13
14
15


import os
Louis Beauloye's avatar
Louis Beauloye committed
16
import ctypes
17

Louis Beauloye's avatar
Louis Beauloye committed
18
import numpy as np
19

20
# importing MbsysPy functions
21
from ..mbs_utilities import str_from_c_pointer
22
23
from ..mbs_utilities import bytes_to_str
from ..mbs_utilities import str_to_bytes
24
from ..mbs_utilities import mbs_msg
Louis Beauloye's avatar
Louis Beauloye committed
25
26

# importing libraries
27
from .._mbsysc_loader.loadlibs import libmodules
28

Louis Beauloye's avatar
Louis Beauloye committed
29

30
# =============================================================================
31
# Global parameter of the current module
32
# =============================================================================
33
__DEBUG__ = False
34
35
__MODULE_DIR__ = os.path.dirname(os.path.abspath(__file__))

36
# =============================================================================
37
# Defining Python MbsEquil class
38
39
# =============================================================================

40
41

class MbsEquil(object):
Olivier Lantsoght's avatar
Olivier Lantsoght committed
42
    """
43
44
    Class of the Equilibrium(static or quasi - static) module.

Olivier Lantsoght's avatar
Olivier Lantsoght committed
45
46
    Attributes
    ----------
47
48
    results: MbsResultEquil
        Instance of MbsResultEquil(dedicated to equilibrium module) containing
Olivier Lantsoght's avatar
Olivier Lantsoght committed
49
        the results of the numerical analysis.
50

Olivier Lantsoght's avatar
Olivier Lantsoght committed
51
52
53
54
    Examples
    --------
    >>> mbs_data = MBsysPy.MbsData("../dataR/ExampleProject.mbs")
    >>> mbs_equil = MBsysPy.MbsEquil(mbs_data)
55
    >>> mbs_equil.set_options(equitol = 1e - 4)
Olivier Lantsoght's avatar
Olivier Lantsoght committed
56
57
58
59
    >>> mbs_equil.get_options(equitol)
        0.0001
    >>> results = mbs_equil.run()
    """
60
61
62

    def __init__(self, mbs, user_path=None, symbolic_path=None):
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
63
            mbs_msg("DEBUG>>  Creating MbsEquil struct for " + mbs.mbs_name + "' MBS.")
64

65
        self.mbs_equil_ptr = libmodules.mbs_new_equil(mbs.mbs_data_ptr)
66
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
67
            mbs_msg("DEBUG>>  MbsEquil structure loaded")
68

69
        self.mbs = mbs
70
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
71
            mbs_msg("DEBUG>>  MbsEquil created.")
72

Louis Beauloye's avatar
Louis Beauloye committed
73
74
        # Path to user function used by partitionning module
        self.user_path = self.mbs.user_path
75
76
        if user_path is not None:
            project_path = self.mbs.project_path
Louis Beauloye's avatar
Louis Beauloye committed
77
78
79
            user_path = os.path.join(project_path, user_path)
            # Error handeling
            if not os.path.isdir(user_path):
Louis Beauloye's avatar
Louis Beauloye committed
80
81
82
                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 + '".')
Louis Beauloye's avatar
Louis Beauloye committed
83
84
85
86
            else:
                self.user_path = user_path
        # Path to user function used by partitionning modue
        self.symbolic_path = self.mbs.symbolic_path
87
88
        if symbolic_path is not None:
            project_path = self.mbs.project_path
Louis Beauloye's avatar
Louis Beauloye committed
89
90
91
            symbolic_path = os.path.join(project_path, symbolic_path)
            # Error handeling
            if not os.path.isdir(symbolic_path):
92
                mbs_msg('The symbolic function directory for direct dynamic module does not exist: "' + symbolic_path + '"')
Louis Beauloye's avatar
Louis Beauloye committed
93
94
                mbs_msg('The current root folder is: "' + os.getcwd() + '"')
                mbs_msg('The following directory is used instead: "' + self.symbolic_path + '".')
Louis Beauloye's avatar
Louis Beauloye committed
95
96
            else:
                self.symbolic_path = symbolic_path
97

Louis Beauloye's avatar
Louis Beauloye committed
98
99
100
101
102
103
104
105
        self.user_fun_list = ['cons_hJ', 'cons_jdqd', 'derivative', 'DrivenJoints',
                              'ExtForces', 'JointForces', 'LinkForces', 'Link3DForces',
                              'equil_init', 'equil_loop', 'equil_finish', 'equil_fxe'
                              ]
        self.symb_fun_list = ['accelred', 'cons_hJ', 'cons_jdqd', 'invdyna',
                              'dirdyna', 'extforces', 'gensensor',
                              'link', 'link3D', 'sensor'
                              ]
106

107
108
        # exchange of equilibrium variables
        self.exchange_list = []
109

110
111
        # addition of equilibrium variables
        self.addition_list = []
112

Louis Beauloye's avatar
Louis Beauloye committed
113
        # storing result
114
        self.store_results = True
Louis Beauloye's avatar
Louis Beauloye committed
115
        self.results = MbsResultEquil()
116

Louis Beauloye's avatar
Louis Beauloye committed
117
        # Exposing some memory
118
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
119
            mbs_msg("DEBUG>>  Exposing MbsEquil fields")
120

Louis Beauloye's avatar
Louis Beauloye committed
121
122
        # Constraints
        self._h = self._Jac = self._jdqd = None
123
124
125
126
        if self.mbs.Ncons > 0:
            self._h = np.ctypeslib.as_array(self.mbs_equil_ptr.contents.aux.contents.h, (self.mbs.Ncons + 1, ))
            self._Jac = np.ctypeslib.as_array(self.mbs_equil_ptr.contents.aux.contents.Jac[0], (self.mbs.Ncons + 1, self.mbs.njoint + 1))
            self._jdqd = np.ctypeslib.as_array(self.mbs_equil_ptr.contents.aux.contents.jdqd, (self.mbs.Ncons + 1, ))
Louis Beauloye's avatar
Louis Beauloye committed
127
        self._huserc = self._Juserc = self._jdqduserc = None
128
129
130
131
        if self.mbs.Nuserc > 0:
            self._huserc = np.ctypeslib.as_array(self.mbs_equil_ptr.contents.aux.contents.huserc, (self.mbs.Nuserc + 1, ))
            self._Juserc = np.ctypeslib.as_array(self.mbs_equil_ptr.contents.aux.contents.Juserc[0], (self.mbs.Nuserc + 1, self.mbs.njoint + 1))
            self._jdqduserc = np.ctypeslib.as_array(self.mbs_equil_ptr.contents.aux.contents.jdqduserc, (self.mbs.Nuserc + 1, ))
Louis Beauloye's avatar
Louis Beauloye committed
132
        # Fields from equation of motion
133
134
135
136
137
        self._M = np.ctypeslib.as_array(self.mbs_equil_ptr.contents.aux.contents.M[0], (self.mbs.njoint + 1, self.mbs.njoint + 1))
        self._c = np.ctypeslib.as_array(self.mbs_equil_ptr.contents.aux.contents.c, (self.mbs.njoint + 1, ))
        self._F = np.ctypeslib.as_array(self.mbs_equil_ptr.contents.aux.contents.F, (self.mbs.njoint + 1, ))
        self._phi = np.ctypeslib.as_array(self.mbs_equil_ptr.contents.aux.contents.phi, (self.mbs.njoint + 1, ))

Louis Beauloye's avatar
Louis Beauloye committed
138
    def __str__(self):
139
140
        """Return str(self)."""
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
141
            mbs_msg("DEBUG>>  start of __str")
142
143
144
145
146

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

Louis Beauloye's avatar
Louis Beauloye committed
147
    def __del__(self):
148
        """Delete the object by freeing the C - related memory."""
Louis Beauloye's avatar
Louis Beauloye committed
149
        libmodules.mbs_delete_equil(self.mbs_equil_ptr, self.mbs.mbs_data_ptr)
150
        self.mbs_equil_ptr = None
151
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
152
            mbs_msg("DEBUG>>  MbsEquil pointer deleted")
153

154
    def run(self, **kwargs):
Louis Beauloye's avatar
Louis Beauloye committed
155
        """
156
157
158
159
160
161
162
163
164
        Run a equilibrium analysis.

        Options can be setted with the function 'set_options()' before calling
        this function. Options can be retrieved with the function 'get_options()'.

        Returns
        -------
        self.results: MbsResultEquil
            The MbsResults containing the results of the analysis.
Louis Beauloye's avatar
Louis Beauloye committed
165
        """
Louis Beauloye's avatar
Louis Beauloye committed
166
        # Assign required user functions
167
        if self.mbs.opt_load_c < 2:
168
            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
169
                mbs_msg("DEBUG>>  Loading user functions")
170
171

            self.mbs.__load_user_fct__(__MODULE_DIR__, self.user_fun_list, self.user_path)
Louis Beauloye's avatar
Louis Beauloye committed
172
            self.mbs.__assign_user_fct__(self.user_fun_list, self)
173

174
175
            self._compute_exchange()
            self._compute_extra_variable()
176

Louis Beauloye's avatar
Louis Beauloye committed
177
        # Assign required symbolic functions
178
        if self.mbs.opt_load_c < 1:
179
180
            # Loading symbolic function
            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
181
                mbs_msg("DEBUG>>  Loading symbolic functions")
182
183

            self.mbs.__load_symbolic_fct__(__MODULE_DIR__, self.symb_fun_list, self.symbolic_path)
Louis Beauloye's avatar
Louis Beauloye committed
184
            self.mbs.__assign_symb_fct__(self.symb_fun_list, self)
185

186
        self.set_options(**kwargs)
187

188
        error = libmodules.mbs_run_equil(self.mbs_equil_ptr, self.mbs.mbs_data_ptr)
189

190
        if not error and self.store_results:
Louis Beauloye's avatar
Louis Beauloye committed
191
            self.load_results()
192

Louis Beauloye's avatar
Louis Beauloye committed
193
        # Unassign user functions
194
        if self.mbs.opt_load_c < 2:
Louis Beauloye's avatar
Louis Beauloye committed
195
            self.mbs.__unassign_user_fct__(self.user_fun_list)
196

Louis Beauloye's avatar
Louis Beauloye committed
197
        # Unassing required symbolic functions
198
        if self.mbs.opt_load_c < 1:
Louis Beauloye's avatar
Louis Beauloye committed
199
            self.mbs.__unassign_symb_fct__(self.symb_fun_list)
200

201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
        if error < 0:
            mbs_msg("\n--------------------------------------------------\n"
                    "READ CAREFULLY !!!\n"
                    "--------------------------------------------------\n\n"
                    "An error occurs during equilibrium module.\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"
                    "    - There is a discontinuity in your system (ie: a tire-ground force with 0 value\n"
                    "      when not touching the ground, ...);\n"
                    "    - None of the variables allows to get closer to the equilibrium;\n"
                    "    - An error in user function (joint force, external forces) return wrong value.\n"
                    "\n"
                    "If the simulation runs a little you should:\n"
216
217
                    "    - In some cases if it was created open the results/animation file and check \n"
                    "      the different equilibrium steps of the module until it fails.\n"
218
                    "    - In case of loop closure problem, you can alos check the dedicated animation\n"
219
                    "      under the name 'Failed_loop_closing_procedure_q.res' if the option was enable.\n"
220
221
222
223
                    "\n--------------------------------------------------\n"
                    "\n--------------------------------------------------\n")
            raise RuntimeError("MbsEquil.run() failed, read previous messages.")

Louis Beauloye's avatar
Louis Beauloye committed
224
        return self.results
225

Louis Beauloye's avatar
Louis Beauloye committed
226
    def load_results(self):
227
        """Load the results of an equilibrium analysis."""
Louis Beauloye's avatar
Louis Beauloye committed
228
229
230
231
232
233
234
235
236
        mode = self.get_options("mode")
        if mode == 1:
            self.results.type = "static"
        elif mode == 2:
            self.results.type = "quasistatic"
        elif mode == 3:
            self.results.type = "dynamic"
        else:
            self.results.type = "not an existing mode"
237

Louis Beauloye's avatar
Louis Beauloye committed
238
239
        self.results.nquch = self.get_options("nquch")
        self.results.nxe = self.get_options("nxe")
240
241
        self.results.nx = self.nx

242
        self.results.x = np.copy(np.ctypeslib.as_array(self.mbs_equil_ptr.contents.x, (self.nx, )))
243
        # Create variable output name
244
        self.results.x_name = []
245
        # independant variables
Louis Beauloye's avatar
Louis Beauloye committed
246
        for u in self.mbs.qu[1:(self.mbs.nqu + 1)]:
247
248
249
            self.results.x_name.append("q" + str(u))
        # User derivative variables
        if self.get_options("compute_uxd"):
250
            for ux in range(1, self.mbs.Nux + 1):
251
252
253
254
                self.results.x_name.append("ux" + str(ux))
        # User defined variable
        for additionnal in self.addition_list:
            self.results.x_name.append(additionnal[1])
255

256
257
        # Exchanged variables
        for exchanged in self.exchange_list:
258
            index = self.results.x_name.index('q' + str(exchanged[1]))
259
260
            self.results.x_name[index] = exchanged[2]

Louis Beauloye's avatar
Louis Beauloye committed
261
262
263
264
        if self.success == 1:
            self.results.success = "PASSED"
        elif self.success == 0:
            self.results.success = "NONE"
265
        elif self.success == - 1:
Louis Beauloye's avatar
Louis Beauloye committed
266
            self.results.success = "FAILED"
267

268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
    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)

303
304
    def add_exchange(self, ptr, index, exchange_var_id, ptr_name=None):
        """Add a new equilibrium variable that will replace the specified one.
305
306
307
308
309
310
311
312
313
314
315

        Exchanged variables are stored and will be set in the memory of the
        external library when 'MbsEquil::run()' is called. Between two run()
        the equilibrium can be modified thanks to replace().
        However as the number of equilibrium variables is locked in the
        external memory after the first call of run(), this function can not
        be called between two run().

        Parameters
        ----------
        ptr: ndarray
316
            Array containing the new equilibrium variable to be used.
317
        index: int
318
319
320
321
322
323
324
            Index in 'ptr' of the new equilibrium variable to be used.
        exchange_var_id: int
            Index of the default equilibrium variable that will be replaced by
            'ptr[index]'.
        ptr_name: str, optionnal
           Name of the variable to be exchanged.
           If not set a generic name will be assigned.
325
326
        """
        if self.get_options("nquch"):
327
328
329
            mbs_msg("As the number of equilibrium variables is locked in the external "
                    "memory after the first call of run(), this function can not "
                    "be called between two run()")
330
        if index > np.size(ptr) - 1:
Louis Beauloye's avatar
Louis Beauloye committed
331
            mbs_msg(">>EQUIL>> variable exchange: Index out of bound")
332
            return
333
        address = ptr.ctypes.data + ctypes.sizeof(ctypes.c_double) * index
334
        if ptr.ctypes.data == self.mbs.q.ctypes.data:
335
336
337
338
339
            mbs_msg(">>EQUIL>> variable exchange: Warning ! Generalized coordinates "
                    "have to be used with caution as exchange variables")

        if not ptr_name:
            ptr_name = "exchange_{:d}".format(len(self.exchange_list) + 1)
340

341
        if len(self.exchange_list) == 0:
342
            if not ptr_name:
343
                self.exchange_list = [[address, exchange_var_id, ptr_name]]
344
345
346
        else:
            for element in self.exchange_list:
                if element[0] == address:
Louis Beauloye's avatar
Louis Beauloye committed
347
                    mbs_msg(">>EQUIL>> variable exchange: Exchange variable is already an equilibrium variable")
348
                else:
349
                    self.exchange_list.append([address, exchange_var_id, ptr_name])
350
                    return
351

Louis Beauloye's avatar
Louis Beauloye committed
352
    def add_extra_variable(self, ptr, index, ptr_name=None):
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
        """
        Add a new equilibrium variable.

        Extra variables are stored and will be set in the memory of the
        external library when 'MbsEquil::run()' is called. Between two run()
        the equilibrium can be modified thanks to replace().
        However as the number of equilibrium variables is locked in the
        external memory after the first call of run(), this function can not
        be called between two run().

        Parameters
        ----------
        ptr: ndarray
            Array containing the extra variable to add to the equilibrium variables
        index: int
            Index in ptr of the extra variable to add to the equilibrium variables
        ptr_name: str
370
            Name of the extra variable, if not set a default name will be given.
371
372
        """
        if self.get_options("nxe"):
Louis Beauloye's avatar
Louis Beauloye committed
373
            mbs_msg("As the number of equilibrium variables is locked in the external memory"
Louis Beauloye's avatar
Louis Beauloye committed
374
                    " after the first call of run(), this function can not be called between two run()")
375
        if index > np.size(ptr) - 1:
Louis Beauloye's avatar
Louis Beauloye committed
376
            mbs_msg(">>EQUIL>> variable exchange: Index out of bound")
377
            return
378
        address = ptr.ctypes.data + ctypes.sizeof(ctypes.c_double) * index
379
        if ptr.ctypes.data == self.mbs.q.ctypes.data:
Louis Beauloye's avatar
Louis Beauloye committed
380
            mbs_msg(">>EQUIL>> extra exchange: Warning ! Generalized coordinates have"
Louis Beauloye's avatar
Louis Beauloye committed
381
                    " to be used with caution as extra variables")
382

383
        if ptr_name is None:
Louis Beauloye's avatar
Louis Beauloye committed
384
            ptr_name = "extra_variable_{:d}".format(len(self.addition_list))
385

386
        if len(self.addition_list) == 0:
Louis Beauloye's avatar
Louis Beauloye committed
387
            self.addition_list = [[address, ptr_name]]
388
389
        else:
            for element in self.addition_list:
Louis Beauloye's avatar
Louis Beauloye committed
390
                if element[0] == address:
Louis Beauloye's avatar
Louis Beauloye committed
391
                    mbs_msg(">>EQUIL>> variable exchange: Extra variable has already been added")
392
                else:
Louis Beauloye's avatar
Louis Beauloye committed
393
                    self.addition_list.append([address, ptr_name])
394
                    return
395

396
    def add_extra_variable_um(self, user_model, param):
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
        """
        Add a user model as new equilibrium variable.

        Extra variables are stored and will be set in the memory of the
        external library when 'MbsEquil::run()' is called. Between two run()
        the equilibrium can be modified thanks to replace().
        However as the number of equilibrium variables is locked in the
        external memory after the first call of run(), this function can not
        be called between two run().

        Parameters
        ----------
        user_model: str
            Name of the user model to add to the equilibrium variables
        param: str
            Parameter of the user model to add to the equilibrium variables
413
414
        """
        if self.get_options("nxe"):
Louis Beauloye's avatar
Louis Beauloye committed
415
            mbs_msg("As the number of equilibrium variables is locked in the external memory after the first call of run(), this function can not be called between two run()")
416
        if self.mbs.user_model[user_model]._type[param] != 1 and self.mbs.user_model[user_model]._type[param] != 7:
Louis Beauloye's avatar
Louis Beauloye committed
417
            mbs_msg(">>EQUIL>> variable exchange: This function does not handle this type of user model(only float and int)")
418
419
            return
        address = self.mbs.user_model[user_model].__getarray__(param)[0].ctypes.data
420
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
421
            mbs_msg("DEBUG>>  add_extra_variable_um, address: ", address)
422
        if len(self.addition_list) == 0:
423
            self.addition_list = [[address, user_model + "." + param]]
424
425
        else:
            for element in self.addtion_list:
Louis Beauloye's avatar
Louis Beauloye committed
426
                if element[0] == address:
Louis Beauloye's avatar
Louis Beauloye committed
427
                    mbs_msg(">>EQUIL>> variable exchange: Extra variable has already been added")
428
                else:
429
                    self.addition_list.append([address, user_model + "." + param])
430
                    return
431

432
433
    def add_exchange_um(self, user_model, param, replaced_var_id):
        """
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
        Add a user model as a new equilibrium variable that replaces the specified one.

        Exchanged variables are stored and will be set in the memory of the
        external library when 'MbsEquil::run()' is called. Between two run()
        the equilibrium can be modified thanks to replace().
        However as the number of equilibrium variables is locked in the
        external memory after the first call of run(), this function can not
        be called between two run().

        Parameters
        ----------
        user_model: str
            Name of the user model to be exchanged
        param: str
            Parameter of the user model to be exchanged
        replaced_var_id: int
            Index of the default variable that will be replaced
        """
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
453
            mbs_msg("DEBUG>>  add_exchange_um, parameters: ", user_model, param, replaced_var_id)
454

455
        if self.get_options("nquch"):
Louis Beauloye's avatar
Louis Beauloye committed
456
            mbs_msg("As the number of equilibrium variables is locked in the external memory"
Louis Beauloye's avatar
Louis Beauloye committed
457
                    " after the first call of run(), this function can not be called between two run()")
458
            return
459
        if self.mbs.user_model[user_model]._type[param] != 1 and self.mbs.user_model[user_model]._type[param] != 7:
Louis Beauloye's avatar
Louis Beauloye committed
460
            mbs_msg(">>EQUIL>> variable exchange: This function does not handle this"
Louis Beauloye's avatar
Louis Beauloye committed
461
                    " type of user model(only float and int)")
462
463
            return
        address = self.mbs.user_model[user_model].__getarray__(param)[0].ctypes.data
464
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
465
            mbs_msg("DEBUG>>  add_exchange_um, address: ", address)
466

467
        if len(self.exchange_list) == 0:
468
            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
469
                mbs_msg("DEBUG>>  add_exchange_um, first exchanged variable")
470
471

            self.exchange_list = [[address, replaced_var_id, user_model + "." + param]]
472
473
        else:
            for element in self.exchange_list:
474
                if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
475
                    mbs_msg("DEBUG>>  add_exchange_um, iterating exchanged variables:", element)
476

477
                if element[0] == address:
Louis Beauloye's avatar
Louis Beauloye committed
478
                    mbs_msg(">>EQUIL>> variable exchange: Exchange variable is already an equilibrium variable")
479
                    return
480
                else:
481
                    if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
482
                        mbs_msg("DEBUG>>  add_exchange_um, vatiable added")
483
484

                    self.exchange_list.append([address, replaced_var_id, user_model + "." + param])
485
                    return
486

487
    def _compute_exchange(self):
488
        """Set exchanged variables in the memory of the external library.
489
490

        This function is automatically called when 'MbsEquil::run()' is called.
491
        """
492
493
494
495
496
        if self.get_options("nquch") == 0 and len(self.exchange_list) > 0:
            self.mbs_equil_ptr.contents.options.contents.nquch = len(self.exchange_list)
            libmodules.mbs_equil_exchange(self.mbs_equil_ptr.contents.options)
        if self.get_options("nquch") == len(self.exchange_list) and len(self.exchange_list) > 0:
            for i, element in enumerate(self.exchange_list):
497
                libmodules.mbs_equil_set_variable(self.mbs_equil_ptr.contents.options, element[0], i + 1, element[1])
498
        elif self.get_options("nquch") != len(self.exchange_list):
Louis Beauloye's avatar
Louis Beauloye committed
499
            mbs_msg(">>EQUIL>> variable exchange: nquch(" + str(self.get_options("nquch")) +
Louis Beauloye's avatar
Louis Beauloye committed
500
                    ") different from the length of the exchange list(" + str(len(self.exchange_list)) + ")")
501

502
    def _compute_extra_variable(self):
503
        """Set extra variables in the memory of the external library.
504

505
        This function is automatically called when 'MbsEquil::run()' is called.
506
507
508
509
510
511
        """
        if self.get_options("nxe") == 0 and len(self.addition_list) > 0:
            self.mbs_equil_ptr.contents.options.contents.nxe = len(self.addition_list)
            libmodules.mbs_equil_addition(self.mbs_equil_ptr.contents.options)
        if self.get_options("nxe") == len(self.addition_list) and len(self.addition_list) > 0:
            for i, element in enumerate(self.addition_list):
512
                libmodules.mbs_equil_add_variable(self.mbs_equil_ptr.contents.options, element[0], i + 1)
513
        elif self.get_options("nxe") != len(self.addition_list):
Louis Beauloye's avatar
Louis Beauloye committed
514
            mbs_msg(">>EQUIL>> extra variable: nxe(" + str(self.get_options("nxe")) +
Louis Beauloye's avatar
Louis Beauloye committed
515
                    ") different from the length of the extra variable list(" + str(len(self.addition_list)) + ")")
516

517
518
    def replace_exchange(self, ptr, index, id_exchanged, ptr_name=None):
        """Replace an existing exchanged variable with another one.
519

520
521
        This function can be called between two 'MbsEquil::run()' as it does not
        change the number of equilibrium variables.
522
523
524
525

        Parameters
        ----------
        ptr: ndarray
526
            Array containing the variable to be exchanged.
527
        index: int
528
            Index in 'ptr' of the variable to be exchanged.
529
530
        id_exchanged: int
            Index in the list of exchanged variable of the variable to be
531
            replaced.
532
        ptr_name: str, optionnal
533
534
535
           Name of the variable to be exchanged. If set to None the existing
           name is kept.
           The default is None.
536
        """
537
        if ptr.ctypes.data == self.mbs.q.ctypes.data:
538
            mbs_msg(">>EQUIL>> variable exchange: Warning generalized coordinates"
Louis Beauloye's avatar
Louis Beauloye committed
539
                    " have to be used with caution as exchange variables")
540
541

        if id_exchanged > len(self.exchange_list) or id_exchanged <= 0:
Louis Beauloye's avatar
Louis Beauloye committed
542
            mbs_msg(">>EQUIL>> variable exchange: There is no exchange variable at this index:" + str(id_exchanged))
543
        else:
544
            self.exchange_list[id_exchanged - 1][0] = ptr.ctypes.data + ctypes.sizeof(ctypes.c_double) * index
545
546
            if ptr_name:
                self.exchange_list[id_exchanged - 1][2] = ptr_name
547

548
549
    def replace_extra_variable(self, ptr, index, id_extra, ptr_name=None):
        """Replace an existing extra variable with another one.
550
551

        This function can be called between two 'MbsEquil::run()' as it does not
552
        change the number of equilibrium variables.
553
554
555
556

        Parameters
        ----------
        ptr: ndarray
557
            Array containing the extra variable to be exchanged.
558
        index: int
559
560
561
562
563
564
565
            Index in 'ptr' of the extra variable to be exchanged.
        id_extra: int
            Index in the list of extra variable of the variable to be replaced.
        ptr_name: str, optionnal
           Name of the variable to be exchanged. If set to None the existing
           name is kept.
           The default is None.
566
567
        """
        if ptr.ctypes.data == self.mbs.q.ctypes.data:
568
            mbs_msg(">>EQUIL>> extra exchange: Warning generalized coordinates"
Louis Beauloye's avatar
Louis Beauloye committed
569
                    " have to be used with caution as extra variables")
570
571

        if id_extra > len(self.addition_list) or id_extra <= 0:
Louis Beauloye's avatar
Louis Beauloye committed
572
            mbs_msg(">>EQUIL>> extra exchange: There is no extra variable at this index:" + str(id_extra))
573
        else:
574
            self.addition_list[id_extra - 1][0] = ptr.ctypes.data + ctypes.sizeof(ctypes.c_double) * index
575
576
            if ptr_name:
                self.addition_list[id_extra - 1][1] = ptr_name
577

578
    def replace_extra_variable_um(self, user_model, param, id_extra):
579
        """Replace an existing extra variable with a user model.
580
581

        This function can be called between two 'MbsEquil::run()' as it does not
582
583
584
        change the number of equilibrium variables.

        The name of the extra equilibrium variable is replaceb by 'user_model.param'.
585
586
587
588

        Parameters
        ----------
        user_model: str
589
            Name of the user model containing the parameter to be exchanged.
590
        param: str
591
592
593
            Parameter of the user model to be exchanged.
        id_extra: int
            Index in the list of extra variable of the variable to be replaced.
594
595
        """
        if self.mbs.user_model[user_model]._type[param] != 1 and self.mbs.user_model[user_model]._type[param] != 7:
Louis Beauloye's avatar
Louis Beauloye committed
596
            mbs_msg(">>EQUIL>> extra exchange: This function does not handle this"
Louis Beauloye's avatar
Louis Beauloye committed
597
                    " type of user model(only float and int)")
598
            return
599
        if id_extra > len(self.addition_list) or id_extra <= 0:
Louis Beauloye's avatar
Louis Beauloye committed
600
            mbs_msg(">>EQUIL>> extra exchange: There is no extra variable at this index:" + str(id_extra))
601
        else:
602
603
604
            self.addition_list[id_extra - 1][0] = self.mbs.user_model[user_model].__getarray__(param)[0].ctypes.data
            self.addition_list[id_extra - 1][1] = user_model + "." + param

605
    def replace_exchange_um(self, user_model, param, id_exchanged):
606
        """Replace an existing exchanged variable with a user model.
607
608
609
610

        This function can be called between two 'MbsEquil::run()' as it does not
        change the number of equilibrium variables.

611
612
        The name of the exchanged equilibrium variable is replaceb by 'user_model.param'.

613
614
615
        Parameters
        ----------
        user_model: str
616
            Name of the user model containing the parameter to be exchanged.
617
        param: str
618
            Parameter of the user model to be exchanged.
619
        id_exchanged: int
620
            Index in the list of exchanged variable of the variable to be replaced.
621
        """
622
        if self.mbs.user_model[user_model]._type[param] != 1 and self.mbs.user_model[user_model]._type[param] != 7:
Louis Beauloye's avatar
Louis Beauloye committed
623
            mbs_msg(">>EQUIL>> variable exchange: This function does not handle "
Louis Beauloye's avatar
Louis Beauloye committed
624
                    "this type of user model(only float and int)")
625
            return
626
        if id_exchanged > len(self.exchange_list) or id_exchanged <= 0:
Louis Beauloye's avatar
Louis Beauloye committed
627
            mbs_msg(">>EQUIL>> variable exchange: There is no exchange variable at this index:" + str(id_exchanged))
628
        else:
629
630
631
            self.exchange_list[id_exchanged - 1][0] = self.mbs.user_model[user_model].__getarray__(param)[0].ctypes.data
            self.exchange_list[id_exchanged - 1][2] = user_model + "." + param

Louis Beauloye's avatar
Louis Beauloye committed
632
    def print_equil(self):
Louis Beauloye's avatar
Louis Beauloye committed
633
        """mbs_msg the results of the equilibrium analysis from C library."""
Louis Beauloye's avatar
Louis Beauloye committed
634
        libmodules.mbs_print_equil(self.mbs_equil_ptr)
Louis Beauloye's avatar
Louis Beauloye committed
635

636
    def set_options(self, **kwargs):
637
        """
638
639
        Set the specified options for Equil module.

640
641
        Parameters
        ----------
642
        method: int
643
644
            fsolvepk = 1 / optim = 2
            default = 1
645
        equitol: float
646
            equilibrium acceptation tolerance
647
648
            default = 1e - 6
        verbose: int
649
650
            no = 0, yes = 1
            default = 1
651
652
        save2file: int
            1: results saved, 0: not saved
653
            default = 1
654
        compute_uxd: int
655
656
657
            no = 0, yes = 1
            flag to compute the extra constitutive differential equations as equil equations
            default = 1
658
        grad_lpk: int
659
660
661
            options to compute the gradient through the lpk parabolic fitting method.
            If not activated, the gradient is computed by small deviation with options devJac
            default = 0
662
        lpk_itermax: int
663
664
            options for the lpk gradient
            default = 10
665
        lpk_relincr: float
666
            options for the lpk gradient
667
668
            default = 1e - 2
        lpk_absincr: float
669
            options for the lpk gradient
670
671
            default = 1e - 3
        lpk_equitol: float
672
            options for the lpk gradient
673
674
            default = 1e - 6
        lpk_lintol: float
675
            options for the lpk gradient
676
677
            default = 1e - 3
        resfilename: char
678
            The keyword used for determining the name of result files
679
680
681
682
683
            default: equil
        respath: char
            Path in which result file are saved.
            default: resultsR folder
        animpath: char
684
            Path in which animation file are saved.
685
686
687
            default: animationR folder
        mode: int
            1=static / 2=quasistatic / 3=dynamic
688
            default = 0
689
        relax: int
690
            relaxation in pk algorithm
691
            no = 0, yes = 1
692
            default = 0
693
        relaxcoeff: float
694
695
            relaxation coefficient
            default = 0.9
696
        relaxIterMax: float
697
698
            maximum number of relaxation iterations
            default = 10
699
        soft: int
700
701
702
            softing in pk algorithm
            no = 0, yes = 1
            default = 0
703
704
705
706
        senstol: float
            tolerance for testing a variable non - sensitivity
            default = 1e - 6
        devjac: float
707
            deviate state for Gradient calculation
708
709
710
            default = 1e - 6
        itermax: int
            maximum number of iteration for solvepk
711
            default = 30
712
        visualize: int
713
714
            no = 0, yes = 1
            default = 0
715
        clearmbsglobal: int
716
            inout = 1, out = 2, none = 3, all = 4
717
718
            default = 1
        saveperiod: int
719
            The number of iteration between two buffer records
720
721
            default = 1(every iteration are recorded)
        save_anim: int
722
            1 to save the anim file
723
724
            0 otherwise(only valid if 'save2file' is set to 1)
        framerate: int
725
            number of frame per second for the .anim file
726
        buffersize: int
727
            The number of time step that can be recorded in the buffer
728
729
            default = - 1
        max_save_user: int
730
731
            The maximal number of user variables saved
            default = 12
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
        store_results: int
            If set to 1, a copy of the results is done.
            default is 1
        """
        options = {'method': {'convert': int, 'c_name': 'method'},
                   'equitol': {'convert': float, 'c_name': 'equitol'},
                   'verbose': {'convert': int, 'c_name': 'verbose'},
                   'save2file': {'convert': int, 'c_name': 'save2file'},
                   'compute_uxd': {'convert': int, 'c_name': 'compute_uxd'},
                   'grad_lpk': {'convert': int, 'c_name': 'grad_lpk'},
                   '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'},
                   'resfilename': {'convert': str_to_bytes, 'c_name': 'resfilename'},
                   'respath': {'convert': str_to_bytes, 'c_name': 'respath'},
                   'animpath': {'convert': str_to_bytes, 'c_name': 'animpath'},
                   'mode': {'convert': int, 'c_name': 'mode'},
                   'relax': {'convert': int, 'c_name': 'relax'},
                   'relaxcoeff': {'convert': float, 'c_name': 'relaxcoeff'},
                   'relaxIterMax': {'convert': float, 'c_name': 'relaxIterMax'},
                   'soft': {'convert': int, 'c_name': 'soft'},
                   'senstol': {'convert': float, 'c_name': 'senstol'},
                   'devjac': {'convert': float, 'c_name': 'devjac'},
                   'itermax': {'convert': int, 'c_name': 'itermax'},
                   'visualize': {'convert': int, 'c_name': 'visualize'},
                   'clearmbsglobal': {'convert': int, 'c_name': 'clearmbsglobal'},
                   'saveperiod': {'convert': int, 'c_name': 'saveperiod'},
                   'save_anim': {'convert': int, 'c_name': 'save_anim'},
                   'framerate': {'convert': int, 'c_name': 'framerate'},
                   'buffersize': {'convert': int, 'c_name': 'buffersize'},
                   'max_save_user': {'convert': int, 'c_name': 'max_save_user'},
                   'store_results': {'convert': int, 'c_name': 'store_results'}
                   }
767
        for key, value in kwargs.items():
768
769
770
771
772
773
774
            if key not in options:
                raise TypeError("{:} is an invalid option name.".format(key))
            try:
                c_name = options[key]['c_name']
                if key == 'store_results':
                    self.store_results = int(value)
                else:
Louis Beauloye's avatar
Louis Beauloye committed
775
                    setattr(self.mbs_equil_ptr.contents.options.contents, c_name, options[key]['convert'](value))
776
777
778
779
            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)
Louis Beauloye's avatar
Louis Beauloye committed
780
                raise TypeError("{:} is str, can not be casted from {:}.".format(key, type(value))).with_traceback(err.__traceback__) from None
781

782
783
    def get_options(self, *args):
        """
784
785
        Get the specified options for Equil module.

786
787
788
        Parameters
        ----------
        The different options specifed in the documentation of set_options
789

790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
        Returns
        -------
        The value of the different options specifed in the documentation of set_options
        """
        options = []
        for key in args:
            if key == "method":
                options.append(self.mbs_equil_ptr.contents.options.contents.methods)
            elif key == "equitol":
                options.append(self.mbs_equil_ptr.contents.options.contents.equitol)
            elif key == "verbose":
                options.append(self.mbs_equil_ptr.contents.options.contents.verbose)
            elif key == "save2file":
                options.append(self.mbs_equil_ptr.contents.options.contents.save2file)
            elif key == "compute_uxd":
                options.append(self.mbs_equil_ptr.contents.options.contents.compute_uxd)
            elif key == "grad_lpk":
                options.append(self.mbs_equil_ptr.contents.options.contents.grad_lpk)
            elif key == "lpk_itermax":
                options.append(self.mbs_equil_ptr.contents.options.contents.lpk_itermax)
            elif key == "lpk_relincr":
                options.append(self.mbs_equil_ptr.contents.options.contents.lpk_relincr)
            elif key == "lpk_absincr":
                options.append(self.mbs_equil_ptr.contents.options.contents.lpk_absincr)
            elif key == "lpk_equitol":
                options.append(self.mbs_equil_ptr.contents.options.contents.lpk_equitol)
            elif key == "lpk_lintol":
                options.append(self.mbs_equil_ptr.contents.options.contents.lpk_lintol)
            elif key == "resfilename":
819
820
821
                address = self.mbs_equil_ptr.contents.options.contents.resfilename
                defaut = 'equil'
                options.append(str_from_c_pointer(address, defaut))
822
            elif key == "respath":
823
824
825
                address = self.mbs_equil_ptr.contents.options.contents.respath
                defaut = os.path.abspath(os.path.join(self.mbs.project_path, 'resultsR'))
                options.append(str_from_c_pointer(address, defaut))
826
            elif key == "animpath":
827
828
829
                address = self.mbs_equil_ptr.contents.options.contents.animpath
                defaut = os.path.abspath(os.path.join(self.mbs.project_path, 'animationR'))
                options.append(str_from_c_pointer(address, defaut))
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
            elif key == "mode":
                options.append(self.mbs_equil_ptr.contents.options.contents.mode)
            elif key == "relax":
                options.append(self.mbs_equil_ptr.contents.options.contents.relax)
            elif key == "relaxcoeff":
                options.append(self.mbs_equil_ptr.contents.options.contents.relaxcoeff)
            elif key == "relaxIterMax":
                options.append(self.mbs_equil_ptr.contents.options.contents.relaxIterMax)
            elif key == "soft":
                options.append(self.mbs_equil_ptr.contents.options.contents.soft)
            elif key == "senstol":
                options.append(self.mbs_equil_ptr.contents.options.contents.senstol)
            elif key == "devjac":
                options.append(self.mbs_equil_ptr.contents.options.contents.devjac)
            elif key == "itermax":
                options.append(self.mbs_equil_ptr.contents.options.contents.itermax)
            elif key == "visualize":
                options.append(self.mbs_equil_ptr.contents.options.contents.visualize)
            elif key == "clearmbsglobal":
                options.append(self.mbs_equil_ptr.contents.options.contents.clearmbsglobal)
            elif key == "nquch":
851
                options.append(self.mbs_equil_ptr.contents.options.contents.nquch)
852
            elif key == "nxe":
853
                options.append(self.mbs_equil_ptr.contents.options.contents.nxe)