mbs_results.py 16.5 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
# -*- coding: utf-8 -*-
"""
Module to handle results from MBS analysis.

Summary
-------
Define the class MbsResults dedicated to load the MbsBuffer C structure. Such
structures contains the result of the multibody analysis, except for modal
analysis.

The module internal class '_buffer_ids' must be updated to be coherent with the
enumeration in the file "mbs_buffer.h". Currently only the value of 'BUFFER_Q'
is used.
"""
# Author: Robotran Team
# (c) Universite catholique de Louvain, 2021

import os

import numpy as np
from enum import IntEnum, unique

# importing MbsysPy functions
from ..mbs_utilities import bytes_to_str
from ..mbs_utilities import mbs_warning
from ..mbs_utilities import mbs_msg


# =============================================================================
# Global parameter of the current module
# =============================================================================
__DEBUG__ = False
__MODULE_DIR__ = os.path.dirname(os.path.abspath(__file__))


# =============================================================================
# Enumeration of the buffer ids
# =============================================================================
@unique
class _buffer_ids(IntEnum):
    """Enum of the buffer ids.

    This class is used as it ensure no duplicated value. Thought it does not
    prevent value jumps.
    """

    BUFFER_Q = 0
    BUFFER_QD = 1
    BUFFER_QDD = 2
    BUFFER_QQ = 3
    BUFFER_UX = 4
    BUFFER_UXD = 5
    BUFFER_LINK_Z = 6
    BUFFER_LINK_ZD = 7
    BUFFER_LINK_F = 8
    BUFFER_VISU = 9
    BUFFER_QC = 10
    BUFFER_QA = 11
    BUFFER_LAMBDA = 12
    BUFFER_X = 13
    BUFFER_F = 14
    BUFFER_R = 15
    BUFFER_OTHER = 16


# =============================================================================
# Defining Python MbsResults class
# =============================================================================
class MbsResult(object):
    """
    Class containing results of multibody analysis (except for modal analysis).

    The attributes are set to None if they are relevant for the current analysis.

    The user-defined outputs (vector or floats) are stored in the dictionnary
    'outputs'.

    Attributes
    ----------
    q : numpy.ndarray
        Array containing the history of the generalized coordinates.
    qd : numpy.ndarray
        Array containing the history of the generalized velocities.
    qdd : numpy.ndarray
        Array containing the history of the generalized accelerations.
    Lambda  : numpy.ndarray
        Array containing the history of the lagrange multipliers related to the
        constraints.
    mbs : MbsData
        Instance of MbsData from where results where computed.
    Fl : numpy.ndarray
        Array containing the history of the forces in each links.
    outputs : dict
        Dict containing the names (as key) and the history (as value) of the user
        outputs. Each history is a numpy.ndarray with first column being the time.
    Qa : numpy.ndarray
        Array containing the history of the actuation force (or torque) in each
        articulations, the value for non-articulated joints is always 0.
        This field is only relevant for inverse dynamic.
    Qc : numpy.ndarray
        Array containing the history of the force introduced in driven joints
        to respect the user-specified trajectory (position, velocity, acceleration).
        The values for non-driven joints is always 0.
    Qq : numpy.ndarray
        Array containing the history of the joint forces.
    t : numpy.ndarray
        Array containing the time vector.
    ux : numpy.ndarray
        Array containing the history of the user variables.
    uxd : numpy.ndarray
        Array containing the history of the time derivative of the user variables.
    Z : numpy.ndarray
        Array containing the history of the lenght of each link.
    Zd : numpy.ndarray
        Array containing the history of the speed (spreading) of each link.

    Notes
    -----
    The module internal class '_buffer_ids' must be updated to be coherent with
    the enumeration in the file "mbs_buffer.h"
    """

    def __init__(self, mbs):
124
        """Initialize the fields to None."""
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
        self.q = None
        self.qd = None
        self.qdd = None
        self.Qq = None
        self.ux = None
        self.uxd = None
        self.Z = None
        self.Zd = None
        self.Fl = None
        self.Qc = None
        self.Qa = None
        self.Lambda = None
        self.outputs = None
        self.t = None
        self.mbs = mbs

