mbs_data.py 71.3 KB
Newer Older
1
2
# -*- coding: utf-8 -*-
"""
3
Main class for Multibody systems.
4

5
6
Summary
-------
7
8
9
10
Define the class MbsData based on the MbsData structure of MBsysC.
This class has the functions required to manipulate the Data (set the number of
user constraints, set the independant or dependant variables...).
"""
11
# MBsysC version : 1.13.0
12
13
# Author : Robotran Team
# (c) Universite catholique de Louvain, 2019
14
15
16
17
18
19
20

import os
import imp
import ctypes

import numpy as np

21
# importing MbsysPy functions
22
from ..mbs_utilities import mbs_msg
23
24
from ..mbs_utilities import bytes_to_str
from ..mbs_utilities import str_to_bytes
25
26
from ..mbs_utilities import convert_numpy_type_to_int
from ..mbs_utilities import convert_numpy_type_to_float
27

28
29
# importing MbsysPy classes
from .mbs_sensor import MbsSensor
30
from .mbs_part import MbsPart
31
32
33
34
35
from .mbs_equil import MbsEquil
from .mbs_dirdyn import MbsDirdyn
from .mbs_modal import MbsModal
from .mbs_solvekin import MbsSolvekin
from .mbs_invdyn import MbsInvdyn
36
37

# importing libraries
38
39
from .._mbsysc_loader.loadlibs import libutilities
from .._mbsysc_loader.loadlibs import libloadXML
40

41
42
43
# importing utilities function
from ..mbs_utilities import callback_undefined

44
# importing wrapping function
45
46
47
48
49
50
from .._mbsysc_loader.callback import user_Derivative_wrap
from .._mbsysc_loader.callback import user_DrivenJoints_wrap
from .._mbsysc_loader.callback import user_ExtForces_wrap
from .._mbsysc_loader.callback import user_JointForces_wrap
from .._mbsysc_loader.callback import user_LinkForces_wrap
from .._mbsysc_loader.callback import user_Link3DForces_wrap
51

52
53
54
55
56
57
from .._mbsysc_loader.callback import mbs_accelred_wrap
from .._mbsysc_loader.callback import mbs_extforces_wrap
from .._mbsysc_loader.callback import mbs_gensensor_wrap
from .._mbsysc_loader.callback import mbs_link_wrap
from .._mbsysc_loader.callback import mbs_link3D_wrap
from .._mbsysc_loader.callback import mbs_sensor_wrap
58

59
# =============================================================================
60
# Global parameter of the current module
61
# =============================================================================
62
__DEBUG__ = False
63
64
65
__MODULE_DIR__ = os.path.dirname(os.path.abspath(__file__))


66
# =============================================================================
67
# Defining Python MbsData class
68
# =============================================================================
69
70
71
72

class MbsData(object):
    """
    Class containing geometrical and dynamical information of robotran Mbs.
73

74
75
    Attributes
    ----------
76
    body_id : dict
77
        Dictionary containing the names of the bodies as keys and their ids as
78
        values.
79
    DonePart : int
80
        Flag that indicates if the coordinate partitioning module has been
81
        executed (default: 0=not done; 1=done).
82
    dpt : numpy.ndarray
83
        Numpy array containing the coordinates of all anchor points in body
84
        fixed frame: [X_i,Y_i,Z_i] = dpt[1:4,i]
85
    extforce_id : dict
86
        Dictionary containing the names of the extforces as keys and their ids
87
        as values.
88
    Fl : numpy.ndarray
89
        Numpy array containing the current values of the forces on each link.
90
    frc: numpy.ndarray
91
        Numpy array containing the components of the resultant external forces
92
        (in the body fixed frame) applied to the center of mass of each body.
93
    g : numpy.ndarray
94
        The 3 gravity components in the inertial frame: g[1:4]=[g_x, g_y, g_z]
95
    In : numpy.ndarray
96
        Numpy array containing the inertia tensor component of each body (in
97
98
        the body fixed frame, relative to the center of mass). For body k:
        In[1:10, k]=[I_11,I_12,I_13,I_21,I_22,I_23,I_31,I_32,I_33]
99
    joint_id : dict
100
        Dictionary containing the names of the joints as keys and their ids as
101
        values.
102
    l : numpy.ndarray
103
104
        Numpy array containing the center of mass coordinates in the body fixed
        frame. For body k: l[1:4, k]=[gc_x, gc_y, gc_z]
105
    lambda_ : numpy.ndarray
106
        Numpy array containing the values of the Lagrange Multipliers related
107
        to the constraints.
108
    link_id : dict
109
        Dictionary containing the names of the links as keys and their ids as
110
        values.
111
    lrod : numpy.ndarray
112
        Numpy array containing the length of each rod.
113
    m : numpy.ndarray
114
115
116
117
118
119
        Numpy array containing the mass of each body.
    mbs_filename : str
        Path to mbs file including the file with the extension (.mbs).
    mbs_name : str
        Filename of the mbs file (excluding path and ".mbs" extension).
    nbody : int
120
        Number of bodies in the system. The fictious bodies are not taken into
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
        account.
    Ncons : int
        Number of algebraic constraints.
    nhu : int
        Number of independent constraints.
    njoint : int
        Number of joints in the system.
    Nlink : int
        Number of forces acting between two points of the system.
    Nloopc : int
        Number of loop constraints.
    npt : int
        Number of anchor points.
    nqa : int
        Number of actuated articulations.
    nqc : int
        Number of driven articulations, it includes qlocked and qdriven..
    nqdriven : int
        Number of driven articulations.
    nqlocked : int
        Number of locked articulations.
    nqu : int
        Number of independent articulations.
    nqv : int
        Number of dependent articulations.
    NRerr : float
147
        Maximal error on constraint allowed in Newton-Raphson algorithm,
148
149
150
151
152
153
154
155
156
157
158
        default 1.0e-9.
    nrod : int
        Number of rod constraints defined on the system.
    Nsensor : int
        Number of kinematic sensors.
    Nuserc : int
        Number of user constraints.
    Nux : int
        Number of user variable.
    Nxfrc : int
        Number of points where an external force is applied into a body.
159
160
161
162
163
    points_id : dict
        Dictionary containing the ids of the anchor points of each bodies. The
        first key is the body name and the second key is the anchor point name.
        The retrieve value is the id of the requested anchor point.
        Example: my_id = mbs_data.points_id["body_1"]["point_0"]
164
    process : int
165
        Flag that indicate which module is currently running:
166
167
168
        1=partitioning; 2=equilibrium; 3=direct dynamic; 4=modal
    project_path : str
        Path to the mbs project folder.
169
    q : numpy.ndarray
170
        Numpy array containing the current values of the generalized
171
        coordinates.
172
    q0 : numpy.ndarray
173
        Numpy array containing the initial values of the generalized
174
        coordinates.
175
    qa : numpy.ndarray of int
176
177
        Numpy array of integers containing the indices of actuated
        articulations (only for inverse dynamic). Those articulations are
178
        controlled by an actuator.
179
180
    Qa : numpy.ndarray
        Numpy array containing the value of actuation forces.
181
    qc : numpy.ndarray of int
182
        Numpy array of integers containing the indices of driven (locked and
183
        driven) articulations.
184
    Qc : numpy.ndarray
185
186
        Numpy array containing the value of joint force introduced in driven
        joints to respect the user function. The driven forces/torques are
187
        saved in the entries given by index vector 'qc'.
188
    qd : numpy.ndarray
189
        Numpy array containing the current values of the generalized
190
        velocities.
191
    qd0 : numpy.ndarray
192
        Numpy array containing the initial values of the generalized
193
        velocities.
194
    qdd : numpy.ndarray
195
        Numpy array containing the current values of the generalized
196
        accelerations.
197
    qdd0 : numpy.ndarray
198
        Numpy array containing the initial values of the generalized
199
        accelerations.
200
    qdriven : numpy.ndarray of int
201
        Numpy array of integers containing the indices of driven articulations.
202
        Those articulations are controlled by a user function:
203
        q[qdriven]=f(tsim)
204
    qlocked : numpy.ndarray of int
205
206
207
        Numpy array of integers containing the indices of locked articulations.
        Those articulations have a constant position defined by the user:
        q[qlocked]=cte
208
    Qq : numpy.ndarray
209
        Numpy array containing the values of the joint forces.
210
    qu : numpy.ndarray of int
211
        Numpy array of integers containing the indices of the independent
212
        articulations.
213
    qv : numpy.ndarray of int
214
        Numpy array of integers containing the indices of the dependent
215
        articulations.
216
    sensor_id : dict
217
        Dictionary containing the names of the sensors as keys and their ids as
218
        values.
219
    sensors : list of MbsSensor
220
221
        List containing one instance of MbsSensor (without defined id). User
        can append as many as needed sensor in the list. Sensors defined in
222
        this list can be used by the user for various computation.
223
    SWr : numpy.ndarray
224
225
226
227
        Numpy array containing the Swr vector for each external forces.
        for force k, Swr[k,1:10] = [Fx, Fy, Fz, Mx, My, Mz, dxF] with
          - Force components (expressed in the inertial frame) : Fx,Fy,Fz
          - Pure torque components (expressed in the inertial frame) : Mx,My,Mz
228
          - Application point local coordinates vector (expressed in the body
229
230
            fixed frame): dxF[1:4]
    t0 : scalar
231
        Initial time of the simulation [s]. For dirdyn and invdyn only. This
232
233
        parameter is set from dirdyn/invyn option at start of time simulation.
    tf : scalar
234
        Final time of the simulation [s]. For dirdyn and invdyn only. This
235
        parameter is set from dirdyn/invyn option at start of time simulation.
236
    trq : numpy.ndarray
237
        Numpy array containing the components of the resultant external torques
238
        (pure torque and couple produced by forces displacement in the body
239
240
241
        fixed frame) applied to each body.
    tsim : scalar
        The time value.
242
243
244
245
    user_model : dict
        Dictionary containing the names of the user models as keys and a second
        dictionary with their parameters as values. The second dictionary
        contains the names of the parameters as keys and their values as values
246
    ux : numpy.ndarray
247
        Numpy array containing the values of the user variables.
248
    ux0 : numpy.ndarray
249
        Numpy array containing the initial values of the user variables.
250
    uxd : numpy.ndarray
251
        Numpy array containing the values of the time derivative of the user
252
        variables.
253
    xfidpt : numpy.ndarray of int
254
255
        Numpy array of integers containing the indices of the points defined as
        force application points.
256
    Z : numpy.ndarray
257
        Numpy array containing the current values of the distances between of
258
        the points of a link.
259
    Zd : numpy.ndarray
260
        Numpy array containing the current values of the speed (spreading)
261
        between of the points of a link.
262
263


Louis Beauloye's avatar
Louis Beauloye committed
264
265
    Example
    -------
266

267
    >>> mbs_data = MBsysPy.MbsData("../dataR/ExampleProject.mbs")
Louis Beauloye's avatar
Louis Beauloye committed
268
269
    >>> mbs_data.user_model["MyUserModel"]["MyScalar"] = 3.0
    >>> print(mbs_data.user_model["MyUserModel"]["MyScalar"])
270
        3.0
Louis Beauloye's avatar
Louis Beauloye committed
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
    >>> #get a copy of the scalar
    >>> a = mbs_data.user_model["MyUserModel"]["MyScalar"]
    >>> a = 2
    >>> print(mbs_data.user_model["MyUserModel"]["MyScalar"])
        3.0
    >>> print(mbs_data.user_model["MyUserModel"]["MyVector"])
        array([1.0, 3.0])
    >>> mbs_data.user_model["MyUserModel"]["MyVector"] = [1.0, 2.0]
    >>> print(mbs_data.user_model["MyUserModel"]["MyVector"])
        array([1.0, 2.0])
    >>> b = mbs_data.user_model["MyUserModel"]["MyVector"]
    >>> b[1] = 10.0
    >>> print(mbs_data.user_model["MyUserModel"]["MyVector"])
        array([1.0, 10.0])
    >>> b = np.array([2.0, 4.0])
    >>> print(mbs_data.user_model["MyUserModel"]["MyVector"])
        array([1.0, 10.0])
288

289
    """
