mbs_dirdyn.py 45 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
14
# MBsysC version: 1.13.0
# Author: Robotran Team
# (c) Universite catholique de Louvain, 2019
15
16

import os
17
import ctypes
18
19
20

import numpy as np

21
22
23
24
25
# importing MbsysPy functions
from ..mbs_utilities import bytes_to_str
from ..mbs_utilities import str_to_bytes
from ..mbs_utilities import mbs_warning
from ..mbs_utilities import mbs_error
Louis Beauloye's avatar
Louis Beauloye committed
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
214
        self.set_options(**kwargs)

215
        if not self.store_results:
216
217
            libmodules.mbs_run_dirdyn(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
        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
            libmodules.mbs_dirdyn_init(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
            libmodules.mbs_dirdyn_loop(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
223
224

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

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

272
273
274
                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
275
                    for i in range(nbOutput):
276
                        user_out = np.copy(np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.user_buffer.contents.X[i], (1, size)))
277
                        name = bytes_to_str(self.mbs_dirdyn_ptr.contents.user_buffer.contents.names[i])
278
                        self.results.outputs[name] = user_out[0]
279

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

Louis Beauloye's avatar
Louis Beauloye committed
282
        # Unassign user functions
283
        if self.mbs.opt_load_c < 2:
Louis Beauloye's avatar
Louis Beauloye committed
284
            self.mbs.__unassign_user_fct__(self.user_fun_list)
285

Louis Beauloye's avatar
Louis Beauloye committed
286
        # Unassing required symbolic functions
287
        if self.mbs.opt_load_c < 1:
Louis Beauloye's avatar
Louis Beauloye committed
288
            self.mbs.__unassign_symb_fct__(self.symb_fun_list)
289

Louis Beauloye's avatar
Louis Beauloye committed
290
        return self.results
291

292
    def set_user_fct_from_file(self, function_name, user_path, user_file):
Louis Beauloye's avatar
Louis Beauloye committed
293
        """Load a user function from a file chosen by the user instead of the default one in the userfctR folder.
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310

        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
311
        """Load a user function chosen by the user instead of the default one in the userfctR folder.
312
313
314
315
316
317
318
319
320
321
322
323
324

        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)

325
326
    def set_options(self, **kwargs):
        """
327
        Set the specified options for Dirdyn module.
328

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

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

        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
547
                else:
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
                    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
566
                elif value == "AlphaM":
567
                    return 8
568
569
                elif value == "Custom":
                    return 9
570
                else:
571
                    raise ValueError
572
            else:
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
607
608
609
610
611
612
613
614
615
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
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
                raise TypeError

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

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

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

            Returns
            -------
            None.

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

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

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

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

            except TypeError as err:
                # if wrong type in integrator or compute_QC
Louis Beauloye's avatar
Louis Beauloye committed
671
672
673
674
675
                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

676
677
    def get_options(self, *args):
        """
678
679
        Get the specified options for Dirdyn module.

680
681
        Parameters
        ----------
682
        The different options specifed in the documentation of 'set_options()'
683

684
685
        Returns
        -------
686
        The value of the different options specifed in the documentation of 'set_options()'
687
688
689
690
691
692
693
694
695
696
697
698
        """
        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":
699
                options.append(bytes_to_str(ctypes.string_at(self.mbs_dirdyn_ptr.contents.options.contents.resfilename)))
700
            elif key == "respath":
701
                options.append(bytes_to_str(ctypes.string_at(self.mbs_dirdyn_ptr.contents.options.contents.respath)))
702
            elif key == "animpath":
703
                options.append(bytes_to_str(ctypes.string_at(self.mbs_dirdyn_ptr.contents.options.contents.animpath)))
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
            elif key == "save_anim":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.save_anim)
            elif key == "save_visu":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.save_visu)
            elif key == "framerate":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.framerate)
            elif key == "saveperiod":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.saveperiod)
            elif key == "max_save_user":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.max_save_user)
            elif key == "buffersize":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.buffersize)
            elif key == "realtime":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.realtime)
            elif key == "accelred":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.accelred)
            elif key == "flag_compute_Qc":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.flag_compute_Qc)
Louis Beauloye's avatar
Louis Beauloye committed
722
723
            elif key == "compute_all_uxd":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.compute_all_uxd)
724
            elif key == "compute_Qc":
725
                compute_Qc_py = np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.options.contents.compute_Qc, (self.mbs.mbs_data_ptr.contents.njoint + 1,))
726
727
728
729
730
731
732
733
734
                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:
735
                    options.append("EulerEx")
736
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 4:
737
                    options.append("Eulaire")
738
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 5:
739
                    options.append("EulerIm")
740
741
742
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 6:
                    options.append("Bader")
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 7:
743
                    options.append("WMethods")
744
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 8:
745
746
                    options.append("AlphaM")
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 9:
747
                    options.append("Custom")
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
            elif key == "verbose":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.verbose)
            elif key == "flag_stop_stiff":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.flag_stop_stiff)
            elif key == "flag_precise_dynamics":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.flag_precise_dynamics)
            elif key == "flag_waypoint":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.flag_waypoint)
            elif key == "flag_solout_wp":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.flag_solout_wp)
            elif key == "delta_t_wp":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.delta_t_wp)
            elif key == "nmax":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.nmax)
            elif key == "toler":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.toler)
            elif key == "rtoler":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.rtoler)
            elif key == "atoler":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.atoler)
            elif key == "dt_max":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.dt_max)
            elif key == "n_freeze":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.n_freeze)
Louis Beauloye's avatar
Louis Beauloye committed
772
773
            elif key == "show_failed_closure":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.show_failed_closure)
Louis Beauloye's avatar
Louis Beauloye committed
774
775
            elif key == "store_results":
                options.append(self.store_results)
776
            else:
Louis Beauloye's avatar
Louis Beauloye committed
777
                mbs_msg(">>DIRDYN>>  The option " + key + " is not defined in this module")
778

779
780
781
782
        if len(options) == 0:
            return
        if len(options) == 1:
            return options[0]
783

784
        return tuple(options)
785
786

    # =========================================================================
787
    # Defining properties
788
    # =========================================================================
789
790
    @property
    def tsim(self):
791
        """Access to `tsim` attribute (read-only)."""
792
        return self.mbs_dirdyn_ptr.contents.tsim
793

794
795
    @property
    def dt(self):
796
        """Access to `dt` attribute (read-only)."""
797
        return self.mbs_dirdyn_ptr.contents.dt
798
799


800
class MbsResult(object):
801
802
    """
    Class containing results.
803

804
    The user-specified vector are not available.
805

806
807
    Attributes
    ----------
808
809
    q: ndarray
        Numpy array containing the current values of the generalized
810
        coordinates.
811
812
    qd: ndarray
        Numpy array containing the current values of the generalized
813
        velocities.
814
815
    qdd: ndarray
        Numpy array containing the current values of the generalized
816
        accelerations.
817
    Qq: ndarray
818
        Numpy array containing the values of the joint forces.
819
820
    Qc: ndarray
        Numpy array containing the value of joint force introduced in driven
821
        joints to respect the user function
822
823
824
    qa: ndarray of int
        Numpy array of integers containing the indices of actuated
        articulations(only for inverse dynamic). Those articulations are
825
        controlled by an actuator.
826
    t: ndarray
827
        Numpy array containing the values of the time vector
828
    ux: ndarray
829
        Numpy array containing the values of the user variables.
830
831
    uxd: ndarray
        Numpy array containing the values of the time derivative of the user
832
        variables.
833
834
    Fl: ndarray
        Numpy array containing the current values of the forces between of
835
        the points of a link.
836
837
    Z: ndarray
        Numpy array containing the current values of the distances between of
838
        the points of a link.
839
840
    Zd: ndarray
        Numpy array containing the current values of the speed(spreading)
841
        between of the points of a link.
842
    outputs: dict
843
        Dict containing the names and the values of the user outputs
844
    mbs: MbsData
845
846
        instance of MbsData from where results come from
    """
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864

    def __init__(self, mbs):
        """Initialize the fields to empty list or dict."""
        self.q = []
        self.qd = []
        self.qdd = []
        self.Fl = []
        self.Z = []
        self.Zd = []
        self.Qq = []
        self.Qa = []
        self.t = []
        self.ux = []
        self.uxd = []
        self.Qc = []
        self.outputs = {}
        self.mbs = mbs

865
    def load_results_from_file(self, filename, result_path="resultsR", module=0):
866
        """
867
        Load the results from the files into a MbsResult instance.
868

869
        This function is called if the buffers don not contain the full simulation.
870

871
872
        Parameters
        ----------
873
        filename: str
874
            The resfilename containing the results to load. The required suffix
875
876
           (ie. "_q.res") will be added automatically
        result_path: str, optional
877
878
            The relative path of the result folder from the project folder.
            default is "resultsR"
879
880
881
        module: int
            The module in which this function is called. In some modules, some
            results files don't exist. Module ids corresponds to the
882
883
            'MbsData.process' value. If set to 0 we try to load all files
            default is 0
884
        """
885
        project_path = self.mbs.project_path
886
        baseFileName = os.path.basename(filename)
887
        baseFileName = baseFileName[:-6]
888
        result_path = os.path.join(project_path, result_path)
889
890
        # Error handeling
        if not os.path.isdir(result_path):
891
            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
892
                mbs_msg('DEBUG>>  The result directory does not exist: "' + result_path + '"')
893

894
        # Generalized coordinates
895
        CurFile = baseFileName + '_q.res'
896
        path = os.path.abspath(os.path.join(result_path, CurFile))
897
        if(os.path.isfile(path)):
898
899
            self.q = np.loadtxt(path)
            if np.array(self.q).ndim > 1:
900
                self.t = self.q[:, 0]
901
902
            else:
                self.t = self.q[0]
903
        else:
904
            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
905
                mbs_msg("DEBUG>>  file '" + CurFile + "' not found in folder '" + os.path.dirname(path))
906

907
        # Generalized velocities
908
        CurFile = baseFileName + '_qd.res'
909
        path = os.path.abspath(os.path.join(result_path, CurFile))
910
        if(os.path.isfile(path)):
911
            self.qd = np.loadtxt(path)
912
        else:
913
            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
914
                mbs_msg("DEBUG>>  file '" + CurFile + "' not found in folder '" + os.path.dirname(path))
915
916
917

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