Commit a6d14494 authored by Olivier Lantsoght's avatar Olivier Lantsoght
Browse files

[InvDyn][NewError] Ongoing [ci skip]

parent e447ff3d
......@@ -20,6 +20,7 @@
#include "mbs_invdyn.h"
#include "mbs_invdyn_struct.h"
#include "mbs_project_interface.h"
#include "mbs_errors_names.h"
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
......@@ -66,6 +67,14 @@ MbsInvdyn* mbs_new_invdyn_aux(MbsData* mbs_data, MbsAux* mbs_aux)
opts->animpath = NULL;
opts->verbose = 1;
// Set pointer to default values.
invdyn->buffers = NULL;
invdyn->user_buffer = NULL;
invdyn->savedArrays = NULL;
invdyn->q = NULL;
invdyn->qd = NULL;
invdyn->qdd = NULL;
return invdyn;
}
......@@ -87,8 +96,9 @@ void mbs_delete_invdyn(MbsInvdyn* invdyn, MbsData* mbs_data)
/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
void mbs_run_invdyn(MbsInvdyn* invd, MbsData* mbs_data)
int mbs_run_invdyn(MbsInvdyn* invd, MbsData* mbs_data)
{
int err = 0;
// 1. Initialize the simulation
// - - - - - - - - - - - - - -
mbs_invdyn_init(invd, mbs_data);
......@@ -102,6 +112,8 @@ void mbs_run_invdyn(MbsInvdyn* invd, MbsData* mbs_data)
// 3. Finish the simulation
// - - - - - - - - - - - -
mbs_invdyn_finish(invd, mbs_data);
return err;
}
void mbs_invdyn_save(MbsInvdyn* invd, MbsData *mbs_data, double t) {
......@@ -133,7 +145,7 @@ void mbs_invdyn_save(MbsInvdyn* invd, MbsData *mbs_data, double t) {
}
}
void mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
int mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
{
int i, err;
......@@ -148,31 +160,40 @@ void mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
// Checking mbs_data coherence
err = mbs_check_mbs_data_values(invd->mbs_aux, mbs_data);
if (err < 0) {
err += _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> Incoherences detected during module initialization! (See message above) \n");
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", -600);
exit(1);
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", err);
return err;
}
// Checking constraints and dependant variable coherence
err = mbs_check_nhu_nqv(mbs_data);
if (err < 0) {
err += _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> Inchoherence detected during module initialization! \n");
mbs_msg(" >> -Number of independent constraints (%d)\n", mbs_data->nhu);
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", -600);
exit(1);
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", err);
return err;
}
// Checking coherence between locked and actuated variables
if (mbs_data->nqlocked && mbs_data->nqa) {
for (i = 1; i <= mbs_data->nqlocked; i++) {
if (find_ivec_1(mbs_data->qa, mbs_data->qlocked[i], mbs_data->nqa) != -1) {
err = _MBS_ERR_INIT + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> Inchoherence detected during module initialization!\n");
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", -600);
exit(1);
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", err);
return err;
}
}
}
......@@ -184,11 +205,14 @@ void mbs_invdyn_init(MbsInvdyn* invd, MbsData* mbs_data)
// Locked joint cannont be actuated
for (i = 1; i <= mbs_data->nqc; i++) {
if (find_ivec_1(mbs_data->qa, mbs_data->nqa, mbs_data->qc[i]) != -1) {
err = _MBS_ERR_INIT + _MBS_ERR_MOD_INV_DYN;
mbs_msg(">>INVDYN>> Inchoherence detected during module initialization!\n");
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", -600);
exit(1);
mbs_error_msg("[%d] in mbs_invdyn_init !! \n", err);
return err;
}
}
......
......@@ -29,6 +29,7 @@ extern "C" {
*/
MbsInvdyn* mbs_new_invdyn(MbsData* mbs_data);
/**
* Fills a new inverse dynamic auxilliary structure.
* A pointer to the given MbsAux structure is kept by the returned structure.
......@@ -39,23 +40,31 @@ MbsInvdyn* mbs_new_invdyn(MbsData* mbs_data);
*/
MbsInvdyn* mbs_new_invdyn_aux(MbsData* mbs_data, MbsAux* mbs_aux);
/**
* Run the inverse kinematic analysis
*
* The MbsData structure associated to solvekin is modified
*
* @param[in,out] invdyn the MbsInvdyn to be run
* @param[in,out] mbs_data the MbsData structure of the model for which the
* \param[in,out] invdyn the MbsInvdyn to be run
* \param[in,out] mbs_data the MbsData structure of the model for which the
* inverse dynamic is computed.
*
* \return Error status, <0 in case of failure.
*/
void mbs_run_invdyn(MbsInvdyn* invdyn, MbsData* mbs_data);
int mbs_run_invdyn(MbsInvdyn* invdyn, MbsData* mbs_data);
/**
* Run the startup operations of the run_invdyn function
*
* \param[in,out] invdyn the MbsInvdyn to be run
* \param[in] mbs_data the MbsData structure of the model for which the
* inverse dynamic is computed.
*
* \return Error status, <0 in case of failure.
*/
void mbs_invdyn_init(MbsInvdyn* invdyn, MbsData* mbs_data);
int mbs_invdyn_init(MbsInvdyn* invdyn, MbsData* mbs_data);
/**
......
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