mbs_dirdyn.py 53.4 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
17

import os
import imp
18
import ctypes
19
20
21

import numpy as np

22
23
24
25
26
# 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
27
28

# importing libraries
29
from .._mbsysc_loader.loadlibs import libmodules
30

31
32
33
# importing utilities function
from ..mbs_utilities import callback_undefined

34
# importing wrapping function
35
36
37
38
39
40
41
42
43
from .._mbsysc_loader.callback import user_cons_hJ_wrap
from .._mbsysc_loader.callback import user_cons_jdqd_wrap
from .._mbsysc_loader.callback import user_dirdyn_init_wrap
from .._mbsysc_loader.callback import user_dirdyn_loop_wrap
from .._mbsysc_loader.callback import user_dirdyn_finish_wrap

from .._mbsysc_loader.callback import mbs_cons_hJ_wrap
from .._mbsysc_loader.callback import mbs_cons_jdqd_wrap
from .._mbsysc_loader.callback import mbs_dirdyna_wrap
44
45


46
# =============================================================================
47
# Global parameter of the current module
48
# =============================================================================
49
__DEBUG__ = False
50
51
52
__MODULE_DIR__ = os.path.dirname(os.path.abspath(__file__))


53
# =============================================================================
54
# Defining Python MbsDirdyn class
55
# =============================================================================
56
57

class MbsDirdyn(object):
58
    """
59
60
    Class of the direct dynamic module.

61
62
    Attributes
    ----------
63
    dt: double
64
        Current integration step size.
65
    mbs: MbsData
66
        Instance of MbsData related to the analysis.
67
    results: MbsResult
68
        Instance of MbsResult containing the results of the direct dynamics analysis.
69
70
    symbolic_path: str
        Path to the folder containing the symbolic functions(python modules)
71
        to be loaded.
72
    tsim: double
73
        Current simulation time.
74
75
76
    user_path: str
        Path to the folder containing the user functions(python modules) to be loaded.

77
78
79
80
81
82
    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")
83
    (5.0, 10.0)
84
    >>> results = mbs_dirdyn.run()
85

86
    """
87

88
    def __init__(self, mbs, user_path=None, symbolic_path=None):
89
90
91
92
93
        """
        Create an instance of the MbsDirdyn class for the provided MbsData instance.

        Parameters
        ----------
94
        mbs: MbsData
95
            Instance of MbsData related to the analysis.
96
97
98
        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'.
99
            default is None
100
101
102
        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'.
103
            default is None
104

105
106
        Returns
        -------
107
        MbsDirdyn: self
108
109
            A MbsDirdyn instance.
        """
110
111
112
        if __DEBUG__:
            print("DEBUG>>  Creating MbsDirdyn struct for " + mbs.mbs_name + "' MBS.")

113
        self.mbs_dirdyn_ptr = libmodules.mbs_new_dirdyn(mbs.mbs_data_ptr)
114
115
116
        if __DEBUG__:
            print("DEBUG>>  MbsDirdyn structure loaded")

117
118
        self.mbs = mbs

119
120
121
        if __DEBUG__:
            print("DEBUG>>  MbsDirdyn created.")

122
        # Path to user function used by partitionning module
123
        self.user_path = self.mbs.user_path
124
125
        if user_path is not None:
            project_path = bytes_to_str(self.mbs.project_path)
126
127
128
            user_path = os.path.join(project_path, user_path)
            # Error handeling
            if not os.path.isdir(user_path):
129
130
131
                print('The user function directory for direct dynamic module does not exist: "' + user_path + '"')
                print('The current root folder is: "' + os.getcwd() + '"')
                print('The following directory is used instead: "' + self.user_path + '".')
132
133
134
135
            else:
                self.user_path = user_path
        # Path to user function used by partitionning modue
        self.symbolic_path = self.mbs.symbolic_path
136
        if symbolic_path is not None:
137
            project_path = bytes_to_str(self.mbs.project_path)
138
139
140
            symbolic_path = os.path.join(project_path, symbolic_path)
            # Error handeling
            if not os.path.isdir(symbolic_path):
141
142
143
                print('Thesymbolic function directory for direct dynamic module does not exist: "' + symbolic_path + '"')
                print('The current root folder is: "' + os.getcwd() + '"')
                print('The following directory is used instead: "' + self.symbolic_path + '".')
