mbs_dirdyn.py 51.5 KB
Newer Older
1
2
# -*- coding: utf-8 -*-
"""
3
Module to handle direct dynamics simulations on Multibody systems.
4

5
6
7
8
9
10
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.
11
"""
12
13
# Author: Robotran Team
# (c) Universite catholique de Louvain, 2019
14
15

import os
16
import ctypes
17
18
19

import numpy as np

20
# importing MbsysPy functions
21
from ..mbs_utilities import str_from_c_pointer
22
23
24
25
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
26
from ..mbs_utilities import mbs_msg
27
28

# importing libraries
29
from .._mbsysc_loader.loadlibs import libmodules
30
from .._mbsysc_loader.loadlibs import libutilities
31
32


33
# =============================================================================
34
# Global parameter of the current module
35
# =============================================================================
36
__DEBUG__ = False
37
38
39
__MODULE_DIR__ = os.path.dirname(os.path.abspath(__file__))


40
# =============================================================================
41
# Defining Python MbsDirdyn class
42
# =============================================================================
43
44

class MbsDirdyn(object):
45
    """
46
47
    Class of the direct dynamic module.

48
49
    Attributes
    ----------
50
    dt: double
51
        Current integration step size.
52
    mbs: MbsData
53
        Instance of MbsData related to the analysis.
54
    results: MbsResult
55
        Instance of MbsResult containing the results of the direct dynamics analysis.
56
57
    symbolic_path: str
        Path to the folder containing the symbolic functions(python modules)
58
        to be loaded.
59
    tsim: double
60
        Current simulation time.
61
62
63
    user_path: str
        Path to the folder containing the user functions(python modules) to be loaded.

64
65
66
67
68
69
    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")
70
    (5.0, 10.0)
71
    >>> results = mbs_dirdyn.run()
72

73
    """
74

75
    def __init__(self, mbs, user_path=None, symbolic_path=None):
76
77
78
79
80
        """
        Create an instance of the MbsDirdyn class for the provided MbsData instance.

        Parameters
        ----------
81
        mbs: MbsData
82
            Instance of MbsData related to the analysis.
83
84
85
        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'.
86
            default is None
87
88
89
        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'.
90
            default is None
91

92
93
        Returns
        -------
94
        MbsDirdyn: self
95
96
            A MbsDirdyn instance.
        """
97
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
98
            mbs_msg("DEBUG>>  Creating MbsDirdyn struct for " + mbs.mbs_name + "' MBS.")
99

100
        self.mbs_dirdyn_ptr = libmodules.mbs_new_dirdyn(mbs.mbs_data_ptr)
101
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
102
            mbs_msg("DEBUG>>  MbsDirdyn structure loaded")
103

104
105
        self.mbs = mbs

106
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
107
            mbs_msg("DEBUG>>  MbsDirdyn created.")
108

109
        # Path to user function used by partitionning module
110
        self.user_path = self.mbs.user_path
111
112
        if user_path is not None:
            project_path = bytes_to_str(self.mbs.project_path)
113
114
115
            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
116
117
118
                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 + '".')
119
120
121
122
            else:
                self.user_path = user_path
        # Path to user function used by partitionning modue
        self.symbolic_path = self.mbs.symbolic_path
123
        if symbolic_path is not None:
124
            project_path = bytes_to_str(self.mbs.project_path)
125
126
127
            symbolic_path = os.path.join(project_path, symbolic_path)
            # Error handeling
            if not os.path.isdir(symbolic_path):
Louis Beauloye's avatar
Louis Beauloye committed
128
129
130
                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 + '".')
131
132
            else:
                self.symbolic_path = symbolic_path
133

134
        # Storing project function pointer
Louis Beauloye's avatar
Louis Beauloye committed
135
136
137
138
139
140
141
142
        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'
                              ]
143

144
145
        # Storing Results
        self.results = MbsResult(self.mbs)