290
291
292

    def __init__(self, name, user_path=None, symbolic_path=None,
                 prj_functions_c=None, prj_lib_path=None):
293
294
295
296
297
298
299
300
301
302
303
        """
        Load a specific *.mbs file into a MbsData class instance.

        Parameters
        ----------
        name : str
            The path to the .mbs file to load, including the file extension.
        user_path : str, optionnal
            The path to the folder containing the user functions if the default
            project structure are not used.
        symbolic_path : str, optionnal
304
            The path to the folder containing the symbolic functions if the
305
            default project structure are not used.
306
        prj_functions_c : None or str, optionnal
307
            Option to load some of the project functions from the project C
308
309
310
311
312
            library. Accepted values are:
             - None : All project function are defined in python
             - "symbolic_only" or "S" : Symbolic function comes from C library
             - "symbolic_and_user" or "SU" : Symbolic and user functions come from C library
            default is None
313
        prj_lib_path : None or str, optionnal
314
            Required if 'prj_functions_c' is not None
315
            - In Linux, it gives the location of the folder containing the
316
317
318
319
320
              "symbolicR/" and, or "userfctR/" folders. Each one of the subfolder
              have to contain the corresponding library (ie: libProject_symbolic.so).
            - In Windows, it gives the location of the folder containing the project
              libraries (Project_symbolic.dll and/or Project_userfct.dll).
            - In MacOs the expected behavior is the same as Linux. It must be tested.
321
            default is None
322

323
324
        Returns
        -------
325
        out : MBsysPy.MbsData
326
            A loaded mbs file in a MbsData instance.
327
        """
328
329
330
331
332
333
        # Retrieve prj_functions_c value
        if prj_functions_c is None:
            self.opt_load_c = 0
            if not (prj_lib_path is None):
                print("Argument 'prj_lib_path' is ignored as option 'prj_functions_c' is 'None'.")
                prj_lib_path = None
334
335
336
        elif (type(prj_functions_c) is str) or (type(prj_functions_c) is bytes):
            if (type(prj_functions_c) is bytes):
                prj_functions_c = bytes_to_str(prj_functions_c)
337
338
339
340
341
            if prj_functions_c == "symbolic_only" or prj_functions_c == "S":
                self.opt_load_c = 1
            elif prj_functions_c == "symbolic_and_user" or prj_functions_c == "SU":
                self.opt_load_c = 2
            else:
342
343
344
                raise ValueError("prj_functions_c is not valid:'"
                                 + prj_functions_c + "'.")

345
346
347
            if (prj_lib_path is None):
                raise ValueError("prj_lib_path must be given if 'prj_functions_c' is set.")
            if not (type(prj_lib_path) is str):
348
349
                raise TypeError("prj_lib_path type must be str but it is '"
                                + str(type(prj_lib_path)) + "'.")
350
            else:
351
                prj_lib_path = str_to_bytes(os.path.abspath(prj_lib_path))
352
        else:
353
354
            raise TypeError("prj_functions_c type is not valid:'"
                            + str(type(prj_functions_c)) + "'.")
355

356
        # Create a byte object from the string
357
        name_c = str_to_bytes(name)
358
        # Load the MbsInfos and keep a copy
359
360
361
362
        if __DEBUG__:
            print("DEBUG>>  Loading: '" + bytes_to_str(name)
                             + "' from:'" + os.getcwd() + "'.")

363
        self.mbs_infos_ptr = libloadXML.mbs_info_reader(name_c)
364
365
366
367
        if __DEBUG__:
            print("DEBUG>>  mbs_infos structure loaded")

        # Preparing loader and its options for loading
