Commit 0851e8ac authored by Olivier Lantsoght's avatar Olivier Lantsoght 🏁
Browse files

[MbsInvDyn][NewErr] Python not checked

parent e89b4156
......@@ -124,37 +124,60 @@ int mbs_run_invdyn(MbsInvdyn* invd, MbsData* mbs_data)
return err;
}
int mbs_invdyn_save(MbsInvdyn* invd, MbsData *mbs_data, double t) {
int i, err = 0;
/**
* \brief Save the values of invdyn analysis in the buffers.
*
* \param invd the MbsInvdyn to be stored in the buffers.
* \param mbs_data the MbsData structure of the model for which the inverse dynamic
* is computed.
* \param t the current time.
*
* \return Error status, <0 in case of failure.
*/
int mbs_invdyn_save(MbsInvdyn* invd, MbsData *mbs_data, double t) {
if (!invd->buffers){
return 0;
}
int i, err = 0, err1 = 0 , err2 = 0;
for (i = 0; i < invd->bufferNb; i++) {
err = mbs_buffer_save(invd->buffers[i], t, invd->savedArrays[i]);
if(!err1){ // Store only one buffer error.
err1 = err;
}
}
if (err < 0) // Error management
{
err += _MBS_ERR_MOD_INV_DYN;
// Ensure trying user buffer before returning the function.
err2 = mbs_growing_buffer_save(invd->user_buffer, t);
mbs_msg(">>INVDYN>> ***** mbs_invdyn_save : impossible to save the files *****\n");
mbs_msg(">>INVDYN>> ***** during buffer save *****\n");
mbs_msg("[%d] in mbs_fct_invdyn !! \n", err);
return err;
if (err1 < 0 && err2 <0){ // Error management
err1 += _MBS_ERR_MOD_INV_DYN;
err2 += _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> mbs_invdyn_save: error [%d] while buffering the modules results.\n", err1);
mbs_msg(">>INVDYN>> mbs_invdyn_save: error [%d] while buffering the user results.\n", err2);
return err1; // arbitrary choice
}
else if (err1 < 0)
{
err1 += _MBS_ERR_MOD_INV_DYN;
err = mbs_growing_buffer_save(invd->user_buffer, t);
mbs_msg(">>INVDYN>> mbs_invdyn_save: error [%d] while buffering the modules results.\n", err);
if (err < 0) // Error management
return err1;
}
else if (err2 < 0) // Error management
{
err += _MBS_ERR_MOD_INV_DYN;
err2 += _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> ***** mbs_invdyn_save : impossible to save the files *****\n");
mbs_msg(">>INVDYN>> ***** during growing buffer save *****\n");
mbs_msg(">>INVDYN>> mbs_invdyn_save: error [%d] while buffering the user results.\n", err2);
mbs_msg("[%d] in mbs_fct_invdyn !! \n", -600 + err);
return err;
return err2;
}
return err;
}
......@@ -643,59 +666,46 @@ int mbs_invdyn_loop(MbsInvdyn* invd, MbsData* mbs_data)
print_ivec_0(&mbs_data->qa[1], mbs_data->nqa);
mbs_msg("\n");
}
int err = 0, err2 = 0, i;
int err = 0, i;
// In case of error, the function mbs_invdyn_finish will write the buffer.
// The function mbs_invdyn_finish is called by the mbs_invdyn_run.
if (!invd->trajectorytype) {
// Oneshot case, just call the mbs_fct_invdyn
err = mbs_fct_invdyn(mbs_data, invd);
if (err < 0) // Error management
{
mbs_msg(">>INVDYN>>\n");
mbs_msg(">>INVDYN>> Error during inverse dynamics analysis at time %g for oneshot case !\n", mbs_data->tsim);
mbs_error_msg("[%d] in mbs_invdyn_loop !! \n", -600 + err);
exit(1);
mbs_msg(">>INVDYN>> Error during inverse dynamics analysis at time %g for oneshot case !\n", mbs_data->tsim);
mbs_msg(" >> [%d] in mbs_invdyn_loop !! \n", err);
return err;
}
// Check contents of vectors
// adding to buffer
mbs_invdyn_save(invd, mbs_data, mbs_data->tsim);
// Check contents of vectors, buffers will be written by finish function
for (i = 1; i <= mbs_data->njoint; i++) {
if (isnan(mbs_data->q[i])) { // Error management
err = _MBS_ERR_LOW_NAN + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> q[%d] is Nan at time %g s ! \n", i, mbs_data->tsim);
err2 = mbs_invdyn_write_buffers(invd);
if (err2 < 0) {
mbs_msg(">>INVDYN>>\n");
mbs_msg(">>INVDYN>> Error [%d] while saving results to disk (coordinates)!\n", -600 + err2);
mbs_msg(">>INVDYN>>\n");
}
mbs_error_msg("[%d] in mbs_invdyn_loop !! \n", -637);
exit(1);
}
if (isnan(mbs_data->qd[i])) {
err = _MBS_ERR_LOW_NAN + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> qd[%d] is Nan at time %g s ! \n", i, mbs_data->tsim);
err2 = mbs_invdyn_write_buffers(invd);
if (err2 < 0) {
mbs_msg(">>INVDYN>>\n");
mbs_msg(">>INVDYN>> Error [%d] while saving results to disk (velocities)!\n", -600 + err2);
mbs_msg(">>INVDYN>>\n");
}
mbs_error_msg("[%d] in mbs_invdyn_loop !! \n", -637);
exit(1);
}
if (isnan(mbs_data->qdd[i])) {
err = _MBS_ERR_LOW_NAN + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> qdd[%d] is Nan at time %g s ! \n", i, mbs_data->tsim);
err2 = mbs_invdyn_write_buffers(invd);
if (err2 < 0) {
mbs_msg(">>INVDYN>>\n");
mbs_msg(">>INVDYN>> Error [%d] while saving results to disk (accelerations)!\n", -600 + err2);
mbs_msg(">>INVDYN>>\n");
}
mbs_error_msg("[%d] in mbs_invdyn_loop !! \n", -637);
exit(1);
}
}
if (err < 0){
mbs_msg(">>INVDYN>> [%d] in mbs_invdyn_loop !! \n", err);
// adding to buffer
mbs_invdyn_save(invd, mbs_data, mbs_data->tsim);
return err;
return err;
}
}
else {
// Time loop, time updated at the end of loop
......@@ -710,65 +720,34 @@ int mbs_invdyn_loop(MbsInvdyn* invd, MbsData* mbs_data)
if (err < 0) // Error management
{
mbs_msg(">>INVDYN>> Error [%d] during inverse dynamics analysis at time %g for trajectory case !\n", err, mbs_data->tsim);
err2 = mbs_invdyn_write_buffers(invd);
if (err2 < 0) {
err2 += _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> Error [%d] while saving results to disk !\n", err2);
}
mbs_msg(">>INVDYN>> Error during inverse dynamics analysis at time %g for trajectory case !\n", err, mbs_data->tsim);
mbs_msg(" >> [%d] in mbs_invdyn_loop !! \n", err);
return err;
}
// Check contents of vectors
// adding to buffer
mbs_invdyn_save(invd, mbs_data, mbs_data->tsim);
for (i = 1; i <= mbs_data->njoint; i++) {
if (isnan(mbs_data->q[i])) {
if (isnan(mbs_data->q[i])) { // Error management
err = _MBS_ERR_LOW_NAN + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> q[%d] is Nan at time %g s ! \n", i, mbs_data->tsim);
err2 = mbs_invdyn_write_buffers(invd);
if (err2 < 0) {
err2 += _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> Error [%d] while saving results to disk !\n", err2);
}
mbs_msg("[%d] in mbs_invdyn_loop !! \n", err);
return err;
}
if (isnan(mbs_data->qd[i])) {
err = _MBS_ERR_LOW_NAN + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> qd[%d] is Nan at time %g s ! \n", i, mbs_data->tsim);
err2 = mbs_invdyn_write_buffers(invd);
if (err2 < 0) {
err2 += _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> Error [%d] while saving results to disk (velocities)!\n", err2);
}
mbs_msg("[%d] in mbs_invdyn_loop !! \n", err);
return err;
}
if (isnan(mbs_data->qdd[i])) {
err = _MBS_ERR_LOW_NAN + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> qdd[%d] is Nan at time %g s ! \n", i, mbs_data->tsim);
err2 = mbs_invdyn_write_buffers(invd);
if (err2 < 0) {
mbs_msg(">>INVDYN>>\n");
mbs_msg(">>INVDYN>> Error [%d] while saving results to disk (accelerations)!\n", -600 + err2);
mbs_msg(">>INVDYN>>\n");
}
mbs_error_msg("[%d] in mbs_invdyn_loop !! \n", -637);
exit(1);
}
}
if (err < 0){
mbs_msg(">>INVDYN>> [%d] in mbs_invdyn_loop !! \n", err);
// adding to buffer
mbs_invdyn_save(invd, mbs_data, mbs_data->tsim);
return err;
}
// Updating time
if (mbs_data->dt0) {
......@@ -787,7 +766,7 @@ int mbs_invdyn_loop(MbsInvdyn* invd, MbsData* mbs_data)
}
}
}
return 0;
return err;
}
int mbs_invdyn_finish(MbsInvdyn* invd, MbsData* mbs_data) {
......@@ -883,7 +862,9 @@ int mbs_invdyn_write_buffers(MbsInvdyn* invd) {
}
for (i = 0; i < invd->bufferNb; i++) {
err = mbs_buffer_write(invd->buffers[i]);
if (err < 0) { return err; }
if (err < 0){
return err;
}
}
return err;
}
......@@ -982,7 +963,6 @@ int mbs_fct_invdyn(MbsData *s, MbsInvdyn *invd)
if (err < 0) // Error management
{
err += _MBS_ERR_MOD_INV_DYN;
mbs_invdyn_save(invd, s, s->tsim);
mbs_msg(">>INVDYN>> Error during inverse dynamics at time %g s !\n", s->tsim);
......
......@@ -109,8 +109,9 @@ void mbs_delete_invdyn(MbsInvdyn* invdyn, MbsData* mbs_data);
* This sepcific function is written to be able to ask for writing buffer from
* python.
*
* \p sk the MbsInvdyn struct for which the buffers must be written
* \return error (0 if no error)
* \param invdyn the MbsInvdyn struct for which the buffers must be written.
*
* \return Error status, <0 in case of failure.
*/
int mbs_invdyn_write_buffers(MbsInvdyn* invdyn);
......
......@@ -194,16 +194,16 @@ libmodules.mbs_new_invdyn.argtypes = [ctypes.POINTER(MbsData_c)]
libmodules.mbs_new_invdyn.restype = ctypes.POINTER(MbsInvdyn_c)
libmodules.mbs_run_invdyn.argtypes = [ctypes.POINTER(MbsInvdyn_c), ctypes.POINTER(MbsData_c)]
libmodules.mbs_run_invdyn.restype = None
libmodules.mbs_run_invdyn.restype = ctypes.c_int
libmodules.mbs_invdyn_init.argtypes = [ctypes.POINTER(MbsInvdyn_c), ctypes.POINTER(MbsData_c)]
libmodules.mbs_invdyn_init.restype = None
libmodules.mbs_invdyn_init.restype = ctypes.c_int
libmodules.mbs_invdyn_loop.argtypes = [ctypes.POINTER(MbsInvdyn_c), ctypes.POINTER(MbsData_c)]
libmodules.mbs_invdyn_loop.restype = None
libmodules.mbs_invdyn_loop.restype = ctypes.c_int
libmodules.mbs_invdyn_finish.argtypes = [ctypes.POINTER(MbsInvdyn_c), ctypes.POINTER(MbsData_c)]
libmodules.mbs_invdyn_finish.restype = None
libmodules.mbs_invdyn_finish.restype = ctypes.c_int
libmodules.mbs_delete_invdyn.argtypes = [ctypes.POINTER(MbsInvdyn_c), ctypes.POINTER(MbsData_c)]
libmodules.mbs_delete_invdyn.restype = None
......
......@@ -322,7 +322,7 @@ class MbsDirdyn(object):
" under the name 'Failed_loop_closing_procedure_q.res'.\n"
"\n--------------------------------------------------\n"
"\n--------------------------------------------------\n")
raise RuntimeError("MbsPart.run() failed, read previous messages.")
raise RuntimeError("MbsDirdyn.run() failed, read previous messages.")
return self.results
......
......@@ -10,6 +10,8 @@ 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.
"""
# Author: Robotran Team
# (c) Universite catholique de Louvain, 2019
import os
import ctypes
......@@ -155,18 +157,24 @@ class MbsInvdyn(object):
return msg
def __del__(self):
"""Delete the object by freeing the C-related memory."""
libmodules.mbs_delete_invdyn(self.mbs_invdyn_ptr, self.mbs.mbs_data_ptr)
if __DEBUG__: mbs_msg("DEBUG>> " + self.module_name + " pointer deleted")
if __DEBUG__:
mbs_msg("DEBUG>> " + self.module_name + " pointer deleted")
def run(self, **kwargs):
"""
Run an inverse dynamics analysis
Run an inverse dynamics analysis.
Options can be setted with set_options.
Options can be retrieved with get_options
Options can be setted with set_options
Options can be retrieved with get_options
Results are stored in the field results(if store_results == True)
Results are stored in the field results(if store_results == True)
Returns
-------
self.results: MbsResult
The MbsResults containing the results of the analysis.
"""
# Assign required user functions
if self.mbs.opt_load_c < 2:
......@@ -188,18 +196,22 @@ class MbsInvdyn(object):
self.set_options(**kwargs)
if not self.store_results:
libmodules.mbs_run_invdyn(self.mbs_invdyn_ptr, self.mbs.mbs_data_ptr)
error = libmodules.mbs_run_invdyn(self.mbs_invdyn_ptr, self.mbs.mbs_data_ptr)
error2 = 0 # Unused for this module
else:
# save2file forced to 1 because if buffers don't have the complete results, results are loaded from files
# save2file forced to 1 because if buffers don't have the complete
# results, results are loaded from files.
self.set_options(save2file=1)
libmodules.mbs_invdyn_init(self.mbs_invdyn_ptr, self.mbs.mbs_data_ptr)
libmodules.mbs_invdyn_loop(self.mbs_invdyn_ptr, self.mbs.mbs_data_ptr)
error = libmodules.mbs_invdyn_init(self.mbs_invdyn_ptr, self.mbs.mbs_data_ptr)
if (error >= 0):
error = libmodules.mbs_invdyn_loop(self.mbs_invdyn_ptr, self.mbs.mbs_data_ptr)
# Results(buffer) memory is kept BUT FILES WILL BE WRITTEN LATER
if self.get_options("save2file"):
self._load_results()
self._load_results()
libmodules.mbs_invdyn_finish(self.mbs_invdyn_ptr, self.mbs.mbs_data_ptr)
# finish function is required to close the module and write the results to disk.
error2 = libmodules.mbs_invdyn_finish(self.mbs_invdyn_ptr, self.mbs.mbs_data_ptr)
# Unassign user functions
if self.mbs.opt_load_c < 2:
......@@ -209,53 +221,73 @@ class MbsInvdyn(object):
if self.mbs.opt_load_c < 1:
self.mbs.__unassign_symb_fct__(self.symb_fun_list)
if error < 0 or error2 < 0:
mbs_msg("\n--------------------------------------------------------------------------------\n"
"READ CAREFULLY !!!\n"
"--------------------------------------------------------------------------------\n\n"
"An error occurs during inverse dynamic 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 an 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("MbsInvdyn.run() failed, read previous messages.")
return self.results
def _load_results(self):
"""
Load the results from the buffers
Load the results from the buffers.
If the beginning of the integration is not available in the buffers,
the complete results are loaded from files
If the beginning of the integration is not available in the buffers,
the complete results are loaded from files
"""
#c_mbs_invdyn_write_buffers(self.mbs_invdyn_ptr)
# c_mbs_invdyn_write_buffers(self.mbs_invdyn_ptr)
size1 = self.mbs_invdyn_ptr.contents.buffers[0].contents.index
if size1 == 0:
size1 = self.mbs_invdyn_ptr.contents.buffers[0].contents.size
size2 = self.mbs_invdyn_ptr.contents.buffers[0].contents.nx + 1
# array are initialized to the time pointer so as to start index of joints at 1(we have to ensure contiguity between t and x in buffers ! ! !)
self.results.q = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[0].contents.tx , (size1, size2)))
self.results.q = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[0].contents.tx, (size1, size2)))
# get time array from the q buffer
self.results.t = self.results.q[:, 0]
if not self.results.t[0] == self.get_options("t0"):
mbs_msg("The beginning of the integration is not available in the buffer.\n The complete results have to be loaded from files.")
filename = bytes_to_str(ctypes.string_at(self.mbs_invdyn_ptr.contents.buffers[0].contents.filename))
self.results.load_results_from_file(filename, module = 6)
self.results.load_results_from_file(filename, module=6)
# get qd and qdd buffer
self.results.qd = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[1].contents.tx , (size1, size2)))
self.results.qdd = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[2].contents.tx , (size1, size2)))
self.results.qd = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[1].contents.tx, (size1, size2)))
self.results.qdd = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[2].contents.tx, (size1, size2)))
size2 = self.mbs_invdyn_ptr.contents.buffers[3].contents.nx + 1
self.results.Qq = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[3].contents.tx , (size1, size2)))
self.results.Qq = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[3].contents.tx, (size1, size2)))
if self.module_name == "MbsInvdyn":
size2 = self.mbs_invdyn_ptr.contents.buffers[4].contents.nx + 1
self.results.Qa = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[4].contents.tx , (size1, size2)))
self.results.Qa = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[4].contents.tx, (size1, size2)))
buffer_id = 5
else:
buffer_id = 4
if self.mbs.Nlink:
size2 = self.mbs_invdyn_ptr.contents.buffers[buffer_id].contents.nx + 1
self.results.Z = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[buffer_id].contents.tx , (size1, size2)))
self.results.Zd = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[buffer_id + 1].contents.tx , (size1, size2)))
self.results.Fl = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[buffer_id + 2].contents.tx , (size1, size2)))
self.results.Z = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[buffer_id].contents.tx, (size1, size2)))
self.results.Zd = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[buffer_id + 1].contents.tx, (size1, size2)))
self.results.Fl = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[buffer_id + 2].contents.tx, (size1, size2)))
buffer_id = buffer_id + 3
if self.mbs.nqc:
size2 = self.mbs_invdyn_ptr.contents.buffers[buffer_id].contents.nx + 1
self.results.Qc = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[buffer_id].contents.tx , (size1, size2)))
self.results.Qc = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[buffer_id].contents.tx, (size1, size2)))
buffer_id = buffer_id + 1
if self.mbs.nhu:
size2 = self.mbs_invdyn_ptr.contents.buffers[buffer_id].contents.nx + 1
self.results.Lambda = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[buffer_id].contents.tx , (size1, size2)))
self.results.Lambda = np.copy(np.ctypeslib.as_array(self.mbs_invdyn_ptr.contents.buffers[buffer_id].contents.tx, (size1, size2)))
if self.mbs_invdyn_ptr.contents.user_buffer.contents.nx:
size = self.mbs_invdyn_ptr.contents.user_buffer.contents.index
......@@ -447,19 +479,15 @@ class MbsInvdyn(object):
def get_options(self, *args):
"""
Get the specified options for Invdyn module
Get the specified options for Invdyn module.
Parameters
----------
The different options specifed in the documentation of set_options
The different options specifed in the documentation of 'set_options()'
Returns
-------
The value of the different options specifed in the documentation of set_options
The value of the different options specifed in the documentation of 'set_options()'
"""
options = []
for key in args:
......@@ -522,4 +550,4 @@ class MbsInvdyn(object):
if len(options) == 1:
return options[0]
return tuple(options)
\ No newline at end of file
return tuple(options)
......@@ -41,8 +41,8 @@ int main(int argc, char const *argv[])
printf("Hello MBS!\n");
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
/* LOADING *
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/* LOADING */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
printf("Loading the PendulumSpring data file !\n");
......@@ -52,8 +52,8 @@ int main(int argc, char const *argv[])
mbs_data = mbs_load(PROJECT_SOURCE_DIR"/../dataR/PendulumSpringC.mbs", BUILD_PATH);
printf("* .mbs file loaded!\n");
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
/* COORDINATE PARTITIONING *
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/* COORDINATE PARTITIONING */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
// Error: only independent joints
// mbs_set_qu(mbs_data, 1);
......@@ -74,8 +74,8 @@ int main(int argc, char const *argv[])
mbs_delete_part(mbs_part);
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
/* EQUILIBRIUM *
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/* EQUILIBRIUM */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
mbs_data->process = 2;
mbs_equil = mbs_new_equil(mbs_data);
......@@ -118,8 +118,8 @@ int main(int argc, char const *argv[])
mbs_delete_equil(mbs_equil, mbs_data);
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
/* MODAL ANALYSIS *
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/* MODAL ANALYSIS */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
mbs_data->process = 3;
mbs_modal = mbs_new_modal(mbs_data);
......@@ -152,8 +152,8 @@ int main(int argc, char const *argv[])
mbs_delete_modal(mbs_modal, mbs_data);
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
/* DIRECT DYNANICS *
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/* DIRECT DYNANICS */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
mbs_data->qd[1] = 1.5;
......@@ -167,7 +167,7 @@ int main(int argc, char const *argv[])
// dirdyn options (see documentations for additional options)
mbs_dirdyn->options->dt0 = 1e-3;
mbs_dirdyn->options->tf = 100.0e-3;
mbs_dirdyn->options->tf = 500.0e-3;
mbs_dirdyn->options->save2file = 1;
mbs_dirdyn->options->respath = PROJECT_SOURCE_DIR"/../resultsR";
mbs_dirdyn->options->animpath = PROJECT_SOURCE_DIR"/../animationR";
......@@ -181,8 +181,8 @@ int main(int argc, char const *argv[])
mbs_delete_dirdyn(mbs_dirdyn, mbs_data);
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * *
/* INVERSE DYNANMICS *
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
/* INVERSE DYNANMICS */
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
mbs_data->process = 6;
mbs_reset_data(mbs_data);
......@@ -191,12 +191,29 @@ int main(int argc, char const *argv[])
mbs_set_qa(mbs_data, 2);
mbs_set_qa(mbs_data, 3);
// Error: impossible cut
// mbs_data->dpt[3][4] = 10;
// Mass matrix full of 0 does not generate errors:
// We solve (roughly) F=m.a with m=0
mbs_invdyn = mbs_new_invdyn(mbs_data);
mbs_invdyn->options->trajectoryqname = PROJECT_SOURCE_DIR"/../resultsR/dirdyn_q.res";
mbs_invdyn->options->trajectoryqdname = PROJECT_SOURCE_DIR"/../resultsR/dirdyn_qd.res";
mbs_invdyn->options->trajectoryqddname = PROJECT_SOURCE_DIR"/../resultsR/dirdyn_qdd.res";
// Error: joints have change
// mbs_set_qdriven(mbs_data, 2);
// Wrong trajectory files, the correct ones must be commented!
// mbs_invdyn->options->trajectoryqname = PROJECT_SOURCE_DIR"/../resultsR/wrong_q.res";
// mbs_invdyn->options->trajectoryqdname = PROJECT_SOURCE_DIR"/../resultsR/wrong_qd.res";
// mbs_invdyn->options->trajectoryqddname = PROJECT_SOURCE_DIR"/../resultsR/wrong_qdd.res";
mbs_invdyn->options->trajectoryqname = PROJECT_SOURCE_DIR"/../resultsR/RK4_q.res";
mbs_invdyn->options->trajectoryqdname = PROJECT_SOURCE_DIR"/../resultsR/RK4_qd.res";
mbs_invdyn->options->trajectoryqddname = PROJECT_SOURCE_DIR"/../resultsR/RK4_qdd.res";
mbs_invdyn->options->motion = trajectory;
mbs_invdyn->options->t0 = 0.;
mbs_invdyn->options->tf = 10.e-3;
mbs_invdyn->options->tf = 0.433;