mbs_dirdyn.py 51.3 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
# -*- coding: utf-8 -*-
"""
-------------------------------
(c) Universite catholique de Louvain, 2019

Creation : 2019 by O. Lantsoght
Last update : 2019

version MBsysC v1.11.2

-------------------------------
Portable Python interface to MBsysC using Ctypes.

Define the class MbsDirdyn based on the MbsDirdyn structure of MBsysC.
This class has the functions required to manipulate the direct dynamic module.
This include setting the options, running an (or multiple) analysis and freeing
the memory.
"""

import os
import imp
22
import ctypes
23
24
25
26
27
28
29
30

import numpy as np

# importing MbsysPy classes

# importing libraries
from ..mbsysc_loader.loadlibs import libmodules

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

34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
# importing wrapping function
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_invdyna_wrap
from ..mbsysc_loader.callback import mbs_dirdyna_wrap


#==============================================================================
# Global parameter of the current module
#==============================================================================
50
__DEBUG__ = False
51
52
53
54
55
56
57
58
__MODULE_DIR__ = os.path.dirname(os.path.abspath(__file__))


#==============================================================================
# Defining Python MbsDirdyn class
#==============================================================================

class MbsDirdyn(object):
59
60
61
62
63
64
65
    """
    Class of the direct dynamic module
    
    Attributes
    ----------
    dt : double
        Current integration step size.
66
67
    mbs : MbsData
        Instance of MbsData related to the analysis.
68
    results : MbsResult
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
        Instance of MbsResult containing the results of the direct dynamics analysis.
    symbolic_path : str
        Path to the folder containing the symbolic functions (python modules) 
        to be loaded.
    tsim : double
        Current simulation time.
    user_path : str
        Path to the folder containing the user functions (python modules) to be loaded.
    
    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")
        (5.0, 10.0)
    >>> results = mbs_dirdyn.run()
    
87
    """
88
    def __init__(self, mbs, user_path=None, symbolic_path=None):
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
        """
        Create an instance of the MbsDirdyn class for the provided MbsData instance.

        Parameters
        ----------
        mbs : MbsData
            Instance of MbsData related to the analysis.
        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'.
            default is None
        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'.
            default is None
            
        Returns
        -------
            A MbsDirdyn instance.
        """
            
110
111
112
113
114
115
116
117
        if __DEBUG__ : print( "DEBUG>>  Creating MbsDirdyn struct for "+mbs.mbs_name+"' MBS.")
        self.mbs_dirdyn_ptr = libmodules.mbs_new_dirdyn(mbs.mbs_data_ptr)
        if __DEBUG__ : print( "DEBUG>>  MbsDirdyn structure loaded")
        self.mbs = mbs

        #Test
        if __DEBUG__ :
            print( "DEBUG>>  MbsDirdyn created.")
118
        # Path to user function used by partitionning module
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
        self.user_path = self.mbs.user_path
        if not (user_path==None):
            project_path  = (self.mbs.project_path).decode("utf-8")
            user_path = os.path.join(project_path, user_path)
            # Error handeling
            if not os.path.isdir(user_path):
                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+'".')
            else:
                self.user_path = user_path
        # Path to user function used by partitionning modue
        self.symbolic_path = self.mbs.symbolic_path
        if not (user_path==None):
            project_path  = (self.mbs.project_path).decode("utf-8")
            symbolic_path = os.path.join(project_path, symbolic_path)
            # Error handeling
            if not os.path.isdir(symbolic_path):
                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+'".')
            else:
                self.symbolic_path = symbolic_path
        
143
        # dictionary containing the pointers to avoid garbage collecting
144
145
        self.ptrs_to_user_fcts = dict()
        self.ptrs_to_symb_fcts = dict()
146
147
148
149
150
151
152
153
154
155
156
157
        
        # 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
        
158
159
        # Storing Results
        self.results = MbsResult(self.mbs)
Louis Beauloye's avatar
Louis Beauloye committed
160
        self.store_results = True
161
        
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
        # Exposing some memory
        if __DEBUG__ : print( "DEBUG>>  Exposing MbsDirdyn fields")
        # Constraints
        self._h = self._Jac = self._jdqd = None
        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,))
        self._huserc = self._Juserc = self._jdqduserc = None
        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,))
        # Fields from equation of motion
        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,))
        
        # Loading user function
181
182
183
        if self.mbs.opt_load_c < 2:
            if __DEBUG__ : print( "DEBUG>>  Loading user functions")
            self.__load_user_fct__(self.user_path)
184
        # Loading symbolic function
185
186
187
        if self.mbs.opt_load_c < 1:
            if __DEBUG__ : print( "DEBUG>>  Loading symbolic functions")
            self.__load_symbolic_fct__(self.symbolic_path)
188
189
190
191
192
193
194
195
196
        

    def __str__(self):
        if __DEBUG__ : print( "DEBUG>>  start of __str")
        
        return "MbsDirdyn instance has nothing to be printed from C library!"
    
    def __load_user_fct__(self, user_path):
        """ 
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
        Load user function where some args depend on MbsDirdyn module.
        
        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 
        and unassigned at the end.
        
        The loader user functions are :
         - user_cons_hJ (from cons_hJ.py)
         - user_cons_jdqd (from cons_hJ.py)
         - user_dirdyn_init (from user_dirdyn.py)
         - user_dirdyn_loop (from user_dirdyn.py)
         - user_dirdyn_finish (from user_dirdyn.py)

        Parameters
        ----------
        user_path : str
            The path to the folder containing the user functions.
215
        """
216
217
218
219
        
        template_path = os.path.join(__MODULE_DIR__,'../templates/user')
        
        
220
221
222
        # cons_hJ
        user_file = "cons_hJ.py"
        path = os.path.abspath(os.path.join(user_path, user_file))
223
        if not os.path.isfile(path):
224
            if __DEBUG__ : print( "DEBUG>>  file '"+user_file+"' not found in folder '"+os.path.dirname(path))
225
226
227
            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
228
229
230
231
        
        # cons_jdqd
        user_file = "cons_jdqd.py"
        path = os.path.abspath(os.path.join(user_path, user_file))
232
        if not os.path.isfile(path):
233
            if __DEBUG__ : print( "DEBUG>>  file '"+user_file+"' not found in folder '"+os.path.dirname(path))
234
235
236
            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
237
        
238
239
240
        # user_dirdyn
        user_file = "user_dirdyn.py"
        path      = os.path.abspath(os.path.join(user_path, user_file))
241
        if not os.path.isfile(path):
242
            print ("file '"+user_file+"' not found in folder '"+os.path.dirname(path))
243
244
245
246
247
248
249
            path = os.path.abspath(os.path.join(template_path, user_file))
        else:
            if __DEBUG__ : print( "DEBUG>>  loading file '"+user_file+"' in folder '"+os.path.dirname(path))
        module = imp.load_source(user_file[:-3], path)
        self.user_dirdyn_init   = module.user_dirdyn_init
        self.user_dirdyn_loop   = module.user_dirdyn_loop
        self.user_dirdyn_finish = module.user_dirdyn_finish
250
251
252
253
        return
        
    def __load_symbolic_fct__(self, symbolic_path):
        """ 
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
        Load symbolic function where some args depend on MbsDirdyn module.
        
        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 
        and unassigned at the end.
        
        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)

        Parameters
        ----------
        symbolic_path : str
            The path to the folder containing the symbolic functions.
271
272
        """
        mbs_name = self.mbs.mbs_name
273
        template_path = os.path.join(__MODULE_DIR__,'../templates/symbolic')
274
275
276
        # mbs_cons_hJ
        symb_file = "mbs_cons_hJ_"+mbs_name+".py"
        path = os.path.abspath(os.path.join(symbolic_path, symb_file))
277
        if not os.path.isfile(path):
278
            if __DEBUG__ : print( "DEBUG>>  file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
279
280
281
282
283
            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
        
284
285
286
287
        
        # mbs_cons_jdqd
        symb_file = "mbs_cons_jdqd_"+mbs_name+".py"
        path = os.path.abspath(os.path.join(symbolic_path, symb_file))
288
        if not os.path.isfile(path):
289
            if __DEBUG__ : print( "DEBUG>>  file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
290
291
292
293
            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
294
295
296
297
        
        # dirdyna
        symb_file = "mbs_dirdyna_"+mbs_name+".py"
        path = os.path.abspath(os.path.join(symbolic_path, symb_file))
298
        if not os.path.isfile(path):
299
            if __DEBUG__ : print( "DEBUG>>  file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
300
301
302
303
            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
304
       
305
306
        return
    
307
308
    def __assign_user_fct__(self):
        """ 
309
310
311
        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
312
313
314
        """
        # cons_hJ
        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))
315
        self.mbs.user_cons_hJ = self.user_cons_hJ
316
        self.mbs.mbs_data_ptr.contents.user_cons_hJ = self.ptrs_to_user_fcts["user_cons_hJ"]
317
318
319
        
        # cons_jdqd
        self.ptrs_to_user_fcts["user_cons_jdqd"] = user_cons_jdqd_wrap(lambda jdqd,mbs,tsim : self.user_cons_jdqd(self._jdqduserc, self.mbs))
320
        self.mbs.user_cons_jdqd = self.user_cons_jdqd
321
        self.mbs.mbs_data_ptr.contents.user_cons_jdqd = self.ptrs_to_user_fcts["user_cons_jdqd"]
322
323
324
325
326

        # user_dirdyn
        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))
327
        self.mbs.user_dirdyn_init = self.user_dirdyn_init
328
        self.mbs.mbs_data_ptr.contents.user_dirdyn_init = self.ptrs_to_user_fcts["user_dirdyn_init"]
329
        self.mbs.user_dirdyn_loop = self.user_dirdyn_loop
330
        self.mbs.mbs_data_ptr.contents.user_dirdyn_loop = self.ptrs_to_user_fcts["user_dirdyn_loop"]
331
        self.mbs.user_dirdyn_finish = self.user_dirdyn_finish
332
        self.mbs.mbs_data_ptr.contents.user_dirdyn_finish = self.ptrs_to_user_fcts["user_dirdyn_finish"]
333
334
335
336
        return
        
    def __assign_symbolic_fct__(self):
        """ 
337
338
339
        Assign and wrap python symbolic 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
340
341
342
343
        """
        
        # mbs_cons_hJ
        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))
344
        self.mbs.mbs_cons_hJ = self.mbs_cons_hJ
345
        self.mbs.mbs_data_ptr.contents.mbs_cons_hJ = self.ptrs_to_symb_fcts["mbs_cons_hJ"]
346
347
348
        
        # mbs_cons_jdqd
        self.ptrs_to_symb_fcts["mbs_cons_jdqd"] = mbs_cons_jdqd_wrap(lambda jdqd,mbs,tsim : self.mbs_cons_jdqd(self._jdqd, self.mbs))
349
        self.mbs.mbs_cons_jdqd = self.mbs_cons_jdqd
350
        self.mbs.mbs_data_ptr.contents.mbs_cons_jdqd = self.ptrs_to_symb_fcts["mbs_cons_jdqd"]
351
352
353
        
        # dirdyna
        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))
354
        self.mbs.mbs_dirdyna = self.mbs_dirdyna
355
        self.mbs.mbs_data_ptr.contents.mbs_dirdyna = self.ptrs_to_symb_fcts["mbs_dirdyna"]
356
357
358
359
360
361
362
363
364
365
366
367
         
        return
    
     # Callback function for function with advanced arguments
    def __callback_mbs_cons_hJ(self, fun, h, Jac):
        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)
    
    
    def __unassign_user_fct__(self):
368
        """ Unassign user function where some args depend on MbsDirdyn module.  """
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
        
        self.ptrs_to_user_fcts = {}
        
        self.mbs.user_cons_hJ = None
        self.mbs.mbs_data_ptr.contents.user_cons_hJ = user_cons_hJ_wrap(lambda h,Jac,mbs,tsim : callback_undefined("user_cons_hJ"))
        self.mbs.user_cons_jdqd = None
        self.mbs.mbs_data_ptr.contents.user_cons_jdqd = user_cons_jdqd_wrap(lambda h,Jac,mbs,tsim : callback_undefined("user_cons_jdqd"))
        self.mbs.user_dirdyn_init = None
        self.mbs.mbs_data_ptr.contents.user_dirdyn_init = user_dirdyn_init_wrap(lambda mbs, dd : callback_undefined("user_dirdyn_init"))
        self.mbs.user_dirdyn_loop = None
        self.mbs.mbs_data_ptr.contents.user_dirdyn_loop = user_dirdyn_loop_wrap(lambda mbs, dd : callback_undefined("user_dirdyn_loop"))
        self.mbs.user_dirdyn_finish = None
        self.mbs.mbs_data_ptr.contents.user_dirdyn_finish = user_dirdyn_finish_wrap(lambda mbs, dd : callback_undefined("user_dirdyn_finish"))
        
    def __unassign_symbolic_fct__(self):
        
385
        """ Unassign symbolic function where some args depend on MbsDirdyn module. """
386
        
387
        self.ptrs_to_symb_fcts = {}
388
        
389
390
391
392
393
394
395
396
397
398
399
400
        self.mbs.mbs_cons_hJ = None
        self.mbs.mbs_data_ptr.contents.mbs_cons_hJ = mbs_cons_hJ_wrap(lambda h,Jac,mbs,tsim : callback_undefined("mbs_cons_hJ"))
        self.mbs.mbs_cons_jdqd = None
        self.mbs.mbs_data_ptr.contents.mbs_cons_jdqd = mbs_cons_jdqd_wrap(lambda jdqd,mbs,tsim : callback_undefined("mbs_cons_jdqd"))
        self.mbs.mbs_dirdyna = None
        self.mbs.mbs_data_ptr.contents.mbs_dirdyna = mbs_dirdyna_wrap(lambda M,c,mbs,tsim : callback_undefined("mbs_dirdyna"))
        
    def __del__(self):
        libmodules.mbs_delete_dirdyn(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
        if __DEBUG__ : print("DEBUG>>  MbsDirdyn pointer deleted")
        
    def run(self):
Louis Beauloye's avatar
Louis Beauloye committed
401
        """
402
403
404
405
406
407
408
        Run a direct dynamics analysis
        
        Options can be setted with the function 'set_options()' 
        Options can be retrieved with the function 'get_options()'
        
        Results are stored in the field 'results' if the options 'store_results'
        is set to True.
Louis Beauloye's avatar
Louis Beauloye committed
409
        
410
411
412
        Returns
        -------
            The MbsResults containing the results of the analysis.
413
        """     
414
        # Assing required user functions
415
416
417
        if self.mbs.opt_load_c < 2:
            self.__assign_user_fct__()
            self.mbs.__assign_user_fct__()
418
419
        
        # Assing required symbolic functions
420
421
422
        if self.mbs.opt_load_c < 1:
            self.__assign_symbolic_fct__()
            self.mbs.__assign_symb_fct__()
423
424
        
        
Louis Beauloye's avatar
Louis Beauloye committed
425
        if not self.store_results:   
426
427
428
429
430
431
432
433
434
435
436
            libmodules.mbs_run_dirdyn(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
        else:
            #save2file forced to 1 because if buffers don't have the complete results, results are loaded from files
            self.set_options(save2file = 1)
            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)
       
            # Results (buffer) memory is kept BUT FILES WILL BE WRITTEN LATER
            if self.get_options("save2file"):
                #c_mbs_dirdyn_write_buffers(self.mbs_dirdyn_ptr)
                size1 = self.mbs_dirdyn_ptr.contents.buffers[0].contents.index
437
438
                if size1 == 0:
                    size1 = self.mbs_dirdyn_ptr.contents.buffers[0].contents.size
439
440
441
442
443
444
445
446
                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)))
                # get time array from the q buffer
                self.results.t = self.results.q[:,0]
                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.")
447
448
                    filename = ctypes.string_at(self.mbs_dirdyn_ptr.contents.buffers[0].contents.filename).decode('utf-8')
                    self.results.load_results_from_file(filename, module = 6)
449
450
451
452
453
                # get qd and qdd buffer
                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)))
454
                buffer_id = 4
455
                if self.mbs.Nux:
456
457
458
459
460
461
462
463
464
465
466
467
468
                    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)))
                    buffer_id = buffer_id + 2
                if self.mbs.Nlink:
                    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)))
                    buffer_id = buffer_id + 3
                if self.mbs.nqc:
                    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)))
469
470
471
472
                    buffer_id = buffer_id + 1
                if self.mbs.nhu:
                    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)))
473
474
475
476
                    
                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
477
                    for i in range(nbOutput):
478
                        user_out = np.copy(np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.user_buffer.contents.X[i],(1, size)))
479
480
                        name = self.mbs_dirdyn_ptr.contents.user_buffer.contents.names[i]
                        name = name.decode('utf-8')
481
                        self.results.outputs[name] = user_out[0]
482
            
483
            #libmodules.mbs_dirdyn_finish(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
484
485
        
        # Anassign user functions
486
487
488
        if self.mbs.opt_load_c < 2:
            self.__unassign_user_fct__()
            self.mbs.__unassign_user_fct__()
489
490
        
        # Anassing required symbolic functions
491
492
493
        if self.mbs.opt_load_c < 1:
            self.__unassign_symbolic_fct__()
            self.mbs.__unassign_symb_fct__()
Louis Beauloye's avatar
Louis Beauloye committed
494
495
        
        return self.results
