mbs_solvekin.py 24 KB
Newer Older
1
2
3
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
4
Module to handle solvekin analysis on Multibody model.
5

6
7
Summary
-------
8
9
10
11
12
Define the class MbsSolvekin based on the MbsSolvekin structure of MBsysC.
This class has the functions required to manipulate the direct dynamic module.
This include setting the options, running an (or multiple) analysis and freeing
the memory.
"""
13
14
# Author: Robotran Team
# (c) Universite catholique de Louvain, 2019
15
16
17
18
19
20

import os
import ctypes

import numpy as np

21
# importing MbsysPy functions
22
from ..mbs_utilities import str_from_c_pointer
23
24
from ..mbs_utilities import bytes_to_str
from ..mbs_utilities import str_to_bytes
Louis Beauloye's avatar
Louis Beauloye committed
25
from ..mbs_utilities import mbs_msg
26

27
# importing MbsysPy classes
28
from .mbs_results import MbsResult
29
30
from .mbs_invdyn import MbsInvdyn

31

32
# importing libraries
33
from .._mbsysc_loader.loadlibs import libmodules
34
from .._mbsysc_loader.loadlibs import libutilities
35

36
# =============================================================================
37
# Global parameter of the current module
38
# =============================================================================
39
__DEBUG__ = False
40
41
42
__MODULE_DIR__ = os.path.dirname(os.path.abspath(__file__))


43
# =============================================================================
44
# Defining Python MbsSolvekin class
45
# =============================================================================
46
47

class MbsSolvekin(MbsInvdyn):
48
49
    """
    Class of the invdyn computation module.
50

51
52
    See MbsInvdyn class for details.
    """
53

54
    def __init__(self, mbs, user_path=None, symbolic_path=None):
55
56
57
58
        """
        Create an instance of the MbsSolvekin class for the provided MbsData instance.

        See MbsInvdyn.__init__() function for details.
59

