mbs_data.py 132 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

import os
16
import importlib
17
18
19
20
import ctypes

import numpy as np

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

30
31
# importing MbsysPy classes
from .mbs_sensor import MbsSensor
32
from .mbs_part import MbsPart
33
34
35
36
37
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
38
39

# importing libraries
40
41
from .._mbsysc_loader.loadlibs import libutilities
from .._mbsysc_loader.loadlibs import libloadXML
42

43
44
45
# importing utilities function
from ..mbs_utilities import callback_undefined

46
# importing wrapping function
47
48
from .._mbsysc_loader.callback import user_cons_hJ_wrap
from .._mbsysc_loader.callback import user_cons_jdqd_wrap
49
50
51
52
53
54
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
55
56
57
58
59
60
61
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 user_equil_init_wrap
from .._mbsysc_loader.callback import user_equil_loop_wrap
from .._mbsysc_loader.callback import user_equil_finish_wrap
from .._mbsysc_loader.callback import user_equil_fxe_wrap
62

63
64
65
66
67
68
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
69
70
71
72
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
73

74
# =============================================================================
75
# Global parameter of the current module
76
# =============================================================================
77
__DEBUG__ = False
78
79
80
__MODULE_DIR__ = os.path.dirname(os.path.abspath(__file__))


81
# =============================================================================
82
# Defining Python MbsData class
83
# =============================================================================
84
85
86
87

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

89
90
    Attributes
    ----------
91
    body_id : dict
92
        Dictionary containing the names of the bodies as keys and their ids as
93
        values.
94
    DonePart : int
95
        Flag that indicates if the coordinate partitioning module has been
96
        executed (default: 0=not done; 1=done).
97
    dpt : numpy.ndarray
98
        Numpy array containing the coordinates of all anchor points in body
99
        fixed frame: [X_i,Y_i,Z_i] = dpt[1:4,i]
100
    extforce_id : dict
101
        Dictionary containing the names of the extforces as keys and their ids
102
        as values.
103
    Fl : numpy.ndarray
104
        Numpy array containing the current values of the forces on each link.
105
    frc: numpy.ndarray
106
        Numpy array containing the components of the resultant external forces
107
        (in the body fixed frame) applied to the center of mass of each body.
108
    g : numpy.ndarray
109
        The 3 gravity components in the inertial frame: g[1:4]=[g_x, g_y, g_z]
110
    In : numpy.ndarray
111
        Numpy array containing the inertia tensor component of each body (in
112
113
        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]
114
    joint_id : dict
115
        Dictionary containing the names of the joints as keys and their ids as
116
        values.
117
    l : numpy.ndarray
118
119
        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]
120
    lambda_ : numpy.ndarray
121
        Numpy array containing the values of the Lagrange Multipliers related
122
123
        to the constraints. The sequence is the same sequence as the constraint
        definition.
124
    link_id : dict
125
        Dictionary containing the names of the links as keys and their ids as
126
        values.
127
    lrod : numpy.ndarray
128
        Numpy array containing the length of each rod.
129
    m : numpy.ndarray
130
131
132
133
134
135
        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
136
        Number of bodies in the system. The fictious bodies are not taken into
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
        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
163
        Maximal error on constraint allowed in Newton-Raphson algorithm,
164
165
166
167
168
169
170
171
172
173
174
        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.
175
176
177
178
179
    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"]
180
    process : int
181
        Flag that indicate which module is currently running:
182
183
184
        1=partitioning; 2=equilibrium; 3=direct dynamic; 4=modal
    project_path : str
        Path to the mbs project folder.
185
    q : numpy.ndarray
186
        Numpy array containing the current values of the generalized
187
        coordinates.
188
    q0 : numpy.ndarray
189
        Numpy array containing the initial values of the generalized
190
        coordinates.
191
    qa : numpy.ndarray of int
192
193
        Numpy array of integers containing the indices of actuated
        articulations (only for inverse dynamic). Those articulations are
194
        controlled by an actuator.
195
196
    Qa : numpy.ndarray
        Numpy array containing the value of actuation forces.
197
    qc : numpy.ndarray of int
198
        Numpy array of integers containing the indices of driven (locked and
199
        driven) articulations.
200
    Qc : numpy.ndarray
201
202
        Numpy array containing the value of joint force introduced in driven
        joints to respect the user function. The driven forces/torques are
