Commit bba6aa02 authored by Olivier Lantsoght's avatar Olivier Lantsoght 🏁
Browse files

[InvDyn][NewErr] mbs_invdyn_init()

parent a6d14494
......@@ -5,6 +5,9 @@
* [C] New function `print_d_vec_0_format` allowing to set the format of the vector values.
* [C] New function to compute matrix product between matrices of specified shape: `doublematrix_product_0`.
* [C] LPK functions new error catched with code `-#16`; Documentation fixed.
* [C] Some functions that my be called by users have return status (<0 in case of error):
* Look Up Table: `mbs_lut_2D_alloc()`, `mbs_lut_res_alloc()`, `mbs_lut_res_load_file()`;
* Useful functions: `read_dmat_0()`
* [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.
......
......@@ -101,7 +101,11 @@ int mbs_run_invdyn(MbsInvdyn* invd, MbsData* mbs_data)
int err = 0;
// 1. Initialize the simulation
// - - - - - - - - - - - - - -
mbs_invdyn_init(invd, mbs_data);
err = mbs_invdyn_init(invd, mbs_data);
if (err < 0){
mbs_invdyn_finish(invd, mbs_data);
return err;
}
// 2. Run the simulation
......@@ -164,7 +168,6 @@ int mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
mbs_msg(">>INVDYN>> Incoherences detected during module initialization! (See message above) \n");
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", err);
return err;
}
......@@ -178,7 +181,6 @@ int mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
mbs_msg(" >> -Number of dependent coordinates (%d)\n", mbs_data->nqv);
mbs_msg(" >> must be equal!\n");
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", err);
return err;
}
......@@ -192,7 +194,6 @@ int mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
mbs_msg(" >> The actuated joint %d is also locked.\n", mbs_data->qlocked[i]);
mbs_msg(" >> This is not allowed!\n");
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", err);
return err;
}
}
......@@ -211,7 +212,6 @@ int mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
mbs_msg(" >> The actuated joint %d is also driven.\n", mbs_data->qc[i]);
mbs_msg(" >> This is not allowed when some joints are independent.\n");
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", err);
return err;
}
}
......@@ -276,16 +276,20 @@ int mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
invd->tindex = 0;
// Motion identification
if (invd->options->motion == closeloop) {
err = _MBS_ERR_INIT + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> Inchoherence detected during module initialization!\n");
mbs_msg(" >> 'MbsInvdyn->options->motion' cannot be set as 'closeloop' for inverse dynamic analysis.\n");
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", -600);
exit(1);
return err;
}
else if (invd->options->motion != oneshot && invd->options->motion != trajectory) {
err = _MBS_ERR_INIT + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> Inchoherence detected during module initialization!\n");
mbs_msg(" >> 'MbsInvdyn->options->motion' has an unsupported value.\n");
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", -600);
exit(1);
return err;
}
// Oneshot : Setting values.
......@@ -302,9 +306,11 @@ int mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
if (invd->options->trajectoryqname == NULL || invd->options->trajectoryqdname == NULL || invd->options->trajectoryqddname == NULL) {
// At least one trajectory file is not provided, we should have a driven inverse dynamic.
if (mbs_data->nqu != 0) {
err = _MBS_ERR_INIT + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> Trajectory (q, qd or qdd) files are mandatory when some joints are independent.\n");
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", -600);
exit(1);
return err;
}
invd->trajectorytype = 1;
invd->q = NULL;
......@@ -317,41 +323,74 @@ int mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
mbs_data->dt0 = invd->options->dt;
invd->tsim = invd->options->t0;
if (mbs_data->dt0 <= 0.0) { // Error management
err = _MBS_ERR_INIT + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> Time step has to be stricly positive (dt0>0) for a fully driven inverse dynamic analysis.\n");
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", -600);
exit(1);
return err;
}
if (mbs_data->t0 >= mbs_data->tf) { // Error management
err = _MBS_ERR_INIT + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> Tnitial time has to be stricly lower than final time (t0<tf).\n");
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", -600);
exit(1);
return err;
}
}
else {
// All trajectory files are provided.
invd->q = mbs_lut_res_load_file(invd->options->trajectoryqname);
if (!invd->q){
err = _MBS_ERR_LOW_FILES + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> Unable to load coordinates trajectory file.\n");
return err;
}
invd->qd = mbs_lut_res_load_file(invd->options->trajectoryqdname);
if (!invd->qd){
err = _MBS_ERR_LOW_FILES + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> Unable to load velocities trajectory file.\n");
return err;
}
invd->qdd = mbs_lut_res_load_file(invd->options->trajectoryqddname);
if (!invd->qdd){
err = _MBS_ERR_LOW_FILES + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> Unable to load accelerations trajectory file.\n");
return err;
}
if (invd->q->nq == mbs_data->njoint) {
// The trajectory contains all coordinates value
mbs_msg(">>INVDYN>>> Provided trajectories contains all joints.");
invd->trajectorytype = 2;
}
else if (invd->q->nq == mbs_data->nqu) {
// The trajectory contains only independent coordinates value
mbs_msg(">>INVDYN>>> Provided trajectories contains only independent coordinates.");
invd->trajectorytype = 3;
}
else { // Error management
err = _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> Provided trajectories number of variable (%d) does not match neither:\n", invd->q->nq);
mbs_msg(" >>> - the number of total joint (%d);\n", mbs_data->njoint);
mbs_msg(" >>> - the number of independant joint (%d);\n", mbs_data->nqu);
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", -600);
exit(1);
return err;
}
if (invd->q->nq != invd->qd->nq || invd->q->nq != invd->qdd->nq) { // Error management
err = _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> Number of coordinates is not consistent between trajectories file.\n");
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", -600);
exit(1);
return err;
}
if (invd->options->dt <= 0) {
// using time vector of the input file
......@@ -368,21 +407,27 @@ int mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
mbs_data->dt0 = invd->options->dt;
invd->tsim = invd->options->t0;
if (mbs_data->t0 < invd->q->tq[0][0]) { // Error management
err = _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> The initial time is below the minimal time of the provided trajectory files.\n");
mbs_msg(" >>> Trajectory extrapolation is not allowed.\n");
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", -600);
exit(1);
return err;
}
if (mbs_data->tf > invd->q->tq[invd->q->nt - 1][0]) { // Error management
err = _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> The final time is over the maximal time of the provided trajectory files.\n");
mbs_msg(" >>> Trajectory extrapolation is not allowed.\n");
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", -600);
exit(1);
return err;
}
if (mbs_data->t0 >= mbs_data->tf) { // Error management
err = _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>>> initial time has to be stricly lower (t0<tf) than final time.\n");
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", -600);
exit(1);
return err;
}
}
......@@ -566,6 +611,7 @@ int mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
else {
reset_flag_output();
}
return err;
}
void mbs_invdyn_loop(MbsInvdyn* invd, MbsData* mbs_data)
......
......@@ -12,6 +12,7 @@
#include "lut.h"
#include "mbs_message.h"
#include "useful_functions.h"
#include "mbs_errors_names.h"
MbsLut1D* mbs_lut_1D_alloc()
{
......@@ -24,6 +25,13 @@ MbsLut2D* mbs_lut_2D_alloc()
{
MbsLut2D* newlut;
newlut = (MbsLut2D*)malloc(sizeof(MbsLut2D));
newlut->nx = 0;
newlut->ny = 0;
newlut->x = NULL;
newlut->y = NULL;
newlut->z = NULL;
return newlut;
}
......@@ -33,6 +41,8 @@ MbsLutRes* mbs_lut_res_alloc()
newlut = (MbsLutRes*)malloc(sizeof(MbsLutRes));
newlut->nq = 0;
newlut->nt = 0;
newlut->tq = NULL;
return newlut;
}
......@@ -82,6 +92,10 @@ void mbs_lut_2D_free(MbsLut2D *lut)
void mbs_lut_res_free(MbsLutRes *lut)
{
if (!lut){
return;
}
free_dmat_0(lut->tq);
free(lut);
}
......@@ -678,15 +692,21 @@ MbsLutRes* mbs_lut_res_load_file(char* fileName)
// Open the file
flut = fopen(fileName, "r");
if (flut == 0) {
mbs_lut_res_free(lut);
mbs_msg("\t>LOOK UP TABLE> Error in mbs_lut_res_load_file.\n");
mbs_msg("\t> > Impossible to find the file %s.\n", fileName);
mbs_error_msg("In Look up table loading file, shuting down the process.\n");
exit(1);
return NULL;
}
// Checking if the file is empty
if (NULL == fgets(line, size, flut)) {
mbs_error_msg("The file %s is empty.", fileName);
exit(1);
mbs_lut_res_free(lut);
fclose(flut);
mbs_msg("\t>LOOK UP TABLE> The file %s is empty.", fileName);
return NULL;
}
// counting number of columns
......@@ -714,15 +734,25 @@ MbsLutRes* mbs_lut_res_load_file(char* fileName)
lut->tq = get_dmat_0(lut->nt, lut->nq + 1);
// filling the array
read_dmat_0(lut->tq, lut->nt, lut->nq + 1, fileName);
int err = read_dmat_0(lut->tq, lut->nt, lut->nq + 1, fileName);
if (err < 0){
mbs_lut_res_free(lut);
mbs_msg("\t>LOOK UP TABLE> Error while loading matrix from file %s.\n", fileName);
return NULL;
}
// Checking time validity
for (len = 0; len < lut->nt - 1; len++) {
if (!(lut->tq[len + 1][0] - lut->tq[len][0] > 0)) {
double time_step = lut->tq[len + 1][0] - lut->tq[len][0];
mbs_lut_res_free(lut);
mbs_msg("\t>LOOK UP TABLE> Error in mbs_lut_res_load_file, time must be a strictly increasing function.\n");
mbs_msg("\t > Time steps between lines %d and %d is %f.\n", len + 1, len + 2, lut->tq[len + 1][0] - lut->tq[len][0]);
mbs_error_msg("In Look up table loading file, shuting down the process.\n");
exit(1);
mbs_msg("\t > Time steps between lines %d and %d is %f.\n", len + 1, len + 2, time_step);
return NULL;
}
}
......
......@@ -70,23 +70,27 @@ typedef struct MbsLut2Dvec
*/
MbsLut1D* mbs_lut_1D_alloc();
/*! \brief Allocate a 2D LUT (Look Up Table) structure
*
* Default values are set, sizes are 0, pointers are NULL.
*
* \return An allocated 2D LUT structure
*/
MbsLut2D* mbs_lut_2D_alloc();
/*! \brief Allocate a LUT (Look Up Table) structure for results files
*
* Default values are set, sizes are 0, pointers are NULL.
* The results files contain the trajectory of coordinates at specified timestep.
*
* \return An allocated Result LUT structure
* \return An allocated Result LUT structure.
*/
MbsLutRes* mbs_lut_res_alloc();
/*! \brief Allocate a 1Dvec LUT (Look Up Table) structure
*
* \param[in] nx number of elements to interpolate
* \param[in] n size of the vector elements to interpolate
* \return An allocated 1Dvec LUT structure
*/
/*! \brief Allocate a 1Dvec LUT (Look Up Table) structure.
*
* \param[in] nx number of elements to interpolate.
* \param[in] n size of the vector elements to interpolate.
*
* \return An allocated 1Dvec LUT structure
*/
MbsLut1Dvec* mbs_lut_1Dvec_alloc(int nx, int n);
/*! \brief Allocate a 1Dvec LUT (Look Up Table) structure
......@@ -222,7 +226,8 @@ MbsLut2D* mbs_lut_2D_load_file(char* fileName);
* \param[in] fileName Name of the file containing the results to load.\n
The first column contains the time.\n
The other columns contain the coordinate.
* \return A new allocated results LUT structure with the data of the file
* \return A new allocated results LUT structure with the data of the file, or
* NULL in case of failure.
*/
MbsLutRes* mbs_lut_res_load_file(char* fileName);
/*! \brief Load a file and return a new allocated results LUT structure
......
......@@ -4,17 +4,16 @@
* author: Nicolas Van der Noot & Aubain Verle
*/
#include "useful_functions.h"
#include "mbs_message.h"
#include <limits.h>
#include <time.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include "useful_functions.h"
#include "mbs_message.h"
#include "mbs_errors_names.h"
void mbs_log(char* msg, ...) {
va_list argptr;
FILE *f = fopen("log", "a");
......@@ -907,8 +906,9 @@ void save_dvec_0(double *vec, int x, char *name)
fclose(file_out);
}
void read_dmat_0(double **mat, int x, int y, char *name)
int read_dmat_0(double **mat, int x, int y, char *name)
{
int err = 0;
int i, j;
char *token;
char *line = malloc(sizeof(char) * (100 * y + 1)); // 100 is for safety...
......@@ -916,10 +916,12 @@ void read_dmat_0(double **mat, int x, int y, char *name)
file_in = NULL; // internal filename
file_in = fopen(name, "r");
if (file_in == NULL)
{
mbs_error_msg("error: cannot open file '%s'\n", name);
exit(1);
if (file_in == NULL){
err = _MBS_ERR_LOW_FILES;
mbs_msg("\t>read_dmat_0>cannot open file '%s'\n", name);
return err;
}
i = 0;
// read the file line by line
......@@ -939,12 +941,17 @@ void read_dmat_0(double **mat, int x, int y, char *name)
if (i != x || j != y)
{
mbs_error_msg("The file contains %d lines and %d columns while the provided matrix has %d lines and %d columns \n", i, j, x, y);
exit(1);
err = _MBS_ERR_LOW_FILES;
fclose(file_in);
mbs_msg("\t>read_dmat_0>The file contains %d lines and %d columns while "
"the provided matrix has %d lines and %d columns \n", i, j, x, y);
return err;
}
fclose(file_in);
free(line);
return err;
}
double** load_dmat_0(int x, int y, char *name)
......
......@@ -443,8 +443,10 @@ void save_dvec_0(double *vec, int x,char *name);
* \param[in] y nb of columns
* \param[in] name the path an name of the file in which reading the matrix
* \param[in,out] mat allocated array for storing the file
*
* \return Error status, <0 in case of failure.
*/
void read_dmat_0(double **mat, int x, int y, char *name);
int read_dmat_0(double **mat, int x, int y, char *name);
/*! \brief load a [x times y] matrix of doubles from a file starting with index 0. The matrix is created (memory allocation) and filled with the values from the file.
*
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment