mbs_dirdyn.py 15.6 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
# -*- 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

import numpy as np

# importing MbsysPy classes

# importing libraries
from ..mbsysc_loader.loadlibs import libmodules

# 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
#==============================================================================
__DEBUG__ = True
__MODULE_DIR__ = os.path.dirname(os.path.abspath(__file__))


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

class MbsDirdyn(object):
        
    def __init__(self, mbs, user_path=None, symbolic_path=None):
        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.")
            print( "     >>  options->tf (5.0): ", self.mbs_dirdyn_ptr.contents.options.contents.tf)
            print( "     >>  options->dt_max (1e-3): ", self.mbs_dirdyn_ptr.contents.options.contents.dt_max)
            print( "     >>  mbs_aux->NRerr (-): ", self.mbs_dirdyn_ptr.contents.mbs_aux.contents.NRerr)
            print( "     >>  mbs_aux->nquc (-): ", self.mbs_dirdyn_ptr.contents.mbs_aux.contents.nquc)
            print( "     >>  mbs_aux->M[1:3][1:3]: ", 
                  self.mbs_dirdyn_ptr.contents.mbs_aux.contents.M[1][1], 
                  self.mbs_dirdyn_ptr.contents.mbs_aux.contents.M[1][2], 
                  self.mbs_dirdyn_ptr.contents.mbs_aux.contents.M[2][1], 
                  self.mbs_dirdyn_ptr.contents.mbs_aux.contents.M[2][2])
        # Path to user function used by partitionning modue
        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
        
        # pointers list to avoid garbage collecting
        self.ptrs_to_fcts = []
        # [0]: user_cons_hJ
        # [1]: user_cons_jdqd
        # [2]: user_dirdyn_init
        # [3]: user_dirdyn_loop
        # [4]: user_dirdyn_finish
        # [5]: mbs_cons_hJ
        # [6]: mbs_cons_jdqd
        # [6]: mbs_invdyna
        # [6]: mbs_dirdyna
        
        # 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
        
        # 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
        if __DEBUG__ : print( "DEBUG>>  Loading user functions")
        self.__load_user_fct__(self.user_path)
        # Loading symbolic function
        if __DEBUG__ : print( "DEBUG>>  Loading symbolic functions")
        self.__load_symbolic_fct__(self.symbolic_path)
        

    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):
        """ 
            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)
        """
        # cons_hJ
        user_file = "cons_hJ.py"
        path = os.path.abspath(os.path.join(user_path, user_file))
        if os.path.isfile(path):
            module = imp.load_source(user_file[:-3], path)
            self.user_cons_hJ = module.user_cons_hJ
            self.ptrs_to_fcts.append(user_cons_hJ_wrap(lambda h,Jac,mbs,tsim : self.user_cons_hJ(self._huserc, self._Juserc, self.mbs, tsim)))
        else:
            if __DEBUG__ : print( "DEBUG>>  file '"+user_file+"' not found in folder '"+os.path.dirname(path))
            self.ptrs_to_fcts.append(user_cons_hJ_wrap(lambda h,Jac,mbs,tsim : None))
            self.user_cons_hJ = lambda *args: None
        
        # cons_jdqd
        user_file = "cons_jdqd.py"
        path = os.path.abspath(os.path.join(user_path, user_file))
        if os.path.isfile(path):
            module = imp.load_source(user_file[:-3], path)
            self.user_cons_jdqd = module.user_cons_jdqd
            self.ptrs_to_fcts.append(user_cons_jdqd_wrap(lambda h,Jac,mbs,tsim : self.user_cons_jdqd(self._jdqduserc, self.mbs, tsim)))
        else:
            if __DEBUG__ : print( "DEBUG>>  file '"+user_file+"' not found in folder '"+os.path.dirname(path))
            self.ptrs_to_fcts.append(user_cons_jdqd_wrap(lambda h,Jac,mbs,tsim : None))
            self.user_cons_jdqd = lambda *args: None

        # user_dirdyn
        user_file = "user_dirdyn.py"
        path      = os.path.abspath(os.path.join(user_path, user_file))
        if os.path.isfile(path):
            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
            self.ptrs_to_fcts.append(user_dirdyn_init_wrap(lambda mbs, dd : self.mbs.user_dirdyn_init(self.mbs, self)))
            self.ptrs_to_fcts.append(user_dirdyn_loop_wrap(lambda mbs, dd : self.mbs.user_dirdyn_loop(self.mbs, self)))
            self.ptrs_to_fcts.append(user_dirdyn_finish_wrap(lambda mbs, dd : self.mbs.user_dirdyn_finish(self.mbs, self)))
        else:
            print ("file '"+user_file+"' not found in folder '"+os.path.dirname(path))
            self.ptrs_to_fcts.append(user_dirdyn_init_wrap(lambda mbs, dd : None))
            self.ptrs_to_fcts.append(user_dirdyn_loop_wrap(lambda mbs, dd : None))
            self.ptrs_to_fcts.append(user_dirdyn_finish_wrap(lambda mbs, dd : None))
            self.mbs.user_dirdyn_init = lambda *args: None
            self.mbs.user_dirdyn_loop = lambda *args: None
            self.mbs.user_dirdyn_finish = lambda *args: None
        return
        
    def __load_symbolic_fct__(self, symbolic_path):
        """ 
            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)
        """
        mbs_name = self.mbs.mbs_name
        # mbs_cons_hJ
        symb_file = "mbs_cons_hJ_"+mbs_name+".py"
        path = os.path.abspath(os.path.join(symbolic_path, symb_file))
        if os.path.isfile(path):
            module = imp.load_source(symb_file[:-3], path)
            self.mbs_cons_hJ = module.cons_hJ
            self.ptrs_to_fcts.append(mbs_cons_hJ_wrap(lambda h,Jac,mbs,tsim : self.mbs_cons_hJ(self._h, self._Jac, self.mbs)))
        else:
            if __DEBUG__ : print( "DEBUG>>  file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
            self.ptrs_to_fcts.append(mbs_cons_hJ_wrap(lambda h,Jac,mbs,tsim : None))
            self.mbs_cons_hJ = lambda *args: None
        
        # mbs_cons_jdqd
        symb_file = "mbs_cons_jdqd_"+mbs_name+".py"
        path = os.path.abspath(os.path.join(symbolic_path, symb_file))
        if os.path.isfile(path):
            module = imp.load_source(symb_file[:-3], path)
            self.mbs_cons_jdqd = module.cons_jdqd
            self.ptrs_to_fcts.append(mbs_cons_jdqd_wrap(lambda jdqd,mbs,tsim : self.mbs_cons_jdqd(self._jdqd, self.mbs)))
        else:
            if __DEBUG__ : print( "DEBUG>>  file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
            self.ptrs_to_fcts.append(mbs_cons_jdqd_wrap(lambda jdqd,mbs,tsim : None))
            self.mbs_cons_jdqd = lambda *args: None
        
        # invdyna
        symb_file = "mbs_invdyna_"+mbs_name+".py"
        path = os.path.abspath(os.path.join(symbolic_path, symb_file))
        if os.path.isfile(path):
            module = imp.load_source(symb_file[:-3], path)
            self.mbs_invdyna = module.invdyna
            self.ptrs_to_fcts.append(mbs_invdyna_wrap(lambda Q,mbs,tsim : self.mbs_invdyna(self.mbs, tsim, None)))
        else:
            if __DEBUG__ : print( "DEBUG>>  file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
            self.ptrs_to_fcts.append(mbs_invdyna_wrap(lambda Q,mbs,tsim : None))
            self.mbs_invdyna = lambda *args: None
        
        # dirdyna
        symb_file = "mbs_dirdyna_"+mbs_name+".py"
        path = os.path.abspath(os.path.join(symbolic_path, symb_file))
        if os.path.isfile(path):
            module = imp.load_source(symb_file[:-3], path)
            self.mbs_dirdyna = module.dirdyna
            self.ptrs_to_fcts.append(mbs_dirdyna_wrap(lambda M,c,mbs,tsim : self.mbs_dirdyna(self._M, self._c, self.mbs, tsim)))
        else:
            if __DEBUG__ : print( "DEBUG>>  file '"+symb_file+"' not found in folder '"+os.path.dirname(path))
            self.ptrs_to_fcts.append(mbs_dirdyna_wrap(lambda M,c,mbs,tsim : None))
            self.mbs_dirdyna = lambda *args: None
        return
    
    def run(self):
        # [0]: user_cons_hJ
        # [1]: user_cons_jdqd
        # [2]: user_dirdyn_init
        # [3]: user_dirdyn_loop
        # [4]: user_dirdyn_finish
        # [5]: mbs_cons_hJ
        # [6]: mbs_cons_jdqd
        # [6]: mbs_invdyna
        # [6]: mbs_dirdyna
        # Assing required user functions
        self.mbs.user_cons_hJ = self.user_cons_hJ
        self.mbs.mbs_data_ptr.contents.user_cons_hJ = self.ptrs_to_fcts[0]
        self.mbs.user_cons_jdqd = self.user_cons_jdqd
        self.mbs.mbs_data_ptr.contents.user_cons_jdqd = self.ptrs_to_fcts[1]
        self.mbs.user_dirdyn_init = self.user_dirdyn_init
        self.mbs.mbs_data_ptr.contents.user_dirdyn_init = self.ptrs_to_fcts[2]
        self.mbs.user_dirdyn_loop = self.user_dirdyn_loop
        self.mbs.mbs_data_ptr.contents.user_dirdyn_loop = self.ptrs_to_fcts[3]
        self.mbs.user_dirdyn_finish = self.user_dirdyn_finish
        self.mbs.mbs_data_ptr.contents.user_dirdyn_finish = self.ptrs_to_fcts[4]
        # Assing required symbolic functions
        self.mbs.mbs_cons_hJ = self.mbs_cons_hJ
        self.mbs.mbs_data_ptr.contents.mbs_cons_hJ = self.ptrs_to_fcts[5]
        self.mbs.mbs_cons_jdqd = self.mbs_cons_jdqd
        self.mbs.mbs_data_ptr.contents.mbs_cons_jdqd = self.ptrs_to_fcts[6]
        self.mbs.mbs_invdyna = self.mbs_invdyna
        self.mbs.mbs_data_ptr.contents.mbs_invdyna = self.ptrs_to_fcts[7]
        self.mbs.mbs_dirdyna = self.mbs_dirdyna
        self.mbs.mbs_data_ptr.contents.mbs_dirdyna = self.ptrs_to_fcts[8]
        
        libmodules.mbs_run_dirdyn(self.mbs_dirdyn_ptr, self.mbs.mbs_data_ptr)
    
        
        
    
    #==========================================================================
    # Defining properties
    #==========================================================================
    @property
    def mbs_filename(self):
        return self.mbs.mbs_filename
    
    @property
    def project_path(self):
        return self.mbs.project_path
    
    @property
    def mbs_name(self):
        return self.mbs.mbs_name