141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
    def clear(self):
        """Reset all data fields to None.

        The mbs is kept.
        """
        self.q = None
        self.qd = None
        self.qdd = None
        self.Qq = None
        self.ux = None
        self.uxd = None
        self.Z = None
        self.Zd = None
        self.Fl = None
        self.Qc = None
        self.Qa = None
        self.Lambda = None
        self.outputs = None
        self.t = None

161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
    def set_buffer(self, buf_id, buf_array, name=None):
        """Set the buffer array in the field corresponding the the buffer id.

        Parameters
        ----------
        buf_id : int
            Value of the buffer id to be set. Buffer id are defined in the
            _buffer_ids class.
        buf_array : numpy.ndarray
            Array with the history of the variable stored in the specified buffer.
        name : str, optionnal
            Name of the user buffer to be set, ignored for other buffer.

        Returns
        -------
        bool
            True if success, False otherwhise.

        """
        # List with ignored buffer ids
        ignored_buffers = [_buffer_ids.BUFFER_X.value,
                           _buffer_ids.BUFFER_F.value,
                           _buffer_ids.BUFFER_R.value,
                           _buffer_ids.BUFFER_VISU.value]
        # Check and set buffer
        if buf_id == _buffer_ids.BUFFER_Q.value:
            self.q = buf_array
        elif buf_id == _buffer_ids.BUFFER_QD.value:
            self.qd = buf_array
        elif buf_id == _buffer_ids.BUFFER_QDD.value:
            self.qdd = buf_array
        elif buf_id == _buffer_ids.BUFFER_QQ.value:
            self.Qq = buf_array
        elif buf_id == _buffer_ids.BUFFER_UX.value:
            self.ux = buf_array
        elif buf_id == _buffer_ids.BUFFER_UXD.value:
            self.uxd = buf_array
        elif buf_id == _buffer_ids.BUFFER_LINK_Z.value:
            self.Z = buf_array
        elif buf_id == _buffer_ids.BUFFER_LINK_ZD.value:
            self.Zd = buf_array
        elif buf_id == _buffer_ids.BUFFER_LINK_F.value:
            self.Fl = buf_array
        elif buf_id == _buffer_ids.BUFFER_QC.value:
            self.Qc = buf_array
        elif buf_id == _buffer_ids.BUFFER_QA.value:
            self.Qa = buf_array
        elif buf_id == _buffer_ids.BUFFER_LAMBDA.value:
            self.Lamda = buf_array
        elif buf_id == _buffer_ids.BUFFER_.value:
            self.outputs[name] = buf_array
        elif buf_id in ignored_buffers:
            pass
        else:
215
            mbs_warning("Invalid buffer index value: {:d}.".format(buf_id))