368
        loader = libloadXML.mbs_new_loader()
369
370
371
        if __DEBUG__:
            print("DEBUG>>  Loader structure created")

372
        loader.contents.opts.contents.no_project_fct = self.opt_load_c
373
374
375
        if __DEBUG__:
            print("DEBUG>>  flag ignoring project function set")

376
        loader.contents.mbs_infos = self.mbs_infos_ptr
377
378
379
        if __DEBUG__:
            print("DEBUG>>  mbs_infos assigned to loader")

380
        # Load and retrieve MbsData structure
381
        self.mbs_data_ptr = libloadXML.mbs_load_with_loader(name_c, prj_lib_path, loader)
382
383
384
        if __DEBUG__:
            print("DEBUG>>  MbsData loaded")

385
        libloadXML.mbs_delete_loader(loader)
386
387
388
        # symbolic and user function folder
        self.user_path = None
        self.symbolic_path = None
389

390
391
392
393
394
395
396
397
398
399
400
401
402
403
        # Storing project function pointer
        # User function that only relies on MbsData structure
        self.user_cons_hJ = None
        self.user_cons_jdqd = None
        self.user_derivative = None
        self.user_DrivenJoints = None
        self.user_ExtForces = None
        self.user_JointForces = None
        self.user_LinkForces = None
        self.user_Link3DForces = None
        # User function also related to dirdyn, equil...
        self.user_dirdyn_init = None
        self.user_dirdyn_loop = None
        self.user_dirdyn_finish = None
404

405
406
407
408
409
410
411
412
413
414
415
        # Symbolic functions
        self.mbs_accelred = None
        self.mbs_cons_hJ = None
        self.mbs_cons_jdqd = None
        self.mbs_dirdyna = None
        self.mbs_extforces = None
        self.mbs_gensensor = None
        self.mbs_invdyna = None
        self.mbs_link = None
        self.mbs_link3D = None
        self.mbs_sensor = None
416

417
418
419
        # pointers dict to avoid garbage collecting
        self.ptrs_to_user_fcts = dict()
        self.ptrs_to_symb_fcts = dict()
420

421
        # Exposing some memory
422
423
424
        if __DEBUG__:
            print("DEBUG>>  Exposing MbsData fields")

425
        # Geometric and dynamic datas
426
427
        self.__dpt = None
        if(self.npt):
428
429
430
431
432
433
434
            self.__dpt = np.ctypeslib.as_array(self.mbs_data_ptr.contents.dpt[0],
                                               (3 + 1, self.npt + 1))
        self.__l = np.ctypeslib.as_array(self.mbs_data_ptr.contents.l[0], (3 + 1, self.njoint + 1))
        self.__m = np.ctypeslib.as_array(self.mbs_data_ptr.contents.m, (self.njoint + 1, ))
        self.__In = np.ctypeslib.as_array(self.mbs_data_ptr.contents.In[0], (9 + 1, self.njoint + 1))
        self.__g = np.ctypeslib.as_array(self.mbs_data_ptr.contents.g, (4, ))

435
        # Partitioning informations
436
437
438
439
440
441
442
        self.__qu = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qu, (self.njoint + 1, ))
        self.__qc = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qc, (self.njoint + 1, ))
        self.__qlocked = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qlocked, (self.njoint + 1, ))
        self.__qdriven = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qdriven, (self.njoint + 1, ))
        self.__qa = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qa, (self.njoint + 1, ))
        self.__qv = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qv, (self.njoint + 1, ))

443
        # Generalized coordinates velocities and acceleration
444
445
446
447
448
449
450
451
        self.__q = np.ctypeslib.as_array(self.mbs_data_ptr.contents.q, (self.njoint + 1, ))
        self.__qd = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qd, (self.njoint + 1, ))
        self.__qdd = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qdd, (self.njoint + 1, ))

        self.__q0 = np.ctypeslib.as_array(self.mbs_data_ptr.contents.q0, (self.njoint + 1, ))
        self.__qd0 = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qd0, (self.njoint + 1, ))
        self.__qdd0 = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qdd0, (self.njoint + 1, ))

452
        # Forces array
453
454
455
456
457
        self.__frc = np.ctypeslib.as_array(self.mbs_data_ptr.contents.frc[0], (3 + 1, self.njoint + 1))
        self.__trq = np.ctypeslib.as_array(self.mbs_data_ptr.contents.trq[0], (3 + 1, self.njoint + 1))
        self.__Qq = np.ctypeslib.as_array(self.mbs_data_ptr.contents.Qq, (self.njoint + 1, ))
        self.__Qa = np.ctypeslib.as_array(self.mbs_data_ptr.contents.Qa, (self.njoint + 1, ))

458
459
460
461
462
        # Constraints datas
        self.nrod = 0
        self.__lrod = None
        if (self.mbs_data_ptr.contents.lrod):
            self.nrod = int(round(self.mbs_data_ptr.contents.lrod[0]))
463
464
465
466
467
            self.__lrod = np.ctypeslib.as_array(self.mbs_data_ptr.contents.lrod, (self.nrod + 1, ))

        self.__lambda_ = np.ctypeslib.as_array(self.mbs_data_ptr.contents.lambda_, (self.njoint + 1, ))
        self.__Qc = np.ctypeslib.as_array(self.mbs_data_ptr.contents.Qc, (self.njoint + 1, ))