496
497
498
    
    def set_options(self, **kwargs):
        """
499
        Set the specified options for Dirdyn module.
500
501
502
503
        
        Parameters
        ----------
        t0 : float
504
505
            Initial time of the simulation.
            default is 0.0
506
        tf : float
507
508
            Final time of the simulation
            default is 5.0
509
        dt0 : float
510
511
512
            Initial value of the integration step size. For fixed-step integrator
            (ie. RK4, Euler explicit...) this is the time step for the whole simulation.
            default is 0.001
513
514
        save2file : int
            Determine whether results are written to files on disk (in resultsR folder):  
515
516
517
             - 1: results are saved
             - 0: results are not saved 
            default is 1
518
        resfilename : str
519
520
            The keyword used for determining the name of result files.
            default is 'dirdyn'
521
        respath : str
522
523
524
            Path in which result file are saved. This is the full path or the
            relative path. The folder must exist. 
            default is the 'resultsR/' folder of the project
525
        animpath : str
526
527
528
            Path in which animation file is saved. This is the full path or the
            relative path. The folder must exist. 
            default is the 'animationR/' folder of the project
529
        save_anim : int
530
531
532
            1 to save the animation file, if 'save2file' is set to 1. Set to 0 
            to not save the animation file.
            default is 1
533
        save_visu : int 
534
535
536
537
            Unused options as realtime is deactivated is MBsysPy.
            1 to save the visualizazion file (as it appears in 'user_realtime_visu.c'),
            if 'save2file' is set to 1. Set to O to not save the visualization.
            default is 0
538
        framerate : int
539
540
            number of frame per second for the animation file.
            default is 1000
541
        saveperiod : int
542
543
            The number of time steps between two buffer records.
            default is 1 (every time step are recorded)
544
        max_save_user : int
545
546
            The maximal number of user variables saved.
            default is 12
547
        buffersize : int 
548
549
550
551
552
            The number of time step that can be recorded in the buffer. Results 
            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
553
        realtime : int
554
555
            Unused options as realtime is deactivated is MBsysPy.
            1 to activate to real-time features, 0 to deactivate them.
556
557
            default = 0
        accelred : int
558
559
            1 to use accelred, 0 otherwise.
            default is 0
560
        flag_compute_Qc : int
561
562
563
564
565
            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
Louis Beauloye's avatar
Louis Beauloye committed
566
        compute_all_uxd : int
567
568
569
            If 1 the user derivative are computed during the analysis. Otherwhise
            If set to 0, they are not computed.
            default is 1
570
        compute_Qc : numpy.ndarray
571
572
573
574
575
576
577
578
            If option "flag_compute_Qc' is set to 0, the vector allows to select
            the (driven) joints on which the force/torque required to follow the 
            trajectory will be computed.
            The shape of the array is '(mbs.njoint+1)', for example with 
            '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
            default is an array full of zero (no forces computation)
579
        integrator : str
580
            Set integrator to use, available value: 
581
             - "RK4": explicit Runge–Kutta method order 4
582
583
584
585
586
587
588
589
590
591
592
593
                      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
594
595
596
                          for stiff problems
             - "WMethods":  Implicit Euler extented to two stages
                            for stiff problems
597
598
             - "Bader": Semi-implicit midpoint rule method
                        adaptative stepsize
599
                        for stiff problems
600
            default is "RK4"
601
        verbose : int 
602
603
604
            Set to 1 to get messages related to adaptive stepsize integrator. To
            disable message set to 0.
            default is 1
605
        flag_stop_stiff : int 
606
607
608
            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
609
        flag_precise_dynamics : int
610
611
612
613
614
615
616
617
618
619
620
621
622
            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.
            
            If set at 1, the direct dynamics (constraint solving, computation of
            the generalized acceleration) will be done at the beginning of each
            time step of integration. The output files will be exact, but the 
            computation will be a little slower.
            If set at 0, the output file will contains the value (constraint 
            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
623
        flag_waypoint : int
624
625
626
627
            If set to 1, the integrator will be forced to give a solution at the
            specified time interval ('delta_t_wp'). If set to 0, the solution
            will be given at every computed time-step.
            default is 0
628
        flag_solout_wp : int
629
630
631
632
633
            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 
            all the timesteps and the waypoints.
            default is 0
634
        delta_t_wp : float
635
636
637
            Time interval between two waypoints [s], unused if 'flag_waypoint' 
            is set to 0.
            default is 1.0e-3
638
        nmax : int
639
640
            maximal number of steps for adaptative stepsize integrators.
            default is 1e9
641
        toler : float 
642
643
            mixed error tolerances for some adaptative stepsize integrators.
            default is 1.0e-3
644
        rtoler : float
645
646
            relative error tolerances for some adaptative stepsize integrators.
            default is 1.0e-3
647
        atoler : float
648
649
            absolute error tolerances for some adaptative stepsize integrators.
            default is 1.0e-6
650
        dt_max : float
651
652
            maximal time step [s] for some adaptative stepsize integrators.
            default is 1.0e-3
653
        n_freeze : int 
654
655
656
            number of time step when the jacobian is freezed (computed once at 
            the start of the n_freeze time steps) for implicit integrators.
            default is 0
Louis Beauloye's avatar
Louis Beauloye committed
657
        show_failed_closure : int 
658
659
660
            If set to 1, an animation of the Newton-Raphson procedure is generated 
            if the closure fails.
            default is 0
Louis Beauloye's avatar
Louis Beauloye committed
661
        store_results : boolean
662
663
            If set to 1, a copy of the results is done.
            default is 1
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
        """
        
        for key, value in kwargs.items():
            if key == "t0":
                self.mbs_dirdyn_ptr.contents.options.contents.t0 = value
            elif key == "tf":
                self.mbs_dirdyn_ptr.contents.options.contents.tf = value
            elif key == "dt0":
                self.mbs_dirdyn_ptr.contents.options.contents.dt0 = value
            elif key == "save2file":
                self.mbs_dirdyn_ptr.contents.options.contents.save2file = value
            elif key == "resfilename":
                self.mbs_dirdyn_ptr.contents.options.contents.resfilename = value.encode('utf-8')
            elif key == "respath":
                self.mbs_dirdyn_ptr.contents.options.contents.respath = value.encode('utf-8')
            elif key == "animpath":
                self.mbs_dirdyn_ptr.contents.options.contents.animpath = value.encode('utf-8')
            elif key == "save_anim":
                self.mbs_dirdyn_ptr.contents.options.contents.save_anim = value
            elif key == "save_visu":
                self.mbs_dirdyn_ptr.contents.options.contents.save_visu = value
            elif key == "framerate":
                self.mbs_dirdyn_ptr.contents.options.contents.framerate = value
            elif key == "saveperiod":
                self.mbs_dirdyn_ptr.contents.options.contents.saveperiod= value
            elif key == "max_save_user":
                self.mbs_dirdyn_ptr.contents.options.contents.max_save_user = value
            elif key == "buffersize":
                self.mbs_dirdyn_ptr.contents.options.contents.buffersize = value
            elif key == "realtime":
                self.mbs_dirdyn_ptr.contents.options.contents.realtime = value
            elif key == "accelred":
                self.mbs_dirdyn_ptr.contents.options.contents.accelred = value
            elif key == "flag_compute_Qc":
                self.mbs_dirdyn_ptr.contents.options.contents.flag_compute_Qc = value
