mbs_data.py 144 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
from .._mbsysc_loader.callback import user_invdyn_init_wrap
from .._mbsysc_loader.callback import user_invdyn_loop_wrap
from .._mbsysc_loader.callback import user_invdyn_finish_wrap
65

66
67
68
69
70
71
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
72
73
74
75
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
76

77
# =============================================================================
78
# Global parameter of the current module
79
# =============================================================================
80
__DEBUG__ = False
81
82
83
__MODULE_DIR__ = os.path.dirname(os.path.abspath(__file__))


84
# =============================================================================
85
# Defining Python MbsData class
86
# =============================================================================
87
88
89
90

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

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


289
290
    Examples
    --------
291

292
    >>> mbs_data = MBsysPy.MbsData("../dataR/ExampleProject.mbs")
Louis Beauloye's avatar
Louis Beauloye committed
293
    >>> mbs_data.user_model["MyUserModel"]["MyScalar"] = 3.0
Louis Beauloye's avatar
Louis Beauloye committed
294
    >>> mbs_msg(mbs_data.user_model["MyUserModel"]["MyScalar"])
295
        3.0
Louis Beauloye's avatar
Louis Beauloye committed
296
297
298
    >>> #get a copy of the scalar
    >>> a = mbs_data.user_model["MyUserModel"]["MyScalar"]
    >>> a = 2
Louis Beauloye's avatar
Louis Beauloye committed
299
    >>> mbs_msg(mbs_data.user_model["MyUserModel"]["MyScalar"])
Louis Beauloye's avatar
Louis Beauloye committed
300
301
302
303
304
305
306
307
308
309
310
311
312
        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])
313

314
    """
315
316
317

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

348
349
        Returns
        -------
350
        out : MBsysPy.MbsData
351
            A loaded mbs file in a MbsData instance.
352
        """
353
354
355
356
        # Assing C pointer to NULL
        self.mbs_infos_ptr = None
        self.mbs_data_ptr = None

357
358
359
360
        # Retrieve prj_functions_c value
        if prj_functions_c is None:
            self.opt_load_c = 0
            if not (prj_lib_path is None):
361
                mbs_msg(">>MBsData>> Argument 'prj_lib_path' is ignored as option 'prj_functions_c' is 'None'\n.")
362
                prj_lib_path = None
363
364
365
        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)
366
367
368
369
370
            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:
371
372
373
                raise ValueError("prj_functions_c is not valid:'"
                                 + prj_functions_c + "'.")

374
375
376
            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):
377
378
                raise TypeError("prj_lib_path type must be str but it is '"
                                + str(type(prj_lib_path)) + "'.")
379
            else:
380
                prj_lib_path = str_to_bytes(os.path.abspath(prj_lib_path))
381
        else:
382
383
            raise TypeError("prj_functions_c type is not valid:'"
                            + str(type(prj_functions_c)) + "'.")
384

385
        # Create a byte object from the string
386
        name_c = str_to_bytes(name)
387
        # Load the MbsInfos and keep a copy
388
        if __DEBUG__:
389
            mbs_msg("DEBUG>>  Loading: '" + bytes_to_str(name) + "'\n" +
390
                    "         from:'" + os.getcwd() + "'.\n")
391

392
        self.mbs_infos_ptr = libloadXML.mbs_info_reader(name_c)
393
394
395
396
397
        if not self.mbs_infos_ptr:
            mbs_msg("\n--------------------------------------------------\n"
                    "READ CAREFULLY !!!\n"
                    "--------------------------------------------------\n\n"
                    "MBsysC was unable to create MbsInfos structure.\n"
398
399
400
                    "The messages above give deeper informations on what went wrong.\n"
                    "The messages below gives classic error and error backtrace.\n\n"

401
402
403
404
405
406
                    "Usual errors are:\n"
                    "    - Incorrect file path and name;\n"
                    "    - Corrupted project file;\n"
                    "\n--------------------------------------------------\n"
                    "\n--------------------------------------------------\n")
            raise RuntimeError("MbsData loading failed, read previous messages.")
407
        if __DEBUG__:
408
            mbs_msg("DEBUG>>  mbs_infos structure loaded")
409
410

        # Preparing loader and its options for loading
411
        loader = libloadXML.mbs_new_loader()
412
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
413
            mbs_msg("DEBUG>>  Loader structure created")
414

415
        loader.contents.opts.contents.no_project_fct = self.opt_load_c
416
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
417
            mbs_msg("DEBUG>>  flag ignoring project function set")
418

419
        loader.contents.mbs_infos = self.mbs_infos_ptr