60
61
62
63
64
        Returns
        -------
        MbsSolvekin: self
            A MbsSolvekin instance.
        """
65
        self.module_name = type(self).__name__
66

67
68
69
        if __DEBUG__:
            mbs_msg("DEBUG>>  Creating " + self.module_name + " struct for " + mbs.mbs_name + "' MBS.")

70
        self.mbs_solvekin_ptr = libmodules.mbs_new_solvekin(mbs.mbs_data_ptr)
71
72
        if __DEBUG__:
            mbs_msg("DEBUG>>  " + self.module_name + " structure loaded")
73

74
        self.mbs = mbs
75

76
        self.user_path = self.mbs.user_path
77
78
        if user_path is not None:
            project_path = bytes_to_str(self.mbs.project_path)
79
80
81
            user_path = os.path.join(project_path, user_path)
            # Error handeling
            if not os.path.isdir(user_path):
82
83
84
                mbs_msg('The user function directory for direct dynamic module does not exist: "' + user_path + '"')
                mbs_msg('The current root folder is: "' + os.getcwd() + '"')
                mbs_msg('The following directory is used instead: "' + self.user_path + '".')
85
86
87
88
            else:
                self.user_path = user_path
        # Path to user function used by partitionning modue
        self.symbolic_path = self.mbs.symbolic_path
89
90
        if symbolic_path is not None:
            project_path = bytes_to_str(self.mbs.project_path)
91
92
93
            symbolic_path = os.path.join(project_path, symbolic_path)
            # Error handeling
            if not os.path.isdir(symbolic_path):
94
95
96
                mbs_msg('Thesymbolic function directory for direct dynamic module does not exist: "' + symbolic_path + '"')
                mbs_msg('The current root folder is: "' + os.getcwd() + '"')
                mbs_msg('The following directory is used instead: "' + self.symbolic_path + '".')
97
98
            else:
                self.symbolic_path = symbolic_path
99

Louis Beauloye's avatar
Louis Beauloye committed
100
101
102
103
104
105
106
        self.user_fun_list = ['cons_hJ', 'cons_jdqd', 'derivative', 'DrivenJoints',
                              'ExtForces', 'JointForces', 'LinkForces', 'Link3DForces'
                              ]
        self.symb_fun_list = ['accelred', 'cons_hJ', 'cons_jdqd', 'invdyna',
                              'dirdyna', 'extforces', 'gensensor',
                              'link', 'link3D', 'sensor'
                              ]
107

108
109
110
        # Storing Results
        self.results = MbsResult(self.mbs)
        self.store_results = True
111

112
        # Exposing some memory
113
114
115
        if __DEBUG__:
            mbs_msg("DEBUG>>  Exposing " + self.module_name + " fields")

116
117
        # Constraints
        self._h = self._Jac = self._jdqd = None
118
119
120
121
        if self.mbs.Ncons > 0:
            self._h = np.ctypeslib.as_array(self.mbs_solvekin_ptr.contents.mbs_aux.contents.h, (self.mbs.Ncons + 1, ))
            self._Jac = np.ctypeslib.as_array(self.mbs_solvekin_ptr.contents.mbs_aux.contents.Jac[0], (self.mbs.Ncons + 1, self.mbs.njoint + 1))
            self._jdqd = np.ctypeslib.as_array(self.mbs_solvekin_ptr.contents.mbs_aux.contents.jdqd, (self.mbs.Ncons + 1, ))
122
        self._huserc = self._Juserc = self._jdqduserc = None
123
124
125
126
        if self.mbs.Nuserc > 0:
            self._huserc = np.ctypeslib.as_array(self.mbs_solvekin_ptr.contents.mbs_aux.contents.huserc, (self.mbs.Nuserc + 1, ))
            self._Juserc = np.ctypeslib.as_array(self.mbs_solvekin_ptr.contents.mbs_aux.contents.Juserc[0], (self.mbs.Nuserc + 1, self.mbs.njoint + 1))
            self._jdqduserc = np.ctypeslib.as_array(self.mbs_solvekin_ptr.contents.mbs_aux.contents.jdqduserc, (self.mbs.Nuserc + 1, ))
127
        # Fields from equation of motion
128
129
130
131
        self._M = np.ctypeslib.as_array(self.mbs_solvekin_ptr.contents.mbs_aux.contents.M[0], (self.mbs.njoint + 1, self.mbs.njoint + 1))
        self._c = np.ctypeslib.as_array(self.mbs_solvekin_ptr.contents.mbs_aux.contents.c, (self.mbs.njoint + 1, ))
        self._F = np.ctypeslib.as_array(self.mbs_solvekin_ptr.contents.mbs_aux.contents.F, (self.mbs.njoint + 1, ))
        self._phi = np.ctypeslib.as_array(self.mbs_solvekin_ptr.contents.mbs_aux.contents.phi, (self.mbs.njoint + 1, ))
132

133
    def run(self, **kwargs):
134
        """
135
136
137
138
        Run an inverse kinematics analysis.

        Options can be setted with set_options.
        Options can be retrieved with get_options
139

140
        Results are stored in the field results(if store_results == True)
141

142
143
144
145
        Returns
        -------
        self.results: MbsResult
            The MbsResults containing the results of the analysis.
146
        """
147
        # Assign required user functions
148
        if self.mbs.opt_load_c < 2:
149
            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
150
                mbs_msg("DEBUG>>  Loading user functions")
151
152

            self.mbs.__load_user_fct__(__MODULE_DIR__, self.user_fun_list, self.user_path)
153
            self.mbs.__assign_user_fct__(self.user_fun_list, self)
154

155
        # Assign required symbolic functions
156
        if self.mbs.opt_load_c < 1:
157
158
            # Loading symbolic function
            if __DEBUG__:
Louis Beauloye's avatar
Louis Beauloye committed
159
                mbs_msg("DEBUG>>  Loading symbolic functions")
160
161

            self.mbs.__load_symbolic_fct__(__MODULE_DIR__, self.symb_fun_list, self.symbolic_path)
162
            self.mbs.__assign_symb_fct__(self.symb_fun_list, self)
163

164
165
        self.set_options(**kwargs)

166
        error_2 = 0  # Default value
167
        if not self.store_results:
Olivier Lantsoght's avatar
Olivier Lantsoght committed
168
            error_1 = libmodules.mbs_run_solvekin(self.mbs_solvekin_ptr, self.mbs.mbs_data_ptr)
169
        else:
170
171
172
            # save2file forced to 1 because if buffers don't have the complete
            # results, results are loaded from files.
            self.set_options(save2file=1)
Olivier Lantsoght's avatar
Olivier Lantsoght committed
173
174
175
            error_1 = libmodules.mbs_solvekin_init(self.mbs_solvekin_ptr, self.mbs.mbs_data_ptr)
            if (error_1 >= 0):
                error_1 = libmodules.mbs_solvekin_loop(self.mbs_solvekin_ptr, self.mbs.mbs_data_ptr)
176

177
            # Results (buffer) memory is kept BUT FILES WILL BE WRITTEN LATER
178
            results_loaded = False
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
            if error_1 >= 0:
                if self.get_options("save2file"):
                    results_loaded = self.results.load_results_from_buffer(self.mbs_solvekin_ptr,
                                                                           self.get_options("t0"),
                                                                           self.get_options("resfilename"))

                    # If failed to load from buffer, save the user output (and vector) name
                    if not results_loaded:
                        results_filename = bytes_to_str(ctypes.string_at(self.mbs_dirdyn_ptr.contents.buffers[0].contents.filename))
                        # Check if user vector output have been defined
                        user_output_vector_filenames = []
                        nb_user_output_vector = libutilities.get_output_vector_nb()
                        first_buffer_id = self.buffer_nb - nb_user_output_vector
                        for i in range(nb_user_output_vector):
                            vector_name = bytes_to_str(self.mbs_dirdyn_ptr.contents.buffers[first_buffer_id + i].contents.filename)
                            user_output_vector_filenames.append(os.path.basename(vector_name))

                        # Check if user auto output have been used
                        user_output_filenames = []
                        nbOutput = self.mbs_dirdyn_ptr.contents.user_buffer.contents.nx
                        for i in range(nbOutput):
                            name = bytes_to_str(self.mbs_dirdyn_ptr.contents.user_buffer.contents.names[i])
                            user_output_filenames.append(self.get_options("resfilename") + '_' + name + '.res')

                error_2 = libmodules.mbs_solvekin_finish(self.mbs_solvekin_ptr, self.mbs.mbs_data_ptr)

                if error_2 >= 0 and error_1 >= 0 and not results_loaded:
                    mbs_msg("The beginning of the integration is not available in the buffer.\n"
                            "The complete results are loaded from files.\n")
                    self.results.load_results_from_file(results_filename,
                                                        user_output=user_output_filenames,
                                                        user_vector=user_output_vector_filenames)
211

212
        # Unassign user functions
213
        if self.mbs.opt_load_c < 2:
214
            self.mbs.__unassign_user_fct__(self.user_fun_list)
215

216
        # Unassing required symbolic functions
217
        if self.mbs.opt_load_c < 1:
218
            self.mbs.__unassign_symb_fct__(self.symb_fun_list)
219

Olivier Lantsoght's avatar
Olivier Lantsoght committed
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
        if error_1 < 0 or error_2 < 0:
            mbs_msg("\n--------------------------------------------------------------------------------\n"
                    "READ CAREFULLY !!!\n"
                    "--------------------------------------------------------------------------------\n\n"
                    "An error occurs during inverse kinematic module.\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"
                    "    - Incompatible module options;\n"
                    "    - Invalid path to trajectory files;\n"
                    "    - Invalid trajectory files contents;\n"
                    "    - The motion reach a singular configuration;\n"
                    "\n"
                    "If the simulation runs a little you should:\n"
                    "    - Open the results files and check the values obtained before failure.\n"
                    "\n--------------------------------------------------\n"
                    "\n--------------------------------------------------\n")
            raise RuntimeError("MbsSolvekin.run() failed, read previous messages.")

240
        return self.results
241

242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
    def set_user_fct_from_file(self, function_name, user_path, user_file):
        """
        Load a user function from a file chosen by the user instead of the default one in the userfctR folder.

        The function is then unassigned by the module at the end of the run().

        Parameters
        ----------
        function_name : str
            name of the user function to replace.
        user_path : str
            path to the new user function file.
        user_file : str
            name of the new user function file.
        """
        self.mbs.__set_user_fct_from_file__(function_name, user_path, user_file)

    def set_user_fct_from_ptr(self, function_name, user_fct_ptr):
        """
        Load a user function chosen by the user instead of the default one in the userfctR folder.

        The function is then unassigned by the module at the end of the run().

        Parameters
        ----------
        function_name : str
            name of the user function to replace.
        user_fct_ptr : ptr
            new user function pointer.
        """
        self.mbs.__set_user_fct_from_ptr__(function_name, user_fct_ptr)

274
    def __del__(self):
275
        """Delete the object by freeing the C-related memory."""
276
        libmodules.mbs_delete_solvekin(self.mbs_solvekin_ptr, self.mbs.mbs_data_ptr)
277
278
        if __DEBUG__:
            mbs_msg("DEBUG>>  " + self.module_name + " pointer deleted")
279
280
281

    def set_options(self, **kwargs):
        """
