mbs_dirdyn.py 41 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
# Import MBsysPy class
from .mbs_results import MbsResult

35

36
# =============================================================================
37
# Global parameter of the current module
38
# =============================================================================
39
__DEBUG__ = False
40
41
42
__MODULE_DIR__ = os.path.dirname(os.path.abspath(__file__))


43
# =============================================================================
44
# Defining Python MbsDirdyn class
45
# =============================================================================
46
47

class MbsDirdyn(object):
48
    """
49
50
    Class of the direct dynamic module.

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

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

76
    """
77

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

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

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

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

107
108
        self.mbs = mbs

109
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
110
            mbs_msg("DEBUG>>  MbsDirdyn created.")
111

112
        # Path to user function used by partitionning module
113
        self.user_path = self.mbs.user_path
114
115
        if user_path is not None:
            project_path = bytes_to_str(self.mbs.project_path)
116
117
118
            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
119
120
121
                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 + '".')
122
123
124
125
            else:
                self.user_path = user_path
        # Path to user function used by partitionning modue
        self.symbolic_path = self.mbs.symbolic_path
126
        if symbolic_path is not None:
127
            project_path = bytes_to_str(self.mbs.project_path)
128
129
130
            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
131
132
133
                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 + '".')
134
135
            else:
                self.symbolic_path = symbolic_path
136

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

147
148
        # Storing Results
        self.results = MbsResult(self.mbs)
Louis Beauloye's avatar
Louis Beauloye committed
149
        self.store_results = True
150

151
        # Exposing some memory
152
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
153
            mbs_msg("DEBUG>>  Exposing MbsDirdyn fields.")
154

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

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

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

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

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

191
192
        Results are stored in the field 'results' if the options 'store_results'
        is set to True.
193

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

204
            self.mbs.__load_user_fct__(__MODULE_DIR__, self.user_fun_list, self.user_path)
Louis Beauloye's avatar
Louis Beauloye committed
205
            self.mbs.__assign_user_fct__(self.user_fun_list, self)
206

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

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

216
        self.set_options(**kwargs)
217

218
        error2 = 0
219
        if not self.store_results:
220
            error = libmodules.mbs_run_dirdyn(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
221
        else:
222
223
224
            # Save2file forced to 1 because if buffers don't have the complete,
            # they are loaded from files
            self.set_options(save2file=1)
225
226
            error = libmodules.mbs_dirdyn_init(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
            if (error >= 0):
227
228
229
230
231
232
                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,
233
234
                                                      self.mbs.mbs_data_ptr,
                                                      self.mbs_dirdyn_ptr
235
                                                      )
236

237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
            if error >= 0:
                if self.get_options("save2file"):
                    # Results(buffer) memory is kept BUT FILES WILL BE WRITTEN LATER.
                    results_loaded = self.results.load_results_from_buffer(self.mbs_dirdyn_ptr,
                                                                           self.get_options("t0"),
                                                                           self.get_options("resfilename"))

                    # If failed to load from buffer, save the user output (and vector) name
                    if not results_loaded:
                        results_filename = bytes_to_str(ctypes.string_at(self.mbs_dirdyn_ptr.contents.buffers[0].contents.filename))
                        # Check if user vector output have been defined
                        user_output_vector_filenames = []
                        nb_user_output_vector = libutilities.get_output_vector_nb()
                        first_buffer_id = self.buffer_nb - nb_user_output_vector
                        for i in range(nb_user_output_vector):
                            vector_name = bytes_to_str(self.mbs_dirdyn_ptr.contents.buffers[first_buffer_id + i].contents.filename)
                            user_output_vector_filenames.append(os.path.basename(vector_name))

                        # Check if user auto output have been used
                        user_output_filenames = []
257
258
259
                        nbOutput = self.mbs_dirdyn_ptr.contents.user_buffer.contents.nx
                        for i in range(nbOutput):
                            name = bytes_to_str(self.mbs_dirdyn_ptr.contents.user_buffer.contents.names[i])
260
                            user_output_filenames.append(self.get_options("resfilename") + '_' + name + '.res')
261

262
263
                # Finalize the simulation (includes writing results files to disk)
                error2 = libmodules.mbs_dirdyn_finish(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
264

265
266
                # Load results from file if required
                if error2 >= 0 and self.get_options("save2file") and not results_loaded:
267
268
                    mbs_msg("The beginning of the integration is not available in the buffer.\n"
                            "The complete results are loaded from files.\n")
269
                    self.results.load_results_from_file(results_filename,
270
271
                                                        user_output=user_output_filenames,
                                                        user_vector=user_output_vector_filenames)
272

Louis Beauloye's avatar
Louis Beauloye committed
273
        # Unassign user functions
274
        if self.mbs.opt_load_c < 2:
Louis Beauloye's avatar
Louis Beauloye committed
275
            self.mbs.__unassign_user_fct__(self.user_fun_list)
276

Louis Beauloye's avatar
Louis Beauloye committed
277
        # Unassing required symbolic functions
278
        if self.mbs.opt_load_c < 1:
Louis Beauloye's avatar
Louis Beauloye committed
279
            self.mbs.__unassign_symb_fct__(self.symb_fun_list)
280

281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
        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"
298
                    "    - The outputs folders does not exists (by default 'resultsR' and 'animationR');\n"
299
300
301
302
303
304
305
306
                    "\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")
307
            raise RuntimeError("MbsDirdyn.run() failed, read previous messages.")
308
309
310
311
312
313
314
315
316
        if error2 < 0:
            mbs_msg("\n--------------------------------------------------\n"
                    "READ CAREFULLY !!!\n"
                    "--------------------------------------------------\n\n"
                    "An error occurs during direct dynamic module finalization process.\n"
                    "It means that one or more results files (or animation file) was not saved to disk.\n"
                    "The messages above give deeper informations on what went wrong.\n"
                    "Check the existence of outputs folders, writing wrigth and free space.\n")
            raise RuntimeError("MbsDirdyn.run() failed during mbs_dirdyn_finish, read previous messages.")
317

Louis Beauloye's avatar
Louis Beauloye committed
318
        return self.results
319

320
    def set_user_fct_from_file(self, function_name, user_path, user_file):
Louis Beauloye's avatar
Louis Beauloye committed
321
        """Load a user function from a file chosen by the user instead of the default one in the userfctR folder.
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338

        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
339
        """Load a user function chosen by the user instead of the default one in the userfctR folder.
340
341
342
343
344
345
346
347
348
349
350
351
352

        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)