420
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
421
            mbs_msg("DEBUG>>  mbs_infos assigned to loader")
422

423
        # Load and retrieve MbsData structure
424
        self.mbs_data_ptr = libloadXML.mbs_load_with_loader(name_c, prj_lib_path, loader)
425
426
427
428
429
430
431
432
433
434
435
436
437
438
        if not self.mbs_data_ptr:
            mbs_msg("\n--------------------------------------------------\n"
                    "READ CAREFULLY !!!\n"
                    "--------------------------------------------------\n\n"
                    "MBsysC was unable to create MbsData structure.\n"
                    "The messages above give deeper informations on what went wrong.\n"
                    "The messages below gives classic error and error backtrace.\n\n"
                    "Usual errors are:\n"
                    "    - Invalid project file;\n"
                    "    - Invalid path for project C libraries;\n"
                    "\n--------------------------------------------------\n"
                    "\n--------------------------------------------------\n")
            libloadXML.mbs_delete_loader(loader)
            raise RuntimeError("MbsData loading failed, read previous messages.")
439
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
440
            mbs_msg("DEBUG>>  MbsData loaded")
441

442
443
        # Project with link3D are not (yet) compatible
        if self.mbs_data_ptr.contents.Nlink3D:
444
            print("The Project contains 3Dlinks which are an advanced feature not"
445
446
                  " yet supported in MBsysPy.\nUncheck the 'Link 3D' box in "
                  "MBsysPAD, or migrate to MBsysC/MBsysLab.")
447
            raise RuntimeError("ERROR: 3Dlinks are not yet supported in MBsysPy, modify your project.")
448

449
        libloadXML.mbs_delete_loader(loader)
450
451
452
        # symbolic and user function folder
        self.user_path = None
        self.symbolic_path = None
453

454
455
456
        # pointers dict to avoid garbage collecting
        self.ptrs_to_user_fcts = dict()
        self.ptrs_to_symb_fcts = dict()
457

458
459
460
461
        # 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',
462
463
                         'equil_init', 'equil_loop', 'equil_finish', 'equil_fxe',
                         'invdyn_init', 'invdyn_loop', 'invdyn_finish',
464
465
466
467
468
                         ]
        symb_fun_list = ['accelred', 'cons_hJ', 'cons_jdqd', 'invdyna', 'dirdyna',
                         'extforces', 'gensensor', 'link', 'link3D', 'sensor'
                         ]

469
        self.__assign_user_fct_to_none__(user_fun_list)
470
471
        if self.opt_load_c < 2:
            self.__assign_user_to_undefined__(user_fun_list)
472
        self.__assign_symb_fct_to_none__(symb_fun_list)
473
474
475
        if self.opt_load_c < 1:
            self.__assign_symb_to_undefined__(symb_fun_list)

476
        # Exposing some memory
477
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
478
            mbs_msg("DEBUG>>  Exposing MbsData fields")
479

480
        # Geometric and dynamic datas
481
482
        self.__dpt = None
        if(self.npt):
483
484
485
486
487
488
489
            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, ))

490
        # Partitioning informations
491
        self.__qu = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qu, (self.njoint + 1, ))
Louis Beauloye's avatar
Louis Beauloye committed
492
        self.__qu.flags["WRITEABLE"] = False
493
        self.__qc = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qc, (self.njoint + 1, ))
Louis Beauloye's avatar
Louis Beauloye committed
494
        self.__qc.flags["WRITEABLE"] = False
495
        self.__qlocked = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qlocked, (self.njoint + 1, ))
Louis Beauloye's avatar
Louis Beauloye committed
496
        self.__qlocked.flags["WRITEABLE"] = False
497
        self.__qdriven = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qdriven, (self.njoint + 1, ))
Louis Beauloye's avatar
Louis Beauloye committed
498
        self.__qdriven.flags["WRITEABLE"] = False
499
        self.__qa = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qa, (self.njoint + 1, ))
Louis Beauloye's avatar
Louis Beauloye committed
500
        self.__qa.flags["WRITEABLE"] = False
501
        self.__qv = np.ctypeslib.as_array(self.mbs_data_ptr.contents.qv, (self.njoint + 1, ))
Louis Beauloye's avatar
Louis Beauloye committed
502
        self.__qv.flags["WRITEABLE"] = False
503

504
        # Generalized coordinates velocities and acceleration
505
506
507
508
509
510
511
512
        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, ))

513
        # Forces array
514
515
516
517
518
        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, ))

519
520
521
522
523
        # 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]))
524
525
526
527
528
            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, ))

529
530
531
        # LinksForces
        self.__Z = self.__Zd = self.__Fl = None
        if self.Nlink:
532
533
534
535
            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, ))

536
537
538
        # ExtForces
        self.__xfidpt = self.__SWr = None
        if self.Nxfrc:
539
            self.__xfidpt = np.ctypeslib.as_array(self.mbs_data_ptr.contents.xfidpt, (self.Nxfrc + 1, ))
Louis Beauloye's avatar
Louis Beauloye committed
540
            self.__xfidpt.flags["WRITEABLE"] = False
541
542
            self.__SWr = np.ctypeslib.as_array(self.mbs_data_ptr.contents.SWr[0], (self.Nxfrc + 1, 9 + 1))

543
544
545
        # User State
        self.__ux = self.__ux0 = self.__uxd = None
        if self.Nux:
546
547
548
549
            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, ))

550
        # Adding internal sensor
551
552
        self.sensors = [MbsSensor(self, None)]

553
        # ID
554
        self.body_id = {}
555
556
557
558
        self.joint_id = {}
        self.link_id = {}
        self.extforce_id = {}
        self.sensor_id = {}
559
        self.points_id = {}
560
        self.__generate_id__()
561
562

        # UserModel
Louis Beauloye's avatar
Louis Beauloye committed
563
        self.user_model = _UserModelDict()
564
        self.__load_user_model__()
565

566
        # Loading user function
567
        if self.opt_load_c < 2:
568
            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
569
                mbs_msg("DEBUG>>  Loading user functions")
570

Louis Beauloye's avatar
Louis Beauloye committed
571
572
573
574
575
576
577
578
579
            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
580
581
                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
582
            self.user_path = user_path
583

584
        # Loading symbolic function
585
        if self.opt_load_c < 1:
586
            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
587
                mbs_msg("DEBUG>>  Loading symbolic functions")
588

Louis Beauloye's avatar
Louis Beauloye committed
589
590
591
592
593
594
595
596
            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
597
598
                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
599
            self.symbolic_path = symbolic_path
600
601

    def __str__(self):
602
603
        """Return str(self)."""
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
604
            mbs_msg("DEBUG>>  start of __str")
605

606
        libutilities.mbs_print_data(self.mbs_data_ptr)
607
        if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
608
            mbs_msg("DEBUG>>  MbsData printed")
609

610
        return ''
611

612
613
    def __assign_user_fct_to_none__(self, functions):
        """Create fields for user functions fields and set them to None.
614
615
616

        Parameters
        ----------
Louis Beauloye's avatar
Louis Beauloye committed
617
618
        functions : List
            List of strings containing the names of the user functions
619
            to assign.
620

Louis Beauloye's avatar
Louis Beauloye committed
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
        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'
639

640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
        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":
671
                    self.user_equil_loop = None
672
673
                elif fun == "equil_finish":
                    self.user_equil_finish = None
Louis Beauloye's avatar
Louis Beauloye committed
674
                elif fun == "equil_fxe":
675
                    self.user_equil_fxe = None
676
677
678
679
680
681
                elif fun == "invdyn_init":
                    self.user_invdyn_init = None
                elif fun == "invdyn_loop":
                    self.user_invdyn_loop = None
                elif fun == "invdyn_finish":
                    self.user_invdyn_finish = None
682
                else:
Louis Beauloye's avatar
Louis Beauloye committed
683
                    raise TypeError(fun + " is not an existing user function")
684
        else:
Louis Beauloye's avatar
Louis Beauloye committed
685
            raise TypeError('The "functions" parameter must be a list, got a {:}.'.format(type(functions)))
686

687
688
    def __assign_symb_fct_to_none__(self, functions):
        """Create fields for symbolic functions fields and set them to None.
689

690
691
        Parameters
        ----------
692
        functions : list
Louis Beauloye's avatar
Louis Beauloye committed
693
694
            List of strings containing the names of the symbolic functions
            to assign
695

Louis Beauloye's avatar
Louis Beauloye committed
696
697
698
699
700
701
702
703
704
705
706
707
708
        Notes
        -----
        Only the following symbolic functions names can be passed as parameter:
            - 'accelred'
            - 'cons_hJ'
            - 'cons_jdqd'
            - 'invdyna'
            - 'dirdyna'
            - 'extforces'
            - 'gensensor'
            - 'link'
            - 'link3D'
            - 'sensor'
709

710
711
712
        Returns
        -------
        None.
713

714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
        """
        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
738
                    raise TypeError(fun + " is not an existing symbolic function")
739
        else:
Louis Beauloye's avatar
Louis Beauloye committed
740
            raise TypeError('The "functions" parameter must be a list, got a {:}.'.format(type(functions)))
741

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

Louis Beauloye's avatar
Louis Beauloye committed
745
        Load only if not yet loaded by the user.
Louis Beauloye's avatar
Louis Beauloye committed
746
747
        The functions will be assigned to the MbsData instance when the 'run'
        functions is called and unassigned at the end.
748

749
750
        Parameters
        ----------
Louis Beauloye's avatar
Louis Beauloye committed
751
752
753
754
755
        module_dir : str
            The path to the module directory
        functions : List
            List of strings containing the names of the user functions
            to assign
756
757
        user_path: str
            The path to the folder containing the user functions.
Louis Beauloye's avatar
Louis Beauloye committed
758

Louis Beauloye's avatar
Louis Beauloye committed
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
        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'
777
778
779
            - 'invdyn_init'
            - 'invdyn_loop'
            - 'invdyn_finish'
780

781
        """
782
        template_path = os.path.join(module_dir, '../templates/user')
783
784
785
786

        if isinstance(functions, list):
            for fun in functions:
                if fun == "cons_hJ":
787
788
789
790
791
792
                    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
793
                                mbs_msg("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))
794
795

                            path = os.path.abspath(os.path.join(template_path, user_file))
796
797
798
                        spec = importlib.util.spec_from_file_location(user_file[:-3], path)
                        module = importlib.util.module_from_spec(spec)
                        spec.loader.exec_module(module)
799
                        self.user_cons_hJ = module.user_cons_hJ
800
801

                elif fun == "cons_jdqd":
802
803
804
805
806
807
                    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
808
                                mbs_msg("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))
809
810

                            path = os.path.abspath(os.path.join(template_path, user_file))
811
812
813
                        spec = importlib.util.spec_from_file_location(user_file[:-3], path)
                        module = importlib.util.module_from_spec(spec)
                        spec.loader.exec_module(module)
814
                        self.user_cons_jdqd = module.user_cons_jdqd
815
816

                elif fun == "derivative":
817
818
819
820
821
822
                    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
823
                                mbs_msg("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))
824
825

                            path = os.path.abspath(os.path.join(template_path, user_file))
826
827
828
                        spec = importlib.util.spec_from_file_location(user_file[:-3], path)
                        module = importlib.util.module_from_spec(spec)
                        spec.loader.exec_module(module)
829
                        self.user_derivative = module.user_derivatives
830
831

                elif fun == "DrivenJoints":
832
833
834
835
836
837
                    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
838
                                mbs_msg("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))
839
840

                            path = os.path.abspath(os.path.join(template_path, user_file))
841
842
843
                        spec = importlib.util.spec_from_file_location(user_file[:-3], path)
                        module = importlib.util.module_from_spec(spec)
                        spec.loader.exec_module(module)
844
                        self.user_DrivenJoints = module.user_DrivenJoints
845
846

                elif fun == "ExtForces":
847
848
849
850
851
852
                    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
853
                                mbs_msg("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))
854
855

                            path = os.path.abspath(os.path.join(template_path, user_file))
856
857
858
                        spec = importlib.util.spec_from_file_location(user_file[:-3], path)
                        module = importlib.util.module_from_spec(spec)
                        spec.loader.exec_module(module)
859
                        self.user_ExtForces = module.user_ExtForces
860
861

                elif fun == "JointForces":
862
863
864
865
866
867
                    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
868
                                mbs_msg("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))
869
870

                            path = os.path.abspath(os.path.join(template_path, user_file))
871
872
873
                        spec = importlib.util.spec_from_file_location(user_file[:-3], path)
                        module = importlib.util.module_from_spec(spec)
                        spec.loader.exec_module(module)
874
                        self.user_JointForces = module.user_JointForces
875
876

                elif fun == "LinkForces":
877
878
879
880
881
882
                    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
883
                                mbs_msg("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))
884
885

                            path = os.path.abspath(os.path.join(template_path, user_file))
886
887
888
                        spec = importlib.util.spec_from_file_location(user_file[:-3], path)
                        module = importlib.util.module_from_spec(spec)
                        spec.loader.exec_module(module)
889
                        self.user_LinkForces = module.user_LinkForces
890
891

                elif fun == "Link3DForces":
892
893
894
895
896
897
                    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
898
                                mbs_msg("DEBUG>>  file '" + user_file + "' not found in folder '" + os.path.dirname(path))
899
900

                            path = os.path.abspath(os.path.join(template_path, user_file))
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_Link3DForces = module.user_Link3DForces
905
906

                elif fun == 'dirdyn_init':
907
908
909
910
911
                    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
912
                            mbs_msg("file '" + user_file + "' not found in folder '" + os.path.dirname(path))