Commit bab98dc8 authored by Sebastien Timmermans's avatar Sebastien Timmermans 🎹
Browse files

Merge branch 'oneshot_dirdyn' into 'dev'

Oneshot dirdyn option

See merge request robotran/mbsysc!400
parents d8ea67c1 23e06afa
......@@ -12,6 +12,9 @@
* [Py] User vector outputs are now available in the MbsDirdyn results field.
* [Py] Using C symbolic library and Python user is possible even with external forces.
* [Py] Adding missing fields in MbsData.
* [dirdyn][new option] A new option option called 'oneshot' is added.
-> It allows to compute the derivative function only once (no time derivation) at time = dt0.
-> See the 'flag_oneshot' in dirdyn options.
* [dirdyn][Fix] the function user_dirdyn_loop is now called only at the real time steps, even for multisteps integration methods.
* [dirdyn][Fix] the mbs_data->flag_stop is taken into account in all the integrators now, the simulation stops.
* [integrators][dirdyn] Add a new integrator called 'AlphaM' for alpha method. It is explicit, fixed step-size but with two stages and a bit smarter than Euler explicit :-)
......
#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Universite catholique de Louvain
CEREM : Centre for research in mechatronics
http://www.robotran.be
Contact : info@robotran.be
"""Example of multibody system computed with MBsysPy using Python project files.
Main script template for complete model:
-----------------------------------------------
This template loads the data file *.mbs and execute:
Summary
-------
This template loads the data file *.mbs and execute:
- the coordinate partitioning module
- the direct dynamic module (time integration of
equations of motion).
- the direct dynamic module (time integration of equations of motion).
- the equilibrium module
- the modal module
- the inverse dynamic module
It may be adapted and completed by the user.
(c) Universite catholique de Louvain
"""
# Author: Robotran Team
# (c) Universite catholique de Louvain, 2021
#==============================================================================
# %%===========================================================================
# Packages loading
#==============================================================================
# =============================================================================
import sys
sys.path.append("../../../MBsysC/mbs_interface") # Relative path for this example script
# Add path to the MBsysC build folder
sys.path.insert(1, "../../../MBsysC/build/python")
import MBsysPy as Robotran
#==============================================================================
# %%===========================================================================
# Project loading
#==============================================================================
mbs_data = Robotran.MbsData("../dataR/PendulumSpringC.mbs", user_path = "userfctR/userfctR_python", symbolic_path = "symbolicR/symbolicR_python")
# =============================================================================
mbs_data = Robotran.MbsData("../dataR/PendulumSpringC.mbs",
user_path="userfctR/userfctR_python",
symbolic_path="symbolicR/symbolicR_python"
)
print(mbs_data)
#==============================================================================
# %%===========================================================================
# Partitionning
#==============================================================================
# =============================================================================
mbs_data.process = 1
mbs_part = Robotran.MbsPart(mbs_data)
mbs_part.set_options(rowperm = 1, verbose = 1)
mbs_part.set_options(rowperm=1, verbose=1)
mbs_part.run()
#==============================================================================
# %%===========================================================================
# Equilibrium
#==============================================================================
# =============================================================================
mbs_data.process = 2
mbs_equil = Robotran.MbsEquil(mbs_data)
mbs_equil.set_options(method = 1, senstol = 1e-2, verbose = 1)
mbs_equil.set_options(method=1, senstol=1e-2, verbose=1)
mbs_equil.run()
#==============================================================================
# %%===========================================================================
# Modal Analysis
#==============================================================================
# =============================================================================
mbs_data.process = 4
mbs_modal = Robotran.MbsModal(mbs_data)
mbs_modal.set_options(save_result = 1, save_anim = 1, mode_ampl = 0.2)
mbs_modal.set_options(save_result=1, save_anim=1, mode_ampl=0.2)
mbs_modal.run()
#==============================================================================
# Direct Dynamics
#==============================================================================
# %%===========================================================================
# Direct Dynamics, oneshot
# =============================================================================
mbs_data.process = 3
mbs_data.qd[1] = 1.5
mbs_dirdyn = Robotran.MbsDirdyn(mbs_data)
mbs_dirdyn.set_options(flag_oneshot=1)
mbs_dirdyn.run()
# %%===========================================================================
# Direct Dynamics, time integration
# =============================================================================
mbs_data.process = 3
mbs_data.qd[1] = 1.5
mbs_dirdyn = Robotran.MbsDirdyn(mbs_data)
mbs_dirdyn.set_options(dt0 = 1e-3, tf = 10.0, save2file = 1)
mbs_dirdyn.set_options(integrator = "RK4", resfilename = "RK4")
mbs_dirdyn.set_options(dt0=1e-3, tf=0.1, save2file=1)
mbs_dirdyn.set_options(integrator="RK4", resfilename="RK4")
mbs_dirdyn.run()
#==============================================================================
# %%===========================================================================
# Inverse Dynamics
#==============================================================================
# =============================================================================
mbs_data.process = 6
mbs_data.reset()
mbs_data.set_qa(4)
......@@ -82,8 +86,8 @@ mbs_data.set_qa(1)
mbs_data.set_qa(3)
mbs_data.set_qa(2)
mbs_invdyn = Robotran.MbsInvdyn(mbs_data)
mbs_invdyn.set_options(trajectoryqname = "../resultsR/RK4_q.res")
mbs_invdyn.set_options(trajectoryqdname = "../resultsR/RK4_qd.res")
mbs_invdyn.set_options(trajectoryqddname = "../resultsR/RK4_qdd.res")
mbs_invdyn.set_options(t0 = 1.0, tf = 2.0, dt = 2.5e-3)
mbs_invdyn.set_options(trajectoryqname="../resultsR/RK4_q.res")
mbs_invdyn.set_options(trajectoryqdname="../resultsR/RK4_qd.res")
mbs_invdyn.set_options(trajectoryqddname="../resultsR/RK4_qdd.res")
mbs_invdyn.set_options(t0=1.0, tf=2.0, dt=2.5e-3)
mbs_invdyn.run()
......@@ -74,6 +74,8 @@ MbsDirdyn* mbs_new_dirdyn_aux(MbsData* mbs_data, MbsAux* mbs_aux)
opts->compute_all_uxd = 1;
opts->flag_oneshot = 0;
opts->flag_compute_Qc = COMPUTE_ALL_QC;
// Initialization for fast computation of Qc
int i;
......@@ -138,6 +140,38 @@ MbsDirdyn* mbs_new_dirdyn_aux(MbsData* mbs_data, MbsAux* mbs_aux)
return dirdyn;
}
int check_user_dirdyn_options(MbsDirdyn* dd, MbsData* mbs_data)
{
int err = 0;
// errors generated
if (dd->options->t0 > dd->options->tf)
{
err = _MBS_ERR_INIT;
mbs_msg("\t >Dirdyn> options error: t0 (%f) > tf (%f).\n", dd->options->t0, dd->options->tf);
mbs_msg("[%d] in check_user_dirdyn_options !! \n", err);
return err;
}
if (dd->options->dt0 <= 0.0)
{
err = _MBS_ERR_INIT;
mbs_msg("\t >Dirdyn> options error: dt0 (%f) is not strictly positive.\n", dd->options->dt0);
mbs_msg("[%d] in check_user_dirdyn_options !! \n", err);
return err;
}
if (dd->options->t0 == dd->options->tf)
{
err = _MBS_ERR_INIT;
mbs_msg("\t >Dirdyn> options error: t0 (%f) == tf (%f).\n", dd->options->t0, dd->options->tf);
mbs_msg("\t >Dirdyn> if you need one evaluation of the derivative function, see the oneshot option in dirdyn. \n");
mbs_msg("[%d] in check_user_dirdyn_options !! \n", err);
return err;
}
return err;
}
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void mbs_delete_dirdyn(MbsDirdyn* dirdyn, MbsData* mbs_data)
......@@ -158,10 +192,11 @@ void mbs_delete_dirdyn(MbsDirdyn* dirdyn, MbsData* mbs_data)
int mbs_run_dirdyn(MbsDirdyn* dd, MbsData* mbs_data)
{
int err = 0;
// 1. Initialize the simulation
// - - - - - - - - - - - - - -
err = mbs_dirdyn_init(dd, mbs_data);
if (err < 0){
if (err < 0) {
// Freeing the buffers, without error catching
mbs_dirdyn_finish(dd, mbs_data);
mbs_error_msg("[%d] in mbs_run_dirdyn !! \n", _MBS_ERR_MOD_DIRDYN + err);
......@@ -170,14 +205,22 @@ int mbs_run_dirdyn(MbsDirdyn* dd, MbsData* mbs_data)
// 2. Run the simulation
// - - - - - - - - - - -
if (dd->options->flag_oneshot)
{
err = mbs_fct_dirdyn(dd->tsim, dd->y, dd->yd, mbs_data, dd);
}
else
{
err = mbs_dirdyn_loop(dd, mbs_data);
if (err < 0){
}
if (err < 0) {
// Freeing the buffers, without error catching
mbs_dirdyn_finish(dd, mbs_data);
mbs_error_msg("[%d] in mbs_run_dirdyn !! \n", _MBS_ERR_MOD_DIRDYN + err);
return _MBS_ERR_MOD_DIRDYN + err;
}
// 3. Finish the simulation
// - - - - - - - - - - - -
err = mbs_dirdyn_finish(dd, mbs_data);
......@@ -262,6 +305,10 @@ int mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
if (dd->options->verbose)
{
mbs_msg("\n>>DIRDYN>> Starting direct dynamics module.\n");
if (dd->options->flag_oneshot)
{
mbs_msg("\n>>DIRDYN>> One shot evaluation of the direct dynamics is activated.\n");
}
}
// Checking mbs_data coherence
......@@ -377,6 +424,14 @@ int mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
}
user_dirdyn_init(mbs_data, dd);
// Checking time options
err = check_user_dirdyn_options(dd, mbs_data);
if (err < 0) {
mbs_msg("\t >Dirdyn> Incoherence detected during module initialization! \n");
mbs_msg("[%d] in mbs_dirdyn_init !! \n", err);
return err;
}
/* TODO : Move in initialize_integrator when 2nd order integrator will be implemented */
// Simulation state initialization
for (i = 1; i <= mbs_data->nqu; i++)
......@@ -417,9 +472,14 @@ int mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
bufElemNb = (int*)malloc(bufSize * sizeof(int));
// set the filename if not specified
if (dd->options->resfilename == NULL) {
if (dd->options->resfilename == NULL && !dd->options->flag_oneshot) {
resfilename = "dirdyn";
}
else if (dd->options->resfilename == NULL && dd->options->flag_oneshot)
{
resfilename = "oneshot";
mbs_msg("\n>>DIRDYN>> Output filename is called 'oneshot' by default.\n");
}
else {
resfilename = dd->options->resfilename;
}
......@@ -728,7 +788,7 @@ int mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data)
mbs_msg("\t >Dirdyn> Error: in dirdyn loop during save realtime update ! \n");
}
mbs_msg(">>DIRDYN>> [%d] during direct dynamics loop.\n", err);
return (err<0)?err:err2;
return (err < 0) ? err : err2;
}
// Update times
cur_t0 = cur_tf;
......@@ -740,6 +800,7 @@ int mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data)
return 0;
}
}
return 0;
}
......@@ -1014,3 +1075,4 @@ int save_realtime_update(MbsDirdyn* dd, MbsData* mbs_data)
return 0;
}
......@@ -174,4 +174,14 @@ int mbs_fct_dirdyn(double t, double y[], double dydt[], MbsData *s, MbsDirdyn *d
*/
int save_realtime_update(MbsDirdyn* dd, MbsData* mbs_data);
/*! \brief check that the dirdyn options performed by the user are correct
*
* \param[in] dd direct dynamics module
* \param[in] mbs_data Robotran main structure
*
* \return Error status, <0 in case of failure.
*/
int check_user_dirdyn_options(MbsDirdyn* dd, MbsData* mbs_data);
#endif
......@@ -31,7 +31,7 @@ int mbs_estim_jac_acc(double x, double htry, double y[], double dydx[], int comp
int(*derivs)(double, double[], double[], MbsData *, MbsDirdyn *), MbsData *s, MbsDirdyn *dd)
{
// setting the flag to ON
dd->options->flag_ongoing_jac_computation = 1;
dd->flag_ongoing_jac_computation = 1;
int i, j, err;
double perturb;
......@@ -100,7 +100,7 @@ int mbs_estim_jac_acc(double x, double htry, double y[], double dydx[], int comp
}
// setting the flag to OFF
dd->options->flag_ongoing_jac_computation = 0;
dd->flag_ongoing_jac_computation = 0;
return 0;
}
......@@ -154,6 +154,8 @@ void set_integrator(MbsDirdyn *mbs_dd) {
int id = opts->integrator;
if (!opts->flag_oneshot)
{
switch (id) {
case RK4:
......@@ -229,6 +231,14 @@ void set_integrator(MbsDirdyn *mbs_dd) {
mbs_warning_msg(" >> Integrator not found, possibly not yet supported !\n");
mbs_warning_msg(" >> Continuing with RK4 (Default) \n");
}
}
else
{
mbs_msg(" >> Integrator : \t rk4 is set but useless because oneshot option is activated \n");
mbs_dd->initialize_integrator = initialize_rk4;
mbs_dd->loop_integrator = loop_rk4;
mbs_dd->finish_integrator = finish_rk4;
}
}
......
......@@ -49,7 +49,8 @@ typedef struct MbsDirdynOptions
* default = 1
*/
int save2file;
/** The keyword used for determining the name of result files */
/** The keyword used for determining the name of result files
Default: 'dirdyn', or 'oneshot' in case the flag_oneshot is activated*/
char* resfilename;
/** Path in which result file are saved.
* Default: the resultsR folder of the project
......@@ -87,6 +88,10 @@ typedef struct MbsDirdynOptions
int accelred; //!< 1 to use accelred, 0 otherwise, default = 0
int flag_oneshot; //!< 1 to compute the derivative function only once (no time integration !), default = 0
//!< The time is the value found in the option t0
//!< The default resfilename is changed to 'oneshot'.
int flag_compute_Qc; //!< choose to compute (or not) the Qc for driven variables. For dirdyn only. ==1 by default
//!< this allows to go faster
......@@ -126,8 +131,6 @@ typedef struct MbsDirdynOptions
// Options related to integrator that need Jacobian computation
/*TO BE COMPLETED*/
int n_freeze; //!< number of time step when the jacobian is freezed (computed once at the start of the n_freeze time steps), default = 0;
int flag_ongoing_jac_computation; //!< flag is ON when the implicit integrator is computing the jacobian.
//!< it allows the user the make a distinction in his user functions (typically a FSM state that should now change during jacobian computation).
// Various options
int show_failed_closure; //!< 1 to generate animation of the failed Newton-Raphson procedure, default = 0.
......@@ -171,6 +174,9 @@ struct MbsDirdyn
int savePeriodCounter;
// Part related to numerical integrator
int flag_ongoing_jac_computation; //!< flag is ON (=1) when the implicit integrator is currently computing the jacobian, default is 0.
//!< it allows the user the make a distinction in his user functions (typically a FSM state that should now change during jacobian computation).
/** pointer to store integrator structure **/
void *integrator_struct; // set by initialize_integrator
/** pointer to integrator initialize function **/
......@@ -180,6 +186,7 @@ struct MbsDirdyn
/** pointer to integrator closing function **/
finish_integrator_ptr finish_integrator;
};
......
# -*- coding: utf-8 -*-
"""
-------------------------------
(c) Universite catholique de Louvain, 2019
Creation : 2019 by O. Lantsoght
Last update : 2019
version MBsysC v1.11.2
-------------------------------
Portable Python interface to MBsysC using Ctypes.
While generating the libraries:
- SENSORKIN MUST BE UNDEFINED
- REALTIME MUST BE UNDEFINED
- PRJ_FCT_PTR MUST BE DEFINED
Declaration of MBsysC structure related to mbs_dirdyn_struct.h file in MBsysC.
"""
"""Declaration of MBsysC structure related to mbs_dirdyn_struct.h file in MBsysC."""
# Author : Robotran Team
# (c) Universite catholique de Louvain, 2021
import ctypes
......@@ -31,15 +13,15 @@ from .forward_decl import MbsBuffer_c
from .forward_decl import MbsGrowingBuffer_c
#==============================================================================
# =============================================================================
# Global parameter of the current module
#==============================================================================
# =============================================================================
__DEBUG__ = False
#==============================================================================
# =============================================================================
# MbsDirdynOptions_c
#==============================================================================
# =============================================================================
MbsDirdynOptions_c._fields_ = [
("t0", ctypes.c_double),
("tf", ctypes.c_double),
......@@ -56,6 +38,7 @@ MbsDirdynOptions_c._fields_ = [
("buffersize", ctypes.c_int),
("realtime", ctypes.c_int),
("accelred", ctypes.c_int),
("flag_oneshot", ctypes.c_int),
("flag_compute_Qc", ctypes.c_int),
("compute_all_uxd", ctypes.c_int),
("compute_Qc", ctypes.POINTER(ctypes.c_int)),
......@@ -63,6 +46,9 @@ MbsDirdynOptions_c._fields_ = [
("verbose", ctypes.c_int),
("flag_stop_stiff", ctypes.c_int),
("flag_precise_dynamics", ctypes.c_int),
("flag_baumgarte_stabilization", ctypes.c_int),
("baumgarte_alpha", ctypes.c_double),
("baumgarte_beta", ctypes.c_double),
("flag_waypoint", ctypes.c_int),
("flag_solout_wp", ctypes.c_int),
("delta_t_wp", ctypes.c_double),
......@@ -76,10 +62,10 @@ MbsDirdynOptions_c._fields_ = [
]
#==============================================================================
# =============================================================================
# MbsDirdyn_c
#==============================================================================
MbsDirdyn_c._fields_ =[
# =============================================================================
MbsDirdyn_c._fields_ = [
("options", ctypes.POINTER(MbsDirdynOptions_c)),
("mbs_aux", ctypes.POINTER(MbsAux_c)),
("tsim", ctypes.c_double),
......@@ -93,6 +79,11 @@ MbsDirdyn_c._fields_ =[
("user_buffer", ctypes.POINTER(MbsGrowingBuffer_c)),
("bufferNb", ctypes.c_int),
("savedArrays", ctypes.POINTER(ctypes.POINTER(ctypes.c_double))),
("SavePeriodCounter", ctypes.c_int)
# Pointer to integrator related functions are not exposed
("SavePeriodCounter", ctypes.c_int),
("flag_ongoing_jac_computation", ctypes.c_int),
# Access to integrator related functions are not implemented
("integrator_struct", ctypes.c_void_p),
("initialize_integrator_ptr", ctypes.c_void_p),
("loop_integrator_ptr", ctypes.c_void_p),
("finish_integrator_ptr", ctypes.c_void_p),
]
# -*- coding: utf-8 -*-
"""Set the arguments and return type of functions from MBsysC libraries."""
# Author: Robotran Team
# (c) Universite catholique de Louvain, 2019
import ctypes
......@@ -156,6 +157,11 @@ libmodules.mbs_dirdyn_init.restype = ctypes.c_int
libmodules.mbs_dirdyn_loop.argtypes = [ctypes.POINTER(MbsDirdyn_c), ctypes.POINTER(MbsData_c)]
libmodules.mbs_dirdyn_loop.restype = ctypes.c_int
libmodules.mbs_fct_dirdyn.argtypes = [ctypes.c_double, ctypes.POINTER(ctypes.c_double),
ctypes.POINTER(ctypes.c_double),
ctypes.POINTER(MbsData_c), ctypes.POINTER(MbsDirdyn_c)]
libmodules.mbs_fct_dirdyn.restype = ctypes.c_int
libmodules.mbs_dirdyn_finish.argtypes = [ctypes.POINTER(MbsDirdyn_c), ctypes.POINTER(MbsData_c)]
libmodules.mbs_dirdyn_finish.restype = None
......
......@@ -59,6 +59,9 @@ 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
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
from .._mbsysc_loader.callback import mbs_accelred_wrap
from .._mbsysc_loader.callback import mbs_extforces_wrap
......@@ -462,7 +465,8 @@ class MbsData(object):
user_fun_list = ['cons_hJ', 'cons_jdqd', 'derivative', 'DrivenJoints',
'ExtForces', 'JointForces', 'LinkForces', 'Link3DForces',
'dirdyn_init', 'dirdyn_loop', 'dirdyn_finish',
'equil_init', 'equil_loop', 'equil_finish', 'equil_fxe'
'equil_init', 'equil_loop', 'equil_finish', 'equil_fxe',
'invdyn_init', 'invdyn_loop', 'invdyn_finish',
]
symb_fun_list = ['accelred', 'cons_hJ', 'cons_jdqd', 'invdyna', 'dirdyna',
'extforces', 'gensensor', 'link', 'link3D', 'sensor'
......@@ -675,6 +679,12 @@ class MbsData(object):
self.user_equil_finish = None
elif fun == "equil_fxe":
self.user_equil_fxe = None
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
else:
raise TypeError(fun + " is not an existing user function")
else:
......@@ -770,6 +780,9 @@ class MbsData(object):
- 'equil_loop'
- 'equil_finish'
- 'equil_fxe'
- 'invdyn_init'
- 'invdyn_loop'
- 'invdyn_finish'
"""
template_path = os.path.join(module_dir, '../templates/user')
......@@ -1019,6 +1032,57 @@ class MbsData(object):
spec.loader.exec_module(module)
self.user_equil_fxe = module.user_equil_fxe
elif fun == 'invdyn_init':
if self.user_invdyn_init is None:
# user_invdyn
user_file = "user_invdyn.py"
path = os.path.abspath(os.path.join(user_path, user_file))
if not os.path.isfile(path):
mbs_msg("file '" + user_file + "' not found in folder '" + os.path.dirname(path))
path = os.path.abspath(os.path.join(template_path, user_file))
else:
if __DEBUG__:
mbs_msg("DEBUG>> loading file '" + user_file + "' in folder '" + os.path.dirname(path))
spec = importlib.util.spec_from_file_location(user_file[:-3], path)
module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(module)
self.user_invdyn_init = module.user_invdyn_init
elif fun == 'invdyn_loop':
if self.user_invdyn_loop is None:
# user_invdyn
user_file = "user_invdyn.py"
path = os.path.abspath(os.path.join(user_path, user_file))
if not os.path.isfile(path):
mbs_msg("file '" + user_file + "' not found in folder '" + os.path.dirname(path))
path = os.path.abspath(os.path.join(template_path, user_file))
else:
if __DEBUG__:
mbs_msg("DEBUG>> loading file '" + user_file + "' in folder '" + os.path.dirname(path))
spec = importlib.util.spec_from_file_location(user_file[:-3], path)
module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(module)
self.user_invdyn_loop = module.user_invdyn_loop
elif fun == 'invdyn_finish':
if self.user_invdyn_finish is None:
# user_invdyn
user_file = "user_invdyn.py"
path = os.path.abspath(os.path.join(user_path, user_file))
if not os.path.isfile(path):
mbs_msg("file '" + user_file + "' not found in folder '" + os.path.dirname(path))
path = os.path.abspath(os.path.join(template_path, user_file))
else:
if __DEBUG__:
mbs_msg("DEBUG>> loading file '" + user_file + "' in folder '" + os.path.dirname(path))
spec = importlib.util.spec_from_file_location(user_file[:-3], path)
module = importlib.util.module_from_spec(spec)
spec.loader.exec_module(module)
self.user_invdyn_finish = module.user_invdyn_finish