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

[Closing animation] Closing animation when NR fail is an option (disable by default)

parent 9159a231
......@@ -79,6 +79,14 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux);
*/
int mbs_step_close_geo(MbsData *s, MbsAux *mbs_aux);
/*! \brief Generate the animation for failed Newton-Raphson procedure
*
* \param[in,out] mbs_data the MbsData structure.
* \param[in] mbs_aux the mbs_aux structure of the failed process. The initial
* coordinates vector values are stored in MbsAux::q_save.
*/
int mbs_anim_close_geo(MbsData *mbs_data, MbsAux *mbs_aux);
/*! \brief compute the dependent velocities that solves the constraints.
*
* The function solves the linear equation: \f$ \dot{v} = \textbf{B}_{vu} \dot{u} \f$.\n
......
......@@ -108,6 +108,9 @@ MbsAux * initMbsAux(MbsData *s)
allocate_sensor(mbs_aux->psens, njoint);
init_sensor(mbs_aux->psens, njoint);
mbs_aux->q_save = get_dvec_1(s->njoint);
mbs_aux->close_anim = 0;
#elif defined SENSORKIN
if (nsensor) {
mbs_aux->psensorStruct = (MbsSensor**) calloc(nsensor+1,sizeof(MbsSensor*));
......@@ -215,22 +218,16 @@ void freeMbsAux(MbsAux *mbs_aux, MbsData *s)
{
#if defined DIRDYNARED || defined INVDYNARED
int njoint, nqu, nqv, nqc, nquc, Ncons, Nuserc;
int Ncons, Nuserc;
#ifdef INVDYNARED
int nqa;
#endif
njoint = s->njoint;
nqu = s->nqu;
nqv = s->nqv;
nqc = s->nqc;
#ifdef INVDYNARED
nqa = s->nqa;
#endif
nquc = nqu + nqc;
Nuserc = s->Nuserc;
Ncons = s->Ncons;
/**/
......@@ -264,6 +261,8 @@ void freeMbsAux(MbsAux *mbs_aux, MbsData *s)
free_sensor(mbs_aux->psens);
free_dvec_1(mbs_aux->q_save);
#elif defined SENSORKIN
int i,j;
......
......@@ -25,17 +25,12 @@ double norminf_vector(double *v, int n);
int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
{
int i,j;
int iter=0;
int nL,nC;
int err = 0;
double d;
// Backup of qv
double *qv = get_dvec_1(s->nqv);
for(i=1;i<=s->nhu;i++)
{
qv[i] = s->q[s->qv[i]];
// if failed closure animation are activated, backup q vector
if (mbs_aux->close_anim){
copy_dvec_1(s->q, mbs_aux->q_save);
}
iter = 0;
......@@ -45,7 +40,6 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
err = mbs_step_close_geo(s, mbs_aux);
if (err <0){
mbs_msg("\t \t >CLOSE GEO> Error in mbs_close_geo during a NR iteration \n");
free_dvec_1(qv);
return -50+err;
}
}
......@@ -53,12 +47,10 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
if (iter >= mbs_aux->MAX_NR_ITER) // Error management
{
mbs_msg("\t \t >CLOSE GEO> Impossible to close the geometry after %d iterations \n", iter);
mbs_anim_close_geo(s, mbs_aux, qv);
free_dvec_1(qv);
mbs_anim_close_geo(s, mbs_aux);
return -50;
}
free_dvec_1(qv);
return iter;
}
......@@ -112,23 +104,22 @@ int mbs_step_close_geo(MbsData *s, MbsAux *mbs_aux){
return 0;
}
int mbs_anim_close_geo(MbsData *mbs_data, MbsAux *mbs_aux, double *qv){
int mbs_anim_close_geo(MbsData *mbs_data, MbsAux *mbs_aux){
mbs_msg("\t \t > Attempting to generate animation of the failed loop closing procedure.\n");
// Restoring qv
int i;
for(i=1;i<=mbs_data->nhu;i++)
{
mbs_data->q[mbs_data->qv[i]] = qv[i];
}
copy_dvec_1(mbs_aux->q_save, mbs_data->q);
// Creating the animation with the solvekin module
MbsSolvekin *mbs_solvekin;
mbs_solvekin = mbs_new_solvekin(mbs_data);
mbs_solvekin->options->motion = closeloop;
mbs_solvekin->options->framerate = 1;
mbs_solvekin->options->resfilename = "Failed_loop_closing_procedure";
mbs_solvekin->mbs_aux->MAX_NR_ITER = mbs_aux->MAX_NR_ITER; // Retrieve max iteration from previous module
// Run the solvekin module
mbs_run_solvekin(mbs_solvekin, mbs_data);
mbs_delete_solvekin(mbs_solvekin, mbs_data);
mbs_msg("\t \t > Loop closure procedure animated in file \"Failed_loop_closing_procedure_q.anim\".\n");
return 1;
}
void mbs_close_kin(MbsData *s, MbsAux *mbs_aux)
......
......@@ -99,6 +99,8 @@ MbsDirdyn* mbs_new_dirdyn_aux(MbsData* mbs_data, MbsAux* mbs_aux)
// Options related to integrator that need Jacobian computation
opts->n_freeze = 0;
// Other options
opts->show_failed_closure = 0;
// initial the saving counter
dirdyn->savePeriodCounter = 0;
......@@ -219,6 +221,8 @@ void mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
mbs_data->tsim = dd->options->t0;
mbs_data->tf = dd->options->tf;
mbs_data->dt0 = dd->options->dt0;
// Other
dd->mbs_aux->close_anim = dd->options->show_failed_closure;
/* TODO : Move in initialize_integrator when 2nd order integrator will be implemented */
// Allocate state vectors
......@@ -270,7 +274,7 @@ void mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
dd->mbs_aux->compute_Qc_vec[i] = 1;
}
}
}
// init time
dd->tsim = dd->options->t0;
......@@ -734,4 +738,4 @@ void save_realtime_update(MbsDirdyn* dd, MbsData* mbs_data)
}
}
#endif
}
\ No newline at end of file
}
......@@ -55,6 +55,9 @@ typedef struct MbsAux_tag
MbsSensor psens[1]; // temporary sensor pointer
double *q_save;
int close_anim;
#elif defined SENSORKIN
MbsSensor **psensorStruct;
#endif
......
......@@ -90,9 +90,9 @@ typedef struct MbsDirdynOptions
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
int *compute_Qc; /// If options->flag_compute_Qc is set to 0, the vector allows to compute only the needed Qc. For dirdyn only.
/// Empty vector by default. If compute_Qc = [njoint 1 0 0 1 0], we will compute Qc(1) and Qc(4) only.
/// length by default to njoint+1, compute_Qc[0] = njoint
int *compute_Qc; ///< If options->flag_compute_Qc is set to 0, the vector allows to compute only the needed Qc. For dirdyn only.
///< Empty vector by default. If compute_Qc = [njoint 1 0 0 1 0], we will compute Qc(1) and Qc(4) only.
///< length by default to njoint+1, compute_Qc[0] = njoint
/*------------------------------------------*/
/* Options related to numerical integration */
......@@ -120,6 +120,9 @@ typedef struct MbsDirdynOptions
/*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;
// Various options
int show_failed_closure; ///< 1 to generate animation of the failed Newton-Raphson procedure, default = 0.
} MbsDirdynOptions;
/**
......
Markdown is supported
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