Louis Beauloye's avatar
Louis Beauloye committed
699
700
            elif key == "compute_all_uxd":
                self.mbs_dirdyn_ptr.contents.options.contents.compute_all_uxd = value
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
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
            elif key == "compute_Qc":
                if (type(value) is np.ndarray) or (type(value) is list):
                    if  np.size(value) == self.mbs.mbs_data_ptr.contents.njoint + 1:
                        for i, val in enumerate(value[1:]):
                            self.mbs_dirdyn_ptr.contents.options.contents.compute_Qc[i+1] = val
                    else:
                        print(">>DIRDYN>> The size of compute_Qc is not consistent with njoint")
                else:
                    print(">>DIRDYN>> The specified value for compute_Qc is not a list or a np.array")
            elif key == "integrator":
                if (type(value) is int) and (value>=0 and value<=8):
                    self.mbs_dirdyn_ptr.contents.options.contents.integrator = value
                elif type(value) is str:
                    if value == "RK4":
                        self.mbs_dirdyn_ptr.contents.options.contents.integrator = 0
                    elif value == "Dopri5":
                        self.mbs_dirdyn_ptr.contents.options.contents.integrator = 1
                    elif value == "Rosenbrock":
                        self.mbs_dirdyn_ptr.contents.options.contents.integrator = 2
                    elif value == "EulerEx":
                        self.mbs_dirdyn_ptr.contents.options.contents.integrator = 3
                    elif value == "Eulaire":
                        self.mbs_dirdyn_ptr.contents.options.contents.integrator = 4
                    elif value == "EulerIm":
                        self.mbs_dirdyn_ptr.contents.options.contents.integrator = 5
                    elif value == "Bader":
                        self.mbs_dirdyn_ptr.contents.options.contents.integrator = 6
                    elif value == "WMethods":
                        self.mbs_dirdyn_ptr.contents.options.contents.integrator = 7
                    elif value == "Custom":
                        self.mbs_dirdyn_ptr.contents.options.contents.integrator = 8
                    else:
                        print(">>DIRDYN>>  " + value + " is not an integrator")
                else:
                    print(">>DIRDYN>>  " + value + " is not an integrator")
            elif key == "verbose":
                self.mbs_dirdyn_ptr.contents.options.contents.verbose = value
            elif key == "flag_stop_stiff":
                self.mbs_dirdyn_ptr.contents.options.contents.flag_stop_stiff = value
            elif key == "flag_precise_dynamics":
                self.mbs_dirdyn_ptr.contents.options.contents.flag_precise_dynamics = value
            elif key == "flag_waypoint":
                self.mbs_dirdyn_ptr.contents.options.contents.flag_waypoint = value
            elif key == "flag_solout_wp":
                self.mbs_dirdyn_ptr.contents.options.contents.flag_solout_wp= value
            elif key == "delta_t_wp":
                self.mbs_dirdyn_ptr.contents.options.contents.delta_t_wp = value
            elif key == "nmax":
                self.mbs_dirdyn_ptr.contents.options.contents.nmax = value
            elif key == "toler":
                self.mbs_dirdyn_ptr.contents.options.contents.toler = value
            elif key == "rtoler":
                self.mbs_dirdyn_ptr.contents.options.contents.rtoler = value
            elif key == "atoler":
                self.mbs_dirdyn_ptr.contents.options.contents.atoler = value
            elif key == "dt_max":
                self.mbs_dirdyn_ptr.contents.options.contents.dt_max = value
            elif key == "n_freeze":
                self.mbs_dirdyn_ptr.contents.options.contents.n_freeze = value
Louis Beauloye's avatar
Louis Beauloye committed
760
761
            elif key == "show_failed_closure":
                self.mbs_dirdyn_ptr.contents.options.contents.show_failed_closure = value
Louis Beauloye's avatar
Louis Beauloye committed
762
763
            elif key == "store_results":
                self.store_results = value
764
765
766
767
768
769
770
771
772
773
            else:
                print(">>DIRDYN>> The option "+key+ " is not defined in this module")
                
        
    def get_options(self, *args):
        """
        Get the specified options for Dirdyn module
        
        Parameters
        ----------
774
        The different options specifed in the documentation of 'set_options()'
775
776
777
        
        Returns
        -------
778
        The value of the different options specifed in the documentation of 'set_options()'
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
        """
        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":
                options.append(ctypes.string_at(self.mbs_dirdyn_ptr.contents.options.contents.resfilename).decode("utf-8"))
            elif key == "respath":
                options.append(ctypes.string_at(self.mbs_dirdyn_ptr.contents.options.contents.respath).decode("utf-8"))
            elif key == "animpath":
                options.append(ctypes.string_at(self.mbs_dirdyn_ptr.contents.options.contents.animpath).decode("utf-8"))
            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