353
354
    def set_options(self, **kwargs):
        """
355
        Set the specified options for Dirdyn module.
356

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

            If set at 1, the direct dynamics(constraint solving, computation of
480
            the generalized acceleration) will be done at the beginning of each
481
            time step of integration. The output files will be exact, but the
482
            computation will be a little slower.
483
            If set at 0, the output file will contains the value(constraint
484
485
486
487
488
            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
489
490
491
492
493
494
495
496
497
        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
498
        flag_waypoint: int
499
            If set to 1, the integrator will be forced to give a solution at the
500
            specified time interval('delta_t_wp'). If set to 0, the solution
501
502
            will be given at every computed time-step.
            default is 0
503
504
505
506
        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
507
508
            all the timesteps and the waypoints.
            default is 0
509
510
        delta_t_wp: float
            Time interval between two waypoints [s], unused if 'flag_waypoint'
511
512
            is set to 0.
            default is 1.0e-3
513
        nmax: int
514
515
            maximal number of steps for adaptative stepsize integrators.
            default is 1e9
516
        toler: float
517
518
            mixed error tolerances for some adaptative stepsize integrators.
            default is 1.0e-3
519
        rtoler: float
520
521
            relative error tolerances for some adaptative stepsize integrators.
            default is 1.0e-3
522
        atoler: float
523
524
            absolute error tolerances for some adaptative stepsize integrators.
            default is 1.0e-6
525
        dt_max: float
526
527
            maximal time step [s] for some adaptative stepsize integrators.
            default is 1.0e-3
528
529
        n_freeze: int
            number of time step when the jacobian is freezed(computed once at
530
531
            the start of the n_freeze time steps) for implicit integrators.
            default is 0
532
533
        show_failed_closure: int
            If set to 1, an animation of the Newton-Raphson procedure is generated
534
535
            if the closure fails.
            default is 0
536
        store_results: int
537
538
            If set to 1, a copy of the results is done.
            default is 1
539
        """
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
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

        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
590
                else:
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
                    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
609
                elif value == "AlphaM":
610
                    return 8
611
612
                elif value == "Custom":
                    return 9
613
                else:
614
                    raise ValueError
615
            else:
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
                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
647
            else:
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
                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'},
665
                   'flag_oneshot': {'convert': int, 'c_name': 'flag_oneshot'},
666
667
668
669
670
671
672
                   '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'},
673
674
675
                   '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'},
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
                   '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
710
                    raise TypeError("{:} is {:}, can not be casted from {:}.".format(key, options[key]['convert'], type(value))).with_traceback(err.__traceback__) from None
711
712
713

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

            except TypeError as err:
                # if wrong type in integrator or compute_QC
Louis Beauloye's avatar
Louis Beauloye committed
718
719
720
721
                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
722

723
724
    def get_options(self, *args):
        """
725
726
        Get the specified options for Dirdyn module.

727
728
        Parameters
        ----------
729
        The different options specifed in the documentation of 'set_options()'
730

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

843
844
845
846
        if len(options) == 0:
            return
        if len(options) == 1:
            return options[0]
847

848
        return tuple(options)
849
850

    # =========================================================================
851
    # Defining properties
852
    # =========================================================================
853
854
    @property
    def tsim(self):
855
        """Access to `tsim` attribute (read-only)."""
856
        return self.mbs_dirdyn_ptr.contents.tsim
857

858
859
    @property
    def dt(self):
860
        """Access to `dt` attribute (read-only)."""
861
        return self.mbs_dirdyn_ptr.contents.dt
862

863
864
865
866
    @property
    def buffer_nb(self):
        """Access to `bufferNb` attribute (read-only)."""
        return self.mbs_dirdyn_ptr.contents.bufferNb