282
        Set the specified options for Solvekin module.
283

284
285
        Parameters
        ----------
286
        motion : int
287
288
289
290
            Determines how the evolution of q, qd qdd are provided.
            oneshot (default) : Uses q, qd and qdd at the current time and configuration of the MBS.
            closeloop : Only for inverse kinematics analysis. Same as oneshot but save the iteration of the close loop process (NR).
            trajectory : Compute the evolution of between t0 and tf.
291
        trajectoryqname : char
292
293
294
295
296
297
298
            Give the path and filename (with extention) to the file containing the input trajectory in position of the joints.
            Only required for motion == 'trajectory' if some joints are independant.
            By default, pointer to NULL.
            Requirement on the file:
            First column is the time, other are the joints coordinates.
            Either provide only the independant joint (by increasing index), user_drivenjoint will be called.
            Either provide all the joint, user_drivenjoint will be neglected.
299
        trajectoryqname : char
300
301
302
303
304
305
306
            Give the path and filename (with extention) to the file containing the input trajectory in velocity of the joints.
            Only required for motion == 'trajectory' if some joints are independant.
            By default, pointer to NULL.
            Requirement on the file:
            First column is the time, other are the joints velocities.
            Either provide only the independant joint (by increasing index), user_drivenjoint will be called.
            Either provide all the joint, user_drivenjoint will be neglected.
307
        trajectoryqname : char
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
            Give the path and filename (with extention) to the file containing the input trajectory in acceleration of the joints.
            Only required for motion == 'trajectory' if some joints are independant.
            By default, pointer to NULL.
            Requirement on the file:
            First column is the time, other are the joints accelerations.
            Either provide only the independant joint (by increasing index), user_drivenjoint will be called.
            Either provide all the joint, user_drivenjoint will be neglected.
        t0 : float
            initial time of the simulation, default = 0.0
        tf : float
            final time of the simulation, default = 5.0
        dt0 : float
            Time step, default = 0.001
            For motion == 'trajectory' (for inverse kinematic analysis, there must be at least one independent joint):
            Set to 0.0 to use the time vector of the coordinate input file (MbsSolvekinOptions::t0 and MbsSolvekinOptions::tf are ignored).
            Specify a value to use specific time vecor.
        save2file : int
325
            Determine whether results are written to files on disk (in MbsSolvekinOptions::respath folder):