Louis Beauloye's avatar
Louis Beauloye committed
146
        self.store_results = True
147

148
        # Exposing some memory
149
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
150
            mbs_msg("DEBUG>>  Exposing MbsDirdyn fields.")
151

152
153
        # Constraints
        self._h = self._Jac = self._jdqd = None
154
155
156
157
        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,))
158
        self._huserc = self._Juserc = self._jdqduserc = None
159
160
161
162
        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,))
163
        # Fields from equation of motion
164
165
166
167
        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,))

168
    def __str__(self):
169
170
        """Return str(self)."""
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
171
            mbs_msg("DEBUG>>  start of __str")
172

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

175
    def __del__(self):
176
        """Delete the object by freeing the C-related memory."""
177
        libmodules.mbs_delete_dirdyn(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
178
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
179
            mbs_msg("DEBUG>>  MbsDirdyn pointer deleted.")
180

181
    def run(self, **kwargs):
Louis Beauloye's avatar
Louis Beauloye committed
182
        """
183
184
185
186
187
        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()'.

188
189
        Results are stored in the field 'results' if the options 'store_results'
        is set to True.
190

191
192
        Returns
        -------
193
        self.results: MbsResult
194
            The MbsResults containing the results of the analysis.
195
        """
196
        # Assign required user functions
197
        if self.mbs.opt_load_c < 2:
198
            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
199
                mbs_msg("DEBUG>>  Loading user functions")
200

201
            self.mbs.__load_user_fct__(__MODULE_DIR__, self.user_fun_list, self.user_path)
Louis Beauloye's avatar
Louis Beauloye committed
202
            self.mbs.__assign_user_fct__(self.user_fun_list, self)
203

204
        # Assign required symbolic functions
205
        if self.mbs.opt_load_c < 1:
206
207
            # Loading symbolic function
            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
208
                mbs_msg("DEBUG>>  Loading symbolic functions")
209
210

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

213
        self.set_options(**kwargs)
214
215

        if not self.store_results:
216
            error = libmodules.mbs_run_dirdyn(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
217
        else:
218
219
220
            # Save2file forced to 1 because if buffers don't have the complete,
            # they are loaded from files
            self.set_options(save2file=1)
221
222
            error = libmodules.mbs_dirdyn_init(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
            if (error >= 0):
223
224
225
226
227
228
                if not self.get_options('flag_oneshot'):
                    error = libmodules.mbs_dirdyn_loop(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
                else:
                    error = libmodules.mbs_fct_dirdyn(self.mbs_dirdyn_ptr.contents.tsim,
                                                      self.mbs_dirdyn_ptr.contents.y,
                                                      self.mbs_dirdyn_ptr.contents.yd,
229
230
                                                      self.mbs.mbs_data_ptr,
                                                      self.mbs_dirdyn_ptr
231
                                                      )
232

233
234
            if (error >= 0 and self.get_options("save2file")):
                # Results(buffer) memory is kept BUT FILES WILL BE WRITTEN LATER
235
                size1 = self.mbs_dirdyn_ptr.contents.buffers[0].contents.index
236
237
                if size1 == 0:
                    size1 = self.mbs_dirdyn_ptr.contents.buffers[0].contents.size
238
239
240
241
                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)))
242
                # get time array from the q buffer
243
                self.results.t = self.results.q[:, 0]
244
                if not self.results.t[0] == self.get_options("t0"):
Louis Beauloye's avatar
Louis Beauloye committed
245
                    mbs_msg("The beginning of the integration is not available in the buffer.\n The complete results have to be loaded from files.")
246
                    filename = bytes_to_str(ctypes.string_at(self.mbs_dirdyn_ptr.contents.buffers[0].contents.filename))
247
                    self.results.load_results_from_file(filename, module=6)
248
                # get qd and qdd buffer
249
250
251
252
                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)))
253
                buffer_id = 4
254
                if self.mbs.Nux:
255
256
257
                    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)))