468
469
470
        # LinksForces
        self.__Z = self.__Zd = self.__Fl = None
        if self.Nlink:
471
472
473
474
            self.__Z = np.ctypeslib.as_array(self.mbs_data_ptr.contents.Z, (self.Nlink + 1, ))
            self.__Zd = np.ctypeslib.as_array(self.mbs_data_ptr.contents.Zd, (self.Nlink + 1, ))
            self.__Fl = np.ctypeslib.as_array(self.mbs_data_ptr.contents.Fl, (self.Nlink + 1, ))

475
476
477
        # ExtForces
        self.__xfidpt = self.__SWr = None
        if self.Nxfrc:
478
479
480
            self.__xfidpt = np.ctypeslib.as_array(self.mbs_data_ptr.contents.xfidpt, (self.Nxfrc + 1, ))
            self.__SWr = np.ctypeslib.as_array(self.mbs_data_ptr.contents.SWr[0], (self.Nxfrc + 1, 9 + 1))

481
482
483
        # User State
        self.__ux = self.__ux0 = self.__uxd = None
        if self.Nux:
484
485
486
487
            self.__ux = np.ctypeslib.as_array(self.mbs_data_ptr.contents.ux, (self.Nux + 1, ))
            self.__ux0 = np.ctypeslib.as_array(self.mbs_data_ptr.contents.ux0, (self.Nux + 1, ))
            self.__uxd = np.ctypeslib.as_array(self.mbs_data_ptr.contents.uxd, (self.Nux + 1, ))

488
        # Adding internal sensor
489
490
        self.sensors = [MbsSensor(self, None)]

491
        # ID
492
        self.body_id = {}
493
494
495
496
        self.joint_id = {}
        self.link_id = {}
        self.extforce_id = {}
        self.sensor_id = {}
497
        self.points_id = {}
498
        self.__generate_id__()
499
500

        # UserModel
Louis Beauloye's avatar
Louis Beauloye committed
501
        self.user_model = _UserModelDict()
502
        self.__load_user_model__()
503

504
        # Loading user function
505
        if self.opt_load_c < 2:
506
507
508
            if __DEBUG__:
                print("DEBUG>>  Loading user functions")

509
            self.__load_user_fct__(user_path)
510
        # Loading symbolic function
511
        if self.opt_load_c < 1:
512
513
514
            if __DEBUG__:
                print("DEBUG>>  Loading symbolic functions")

515
            self.__load_symbolic_fct__(symbolic_path)
516
517

    def __str__(self):
518
519
520
521
        """Return str(self)."""
        if __DEBUG__:
            print("DEBUG>>  start of __str")

522
        libutilities.mbs_print_data(self.mbs_data_ptr)
523
524
525
        if __DEBUG__:
            print("DEBUG>>  MbsData printed")

526
        return ''
527

528
    def __load_user_fct__(self, user_path=None):
529
        """
Olivier Lantsoght's avatar
Olivier Lantsoght committed
530
        Load all user functions where arguments are in MbsData instance.
531
532

        Only load the user functions in which the arguments are not
Olivier Lantsoght's avatar
Olivier Lantsoght committed
533
534
        dependent of another module instance (ie. MbsPart, MbsDirdyn...).
        The other user functions will be loaded when a module is created.
535
536
        The functions will be assigned to the MbsData instance when
        the 'run' function in other modules is called and unassigned
Olivier Lantsoght's avatar
Olivier Lantsoght committed
537
        at the end.
538

Olivier Lantsoght's avatar
Olivier Lantsoght committed
539
        The loader user functions are :
540
541
542
543
544
545
         - user_derivatives (from user_Derivative.py)
         - user_DrivenJoints (from user_DrivenJoints.py)
         - user_ExtForces (from user_ExtForces.py)
         - user_JointForces (from user_JointForces.py)
         - user_LinkForces (from user_LinkForces.py)
         - user_Link3DForces (from user_Link3DForces.py)
546
547
548
549
550
551
552

        Parameters
        ----------
        user_path : str or None, optionnal
            The path to the folder containing the user functions if the default
            project structure are not used.
            default is None
553
        """
554
        template_path = os.path.join(__MODULE_DIR__, '../templates/user')
555
        project_path = self.project_path
556

557
        # Creating user path
558
        if user_path is None:
559
560
561
562
563
            user_path = os.path.join(project_path, "userfctR")
        else:
            user_path = os.path.join(project_path, user_path)
        # Error handeling
        if not os.path.isdir(user_path):
564
565
            print('The user function directory does not exist: "' + user_path + '"')
            print('The current root folder is: "' + os.getcwd() + '"')
566
567
568
        self.user_path = user_path

        # derivative
569
        user_file = "user_Derivative.py"
570
        path = os.path.abspath(os.path.join(user_path, user_file))
571
        if not os.path.isfile(path):