144
145
            else:
                self.symbolic_path = symbolic_path
146

147
        # dictionary containing the pointers to avoid garbage collecting
148
149
        self.ptrs_to_user_fcts = dict()
        self.ptrs_to_symb_fcts = dict()
150

151
152
153
154
155
156
157
158
159
160
        # Storing project function pointer
        self.user_cons_hJ = None
        self.user_cons_jdqd = None
        self.user_dirdyn_init = None
        self.user_dirdyn_loop = None
        self.user_dirdyn_finish = None
        self.mbs_cons_hJ = None
        self.mbs_cons_jdqd = None
        self.mbs_invdyna = None
        self.mbs_dirdyna = None
161

162
163
        # Storing Results
        self.results = MbsResult(self.mbs)
Louis Beauloye's avatar
Louis Beauloye committed
164
        self.store_results = True
165

166
        # Exposing some memory
167
168
169
        if __DEBUG__:
            print("DEBUG>>  Exposing MbsDirdyn fields.")

170
171
        # Constraints
        self._h = self._Jac = self._jdqd = None
172
173
174
175
        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,))
176
        self._huserc = self._Juserc = self._jdqduserc = None
177
178
179
180
        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,))
181
        # Fields from equation of motion
182
183
184
185
        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,))

186
        # Loading user function
187
        if self.mbs.opt_load_c < 2:
188
189
190
            if __DEBUG__:
                print("DEBUG>>  Loading user functions")

191
            self.__load_user_fct__(self.user_path)
192
        # Loading symbolic function
193
        if self.mbs.opt_load_c < 1:
194
195
196
            if __DEBUG__:
                print("DEBUG>>  Loading symbolic functions")

197
            self.__load_symbolic_fct__(self.symbolic_path)
198
199

    def __str__(self):
200
201
202
203
        """Return str(self)."""
        if __DEBUG__:
            print("DEBUG>>  start of __str")

204
        return "MbsDirdyn instance has nothing to be printed from C library!"
205

206
    def __load_user_fct__(self, user_path):
207
        """
208
        Load user function where some args depend on MbsDirdyn module.
209
210
211
212

        Load the user functions in which some of the arguments are
        dependent of MbsDirdyn module instance. The functions will be
        assigned to the MbsData instance when the 'run' functions is called
213
        and unassigned at the end.
214
215
216
217
218
219
220

        The loader user functions are:
         - user_cons_hJ(from user_cons_hJ.py)
         - user_cons_jdqd(from user_cons_jdqd.py)
         - user_dirdyn_init(from user_dirdyn.py)
         - user_dirdyn_loop(from user_dirdyn.py)
         - user_dirdyn_finish(from user_dirdyn.py)
221
222
223

        Parameters
        ----------
224
        user_path: str
225
            The path to the folder containing the user functions.
226
        """
227
228
        template_path = os.path.join(__MODULE_DIR__, '../templates/user')

229
        # cons_hJ
230
        user_file = "user_cons_hJ.py"
231
        path = os.path.abspath(os.path.join(user_path, user_file))
232
        if not os.path.isfile(path):