258
259
                    buffer_id = buffer_id + 2
                if self.mbs.Nlink:
260
261
262
263
                    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)))
264
265
                    buffer_id = buffer_id + 3
                if self.mbs.nqc:
266
267
                    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)))
268
269
                    buffer_id = buffer_id + 1
                if self.mbs.nhu:
270
271
                    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)))
272
273
274
275
                    buffer_id = buffer_id + 1

                nb_user_output_vector = libutilities.get_output_vector_nb()
                for i in range(nb_user_output_vector):
276
277
                    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)))
278
279
280
281
282

                    # Name of the output vector should be recoverable with function
                    # 'libutilities.get_output_vector_label(i)' returning char*.
                    # However on MacOs (see comments of merge request !383) this
                    # does not work (pointer seems to be random).
283
                    name = bytes_to_str(os.path.basename(self.mbs_dirdyn_ptr.contents.buffers[buffer_id + i].contents.filename)[:-4])
284
285
286
                    len_prefix = len(self.get_options("resfilename"))
                    name = name[len_prefix + 1:]

287
                    self.results.outputs[name] = user_out
288

289
290
291
                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
292
                    for i in range(nbOutput):
293
                        user_out = np.copy(np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.user_buffer.contents.X[i], (1, size)))
294
                        name = bytes_to_str(self.mbs_dirdyn_ptr.contents.user_buffer.contents.names[i])
295
                        self.results.outputs[name] = user_out[0]
296

297
298
            if (error >= 0):
                libmodules.mbs_dirdyn_finish(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
299

Louis Beauloye's avatar
Louis Beauloye committed
300
        # Unassign user functions
301
        if self.mbs.opt_load_c < 2:
Louis Beauloye's avatar
Louis Beauloye committed
302
            self.mbs.__unassign_user_fct__(self.user_fun_list)
303

Louis Beauloye's avatar
Louis Beauloye committed
304
        # Unassing required symbolic functions
305
        if self.mbs.opt_load_c < 1:
Louis Beauloye's avatar
Louis Beauloye committed
306
            self.mbs.__unassign_symb_fct__(self.symb_fun_list)
307

308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
        if error < 0:
            mbs_msg("\n--------------------------------------------------\n"
                    "READ CAREFULLY !!!\n"
                    "--------------------------------------------------\n\n"
                    "An error occurs during direct dynamic 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"
                    "    - Some mass or Inertia component is missing, leading to a invalid mass matrix\n"
                    "      during the motion;\n"
                    "    - The motion reach an singular configuration;\n"
                    "    - Some integrators do not allow system without independent joints;\n"
                    "    - An error in user function (joint force, external forces) return wrong value.\n"
                    "      Excessive force can generate infinte acceleration.\n"
                    "    - The parameters of the integrator are not compatible with the system dynamics.\n"
                    "    - The integrator is not adapted to the current system dynamics;\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'.\n"
                    "\n--------------------------------------------------\n"
                    "\n--------------------------------------------------\n")
333
            raise RuntimeError("MbsDirdyn.run() failed, read previous messages.")
334

Louis Beauloye's avatar
Louis Beauloye committed
335
        return self.results
336

337
    def set_user_fct_from_file(self, function_name, user_path, user_file):
Louis Beauloye's avatar
Louis Beauloye committed
338
        """Load a user function from a file chosen by the user instead of the default one in the userfctR folder.
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355

        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):
Louis Beauloye's avatar
Louis Beauloye committed
356
        """Load a user function chosen by the user instead of the default one in the userfctR folder.
357
358
359
360
361
362
363
364
365
366
367
368
369

        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)