572
573
574
            if __DEBUG__:
                print("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))

575
576
577
            path = os.path.abspath(os.path.join(template_path, user_file))
        module = imp.load_source(user_file[:-3], path)
        self.user_derivative = module.user_derivatives
578
579

        # drivenJoints
580
        user_file = "user_DrivenJoints.py"
581
        path = os.path.abspath(os.path.join(user_path, user_file))
582
        if not os.path.isfile(path):
583
584
585
            if __DEBUG__:
                print("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))

586
587
588
            path = os.path.abspath(os.path.join(template_path, user_file))
        module = imp.load_source(user_file[:-3], path)
        self.user_DrivenJoints = module.user_DrivenJoints
589
590

        # ext_forces
591
        user_file = "user_ExtForces.py"
592
        path = os.path.abspath(os.path.join(user_path, user_file))
593
        if not os.path.isfile(path):
594
595
596
            if __DEBUG__:
                print("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))

597
598
599
            path = os.path.abspath(os.path.join(template_path, user_file))
        module = imp.load_source(user_file[:-3], path)
        self.user_ExtForces = module.user_ExtForces
600
601

        # joint_forces
602
        user_file = "user_JointForces.py"
603
        path = os.path.abspath(os.path.join(user_path, user_file))
604
        if not os.path.isfile(path):
605
606
607
            if __DEBUG__:
                print("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))

608
609
610
            path = os.path.abspath(os.path.join(template_path, user_file))
        module = imp.load_source(user_file[:-3], path)
        self.user_JointForces = module.user_JointForces
611
612

        # link_forces
613
        user_file = "user_LinkForces.py"
614
        path = os.path.abspath(os.path.join(user_path, user_file))
615
        if not os.path.isfile(path):
616
617
618
            if __DEBUG__:
                print("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))

619
620
621
            path = os.path.abspath(os.path.join(template_path, user_file))
        module = imp.load_source(user_file[:-3], path)
        self.user_LinkForces = module.user_LinkForces
622
623

        # link3D_forces
624
        user_file = "user_Link3DForces.py"
625
        path = os.path.abspath(os.path.join(user_path, user_file))
626
        if not os.path.isfile(path):
627
628
629
            if __DEBUG__:
                print("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))

630
631
632
            path = os.path.abspath(os.path.join(template_path, user_file))
        module = imp.load_source(user_file[:-3], path)
        self.user_Link3DForces = module.user_Link3DForces
633

634
        return
635

636
    def __load_symbolic_fct__(self, symb_path=None):
637
        """
Olivier Lantsoght's avatar
Olivier Lantsoght committed
638
        Load all symbolic functions if all args are in MbsData instance.
639
640

        Only load the symbolic functions in which the arguments are not
Olivier Lantsoght's avatar
Olivier Lantsoght committed
641
        dependent of another module instance (ie. MbsPart, MbsDirdyn...).
642
        The other symbolic functions will be loaded when a module is
Olivier Lantsoght's avatar
Olivier Lantsoght committed
643
        instancied.
644
645
        The functions will be assigned to the MbsData instance when
        the 'run' function in other modules is called and unassigned
Olivier Lantsoght's avatar
Olivier Lantsoght committed
646
        at the end.
647

Olivier Lantsoght's avatar
Olivier Lantsoght committed
648
649
650
651
652
653
654
        The loader symbolic functions are :
         - mbs_accelred (from mbs_accelred_MBSNAME.py)
         - extforces (from mbs_extforces_MBSNAME.py)
         - sensor (from mbs_gensensor_MBSNAME.py)
         - sensor (from mbs_sensor_MBSNAME.py)
         - link (from mbs_link_MBSNAME.py)
         - link3D (from mbs_link3D_MBSNAME.py)
655
656
657
658

        Parameters
        ----------
        symbolic_path : str, optionnal
659
            The path to the folder containing the symbolic functions if the
660
661
            default project structure are not used.
            defult is None
662
663
        """
        mbs_name = self.mbs_name
664
        template_path = os.path.join(__MODULE_DIR__, '../templates/symbolic')
665
        project_path = self.project_path
666

667
        # Creating user path
668
        if symb_path is None:
669
670
671
672
673
            symb_path = os.path.join(project_path, "symbolicR")
        else:
            symb_path = os.path.join(project_path, symb_path)
        # Error handeling
        if not os.path.isdir(symb_path):
674
675
            print('The symbolic function directory does not exist: "' + symb_path + '"')
            print('The current root folder is: "' + os.getcwd() + '"')
676
        self.symbolic_path = symb_path
677

678
        # mbs_accelred
679
        symb_file = "mbs_accelred_" + mbs_name + ".py"
680
        path = os.path.abspath(os.path.join(symb_path, symb_file))
681
        if not os.path.isfile(path):
682
683
684
            if __DEBUG__:
                print("DEBUG>>  file '" + symb_file + "' not found in folder '" + os.path.dirname(path))

685
            symb_file = "mbs_accelred_PRJ.py"
686
            path = os.path.abspath(os.path.join(template_path, symb_file))
687
688
        module = imp.load_source(symb_file[:-3], path)
        self.mbs_accelred = module.mbs_accelred
689

690
        # mbs_extforces
691
        symb_file = "mbs_extforces_" + mbs_name + ".py"
692
        path = os.path.abspath(os.path.join(symb_path, symb_file))
693
        if not os.path.isfile(path):
694
695
696
            if __DEBUG__:
                print("DEBUG>>  file '" + symb_file + "' not found in folder '" + os.path.dirname(path))

697
698
699
700
            symb_file = "mbs_extforces_PRJ.py"
            path = os.path.abspath(os.path.join(template_path, symb_file))
        module = imp.load_source(symb_file[:-3], path)
        self.mbs_extforces = module.extforces
701

702
        # mbs_gensensor
703
        symb_file = "mbs_gensensor_" + mbs_name + ".py"
704
        path = os.path.abspath(os.path.join(symb_path, symb_file))
705
        if not os.path.isfile(path):
706
707
708
            if __DEBUG__:
                print("DEBUG>>  file '" + symb_file + "' not found in folder '" + os.path.dirname(path))

709
710
711
712
            symb_file = "mbs_gensensor_PRJ.py"
            path = os.path.abspath(os.path.join(template_path, symb_file))
        module = imp.load_source(symb_file[:-3], path)
        self.mbs_gensensor = module.sensor
713

714
        # mbs_link
715
        symb_file = "mbs_link_" + mbs_name + ".py"
716
        path = os.path.abspath(os.path.join(symb_path, symb_file))
717
        if not os.path.isfile(path):
718
719
720
            if __DEBUG__:
                print("DEBUG>>  file '" + symb_file + "' not found in folder '" + os.path.dirname(path))

721
722
723
724
            symb_file = "mbs_link_PRJ.py"
            path = os.path.abspath(os.path.join(template_path, symb_file))
        module = imp.load_source(symb_file[:-3], path)
        self.mbs_link = module.link
725

726
        # mbs_link3D
727
        symb_file = "mbs_link3D_" + mbs_name + ".py"
728
        path = os.path.abspath(os.path.join(symb_path, symb_file))
729
        if not os.path.isfile(path):
730
731
732
            if __DEBUG__:
                print("DEBUG>>  file '" + symb_file + "' not found in folder '" + os.path.dirname(path))

733
734
735
736
            symb_file = "mbs_link3D_PRJ.py"
            path = os.path.abspath(os.path.join(template_path, symb_file))
        module = imp.load_source(symb_file[:-3], path)
        self.mbs_link3D = module.link3D
737

738
        # mbs_sensor
739
        symb_file = "mbs_sensor_" + mbs_name + ".py"
740
        path = os.path.abspath(os.path.join(symb_path, symb_file))
741
        if not os.path.isfile(path):
742
743
744
            if __DEBUG__:
                print("DEBUG>>  file '" + symb_file + "' not found in folder '" + os.path.dirname(path))

745
746
747
748
            symb_file = "mbs_sensor_PRJ.py"
            path = os.path.abspath(os.path.join(template_path, symb_file))
        module = imp.load_source(symb_file[:-3], path)
        self.mbs_sensor = module.sensor
749

750
    def __assign_user_fct__(self):
751
        """
Olivier Lantsoght's avatar
Olivier Lantsoght committed
752
        Assign all user functions if all args are in MbsData instance.
753
754

        Assign and wrap python user functions in which the arguments are
Olivier Lantsoght's avatar
Olivier Lantsoght committed
755
        not dependent of another module instance (ie. MbsPart, MbsDirdyn...).
756
757
        Store the functions in the MbsData instance and assign the pointer
        of functions in the C structure.
Olivier Lantsoght's avatar
Olivier Lantsoght committed
758
759
        The other user functions will be assigned when a module is
        instancied.
760
        """
761
        data_c_ptr = self.mbs_data_ptr.contents
762
763

        # derivative
764
        self.ptrs_to_user_fcts["user_Derivative"] = user_Derivative_wrap(lambda mbs: self.user_derivative(self))
765
766
767
        data_c_ptr.user_Derivative = self.ptrs_to_user_fcts["user_Derivative"]

        # drivenJoints
768
        self.ptrs_to_user_fcts["user_DrivenJoints"] = user_DrivenJoints_wrap(lambda mbs, t: self.user_DrivenJoints(self, t))
769
770
771
        data_c_ptr.user_DrivenJoints = self.ptrs_to_user_fcts["user_DrivenJoints"]

        # ext_forces
772
        self.ptrs_to_user_fcts["user_ExtForces"] = user_ExtForces_wrap(lambda PxF, RxF, VxF, OMxF, AxF, OMPxF, mbs, tsim, ixF: self.__callback_user_ExtForces(self.user_ExtForces, PxF, RxF, VxF, OMxF, AxF, OMPxF, tsim, ixF))
773
774
775
        data_c_ptr.user_ExtForces = self.ptrs_to_user_fcts["user_ExtForces"]

        # joint_forces
776
        self.ptrs_to_user_fcts["user_JointForces"] = user_JointForces_wrap(lambda mbs, t: self.user_JointForces(self, t))
777
778
779
        data_c_ptr.user_JointForces = self.ptrs_to_user_fcts["user_JointForces"]

        # link_forces
780
        self.ptrs_to_user_fcts["user_LinkForces"] = user_LinkForces_wrap(lambda Z, Zd, mbs, tsim, ilnk: self.user_LinkForces(Z, Zd, self, tsim, ilnk))
781
782
783
        data_c_ptr.user_LinkForces = self.ptrs_to_user_fcts["user_LinkForces"]

        # link3D_forces
784
        self.ptrs_to_user_fcts["user_Link3DForces"] = user_Link3DForces_wrap(lambda PxF, RxF, VxF, OMxF, AxF, OMPxF, mbs, tsim, ixF: self.__callback_user_Link3DForces(self.user_Link3DForces, PxF, RxF, VxF, OMxF, AxF, OMPxF, tsim, ixF))
785
        data_c_ptr.user_Link3DForces = self.ptrs_to_user_fcts["user_Link3DForces"]
786

787
        return
788

789
    def __assign_symb_fct__(self):
790
        """
Olivier Lantsoght's avatar
Olivier Lantsoght committed
791
        Assign all symbolic functions if all args are in MbsData instance.
792
793

        Assign and wrap python symbolic functions in which the arguments are
Olivier Lantsoght's avatar
Olivier Lantsoght committed
794
        not dependent of another module instance (ie. MbsPart, MbsDirdyn...).
795
796
        Store the functions in the MbsData instance and assign the pointer
        of functions in the C structure.
Olivier Lantsoght's avatar
Olivier Lantsoght committed
797
798
        The other user functions will be assigned when a module is
        instancied.
799
        """
800
801
        data_c_ptr = self.mbs_data_ptr.contents

802
        # mbs_accelred
803
        self.ptrs_to_symb_fcts["mbs_accelred"] = mbs_accelred_wrap(lambda mbs, tsim: self.mbs_accelred(self, tsim))
804
        data_c_ptr.mbs_accelred = self.ptrs_to_symb_fcts["mbs_accelred"]
805

806
        # mbs_extforces
807
        self.ptrs_to_symb_fcts["mbs_extforces"] = mbs_extforces_wrap(lambda frc, trq, mbs, tsim: self.mbs_extforces(self.frc, self.trq, self, tsim))
808
        data_c_ptr.mbs_extforces = self.ptrs_to_symb_fcts["mbs_extforces"]
809

810
        # mbs_gensensor
811
        self.ptrs_to_symb_fcts["mbs_gensensor"] = mbs_gensensor_wrap(lambda sens, mbs, isens: self.__callback_mbs_sensor(self.mbs_gensensor, sens, isens))
812
        data_c_ptr.mbs_gensensor = self.ptrs_to_symb_fcts["mbs_gensensor"]
813

814
        # mbs_link
815
        self.ptrs_to_symb_fcts["mbs_link"] = mbs_link_wrap(lambda frc, trq, Flnk, Z, Zd, mbs, tsim: self.mbs_link(self.frc, self.trq, self.Fl, self.Z, self.Zd, self, tsim))
816
        data_c_ptr.mbs_link = self.ptrs_to_symb_fcts["mbs_link"]
817

818
        # mbs_link3D
819
        self.ptrs_to_symb_fcts["mbs_link3D"] = mbs_link3D_wrap(lambda frc, trq, mbs, tsim: self.mbs_link3D(self.frc, self.trq, self, tsim))
820
        data_c_ptr.mbs_link3D = self.ptrs_to_symb_fcts["mbs_link3D"]
821

822
        # mbs_sensor
823
        self.ptrs_to_symb_fcts["mbs_sensor"] = mbs_sensor_wrap(lambda sens, mbs, isens: self.__callback_mbs_sensor(self.mbs_sensor, sens, isens))
824
        data_c_ptr.mbs_sensor = self.ptrs_to_symb_fcts["mbs_sensor"]
825
826
827
828
829
830
831
832

    def __callback_user_ExtForces(self, fun, PxF, RxF, VxF, OMxF, AxF, OMPxF, tsim, ixF):
        __PxF = np.ctypeslib.as_array(PxF, (4, ))
        __RxF = np.ctypeslib.as_array(RxF, (4, 4))
        __VxF = np.ctypeslib.as_array(VxF, (4, ))
        __OMxF = np.ctypeslib.as_array(OMxF, (4, ))
        __AxF = np.ctypeslib.as_array(AxF, (4, ))
        __OMPxF = np.ctypeslib.as_array(OMPxF, (4, ))
833
        SWr = fun(__PxF, __RxF, __VxF, __OMxF, __AxF, __OMPxF, self, tsim, ixF)
834
835
836
837
838
839
840
841
842
        self.SWr[ixF, :] = SWr[:]

    def __callback_user_Link3DForces(self, fun, PxF, RxF, VxF, OMxF, AxF, OMPxF, tsim, ixF):
        __PxF = np.ctypeslib.as_array(PxF, (4, ))
        __RxF = np.ctypeslib.as_array(RxF, (4, 4))
        __VxF = np.ctypeslib.as_array(VxF, (4, ))
        __OMxF = np.ctypeslib.as_array(OMxF, (4, ))
        __AxF = np.ctypeslib.as_array(AxF, (4, ))
        __OMPxF = np.ctypeslib.as_array(OMPxF, (4, ))
843
        SWr = fun(__PxF, __RxF, __VxF, __OMxF, __AxF, __OMPxF, self, tsim, ixF)
844
845
        self.SWr[ixF, 1:7] = SWr[1:7]

846
    def __callback_mbs_sensor(self, fun, sens, isens):
847
        """Do the callback for gensensor, forcesensor and sensor."""
848
        self.__sens.__reassign_memory__(sens)
849
850
851
852
        fun(self.__sens, self, isens)

    def __callback_mbs_invdyna(self, fun, Q, tsim):
        __Q = np.ctypeslib.as_array(Q, (self.njoint + 1, ))
853
        fun(self, tsim, __Q)
854

855
856
    def __unassign_user_fct__(self):
        """