233
234
235
            if __DEBUG__:
                print("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))

236
237
238
            path = os.path.abspath(os.path.join(template_path, user_file))
        module = imp.load_source(user_file[:-3], path)
        self.user_cons_hJ = module.user_cons_hJ
239

240
        # cons_jdqd
241
        user_file = "user_cons_jdqd.py"
242
        path = os.path.abspath(os.path.join(user_path, user_file))
243
        if not os.path.isfile(path):
244
245
246
            if __DEBUG__:
                print("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))

247
248
249
            path = os.path.abspath(os.path.join(template_path, user_file))
        module = imp.load_source(user_file[:-3], path)
        self.user_cons_jdqd = module.user_cons_jdqd
250

251
252
        # user_dirdyn
        user_file = "user_dirdyn.py"
253
        path = os.path.abspath(os.path.join(user_path, user_file))
254
        if not os.path.isfile(path):
255
            print("file '" + user_file + "' not found in folder '" + os.path.dirname(path))
256
257
            path = os.path.abspath(os.path.join(template_path, user_file))
        else:
258
259
260
            if __DEBUG__:
                print("DEBUG>>  loading file '" + user_file + "' in folder '" + os.path.dirname(path))

261
        module = imp.load_source(user_file[:-3], path)
262
263
        self.user_dirdyn_init = module.user_dirdyn_init
        self.user_dirdyn_loop = module.user_dirdyn_loop
264
        self.user_dirdyn_finish = module.user_dirdyn_finish
265
        return
266

267
    def __load_symbolic_fct__(self, symbolic_path):
268
        """
269
        Load symbolic function where some args depend on MbsDirdyn module.
270
271
272
273

        Load the symb functions in which some of the arguments are
        dependent of MbsDirdyn module instance. The functions will be
        assigned to the MbsData instance when the 'run' functions is called
274
        and unassigned at the end.
275
276
277
278
279
280

        The loader user functions are:
         - cons_hJ(from mbs_cons_hJ_MBSNAME.py)
         - cons_jdqd(from mbs_cons_jdqd_MBSNAME.py)
         - dirdyna(from mbs_dirdyna_MBSNAME.py)
         - invdyna(from mbs_invdyna_MBSNAME.py)
281
282
283

        Parameters
        ----------
284
        symbolic_path: str
285
            The path to the folder containing the symbolic functions.
286
287
        """
        mbs_name = self.mbs.mbs_name
288
        template_path = os.path.join(__MODULE_DIR__, '../templates/symbolic')
289
        # mbs_cons_hJ
290
        symb_file = "mbs_cons_hJ_" + mbs_name + ".py"
291
        path = os.path.abspath(os.path.join(symbolic_path, symb_file))
292
        if not os.path.isfile(path):
293
294
295
            if __DEBUG__:
                print("DEBUG>>  file '" + symb_file + "' not found in folder '" + os.path.dirname(path))

296
297
298
299
            symb_file = "mbs_cons_hJ_PRJ.py"
            path = os.path.abspath(os.path.join(template_path, symb_file))
        module = imp.load_source(symb_file[:-3], path)
        self.mbs_cons_hJ = module.cons_hJ
300

301
        # mbs_cons_jdqd
302
        symb_file = "mbs_cons_jdqd_" + mbs_name + ".py"
303
        path = os.path.abspath(os.path.join(symbolic_path, symb_file))
304
        if not os.path.isfile(path):
305
306
307
            if __DEBUG__:
                print("DEBUG>>  file '" + symb_file + "' not found in folder '" + os.path.dirname(path))

308
309
310
311
            symb_file = "mbs_cons_jdqd_PRJ.py"
            path = os.path.abspath(os.path.join(template_path, symb_file))
        module = imp.load_source(symb_file[:-3], path)
        self.mbs_cons_jdqd = module.cons_jdqd
312

313
        # dirdyna
314
        symb_file = "mbs_dirdyna_" + mbs_name + ".py"
315
        path = os.path.abspath(os.path.join(symbolic_path, symb_file))
316
        if not os.path.isfile(path):
317
318
319
            if __DEBUG__:
                print("DEBUG>>  file '" + symb_file + "' not found in folder '" + os.path.dirname(path))

320
321
322
323
            symb_file = "mbs_dirdyna_PRJ.py"
            path = os.path.abspath(os.path.join(template_path, symb_file))
        module = imp.load_source(symb_file[:-3], path)
        self.mbs_dirdyna = module.dirdyna
324

325
        return
326

327
    def __assign_user_fct__(self):
328
329
330
331
332
333
        """
        Assign all user functions if some args are in MbsDirdyn instance.

        Assign and wrap python user function where some args depend on MbsDirdyn
        module. Store the functions in the MbsData instance and assign the
        pointer of functions in the C structure.
334
335
        """
        # cons_hJ
336
        self.ptrs_to_user_fcts["user_cons_hJ"] = user_cons_hJ_wrap(lambda h, Jac, mbs, tsim: self.user_cons_hJ(self._huserc, self._Juserc, self.mbs, tsim))
337
        self.mbs.user_cons_hJ = self.user_cons_hJ
338
        self.mbs.mbs_data_ptr.contents.user_cons_hJ = self.ptrs_to_user_fcts["user_cons_hJ"]
339

340
        # cons_jdqd
341
        self.ptrs_to_user_fcts["user_cons_jdqd"] = user_cons_jdqd_wrap(lambda jdqd, mbs, tsim: self.user_cons_jdqd(self._jdqduserc, self.mbs))
342
        self.mbs.user_cons_jdqd = self.user_cons_jdqd
343
        self.mbs.mbs_data_ptr.contents.user_cons_jdqd = self.ptrs_to_user_fcts["user_cons_jdqd"]
344
345

        # user_dirdyn
346
347
348
        self.ptrs_to_user_fcts["user_dirdyn_init"] = user_dirdyn_init_wrap(lambda mbs, dd: self.mbs.user_dirdyn_init(self.mbs, self))
        self.ptrs_to_user_fcts["user_dirdyn_loop"] = user_dirdyn_loop_wrap(lambda mbs, dd: self.mbs.user_dirdyn_loop(self.mbs, self))
        self.ptrs_to_user_fcts["user_dirdyn_finish"] = user_dirdyn_finish_wrap(lambda mbs, dd: self.mbs.user_dirdyn_finish(self.mbs, self))
349
        self.mbs.user_dirdyn_init = self.user_dirdyn_init
350
        self.mbs.mbs_data_ptr.contents.user_dirdyn_init = self.ptrs_to_user_fcts["user_dirdyn_init"]
351
        self.mbs.user_dirdyn_loop = self.user_dirdyn_loop
352
        self.mbs.mbs_data_ptr.contents.user_dirdyn_loop = self.ptrs_to_user_fcts["user_dirdyn_loop"]
353
        self.mbs.user_dirdyn_finish = self.user_dirdyn_finish
354
        self.mbs.mbs_data_ptr.contents.user_dirdyn_finish = self.ptrs_to_user_fcts["user_dirdyn_finish"]
355
        return
356

357
    def __assign_symbolic_fct__(self):
358
359
360
361
362
        """
        Assign all symbolic functions if some args are in MbsDirdyn instance.

        Assign and wrap python symbolic function where some args depend
        on MbsDirdyn module. Store the functions in the MbsData instance
363
        and assign the pointer of functions in the C structure
364
365
        """
        # mbs_cons_hJ
366
        self.ptrs_to_symb_fcts["mbs_cons_hJ"] = mbs_cons_hJ_wrap(lambda h, Jac, mbs, tsim: self.mbs_cons_hJ(self._h, self._Jac, self.mbs))
367
        self.mbs.mbs_cons_hJ = self.mbs_cons_hJ
368
        self.mbs.mbs_data_ptr.contents.mbs_cons_hJ = self.ptrs_to_symb_fcts["mbs_cons_hJ"]
369

370
        # mbs_cons_jdqd
371
        self.ptrs_to_symb_fcts["mbs_cons_jdqd"] = mbs_cons_jdqd_wrap(lambda jdqd, mbs, tsim: self.mbs_cons_jdqd(self._jdqd, self.mbs))
372
        self.mbs.mbs_cons_jdqd = self.mbs_cons_jdqd
373
        self.mbs.mbs_data_ptr.contents.mbs_cons_jdqd = self.ptrs_to_symb_fcts["mbs_cons_jdqd"]
374

375
        # dirdyna
376
        self.ptrs_to_symb_fcts["mbs_dirdyna"] = mbs_dirdyna_wrap(lambda M, c, mbs, tsim: self.mbs_dirdyna(self._M, self._c, self.mbs, tsim))
377
        self.mbs.mbs_dirdyna = self.mbs_dirdyna
378
        self.mbs.mbs_data_ptr.contents.mbs_dirdyna = self.ptrs_to_symb_fcts["mbs_dirdyna"]
379

380
        return
381
382

    # Callback function for function with advanced arguments
383
    def __callback_mbs_cons_hJ(self, fun, h, Jac):
384
385
386
387
388
389
390
        if __DEBUG__:
            print("DEBUG>>  callback_mbs_cons_hJ")

        __h = np.ctypeslib.as_array(h, (self.mbs.Ncons + 1,))
        __Jac = np.ctypeslib.as_array(Jac[0], (self.mbs.Ncons + 1, self.mbs.njoint + 1))
        fun(__h, __Jac, self.mbs)

391
    def __unassign_user_fct__(self):
392
        """Unassign user function where some args depend on MbsDirdyn module."""
393
        self.ptrs_to_user_fcts = {}
394

395
        self.mbs.user_cons_hJ = None
396
        self.mbs.mbs_data_ptr.contents.user_cons_hJ = user_cons_hJ_wrap(lambda h, Jac, mbs, tsim: callback_undefined("user_cons_hJ"))
397
        self.mbs.user_cons_jdqd = None
398
        self.mbs.mbs_data_ptr.contents.user_cons_jdqd = user_cons_jdqd_wrap(lambda h, Jac, mbs, tsim: callback_undefined("user_cons_jdqd"))
399
        self.mbs.user_dirdyn_init = None
400
        self.mbs.mbs_data_ptr.contents.user_dirdyn_init = user_dirdyn_init_wrap(lambda mbs, dd: callback_undefined("user_dirdyn_init"))
401
        self.mbs.user_dirdyn_loop = None
402
        self.mbs.mbs_data_ptr.contents.user_dirdyn_loop = user_dirdyn_loop_wrap(lambda mbs, dd: callback_undefined("user_dirdyn_loop"))
403
        self.mbs.user_dirdyn_finish = None
404
405
        self.mbs.mbs_data_ptr.contents.user_dirdyn_finish = user_dirdyn_finish_wrap(lambda mbs, dd: callback_undefined("user_dirdyn_finish"))

406
    def __unassign_symbolic_fct__(self):
407
        """Unassign symbolic function where some args depend on MbsDirdyn module."""
408
        self.ptrs_to_symb_fcts = {}
409

410
        self.mbs.mbs_cons_hJ = None
411
        self.mbs.mbs_data_ptr.contents.mbs_cons_hJ = mbs_cons_hJ_wrap(lambda h, Jac, mbs, tsim: callback_undefined("mbs_cons_hJ"))
412
        self.mbs.mbs_cons_jdqd = None
413
        self.mbs.mbs_data_ptr.contents.mbs_cons_jdqd = mbs_cons_jdqd_wrap(lambda jdqd, mbs, tsim: callback_undefined("mbs_cons_jdqd"))
414
        self.mbs.mbs_dirdyna = None
415
416
        self.mbs.mbs_data_ptr.contents.mbs_dirdyna = mbs_dirdyna_wrap(lambda M, c, mbs, tsim: callback_undefined("mbs_dirdyna"))

417
    def __del__(self):
418
        """Delete the object by freeing the C-related memory."""
419
        libmodules.mbs_delete_dirdyn(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
420
421
422
        if __DEBUG__:
            print("DEBUG>>  MbsDirdyn pointer deleted.")

423
    def run(self):
Louis Beauloye's avatar
Louis Beauloye committed
424
        """
425
426
427
428
429
        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()'.

430
431
        Results are stored in the field 'results' if the options 'store_results'
        is set to True.
432

433
434
        Returns
        -------
435
        self.results: MbsResult
436
            The MbsResults containing the results of the analysis.
437
        """
438
        # Assing required user functions
439
440
441
        if self.mbs.opt_load_c < 2:
            self.__assign_user_fct__()
            self.mbs.__assign_user_fct__()
442

443
        # Assing required symbolic functions
444
445
446
        if self.mbs.opt_load_c < 1:
            self.__assign_symbolic_fct__()
            self.mbs.__assign_symb_fct__()
447
448

        if not self.store_results:
449
450
            libmodules.mbs_run_dirdyn(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
        else:
451
452
453
            # Save2file forced to 1 because if buffers don't have the complete,
            # they are loaded from files
            self.set_options(save2file=1)
454
455
            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)
456
457

            # Results(buffer) memory is kept BUT FILES WILL BE WRITTEN LATER
458
459
            if self.get_options("save2file"):
                size1 = self.mbs_dirdyn_ptr.contents.buffers[0].contents.index
460
461
                if size1 == 0:
                    size1 = self.mbs_dirdyn_ptr.contents.buffers[0].contents.size
462
463
464
465
                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)))