370
371
    def set_options(self, **kwargs):
        """
372
        Set the specified options for Dirdyn module.
373

374
375
        Parameters
        ----------
376
        t0: float
377
378
            Initial time of the simulation.
            default is 0.0
379
        tf: float
380
381
            Final time of the simulation
            default is 5.0
382
        dt0: float
383
            Initial value of the integration step size. For fixed-step integrator
384
           (ie. RK4, Euler explicit...) this is the time step for the whole simulation.
385
            default is 0.001
386
387
        save2file: int
            Determine whether results are written to files on disk(in resultsR folder):
388
             - 1: results are saved
389
             - 0: results are not saved
390
            default is 1
391
        resfilename: str
392
393
            The keyword used for determining the name of result files. The default
            value is set to 'oneshot' (instead of 'dirdyn') if 'flag_oneshot' is True.
394
            default is 'dirdyn'
395
        respath: str
396
            Path in which result file are saved. This is the full path or the
397
            relative path. The folder must exist.
398
            default is the 'resultsR/' folder of the project
399
        animpath: str
400
            Path in which animation file is saved. This is the full path or the
401
            relative path. The folder must exist.
402
            default is the 'animationR/' folder of the project
403
404
        save_anim: int
            1 to save the animation file, if 'save2file' is set to 1. Set to 0
405
406
            to not save the animation file.
            default is 1
407
        save_visu: int
408
            Unused options as realtime is deactivated is MBsysPy.
409
            1 to save the visualizazion file(as it appears in 'user_realtime_visu.c'),
410
411
            if 'save2file' is set to 1. Set to O to not save the visualization.
            default is 0
412
        framerate: int
413
414
            number of frame per second for the animation file.
            default is 1000
415
        saveperiod: int
416
            The number of time steps between two buffer records.
417
418
            default is 1(every time step are recorded)
        max_save_user: int
419
420
            The maximal number of user variables saved.
            default is 12
421
422
        buffersize: int
            The number of time step that can be recorded in the buffer. Results
423
424
425
426
            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
427
        realtime: int
428
429
            Unused options as realtime is deactivated is MBsysPy.
            1 to activate to real-time features, 0 to deactivate them.
430
            default = 0
431
        accelred: int
432
433
            1 to use accelred, 0 otherwise.
            default is 0
434
435
436
437
438
        flag_oneshot: int
            Set to 1 to compute the derivative function only once (no time integration).
            Then default resfilename is set to 'oneshot'.
            The time is the value found in the option t0 (used by user functions).
            default is 0
439
        flag_compute_Qc: int
440
441
442
443
444
            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
445
        compute_all_uxd: int
446
447
448
            If 1 the user derivative are computed during the analysis. Otherwhise
            If set to 0, they are not computed.
            default is 1
449
        compute_Qc: numpy.ndarray
450
            If option "flag_compute_Qc' is set to 0, the vector allows to select
451
            the(driven) joints on which the force/torque required to follow the
452
            trajectory will be computed.
453
            The shape of the array is '(mbs.njoint + 1)', for example with
454
455
456
            '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
457
458
            default is an array full of zero(no forces computation)
        integrator: str
Olivier Lantsoght's avatar
Olivier Lantsoght committed
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
            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
483
            default is "RK4"
484
        verbose: int
485
486
487
            Set to 1 to get messages related to adaptive stepsize integrator. To
            disable message set to 0.
            default is 1
488
        flag_stop_stiff: int
489
490
491
            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
492
        flag_precise_dynamics: int
493
494
            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.
495
496

            If set at 1, the direct dynamics(constraint solving, computation of
497
            the generalized acceleration) will be done at the beginning of each
498
            time step of integration. The output files will be exact, but the
499
            computation will be a little slower.
500
            If set at 0, the output file will contains the value(constraint
501
502
503
504
505
            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
506
507
508
509
510
511
512
513
514
        flag_baumgarte_stabilization: int
            Flag to use the baumgarte stabilization of the independant accelerations.
            default is 0
        baumgarte_alpha: float
            Alpha parameter of baumgarte stabilization (>0) in 1/s.
            default = 0.1
        baumgarte_beta: float
            Beta parameter of baumgarte stabilization (>0) in 1/s.
            default = 0.1
515
        flag_waypoint: int
516
            If set to 1, the integrator will be forced to give a solution at the
517
            specified time interval('delta_t_wp'). If set to 0, the solution
518
519
            will be given at every computed time-step.
            default is 0
520
521
522
523
        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
524
525
            all the timesteps and the waypoints.
            default is 0
526
527
        delta_t_wp: float
            Time interval between two waypoints [s], unused if 'flag_waypoint'
528
529
            is set to 0.
            default is 1.0e-3
530
        nmax: int
531
532
            maximal number of steps for adaptative stepsize integrators.
            default is 1e9
533
        toler: float
534
535
            mixed error tolerances for some adaptative stepsize integrators.
            default is 1.0e-3
536
        rtoler: float
537
538
            relative error tolerances for some adaptative stepsize integrators.
            default is 1.0e-3
539
        atoler: float
540
541
            absolute error tolerances for some adaptative stepsize integrators.
            default is 1.0e-6
542
        dt_max: float
543
544
            maximal time step [s] for some adaptative stepsize integrators.
            default is 1.0e-3
545
546
        n_freeze: int
            number of time step when the jacobian is freezed(computed once at
547
548
            the start of the n_freeze time steps) for implicit integrators.
            default is 0
549
550
        show_failed_closure: int
            If set to 1, an animation of the Newton-Raphson procedure is generated
551
552
            if the closure fails.
            default is 0
553
        store_results: int
554
555
            If set to 1, a copy of the results is done.
            default is 1
556
        """
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606

        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