814
815
            elif key == "compute_all_uxd":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.compute_all_uxd)
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
860
861
            elif key == "compute_Qc":
                compute_Qc_py = np.ctypeslib.as_array(self.mbs_dirdyn_ptr.contents.options.contents.compute_Qc, (self.mbs.mbs_data_ptr.contents.njoint+1,))
                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:
                    options.append("EulerEx")    
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 4:
                    options.append("Eulaire") 
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 5:
                    options.append("EulerIm")  
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 6:
                    options.append("Bader")
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 7:
                    options.append("WMethods")   
                elif self.mbs_dirdyn_ptr.contents.options.contents.integrator == 8:
                    options.append("Custom")   
            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
862
863
            elif key == "show_failed_closure":
                options.append(self.mbs_dirdyn_ptr.contents.options.contents.show_failed_closure)
Louis Beauloye's avatar
Louis Beauloye committed
864
865
            elif key == "store_results":
                options.append(self.store_results)
866
867
            else:
                print(">>DIRDYN>> The option "+key+ " is not defined in this module")
868
869
870
871
872
                
        if len(options) == 0:
            return
        if len(options) == 1:
            return options[0]
873
874
            
        return tuple(options)
875
    
876
                
877
878
879
    #==========================================================================
    # Defining properties
    #==========================================================================
880
881
882
883
884
885
886
887
    @property
    def tsim(self):
        return self.mbs_dirdyn_ptr.contents.tsim
    
    @property
    def dt(self):
        return self.mbs_dirdyn_ptr.contents.dt
    
888
889
    
class MbsResult(object):
890
891
892
    """
    Class containing results.
    
893
894
    The user-specified vector are not available.
    
895
896
897
898
899
900
901
902
903
904
905
906
    Attributes
    ----------
    q : ndarray
        Numpy array containing the current values of the generalized 
        coordinates.
    qd : ndarray
        Numpy array containing the current values of the generalized 
        velocities.
    qdd : ndarray
        Numpy array containing the current values of the generalized 
        accelerations.
    Qq : ndarray
907
908
909
910
911
912
913
914
        Numpy array containing the values of the joint forces.
    Qc : ndarray
        Numpy array containing the value of joint force introduced in driven 
        joints to respect the user function
    qa : ndarray of int
        Numpy array of integers containing the indices of actuated 
        articulations (only for inverse dynamic). Those articulations are 
        controlled by an actuator.
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
    t : ndarray
        Numpy array containing the values of the time vector
    ux : ndarray
        Numpy array containing the values of the user variables.
    uxd : ndarray
        Numpy array containing the values of the time derivative of the user 
        variables.
    Fl : ndarray
        Numpy array containing the current values of the forces between of 
        the points of a link.
    Z : ndarray
        Numpy array containing the current values of the distances between of 
        the points of a link.
    Zd : ndarray
        Numpy array containing the current values of the speed (spreading) 
        between of the points of a link.
    outputs : dict
        Dict containing the names and the values of the user outputs
    mbs : MbsData
        instance of MbsData from where results come from
    """
936
937
        
    def __init__(self,mbs):
938
939
940
        """
            
        """
941
942
943
944
945
946
947
        self.q=[]
        self.qd=[]
        self.qdd=[]
        self.Fl=[]
        self.Z=[]
        self.Zd=[]
        self.Qq=[]
948
        self.Qa=[]
949
950
951
        self.t=[]
        self.ux=[]
        self.uxd=[]
952
        self.Qc=[]
953
954
955
        self.outputs={}
        self.mbs=mbs
    
956
    def load_results_from_file(self, filename, result_path="resultsR", module=0):
957
        """
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
        Load the results from the files into a MbsResult instance.
        
        This function is called if the buffers don not contain the full simulation.
        
        Parameters
        ----------
        filename : str
            The resfilename containing the results to load. The required suffix
            (ie. "_q.res") will be added automatically
        result_path : str, optional
            The relative path of the result folder from the project folder.
            default is "resultsR"
        module : int
            The module in which this function is called. In some modules, some 
            results files don't exist. Module ids corresponds to the 
            'MbsData.process' value. If set to 0 we try to load all files
            default is 0
975
        """
976
977
        
        project_path = self.mbs.project_path
978
979
        baseFileName = os.path.basename(filename)    
        baseFileName = baseFileName[:-6]
980
        result_path = os.path.join(project_path, result_path)
981
982
983
984
985
986
987
988
        # Error handeling
        if not os.path.isdir(result_path):
            if __DEBUG__ : print ('DEBUG>> The result directory does not exist: "'+ result_path+'"')
            
        # Generalized coordinates
        CurFile = baseFileName+'_q.res'
        path = os.path.abspath(os.path.join(result_path, CurFile))
        if (os.path.isfile(path) ):