326
            1: results are saved
327
            0: results are not saved
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
            default = 1
        resfilename : str
            The keyword used for determining the name of result files
        respath : str
            Path in which result file are saved.
            Default: the resultsR folder of the project
        animpath : str
            Path in which anim file is saved.
            Default: the animationR folder of the project
        save_anim : int
            1 to save the anim file
            0 otherwise (only valid if 'save2file' is set to 1)
            default = 1
        framerate : int
            number of frame per second for the .anim file
            default = 1000
        saveperiod : int
            The number of time steps between two buffer records
            default: 1 (every time step are recorded)
        max_save_user : int
            The maximal number of user variables saved
            default: 12
350
        buffersize : int
351
352
353
354
355
356
357
358
359
360
            The number of time step that can be recorded in the buffer.
            Results are written to disk when the buffer is full.
            default: -1
            compute the buffer size for saving results only once at the end according to dt0, t0 and tf.
        verbose : int
            Gives informations during the process.
            default: 1, to disable set it to 0.
        store_results : boolean
            1 to save a copy of the results from the buffers
            default = 1
361

362
        """
363
364
365
366
        def motion(value):
            if(type(value) is int):
                if (value >= 0 and value <= 2):
                    return value
367
                else:
368
369
370
371
372
373
374
375
376
377
                    raise ValueError
            elif type(value) is str:
                if value == "oneshot":
                    return 0
                elif value == "closeloop":
                    return 1
                elif value == "trajectory":
                    return 2
                else:
                    raise ValueError
378
            else:
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
                raise TypeError

        options = {'motion': {'convert': motion, 'c_name': 'motion'},
                   'trajectoryqname': {'convert': str_to_bytes, 'c_name': 'trajectoryqname'},
                   'trajectoryqdname': {'convert': str_to_bytes, 'c_name': 'trajectoryqdname'},
                   'trajectoryqddname': {'convert': str_to_bytes, 'c_name': 'trajectoryqddname'},
                   't0': {'convert': float, 'c_name': 't0'},
                   'tf': {'convert': float, 'c_name': 'tf'},
                   'dt': {'convert': float, 'c_name': 'dt'},
                   'save2file': {'convert': int, 'c_name': 'save2file'},
                   'resfilename': {'convert': str_to_bytes, 'c_name': 'resfilename'},
                   'respath': {'convert': str_to_bytes, 'c_name': 'respath'},
                   'animpath': {'convert': str_to_bytes, 'c_name': 'animpath'},
                   'save_anim': {'convert': int, 'c_name': 'save_anim'},
                   'framerate': {'convert': int, 'c_name': 'framerate'},
                   'saveperiod': {'convert': int, 'c_name': 'saveperiod'},
                   'max_save_user': {'convert': int, 'c_name': 'max_save_user'},
                   'buffersize': {'convert': int, 'c_name': 'buffersize'},
                   'verbose': {'convert': int, 'c_name': 'verbose'},
                   'store_results': {'convert': int, 'c_name': 'store_results'},
                   }

        for key, value in kwargs.items():
            if key not in options:
                raise TypeError("{:} is an invalid option name.".format(key))
            try:
                c_name = options[key]['c_name']
                setattr(self.mbs_solvekin_ptr.contents.options.contents, c_name, options[key]['convert'](value))
            except ValueError as err:
                # if wrong value in motion
                if key == 'motion':
                    raise ValueError('>>SOLVEKIN>>  {:} is not a valid motion'.format(value)).with_traceback(err.__traceback__) from None
                # if error during the cast
                else:
                    raise TypeError("{:} is {:}, can not be casted from {:}.".format(key, options[key]['convert'], type(value))).with_traceback(err.__traceback__) from None

            except AttributeError as err:
                # if error during cast of strings (i.e in str_to_bytes)
                raise TypeError("{:} is str, can not be casted from {:}".format(key, type(value))).with_traceback(err.__traceback__) from None

            except TypeError as err:
                # if wrong type in motion
Louis Beauloye's avatar
Louis Beauloye committed
421
422
                if key == 'motion':
                    raise TypeError("{:} is str or int, can not be casted from {:}.".format(key, type(value))).with_traceback(err.__traceback__) from None
423

424
425
    def get_options(self, *args):
        """