607
                else:
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
                    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
626
                elif value == "AlphaM":
627
                    return 8
628
629
                elif value == "Custom":
                    return 9
630
                else:
631
                    raise ValueError
632
            else:
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
                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
664
            else:
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
                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'},
682
                   'flag_oneshot': {'convert': int, 'c_name': 'flag_oneshot'},
683
684
685
686
687
688
689
                   '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'},
690
691
692
                   'flag_baumgarte_stabilization': {'convert': int, 'c_name': 'flag_baumgarte_stabilization'},
                   'baumgarte_alpha': {'convert': float, 'c_name': 'baumgarte_alpha'},
                   'baumgarte_beta': {'convert': float, 'c_name': 'baumgarte_beta'},
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
                   '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:
Louis Beauloye's avatar
Louis Beauloye committed
727
                    raise TypeError("{:} is {:}, can not be casted from {:}.".format(key, options[key]['convert'], type(value))).with_traceback(err.__traceback__) from None
728
729
730

            except AttributeError as err:
                # if error during cast of strings (i.e in str_to_bytes)
Louis Beauloye's avatar
Louis Beauloye committed
731
                raise TypeError("{:} is str, can not be casted from {:}".format(key, type(value))).with_traceback(err.__traceback__) from None
732
733
734

            except TypeError as err:
                # if wrong type in integrator or compute_QC
Louis Beauloye's avatar
Louis Beauloye committed
735
736
737
738
                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
739

740
741
    def get_options(self, *args):
        """
742
743
        Get the specified options for Dirdyn module.

744
745
        Parameters
        ----------
746
        The different options specifed in the documentation of 'set_options()'
747

748
749
        Returns
        -------
750
        The value of the different options specifed in the documentation of 'set_options()'
751
752
753
754
755
756
757
758
759
760
761
762
        """
        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":
763
                address = self.mbs_dirdyn_ptr.contents.options.contents.resfilename
764
765
766
767
                if self.get_options('flag_oneshot'):
                    defaut = "oneshot"
                else:
                    defaut = "dirdyn"
768
                options.append(str_from_c_pointer(address, defaut))
769
            elif key == "respath":
770
771
772
                address = self.mbs_dirdyn_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))
773
            elif key == "animpath":
774
775
776
                address = self.mbs_dirdyn_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))
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
            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)
793
794
            elif key == "flag_oneshot":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.flag_oneshot)