216
217
218
219
220
221
222
223
224
225
226
227
228
            return False
        return True

    def load_results_from_buffer(self, c_ptr, t0=None, resfilename=''):
        """Load the results from the buffer if completely available.

        Parameters
        ----------
        c_ptr : ctypes.POINTER to either MbsDirdyn_c or MbsSolvekin_c or MbsInvdyn_c
            Pointer to the c-structure of the current module.
            The module must contain the fields 'buffers' (MbsBuffer**), 'bufferNb'
            (int) and 'user_buffer' (MbsGrowingBuffer*).
        t0 : float, optional
229
230
            Initial time (if relevant) of the analysis. If the initial time in the
            buffer does not match the provided value, the buffer are not loaded.
231
            If the value is None, then the buffers are loaded whatever the available
232
            timesteps.
233
234
235
            The default is None.
        resfilename : str, optionnal
            The option of 'resfilename' of the module, this variable is only used
236
            to create the user output vectors name from the user filename.
237
238
239
240
241
242
            The default is '' (empty str).

        Returns
        -------
        bool
            True if the whole analysis was loaded from the buffers.
243
244
            False if nothing was loaded from the buffers in case of incomplete
            or invalid buffers.
245
246
247
248
249
250

        Notes
        -----
        Usually the first buffer is dedicated to generalized coordinates. At the
        exception of equilibrium analysis, where it is the fourth buffer. Even
        if equilibrium analysis is not (yet) handled by this function the loading
251
        iterates on the buffers unil finding the one dedicated to generalized
252
253
        coordinates.

254
        The buffer of the generalized coordinates is used to check if the full
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
        simulation is available in the buffer.
        """
        # get usefull contents
        nb_buffers = c_ptr.contents.bufferNb  # Size of the buffer_list
        buffer_list = c_ptr.contents.buffers  # "list" to each buffer pointer
        user_buffer = c_ptr.contents.user_buffer  # Pointer to the user_buffer

        # Locate generalized coordinate buffer
        index_q = -1
        for i in range(nb_buffers):
            buffer_id = buffer_list[i].contents.id
            if buffer_id == _buffer_ids.BUFFER_Q.value:
                index_q = i
                break

        # Raise warning if it was not located
        if index_q < 0:
            raise RuntimeWarning("Generalized coordinates buffer missing in current module.")
            return False

        # Load generalized buffer
        nb_var = buffer_list[index_q].contents.nx + 1  # Time is not counted in nx.
        nb_steps = buffer_list[index_q].contents.index  # Filled values in the buffer.
        if nb_steps == 0:
            # nb_steps is the number of steps not (yet) save to disk.
            # If the value is 0 it means that all has been saved to disk.
            # But all is also still in memory.
            nb_steps = buffer_list[index_q].contents.size

        # Retrieve time and generalized coordinates.
        q = np.copy(np.ctypeslib.as_array(buffer_list[index_q].contents.tx, (nb_steps, nb_var)))
        t = q[:, 0]
        if t0 is not None and t[0] != t0:
            return False

        # Load each buffer, number of step is constant
        for i in range(nb_buffers):
            nb_var = buffer_list[i].contents.nx + 1  # Time is not counted in nx.
            buffer_array = np.copy(np.ctypeslib.as_array(buffer_list[i].contents.tx, (nb_steps, nb_var)))
            buffer_id = buffer_list[i].contents.id
            name = None

297
            # For user-vector-buffer, retrieve the buffer name
298
299
300
301
302
303
304
305
306
307
            if buffer_id == _buffer_ids.BUFFER_OTHER.value:
                # get filename
                name = bytes_to_str(os.path.basename(buffer_list[i].contents.filename)[:-4])
                # Remove module prefixe from filename
                name = name[len(self.get_options("resfilename")) + 1:]

            # Set the buffer
            self.set_buffer(buffer_id, buffer_array, name)

        # Load scalar user buffer
308
        nb_steps = user_buffer.contents.index  # Filled values in the buffer.
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
        if not nb_steps:
            # nb_steps is the number of steps not (yet) save to disk.
            # If the value is 0 it means that all has been saved to disk.
            # But all is also still in memory.
            nb_steps = user_buffer.contents.size
        nb_var = user_buffer.contents.nx
        for i in range(nb_var):
            user_out = np.copy(np.ctypeslib.as_array(user_buffer.contents.X[i], (1, nb_steps)))
            name = bytes_to_str(user_buffer.contents.names[i])
            self.outputs[name] = user_out[0]

        return True

    def load_results_from_file(self, filename, result_path="resultsR", module=0,
                               user_output=None, user_vector=None):
324
        """Load the results from the files.
325
326
327
328
329
330

        This function is called if the buffers do not contain the full simulation.

        Parameters
        ----------
        filename : str
331
332
333
334
335
336
            The filename of the results to load. It is either the base filename
            (without the suffix and extension) or the name of a file containing
            basic buffers (ie: q, qd, qdd...) to be loaded.
            In the second case the suffix and extension will be stripped to
            retrieve the basefilename.
            It can contain the full path to files, but it will be ignored.
337
338
        result_path : str, optional
            The relative path of the result folder from the project folder.
339
            The default is "resultsR".
340
341
342
343
344
345
        user_output : list
            List containing the user output files to be loaded if not None.
            default is None.
        user_vector : list
            List containing the user vector output files to be loaded if not None.
            default is None.
346
347
348
349
350

        Returns
        -------
        bool
            False if the result path was not located.
351
        """