466
                # get time array from the q buffer
467
                self.results.t = self.results.q[:, 0]
468
469
                if not self.results.t[0] == self.get_options("t0"):
                    print("The beginning of the integration is not available in the buffer.\n The complete results have to be loaded from files.")
470
                    filename = bytes_to_str(ctypes.string_at(self.mbs_dirdyn_ptr.contents.buffers[0].contents.filename))
471
                    self.results.load_results_from_file(filename, module=6)
472
                # get qd and qdd buffer
473
474
475
476
                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)))
477
                buffer_id = 4
478
                if self.mbs.Nux:
479
480
481
                    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)))
482
483
                    buffer_id = buffer_id + 2
                if self.mbs.Nlink:
484
485
486
487
                    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)))
488
489
                    buffer_id = buffer_id + 3
                if self.mbs.nqc:
490
491
                    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)))
492
493
                    buffer_id = buffer_id + 1
                if self.mbs.nhu:
494
495
496
                    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)))

497
498
499
                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
500
                    for i in range(nbOutput):
501
                        user_out = np.copy(np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.user_buffer.contents.X[i], (1, size)))
502
                        name = bytes_to_str(self.mbs_dirdyn_ptr.contents.user_buffer.contents.names[i])
503
                        self.results.outputs[name] = user_out[0]