795
796
            elif key == "flag_compute_Qc":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.flag_compute_Qc)
Louis Beauloye's avatar
Louis Beauloye committed
797
798
            elif key == "compute_all_uxd":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.compute_all_uxd)
799
            elif key == "compute_Qc":
800
                compute_Qc_py = np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.options.contents.compute_Qc, (self.mbs.mbs_data_ptr.contents.njoint + 1,))
801
802
803
804
805
806
807
808
809
                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:
810
                    options.append("EulerEx")
811
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 4:
812
                    options.append("Eulaire")
813
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 5:
814
                    options.append("EulerIm")
815
816
817
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 6:
                    options.append("Bader")
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 7:
818
                    options.append("WMethods")
819
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 8:
820
821
                    options.append("AlphaM")
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 9:
822
                    options.append("Custom")
823
824
825
826
827
828
            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)
829
830
831
832
833
834
            elif key == "flag_baumgarte_stabilization":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.flag_baumgarte_stabilization)
            elif key == "baumgarte_alpha":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.baumgarte_alpha)
            elif key == "baumgarte_beta":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.baumgarte_beta)
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
            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)
Louis Beauloye's avatar
Louis Beauloye committed
853
854
            elif key == "show_failed_closure":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.show_failed_closure)
Louis Beauloye's avatar
Louis Beauloye committed
855
856
            elif key == "store_results":
                options.append(self.store_results)
857
            else:
Louis Beauloye's avatar
Louis Beauloye committed
858
                mbs_msg(">>DIRDYN>>  The option " + key + " is not defined in this module")
859

860
861
862
863
        if len(options) == 0:
            return
        if len(options) == 1:
            return options[0]
864

865
        return tuple(options)
866
867

    # =========================================================================
868
    # Defining properties
869
    # =========================================================================
870
871
    @property
    def tsim(self):
872
        """Access to `tsim` attribute (read-only)."""
873
        return self.mbs_dirdyn_ptr.contents.tsim
874

875
876
    @property
    def dt(self):
877
        """Access to `dt` attribute (read-only)."""
878
        return self.mbs_dirdyn_ptr.contents.dt
879

880
881
882
883
884
    @property
    def buffer_nb(self):
        """Access to `bufferNb` attribute (read-only)."""
        return self.mbs_dirdyn_ptr.contents.bufferNb

885

886
class MbsResult(object):
887
888
    """
    Class containing results.
889

890
    The user-specified vector are not available.
891

892
893
    Attributes
    ----------
894
895
    q: ndarray
        Numpy array containing the current values of the generalized
896
        coordinates.
897
898
    qd: ndarray
        Numpy array containing the current values of the generalized
899
        velocities.
900
901
    qdd: ndarray
        Numpy array containing the current values of the generalized
902
        accelerations.
903
    Qq: ndarray
904
        Numpy array containing the values of the joint forces.
905
906
    Qc: ndarray
        Numpy array containing the value of joint force introduced in driven
907
        joints to respect the user function
908
909
910
    qa: ndarray of int
        Numpy array of integers containing the indices of actuated
        articulations(only for inverse dynamic). Those articulations are
911
        controlled by an actuator.
912
    t: ndarray
913
        Numpy array containing the values of the time vector
914
    ux: ndarray
915
        Numpy array containing the values of the user variables.
916
917
    uxd: ndarray
        Numpy array containing the values of the time derivative of the user
918
        variables.
919
920
    Fl: ndarray
        Numpy array containing the current values of the forces between of
921
        the points of a link.
922
923
    Z: ndarray
        Numpy array containing the current values of the distances between of
924
        the points of a link.
925
926
    Zd: ndarray
        Numpy array containing the current values of the speed(spreading)
927
        between of the points of a link.
928
    outputs: dict
929
        Dict containing the names and the values of the user outputs
930
    mbs: MbsData
931
932
        instance of MbsData from where results come from
    """
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950

    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 =