203
        saved in the entries given by index vector 'qc'.
204
    qd : numpy.ndarray
205
        Numpy array containing the current values of the generalized
206
        velocities.
207
    qd0 : numpy.ndarray
208
        Numpy array containing the initial values of the generalized
209
        velocities.
210
    qdd : numpy.ndarray
211
        Numpy array containing the current values of the generalized
212
        accelerations.
213
    qdd0 : numpy.ndarray
214
        Numpy array containing the initial values of the generalized
215
        accelerations.
216
    qdriven : numpy.ndarray of int
217
        Numpy array of integers containing the indices of driven articulations.
218
        Those articulations are controlled by a user function:
219
        q[qdriven]=f(tsim)
220
    qlocked : numpy.ndarray of int
221
222
223
        Numpy array of integers containing the indices of locked articulations.
        Those articulations have a constant position defined by the user:
        q[qlocked]=cte
224
    Qq : numpy.ndarray
225
        Numpy array containing the values of the joint forces.
226
    qu : numpy.ndarray of int
227
        Numpy array of integers containing the indices of the independent
228
        articulations.
229
    qv : numpy.ndarray of int
230
        Numpy array of integers containing the indices of the dependent
231
        articulations.
232
    sensor_id : dict
233
        Dictionary containing the names of the sensors as keys and their ids as
234
        values.
235
    sensors : list of MbsSensor
236
237
        List containing one instance of MbsSensor (without defined id). User
        can append as many as needed sensor in the list. Sensors defined in
238
        this list can be used by the user for various computation.
239
    SWr : numpy.ndarray
240
241
242
243
        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
244
          - Application point local coordinates vector (expressed in the body
245
246
            fixed frame): dxF[1:4]
    t0 : scalar
247
        Initial time of the simulation [s]. For dirdyn and invdyn only. This
248
249
        parameter is set from dirdyn/invyn option at start of time simulation.
    tf : scalar
250
        Final time of the simulation [s]. For dirdyn and invdyn only. This
251
        parameter is set from dirdyn/invyn option at start of time simulation.
252
    trq : numpy.ndarray
253
        Numpy array containing the components of the resultant external torques
254
        (pure torque and couple produced by forces displacement in the body
255
256
257
        fixed frame) applied to each body.
    tsim : scalar
        The time value.
258
259
260
261
    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
262
    ux : numpy.ndarray
263
        Numpy array containing the values of the user variables.
264
    ux0 : numpy.ndarray
265
        Numpy array containing the initial values of the user variables.
266
    uxd : numpy.ndarray
267
        Numpy array containing the values of the time derivative of the user
268
        variables.
269
    xfidpt : numpy.ndarray of int
270
271
        Numpy array of integers containing the indices of the points defined as
        force application points.
272
    Z : numpy.ndarray
273
        Numpy array containing the current values of the distances between of
274
        the points of a link.
275
    Zd : numpy.ndarray
276
        Numpy array containing the current values of the speed (spreading)
277
        between of the points of a link.
278
279


Louis Beauloye's avatar
Louis Beauloye committed
280
281
    Example
    -------
282

283
    >>> mbs_data = MBsysPy.MbsData("../dataR/ExampleProject.mbs")
Louis Beauloye's avatar
Louis Beauloye committed
284
    >>> mbs_data.user_model["MyUserModel"]["MyScalar"] = 3.0
Louis Beauloye's avatar
Louis Beauloye committed
285
    >>> mbs_msg(mbs_data.user_model["MyUserModel"]["MyScalar"])
286
        3.0
Louis Beauloye's avatar
Louis Beauloye committed
287
288
289
    >>> #get a copy of the scalar
    >>> a = mbs_data.user_model["MyUserModel"]["MyScalar"]
    >>> a = 2
Louis Beauloye's avatar
Louis Beauloye committed
290
    >>> mbs_msg(mbs_data.user_model["MyUserModel"]["MyScalar"])
Louis Beauloye's avatar
Louis Beauloye committed
291
        3.0
Louis Beauloye's avatar
Louis Beauloye committed
292
    >>> print(mbs_data.user_model["MyUserModel"]["MyVector"])
Louis Beauloye's avatar
Louis Beauloye committed
293
294
        array([1.0, 3.0])
    >>> mbs_data.user_model["MyUserModel"]["MyVector"] = [1.0, 2.0]
Louis Beauloye's avatar
Louis Beauloye committed
295
    >>> print(mbs_data.user_model["MyUserModel"]["MyVector"])
Louis Beauloye's avatar
Louis Beauloye committed
296
297
298
        array([1.0, 2.0])
    >>> b = mbs_data.user_model["MyUserModel"]["MyVector"]
    >>> b[1] = 10.0
Louis Beauloye's avatar
Louis Beauloye committed
299
    >>> print(mbs_data.user_model["MyUserModel"]["MyVector"])
Louis Beauloye's avatar
Louis Beauloye committed
300
301
        array([1.0, 10.0])
    >>> b = np.array([2.0, 4.0])
Louis Beauloye's avatar
Louis Beauloye committed
302
    >>> print(mbs_data.user_model["MyUserModel"]["MyVector"])
Louis Beauloye's avatar
Louis Beauloye committed
303
        array([1.0, 10.0])
304

305
    """
306
307
308

    def __init__(self, name, user_path=None, symbolic_path=None,
                 prj_functions_c=None, prj_lib_path=None):
309
310
311
312
313
314
315
316
317
318
319
        """
        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
320
            The path to the folder containing the symbolic functions if the
321
            default project structure are not used.
322
        prj_functions_c : None or str, optionnal
323
            Option to load some of the project functions from the project C
324
325
326
327
328
            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
329
        prj_lib_path : None or str, optionnal
330
            Required if 'prj_functions_c' is not None
331
            - In Linux, it gives the location of the folder containing the
332
333
334
335
336
              "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.
337
            default is None
338

339
340
        Returns
        -------
341
        out : MBsysPy.MbsData
342
            A loaded mbs file in a MbsData instance.