989
990
991
992
993
            self.q = np.loadtxt(path)
            if np.array(self.q).ndim > 1:
                self.t = self.q[:,0]
            else:
                self.t = self.q[0]
994
995
996
997
998
999
1000
        else:
            if __DEBUG__ : print( "DEBUG>>  file '"+CurFile+"' not found in folder '"+os.path.dirname(path))
        
        # Generalized velocities
        CurFile = baseFileName+'_qd.res'
        path = os.path.abspath(os.path.join(result_path, CurFile))
        if (os.path.isfile(path)):
1001
            self.qd = np.loadtxt(path)
1002
1003
1004
1005
1006
1007
1008
        else:
             if __DEBUG__ : print( "DEBUG>>  file '"+CurFile+"' not found in folder '"+os.path.dirname(path))
             
        # Generalized accelerations   
        CurFile = baseFileName+'_qdd.res'
        path = os.path.abspath(os.path.join(result_path, CurFile))
        if (os.path.isfile(path)):
1009
            self.qdd = np.loadtxt(path)
1010
1011
1012
        else:
             if __DEBUG__ : print( "DEBUG>>  file '"+CurFile+"' not found in folder '"+os.path.dirname(path))
             
1013
1014
1015
1016
1017
1018
1019
        if module != 5: 
            CurFile = baseFileName+'_Qq.res'
            path = os.path.abspath(os.path.join(result_path, CurFile))
            if (os.path.isfile(path)):
                self.Qq = np.loadtxt(path)
            else:
                 if __DEBUG__ : print( "DEBUG>>  file '"+CurFile+"' not found in folder '"+os.path.dirname(path))
1020
1021
        
        # Generalized user state
1022
        if self.mbs.Nux and (module != 5 and module != 6):
1023
1024
1025
            CurFile = baseFileName+'_ux.res'
            path = os.path.abspath(os.path.join(result_path, CurFile))
            if (os.path.isfile(path) ):
1026
                self.ux = np.loadtxt(path)
1027
1028
1029
1030
1031
1032
            else:
                if __DEBUG__ : print( "DEBUG>>  file '"+CurFile+"' not found in folder '"+os.path.dirname(path))
            
            CurFile = baseFileName+'_uxd.res'
            path = os.path.abspath(os.path.join(result_path, CurFile))
            if (os.path.isfile(path)):
1033
                self.uxd = np.loadtxt(path)
1034
1035
1036
            else:
                if __DEBUG__ : print( "DEBUG>>  file '"+CurFile+"' not found in folder '"+os.path.dirname(path))
                
1037
        # Link        
1038
        if self.mbs.Nlink and module != 5: 
1039
1040
1041
            CurFile = baseFileName+'_linkF.res'
            path = os.path.abspath(os.path.join(result_path, CurFile))
            if (os.path.isfile(path) ):
1042
                self.Fl = np.loadtxt(path)
1043
1044
1045
1046
1047
1048
            else:
                if __DEBUG__ : print( "DEBUG>>  file '"+CurFile+"' not found in folder '"+os.path.dirname(path))
            
            CurFile = baseFileName+'_linkZ.res'
            path = os.path.abspath(os.path.join(result_path, CurFile))
            if (os.path.isfile(path)):
1049
                self.Z = np.loadtxt(path)
1050
1051
1052
1053
1054
1055
            else:
                if __DEBUG__ : print( "DEBUG>>  file '"+CurFile+"' not found in folder '"+os.path.dirname(path))
            
            CurFile = baseFileName+'_linkZd.res'
            path = os.path.abspath(os.path.join(result_path, CurFile))
            if (os.path.isfile(path)):
1056
                self.Zd = np.loadtxt(path)
1057
1058
            else:
                if __DEBUG__ : print( "DEBUG>>  file '"+CurFile+"' not found in folder '"+os.path.dirname(path))
1059
1060
        
        # Qc
1061
        if self.mbs.nqc and module != 5:
1062
1063
1064
            CurFile = baseFileName+'_Qc.res'
            path = os.path.abspath(os.path.join(result_path, CurFile))
            if (os.path.isfile(path) ):
1065
                self.Qc = np.loadtxt(path)
1066
1067
            else:
                if __DEBUG__ : print( "DEBUG>>  file '"+CurFile+"' not found in folder '"+os.path.dirname(path))
1068
         
1069
1070
1071
1072
1073
1074
1075
1076
1077
            
        # Qa
        if self.mbs.nqa and (module != 5 and module != 3):
            CurFile = baseFileName+'_Qa.res'
            path = os.path.abspath(os.path.join(result_path, CurFile))
            if (os.path.isfile(path) ):
                self.Qa = np.loadtxt(path)
            else:
                if __DEBUG__ : print( "DEBUG>>  file '"+CurFile+"' not found in folder '"+os.path.dirname(path))
1078
1079