426
        Get the specified options for Solvekin module.
427

428
429
430
        Parameters
        ----------
        The different options specifed in the documentation of set_options
431

432
433
434
435
436
437
438
439
440
441
442
443
444
445
        Returns
        -------
        The value of the different options specifed in the documentation of set_options
        """
        options = []
        for key in args:
            if key == "motion":
                if self.mbs_solvekin_ptr.contents.options.contents.motion == 0:
                    options.append("oneshot")
                elif self.mbs_solvekin_ptr.contents.options.contents.motion == 1:
                    options.append("closeloop")
                elif self.mbs_solvekin_ptr.contents.options.contents.motion == 2:
                    options.append("trajectory")
            elif key == "trajectoryqname":
446
447
448
                address = self.mbs_solvekin_ptr.contents.options.contents.trajectoryqname
                defaut = None
                options.append(str_from_c_pointer(address, defaut))
449
            elif key == "trajectoryqdname":
450
451
452
                address = self.mbs_solvekin_ptr.contents.options.contents.trajectoryqdname
                defaut = None
                options.append(str_from_c_pointer(address, defaut))
453
            elif key == "trajectoryqddname":
454
455
456
                address = self.mbs_solvekin_ptr.contents.options.contents.trajectoryqddname
                defaut = None
                options.append(str_from_c_pointer(address, defaut))
457
458
459
460
461
462
463
464
465
            elif key == "t0":
                options.append(self.mbs_solvekin_ptr.contents.options.contents.t0)
            elif key == "tf":
                options.append(self.mbs_solvekin_ptr.contents.options.contents.tf)
            elif key == "dt":
                options.append(self.mbs_solvekin_ptr.contents.options.contents.dt)
            elif key == "save2file":
                options.append(self.mbs_solvekin_ptr.contents.options.contents.save2file)
            elif key == "resfilename":
466
467
468
                address = self.mbs_solvekin_ptr.contents.options.contents.resfilename
                defaut = 'solvekin'
                options.append(str_from_c_pointer(address, defaut))
469
            elif key == "respath":
470
471
472
                address = self.mbs_solvekin_ptr.contents.options.contents.respath
                defaut = os.path.abspath(os.path.join(self.mbs.project_path, 'resultsR'))
                options.append(str_from_c_pointer(address, defaut))
473
            elif key == "animpath":
474
475
476
                address = self.mbs_solvekin_ptr.contents.options.contents.animpath
                defaut = os.path.abspath(os.path.join(self.mbs.project_path, 'animationR'))
                options.append(str_from_c_pointer(address, defaut))
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
            elif key == "save_anim":
                options.append(self.mbs_solvekin_ptr.contents.options.contents.save_anim)
            elif key == "framerate":
                options.append(self.mbs_solvekin_ptr.contents.options.contents.framerate)
            elif key == "saveperiod":
                options.append(self.mbs_solvekin_ptr.contents.options.contents.saveperiod)
            elif key == "max_save_user":
                options.append(self.mbs_solvekin_ptr.contents.options.contents.max_save_user)
            elif key == "buffersize":
                options.append(self.mbs_solvekin_ptr.contents.options.contents.buffersize)
            elif key == "verbose":
                options.append(self.mbs_solvekin_ptr.contents.options.contents.verbose)
            elif key == "store_results":
                options.append(self.store_results)
            else:
492
                mbs_msg(">>" + self.module_name + ">> The option " + key + " is not defined in this module")
493

494
495
496
497
        if len(options) == 0:
            return
        if len(options) == 1:
            return options[0]
498

499
        return tuple(options)