343
        """
344
345
346
347
        # Retrieve prj_functions_c value
        if prj_functions_c is None:
            self.opt_load_c = 0
            if not (prj_lib_path is None):
Louis Beauloye's avatar
Louis Beauloye committed
348
                mbs_msg("Argument 'prj_lib_path' is ignored as option 'prj_functions_c' is 'None'.")
349
                prj_lib_path = None
350
351
352
        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)
353
354
355
356
357
            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:
358
359
360
                raise ValueError("prj_functions_c is not valid:'"
                                 + prj_functions_c + "'.")

361
362
363
            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):
364
365
                raise TypeError("prj_lib_path type must be str but it is '"
                                + str(type(prj_lib_path)) + "'.")
366
            else:
367
                prj_lib_path = str_to_bytes(os.path.abspath(prj_lib_path))
368
        else:
369
370
            raise TypeError("prj_functions_c type is not valid:'"
                            + str(type(prj_functions_c)) + "'.")
371

372
        # Create a byte object from the string
373
        name_c = str_to_bytes(name)
374
        # Load the MbsInfos and keep a copy
375
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
376
            mbs_msg("DEBUG>>  Loading: '" + bytes_to_str(name)
377
378
                             + "' from:'" + os.getcwd() + "'.")

379
        self.mbs_infos_ptr = libloadXML.mbs_info_reader(name_c)
380
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
381
            mbs_msg("DEBUG>>  mbs_infos structure loaded")
382
383

        # Preparing loader and its options for loading
384
        loader = libloadXML.mbs_new_loader()
385
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
386
            mbs_msg("DEBUG>>  Loader structure created")
387

388
        loader.contents.opts.contents.no_project_fct = self.opt_load_c
389
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
390
            mbs_msg("DEBUG>>  flag ignoring project function set")
391

392
        loader.contents.mbs_infos = self.mbs_infos_ptr
393
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
394
            mbs_msg("DEBUG>>  mbs_infos assigned to loader")
395

396
        # Load and retrieve MbsData structure
397
        self.mbs_data_ptr = libloadXML.mbs_load_with_loader(name_c, prj_lib_path, loader)
398
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
399
            mbs_msg("DEBUG>>  MbsData loaded")
400

401
402
403
404
405
406
407
        # Project with link3D are not (yet) compatible
        if self.mbs_data_ptr.contents.Nlink3D:
            print("The system contains 3Dlinks which are an advanced feature not"
                  " yet supported in MBsysPy.\nUncheck the 'Link 3D' box in "
                  "MBsysPAD, or migrate to MBsysC/MBsysLab.")
            raise RuntimeError("ERROR: 3Dlinks are not yet supported in MBsysPy, modify the multibody sytem.")

408
        libloadXML.mbs_delete_loader(loader)
409
410
411
        # symbolic and user function folder
        self.user_path = None
        self.symbolic_path = None
412

413
414
415
        # pointers dict to avoid garbage collecting
        self.ptrs_to_user_fcts = dict()
        self.ptrs_to_symb_fcts = dict()
416

417
418
419
420
421
422
423
424
425
426
        # Creating field for all project function.
        user_fun_list = ['cons_hJ', 'cons_jdqd', 'derivative', 'DrivenJoints',
                         'ExtForces', 'JointForces', 'LinkForces', 'Link3DForces',
                         'dirdyn_init', 'dirdyn_loop', 'dirdyn_finish',
                         'equil_init', 'equil_loop', 'equil_finish', 'equil_fxe'
                         ]
        symb_fun_list = ['accelred', 'cons_hJ', 'cons_jdqd', 'invdyna', 'dirdyna',
                         'extforces', 'gensensor', 'link', 'link3D', 'sensor'
                         ]

427
        self.__assign_user_fct_to_none__(user_fun_list)
428
429
        if self.opt_load_c < 2:
            self.__assign_user_to_undefined__(user_fun_list)
430
        self.__assign_symb_fct_to_none__(symb_fun_list)
431
432
433
        if self.opt_load_c < 1:
            self.__assign_symb_to_undefined__(symb_fun_list)

434
        # Exposing some memory
435
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
436
            mbs_msg("DEBUG>>  Exposing MbsData fields")
437

438
        # Geometric and dynamic datas
439
440
        self.__dpt = None
        if(self.npt):
441
442
443
444
445
446
447
            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, ))

448
        # Partitioning informations
449
        self.__qu = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qu, (self.njoint + 1, ))
Louis Beauloye's avatar
Louis Beauloye committed
450
        self.__qu.flags["WRITEABLE"] = False
451
        self.__qc = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qc, (self.njoint + 1, ))
Louis Beauloye's avatar
Louis Beauloye committed
452
        self.__qc.flags["WRITEABLE"] = False
453
        self.__qlocked = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qlocked, (self.njoint + 1, ))
Louis Beauloye's avatar
Louis Beauloye committed
454
        self.__qlocked.flags["WRITEABLE"] = False
455
        self.__qdriven = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qdriven, (self.njoint + 1, ))
Louis Beauloye's avatar
Louis Beauloye committed
456
        self.__qdriven.flags["WRITEABLE"] = False
457
        self.__qa = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qa, (self.njoint + 1, ))
Louis Beauloye's avatar
Louis Beauloye committed
458
        self.__qa.flags["WRITEABLE"] = False
459
        self.__qv = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qv, (self.njoint + 1, ))
Louis Beauloye's avatar
Louis Beauloye committed
460
        self.__qv.flags["WRITEABLE"] = False
461

462
        # Generalized coordinates velocities and acceleration
463
464
465
466
467
468
469
470
        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, ))

471
        # Forces array
472
473
474
475
476
        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, ))

477
478
479
480
481
        # 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]))
482
483
484
485
486
            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, ))

487
488
489
        # LinksForces
        self.__Z = self.__Zd = self.__Fl = None
        if self.Nlink:
490
491
492
493
            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, ))

494
495
496
        # ExtForces
        self.__xfidpt = self.__SWr = None
        if self.Nxfrc:
497
            self.__xfidpt = np.ctypeslib.as_array(self.mbs_data_ptr.contents.xfidpt, (self.Nxfrc + 1, ))
Louis Beauloye's avatar
Louis Beauloye committed
498
            self.__xfidpt.flags["WRITEABLE"] = False
499
500
            self.__SWr = np.ctypeslib.as_array(self.mbs_data_ptr.contents.SWr[0], (self.Nxfrc + 1, 9 + 1))

501
502
503
        # User State
        self.__ux = self.__ux0 = self.__uxd = None
        if self.Nux:
504
505
506
507
            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, ))

508
        # Adding internal sensor
509
510
        self.sensors = [MbsSensor(self, None)]

511
        # ID
512
        self.body_id = {}
513
514
515
516
        self.joint_id = {}
        self.link_id = {}
        self.extforce_id = {}
        self.sensor_id = {}
517
        self.points_id = {}
518
        self.__generate_id__()
519
520

        # UserModel
Louis Beauloye's avatar
Louis Beauloye committed
521
        self.user_model = _UserModelDict()
522
        self.__load_user_model__()
523

524
        # Loading user function
525
        if self.opt_load_c < 2:
526
            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
527
                mbs_msg("DEBUG>>  Loading user functions")
528

Louis Beauloye's avatar
Louis Beauloye committed
529
530
531
532
533
534
535
536
537
            project_path = self.project_path

            # Creating user path
            if user_path is None:
                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):
Louis Beauloye's avatar
Louis Beauloye committed
538
539
                mbs_msg('The user function directory does not exist: "' + user_path + '"')
                mbs_msg('The current root folder is: "' + os.getcwd() + '"')
Louis Beauloye's avatar
Louis Beauloye committed
540
541
            self.user_path = user_path

542
        # Loading symbolic function
543
        if self.opt_load_c < 1:
544
            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
545
                mbs_msg("DEBUG>>  Loading symbolic functions")
546

Louis Beauloye's avatar
Louis Beauloye committed
547
548
549
550
551
552
553
554
            project_path = self.project_path
            # Creating user path
            if symbolic_path is None:
                symbolic_path = os.path.join(project_path, "symbolicR")
            else:
                symbolic_path = os.path.join(project_path, symbolic_path)
            # Error handeling
            if not os.path.isdir(symbolic_path):
Louis Beauloye's avatar
Louis Beauloye committed
555
556
                mbs_msg('The symbolic function directory does not exist: "' + symbolic_path + '"')
                mbs_msg('The current root folder is: "' + os.getcwd() + '"')
Louis Beauloye's avatar
Louis Beauloye committed
557
            self.symbolic_path = symbolic_path
558
559

    def __str__(self):
560
561
        """Return str(self)."""
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
562
            mbs_msg("DEBUG>>  start of __str")
563

564
        libutilities.mbs_print_data(self.mbs_data_ptr)
565
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
566
            mbs_msg("DEBUG>>  MbsData printed")
567

568
        return ''
569

570
571
    def __assign_user_fct_to_none__(self, functions):
        """Create fields for user functions fields and set them to None.
572
573
574

        Parameters
        ----------
Louis Beauloye's avatar
Louis Beauloye committed
575
576
        functions : List
            List of strings containing the names of the user functions
577
            to assign.
578

Louis Beauloye's avatar
Louis Beauloye committed
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
        Notes
        -----
        Only the following user functions names can be passed as parameter:
            - 'cons_hJ'
            - 'cons_jdqd'
            - 'derivative'
            - 'DrivenJoints'
            - 'ExtForces'
            - 'JointForces'
            - 'LinkForces'
            - 'Link3DForces'
            - 'dirdyn_init'
            - 'dirdyn_loop'
            - 'dirdyn_finish'
            - 'equil_init'
            - 'equil_loop'
            - 'equil_finish'
            - 'equil_fxe'

598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
        Returns
        -------
        None.
        """
        if isinstance(functions, list):
            for fun in functions:
                if fun == "cons_hJ":
                    self.user_cons_hJ = None
                elif fun == "cons_jdqd":
                    self.user_cons_jdqd = None
                elif fun == "derivative":
                    self.user_derivative = None
                elif fun == "DrivenJoints":
                    self.user_DrivenJoints = None
                elif fun == "ExtForces":
                    self.user_ExtForces = None
                elif fun == "JointForces":
                    self.user_JointForces = None
                elif fun == "LinkForces":
                    self.user_LinkForces = None
                elif fun == "Link3DForces":
                    self.user_Link3DForces = None
                elif fun == "dirdyn_init":
                    self.user_dirdyn_init = None
                elif fun == "dirdyn_loop":
                    self.user_dirdyn_loop = None
                elif fun == "dirdyn_finish":
                    self.user_dirdyn_finish = None
                elif fun == "equil_init":
                    self.user_equil_init = None
                elif fun == "equil_loop":
629
                    self.user_equil_loop = None
630
631
                elif fun == "equil_finish":
                    self.user_equil_finish = None
Louis Beauloye's avatar
Louis Beauloye committed
632
                elif fun == "equil_fxe":
633
                    self.user_equil_fxe = None
634
                else:
Louis Beauloye's avatar
Louis Beauloye committed
635
                    raise TypeError(fun + " is not an existing user function")
636
        else:
Louis Beauloye's avatar
Louis Beauloye committed
637
            raise TypeError('The "functions" parameter must be a list, got a {:}.'.format(type(functions)))
638

639
640
    def __assign_symb_fct_to_none__(self, functions):
        """Create fields for symbolic functions fields and set them to None.