504

505
            libmodules.mbs_dirdyn_finish(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
506

507
        # Anassign user functions
508
509
510
        if self.mbs.opt_load_c < 2:
            self.__unassign_user_fct__()
            self.mbs.__unassign_user_fct__()
511

512
        # Anassing required symbolic functions
513
514
515
        if self.mbs.opt_load_c < 1:
            self.__unassign_symbolic_fct__()
            self.mbs.__unassign_symb_fct__()
516

Louis Beauloye's avatar
Louis Beauloye committed
517
        return self.results
518

519
520
    def set_options(self, **kwargs):
        """
521
        Set the specified options for Dirdyn module.
522

523
524
        Parameters
        ----------
525
        t0: float
526
527
            Initial time of the simulation.
            default is 0.0
528
        tf: float
529
530
            Final time of the simulation
            default is 5.0
531
        dt0: float
532
            Initial value of the integration step size. For fixed-step integrator
533
           (ie. RK4, Euler explicit...) this is the time step for the whole simulation.
534
            default is 0.001
535
536
        save2file: int
            Determine whether results are written to files on disk(in resultsR folder):
537
             - 1: results are saved
538
             - 0: results are not saved
539
            default is 1
540
        resfilename: str
541
542
            The keyword used for determining the name of result files.
            default is 'dirdyn'
543
        respath: str
544
            Path in which result file are saved. This is the full path or the
545
            relative path. The folder must exist.
546
            default is the 'resultsR/' folder of the project
547
        animpath: str
548
            Path in which animation file is saved. This is the full path or the
549
            relative path. The folder must exist.
550
            default is the 'animationR/' folder of the project
551
552
        save_anim: int
            1 to save the animation file, if 'save2file' is set to 1. Set to 0
553
554
            to not save the animation file.
            default is 1
555
        save_visu: int
556
            Unused options as realtime is deactivated is MBsysPy.
557
            1 to save the visualizazion file(as it appears in 'user_realtime_visu.c'),
558
559
            if 'save2file' is set to 1. Set to O to not save the visualization.
            default is 0
560
        framerate: int
561
562
            number of frame per second for the animation file.
            default is 1000
563
        saveperiod: int
564
            The number of time steps between two buffer records.
565
566
            default is 1(every time step are recorded)
        max_save_user: int
567
568
            The maximal number of user variables saved.
            default is 12
569
570
        buffersize: int
            The number of time step that can be recorded in the buffer. Results
571
572
573
574
            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
575
        realtime: int
576
577
            Unused options as realtime is deactivated is MBsysPy.
            1 to activate to real-time features, 0 to deactivate them.
578
            default = 0
579
        accelred: int
580
581
            1 to use accelred, 0 otherwise.
            default is 0
582
        flag_compute_Qc: int
583
584
585
586
587
            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
588
        compute_all_uxd: int
589
590
591
            If 1 the user derivative are computed during the analysis. Otherwhise
            If set to 0, they are not computed.
            default is 1
592
        compute_Qc: numpy.ndarray
593
            If option "flag_compute_Qc' is set to 0, the vector allows to select
594
            the(driven) joints on which the force/torque required to follow the
595
            trajectory will be computed.
596
            The shape of the array is '(mbs.njoint + 1)', for example with
597
598
599
            '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
600
601
602
            default is an array full of zero(no forces computation)
        integrator: str
            Set integrator to use, available value:
603
             - "RK4": explicit Runge–Kutta method order 4
604
                      fixed stepsize
605
             - "Dopri5": explicit Runge–Kutta method of order(4)5
606
607
608
609
610
611
612
613
614
615
                         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
616
617
618
                          for stiff problems
             - "WMethods":  Implicit Euler extented to two stages
                            for stiff problems
619
620
             - "Bader": Semi-implicit midpoint rule method
                        adaptative stepsize
621
                        for stiff problems
622
            default is "RK4"
623
        verbose: int
624
625
626
            Set to 1 to get messages related to adaptive stepsize integrator. To
            disable message set to 0.
            default is 1
627
        flag_stop_stiff: int
628
629
630
            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
631
        flag_precise_dynamics: int
632
633
            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.
634
635

            If set at 1, the direct dynamics(constraint solving, computation of
636
            the generalized acceleration) will be done at the beginning of each
637
            time step of integration. The output files will be exact, but the
638
            computation will be a little slower.
639
            If set at 0, the output file will contains the value(constraint
640
641
642
643
644
            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
645
        flag_waypoint: int
646
            If set to 1, the integrator will be forced to give a solution at the
647
            specified time interval('delta_t_wp'). If set to 0, the solution
648
649
            will be given at every computed time-step.
            default is 0
650
651
652
653
        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
654
655
            all the timesteps and the waypoints.
            default is 0
656
657
        delta_t_wp: float
            Time interval between two waypoints [s], unused if 'flag_waypoint'
658
659
            is set to 0.
            default is 1.0e-3
660
        nmax: int
661
662
            maximal number of steps for adaptative stepsize integrators.
            default is 1e9
663
        toler: float
664
665
            mixed error tolerances for some adaptative stepsize integrators.
            default is 1.0e-3
666
        rtoler: float
667
668
            relative error tolerances for some adaptative stepsize integrators.
            default is 1.0e-3
669
        atoler: float
670
671
            absolute error tolerances for some adaptative stepsize integrators.
            default is 1.0e-6
672
        dt_max: float
673
674
            maximal time step [s] for some adaptative stepsize integrators.
            default is 1.0e-3
675
676
        n_freeze: int
            number of time step when the jacobian is freezed(computed once at
677
678
            the start of the n_freeze time steps) for implicit integrators.
            default is 0
679
680
        show_failed_closure: int
            If set to 1, an animation of the Newton-Raphson procedure is generated
681
682
            if the closure fails.
            default is 0
683
        store_results: int
684
685
            If set to 1, a copy of the results is done.
            default is 1
686
        """
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736

        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
737
                else:
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
                    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
                elif value == "Custom":
                    return 8
758
                else:
759
                    raise ValueError
760
            else:
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
                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:
                    raise TypeError("{:} is {:}, can not be casted from {:}.".format(key, options[key]['convert'], type(value))).with_traceback(err.__traceback__)

            except AttributeError as err:
                # if error during cast of strings (i.e in str_to_bytes)
                raise TypeError("{:} is str, {:} can not be casted.".format(key, type(value))).with_traceback(err.__traceback__)

            except TypeError as err:
                # if wrong type in integrator or compute_QC
                raise TypeError("{:} is {:}, can not be casted from {:}.".format(key, options[key]['convert'], type(value))).with_traceback(err.__traceback__) from None
860

861
862
    def get_options(self, *args):
        """
863
864
        Get the specified options for Dirdyn module.

865
866
        Parameters
        ----------
867
        The different options specifed in the documentation of 'set_options()'
868

869
870
        Returns
        -------
871
        The value of the different options specifed in the documentation of 'set_options()'
872
873
874
875
876
877
878
879
880
881
882
883
        """
        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":
884
                options.append(bytes_to_str(ctypes.string_at(self.mbs_dirdyn_ptr.contents.options.contents.resfilename)))
885
            elif key == "respath":
886
                options.append(bytes_to_str(ctypes.string_at(self.mbs_dirdyn_ptr.contents.options.contents.respath)))
887
            elif key == "animpath":
888
                options.append(bytes_to_str(ctypes.string_at(self.mbs_dirdyn_ptr.contents.options.contents.animpath)))
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
            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
907
908
            elif key == "compute_all_uxd":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.compute_all_uxd)
909
            elif key == "compute_Qc":