352
353
354
355
356
357
        # remove extension and suffix if required
        if filename.endswith(".res"):
            index_last_char = filename.rfind('_')
            filename = filename[:index_last_char]

        # remove dirname
358
        baseFileName = os.path.basename(filename)
359
360
361

        # build result path
        project_path = self.mbs.project_path
362
        result_path = os.path.join(project_path, result_path)
363

364
365
        # Error handeling
        if not os.path.isdir(result_path):
366
367
            raise RuntimeWarning('The result directory does not exist, no results loaded.')
            return False
368
369
370
371
372
373
374
375
376
377

        # Generalized coordinates
        CurFile = baseFileName + '_q.res'
        path = os.path.abspath(os.path.join(result_path, CurFile))
        if(os.path.isfile(path)):
            self.q = np.loadtxt(path)
            if np.array(self.q).ndim > 1:
                self.t = self.q[:, 0]
            else:
                self.t = self.q[0]
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
        elif __DEBUG__:
            mbs_msg("DEBUG>>  file '" + CurFile + "' not found in folder '" + os.path.dirname(path))

        # Other base buffers
        ext = '.res'
        base_buffers = {'_qd': _buffer_ids.BUFFER_QD.value,
                        '_qdd': _buffer_ids.BUFFER_QDD.value,
                        '_Qq': _buffer_ids.BUFFER_QQ.value,
                        '_ux': _buffer_ids.BUFFER_UX.value,
                        '_uxd': _buffer_ids.BUFFER_UXD.value,
                        '_linkZ': _buffer_ids.BUFFER_LINK_Z.value,
                        '_linkZD': _buffer_ids.BUFFER_LINK_ZD.value,
                        '_linkF': _buffer_ids.BUFFER_LINK_F.value,
                        '_Qc': _buffer_ids.BUFFER_QC.value,
                        '_Qa': _buffer_ids.BUFFER_QA.value,
                        '_Lambda': _buffer_ids.BUFFER_LAMBDA.value,
                        }

        for suffix, buffer_id in base_buffers.items():
            CurFile = baseFileName + suffix + ext
398
399
            path = os.path.abspath(os.path.join(result_path, CurFile))
            if(os.path.isfile(path)):
400
401
402
403
                file_array = np.loadtxt(path)
                self.set_buffer(buffer_id, file_array, None)
            elif __DEBUG__:
                mbs_msg("DEBUG>>  file '" + CurFile + "' not found in folder '" + os.path.dirname(path))
404
405
406
407
408
409
410
411
412
413
414
415

        if user_output:
            for CurFile in user_output:
                path = os.path.abspath(os.path.join(result_path, CurFile))
                name = CurFile[len(baseFileName) + 1:-4]
                if(os.path.isfile(path)):
                    self.outputs[name] = np.loadtxt(path)[:, 1:]
                else:
                    if __DEBUG__:
                        mbs_msg("DEBUG>>  file '" + CurFile + "' not found in folder '" + os.path.dirname(path))

        if user_vector:
416
            buffer_id = _buffer_ids.BUFFER_OTHER.value
417
            for CurFile in user_vector:
418
419
420
                # Remove the eventual path from filename
                CurFile = os.path.basename(CurFile)

421
                path = os.path.abspath(os.path.join(result_path, CurFile))
422
423
424
425
426
427
428
429
430
431

                # Remove the basefilename
                buffer_name = CurFile
                if buffer_name.startswith(baseFileName):
                    buffer_name = buffer_name[len(baseFileName) + 1:]

                # Remove extension
                if buffer_name.endswith(ext):
                    buffer_name = buffer_name[:-len(ext)]

432
                if(os.path.isfile(path)):
433
434
435
436
                    file_array = np.loadtxt(path)
                    self.set_buffer(buffer_id, file_array, buffer_name)
                elif __DEBUG__:
                    mbs_msg("DEBUG>>  file '" + CurFile + "' not found in folder '" + os.path.dirname(path))