641
642
643

        Parameters
        ----------
644
        functions : list
Louis Beauloye's avatar
Louis Beauloye committed
645
646
            List of strings containing the names of the symbolic functions
            to assign
647

Louis Beauloye's avatar
Louis Beauloye committed
648
649
650
651
652
653
654
655
656
657
658
659
660
661
        Notes
        -----
        Only the following symbolic functions names can be passed as parameter:
            - 'accelred'
            - 'cons_hJ'
            - 'cons_jdqd'
            - 'invdyna'
            - 'dirdyna'
            - 'extforces'
            - 'gensensor'
            - 'link'
            - 'link3D'
            - 'sensor'

662
663
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
        Returns
        -------
        None.

        """
        if isinstance(functions, list):
            for fun in functions:
                if fun == "accelred":
                    self.mbs_accelred = None
                elif fun == "cons_hJ":
                    self.mbs_cons_hJ = None
                elif fun == "cons_jdqd":
                    self.mbs_cons_jdqd = None
                elif fun == "dirdyna":
                    self.mbs_dirdyna = None
                elif fun == "extforces":
                    self.mbs_extforces = None
                elif fun == "gensensor":
                    self.mbs_gensensor = None
                elif fun == "invdyna":
                    self.mbs_invdyna = None
                elif fun == "link":
                    self.mbs_link = None
                elif fun == "link3D":
                    self.mbs_link3D = None
                elif fun == "sensor":
                    self.mbs_sensor = None
                else:
Louis Beauloye's avatar
Louis Beauloye committed
690
                    raise TypeError(fun + " is not an existing symbolic function")
691
        else:
Louis Beauloye's avatar
Louis Beauloye committed
692
            raise TypeError('The "functions" parameter must be a list, got a {:}.'.format(type(functions)))
693

Louis Beauloye's avatar
Louis Beauloye committed
694
    def __load_user_fct__(self, module_dir, functions, user_path):
Louis Beauloye's avatar
Louis Beauloye committed
695
        """Load specified user functions in MbsData instance.
696

Louis Beauloye's avatar
Louis Beauloye committed
697
        Load only if not yet loaded by the user.
Louis Beauloye's avatar
Louis Beauloye committed
698
699
        The functions will be assigned to the MbsData instance when the 'run'
        functions is called and unassigned at the end.
700
701
702

        Parameters
        ----------
Louis Beauloye's avatar
Louis Beauloye committed
703
704
705
706
707
        module_dir : str
            The path to the module directory
        functions : List
            List of strings containing the names of the user functions
            to assign
708
709
        user_path: str
            The path to the folder containing the user functions.
Louis Beauloye's avatar
Louis Beauloye committed
710

Louis Beauloye's avatar
Louis Beauloye committed
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
        Notes
        -----
        Only the following user functions names can be passed as parameter:
            - 'cons_hJ'
            - 'cons_jdqd'
            - 'derivative'
            - 'DrivenJoints'
            - 'ExtForces'
            - 'JointForces'
            - 'LinkForces'
            - 'Link3DForces'
            - 'dirdyn_init'
            - 'dirdyn_loop'
            - 'dirdyn_finish'
            - 'equil_init'
            - 'equil_loop'
            - 'equil_finish'
            - 'equil_fxe'

730
        """
731
        template_path = os.path.join(module_dir, '../templates/user')
732
733
734
735

        if isinstance(functions, list):
            for fun in functions:
                if fun == "cons_hJ":
736
737
738
739
740
741
                    if self.user_cons_hJ is None:
                        # cons_hJ
                        user_file = "user_cons_hJ.py"
                        path = os.path.abspath(os.path.join(user_path, user_file))
                        if not os.path.isfile(path):
                            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
742
                                mbs_msg("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))
743
744

                            path = os.path.abspath(os.path.join(template_path, user_file))
745
746
747
                        spec = importlib.util.spec_from_file_location(user_file[:-3], path)
                        module = importlib.util.module_from_spec(spec)
                        spec.loader.exec_module(module)
748
                        self.user_cons_hJ = module.user_cons_hJ
749
750

                elif fun == "cons_jdqd":
751
752
753
754
755
756
                    if self.user_cons_jdqd is None:
                        # cons_jdqd
                        user_file = "user_cons_jdqd.py"
                        path = os.path.abspath(os.path.join(user_path, user_file))
                        if not os.path.isfile(path):
                            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
757
                                mbs_msg("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))
758
759

                            path = os.path.abspath(os.path.join(template_path, user_file))
760
761
762
                        spec = importlib.util.spec_from_file_location(user_file[:-3], path)
                        module = importlib.util.module_from_spec(spec)
                        spec.loader.exec_module(module)
763
                        self.user_cons_jdqd = module.user_cons_jdqd
764
765

                elif fun == "derivative":
766
767
768
769
770
771
                    if self.user_derivative is None:
                        # derivative
                        user_file = "user_Derivative.py"
                        path = os.path.abspath(os.path.join(user_path, user_file))
                        if not os.path.isfile(path):
                            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
772
                                mbs_msg("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))
773
774

                            path = os.path.abspath(os.path.join(template_path, user_file))
775
776
777
                        spec = importlib.util.spec_from_file_location(user_file[:-3], path)
                        module = importlib.util.module_from_spec(spec)
                        spec.loader.exec_module(module)
778
                        self.user_derivative = module.user_derivatives
779
780

                elif fun == "DrivenJoints":
781
782
783
784
785
786
                    if self.user_DrivenJoints is None:
                        # drivenJoints
                        user_file = "user_DrivenJoints.py"
                        path = os.path.abspath(os.path.join(user_path, user_file))
                        if not os.path.isfile(path):
                            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
787
                                mbs_msg("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))
788
789

                            path = os.path.abspath(os.path.join(template_path, user_file))
790
791
792
                        spec = importlib.util.spec_from_file_location(user_file[:-3], path)
                        module = importlib.util.module_from_spec(spec)
                        spec.loader.exec_module(module)
793
                        self.user_DrivenJoints = module.user_DrivenJoints
794
795

                elif fun == "ExtForces":
796
797
798
799
800
801
                    if self.user_ExtForces is None:
                        # ext_forces
                        user_file = "user_ExtForces.py"
                        path = os.path.abspath(os.path.join(user_path, user_file))
                        if not os.path.isfile(path):
                            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
802
                                mbs_msg("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))
803
804

                            path = os.path.abspath(os.path.join(template_path, user_file))
805
806
807
                        spec = importlib.util.spec_from_file_location(user_file[:-3], path)
                        module = importlib.util.module_from_spec(spec)
                        spec.loader.exec_module(module)
808
                        self.user_ExtForces = module.user_ExtForces
809
810

                elif fun == "JointForces":
811
812
813
814
815
816
                    if self.user_JointForces is None:
                        # joint_forces
                        user_file = "user_JointForces.py"
                        path = os.path.abspath(os.path.join(user_path, user_file))
                        if not os.path.isfile(path):
                            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
817
                                mbs_msg("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))
818
819

                            path = os.path.abspath(os.path.join(template_path, user_file))
820
821
822
                        spec = importlib.util.spec_from_file_location(user_file[:-3], path)
                        module = importlib.util.module_from_spec(spec)
                        spec.loader.exec_module(module)
823
                        self.user_JointForces = module.user_JointForces
824
825

                elif fun == "LinkForces":
826
827
828
829
830
831
                    if self.user_LinkForces is None:
                        # link_forces
                        user_file = "user_LinkForces.py"
                        path = os.path.abspath(os.path.join(user_path, user_file))
                        if not os.path.isfile(path):
                            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
832
                                mbs_msg("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))
833
834

                            path = os.path.abspath(os.path.join(template_path, user_file))
835
836
837
                        spec = importlib.util.spec_from_file_location(user_file[:-3], path)
                        module = importlib.util.module_from_spec(spec)
                        spec.loader.exec_module(module)
838
                        self.user_LinkForces = module.user_LinkForces
839
840

                elif fun == "Link3DForces":
841
842
843
844
845
846
                    if self.user_Link3DForces is None:
                        # link3D_forces
                        user_file = "user_Link3DForces.py"
                        path = os.path.abspath(os.path.join(user_path, user_file))
                        if not os.path.isfile(path):
                            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
847
                                mbs_msg("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))
848
849

                            path = os.path.abspath(os.path.join(template_path, user_file))
850
851
852
                        spec = importlib.util.spec_from_file_location(user_file[:-3], path)
                        module = importlib.util.module_from_spec(spec)
                        spec.loader.exec_module(module)
853
                        self.user_Link3DForces = module.user_Link3DForces
854
855

                elif fun == 'dirdyn_init':
856
857
858
859
860
                    if self.user_dirdyn_init is None:
                        # user_dirdyn
                        user_file = "user_dirdyn.py"
                        path = os.path.abspath(os.path.join(user_path, user_file))
                        if not os.path.isfile(path):
Louis Beauloye's avatar
Louis Beauloye committed
861
                            mbs_msg("file '" + user_file + "' not found in folder '" + os.path.dirname(path))
862
863
864
                            path = os.path.abspath(os.path.join(template_path, user_file))
                        else:
                            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
865
                                mbs_msg("DEBUG>>  loading file '" + user_file + "' in folder '" + os.path.dirname(path))
866

867
868
869
                        spec = importlib.util.spec_from_file_location(user_file[:-3], path)
                        module = importlib.util.module_from_spec(spec)
                        spec.loader.exec_module(module)
870
                        self.user_dirdyn_init = module.user_dirdyn_init
871
872

                elif fun == 'dirdyn_loop':
873
874
875
876
877
                    if self.user_dirdyn_loop is None:
                        # user_dirdyn
                        user_file = "user_dirdyn.py"
                        path = os.path.abspath(os.path.join(user_path, user_file))
                        if not os.path.isfile(path):
Louis Beauloye's avatar
Louis Beauloye committed
878
                            mbs_msg("file '" + user_file + "' not found in folder '" + os.path.dirname(path))
879
880
881
                            path = os.path.abspath(os.path.join(template_path, user_file))
                        else:
                            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
882
                                mbs_msg("DEBUG>>  loading file '" + user_file + "' in folder '" + os.path.dirname(path))
883

884
885
886
                        spec = importlib.util.spec_from_file_location(user_file[:-3], path)
                        module = importlib.util.module_from_spec(spec)
                        spec.loader.exec_module(module)
887
                        self.user_dirdyn_loop = module.user_dirdyn_loop
888
889

                elif fun == 'dirdyn_finish':
890
891
892
893
894
                    if self.user_dirdyn_finish is None:
                        # user_dirdyn
                        user_file = "user_dirdyn.py"
                        path = os.path.abspath(os.path.join(user_path, user_file))
                        if not os.path.isfile(path):
Louis Beauloye's avatar
Louis Beauloye committed
895
                            mbs_msg("file '" + user_file + "' not found in folder '" + os.path.dirname(path))
896
897
898
                            path = os.path.abspath(os.path.join(template_path, user_file))
                        else:
                            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
899
                                mbs_msg("DEBUG>>  loading file '" + user_file + "' in folder '" + os.path.dirname(path))
900

901
902
903
                        spec = importlib.util.spec_from_file_location(user_file[:-3], path)
                        module = importlib.util.module_from_spec(spec)
                        spec.loader.exec_module(module)
904
                        self.user_dirdyn_finish = module.user_dirdyn_finish
905
906

                elif fun == 'equil_init':
907
908
909
910
                    if self.user_equil_init is None:
                        user_file = "user_equil.py"
                        path = os.path.abspath(os.path.join(user_path, user_file))
                        if not os.path.isfile(path):
Louis Beauloye's avatar
Louis Beauloye committed
911
                            mbs_msg("file '" + user_file + "' not found in folder '" + os.path.dirname(path))
912
913
914
                            path = os.path.abspath(os.path.join(template_path, user_file))
                        else:
                            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
915
                                mbs_msg("DEBUG>>  loading file '" + user_file + "' in folder '" + os.path.dirname(path))
916

917
918
919
                        spec = importlib.util.spec_from_file_location(user_file[:-3], path)
                        module = importlib.util.module_from_spec(spec)
                        spec.loader.exec_module(module)
920
                        self.user_equil_init = module.user_equil_init