Commit ed5fcee9 authored by Quentin Docquier's avatar Quentin Docquier
Browse files

equil and modal (+ linearization)

parent f2272cb1
......@@ -34,6 +34,17 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s);
int invdynared(MbsAux *mbs_aux, MbsData *s);
#endif
/**
* Compute the reduced Phi vector of size nquc=nqu+nqc for qu and qc variables (Robotran Basics Eq(7,20))
* necessary for
* - equilibrium (while acceleration are set to 0 beforehand)
* - linearization of MBS system: get the tangent matrices (for control or modal analysis)
*
* \p Fruc the force vector in which the force must be returned
* \p aux the local data structure
* \p s the MbsData of the system
*/
int mbs_calc_Fruc(double Fruc[], MbsAux *aux, MbsData *s);
int mbs_close_geo(MbsData *s, MbsAux *mbs_aux);
......
/**
*
* author Quentin Docquier
*
*/
#include "mbs_data.h"
#include "mbs_project_interface.h"
#include "MBSfun.h"
int mbs_calc_Fruc(double Fruc[], MbsAux *aux, MbsData *s)
{
int ok, err = 0;
int i, j;
int *iquc;
int nuc;
double *F;// *Fr_uc;
double val;
int ind_x;
nuc = s->nqu + s->nqc;
iquc = get_int_vec(nuc + 1);
mbs_conc_int_vec(s->qu, s->nqu, s->qc, s->nqc, iquc);
F = get_double_vec(s->njoint + 1);
// ?Q need for a reference to this sequence of calculation (something like a diagram)...
// 1. driven variables, qc as the user defines it
if (s->nqc > 0)
{
user_DrivenJoints(s, s->tsim);
for (i = 1; i <= s->nqc; i++) // Rob convention starts at 1
{
if (s->qdd[s->qc[i]] != 0.0)
{
s->qdd[s->qc[i]] = 0.0;
printf(">>MBS>> MBS_equil - user file : non-zero driven accelerations : forced to zero \n");
}
}
}
// 2. Geometric Constraints, qv as function of qu and qc
if (s->nqv > 0)
{
if (mbs_close_geo(s, aux) >= aux->MAX_NR_ITER)
{
err = 501;
free_int_vec(iquc);
free_double_vec(F);
return err;
}
}
// 3. Kinematic Constraints, qv_d as function of qu_d and qc_d
if (s->nqv > 0)
{
mbs_close_kin(s, aux);
}
// 4. External (and link) and Joint forces
mbs_calc_force(s);
// 5.1 Equations of motion (unconstrained MBS)
//zeros_double_vec(&(s->qdd[1]), s->njoint); // ?Q ... sécurité en attente d'une fonction symb c(q,qd)
if (s->process == 92)
{
mbs_nonlindyna(aux->c, s, s->tsim); // ?Q futur : c(q,qd)
}
else
{
mbs_invdyna(aux->c, s, s->tsim); // ?Q futur : phi(q,qd) including the mass matrix. the term is then derived with regards to q and qd !
}
for (i = 1; i <= s->njoint; i++)
{
F[i] = aux->c[i] - s->Qq[i];
}
// 5.2 Equations of motion (reduction for constrained MBS)
if (s->nqv > 0) // Fr_uc(i) = F(iquc) + Bvuc'*F(iquc);
{
for (i = 1; i < nuc; i++)
{
val = 0.0;
for (j = 1; j < s->nqv; j++)
{
val += aux->Bvuc[j][i] * F[s->qv[j]];
}
Fruc[i] = F[iquc[i]] + val;
}
}
else // Fr_uc = F(iquc);
{
for (i = 1; i <= nuc; i++)
{
Fruc[i] = F[iquc[i]];
}
}
free_int_vec(iquc);
free_double_vec(F);
return err;
}
......@@ -32,9 +32,12 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
mbs_aux->norm_h=1.0;
while((mbs_aux->norm_h > mbs_aux->NRerr) && (iter++ <= mbs_aux->MAX_NR_ITER))
{
// Calcul des contraintes et de la Jacobienne
mbs_calc_hJ(s,mbs_aux);
// Norme des contraintes (en supposant que toutes les contraintes indpendantes sont au dbut ???)
mbs_aux->norm_h = norminf_vector(mbs_aux->h,s->nhu);
......@@ -48,7 +51,7 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
mbs_aux->mJv[i][j] = -mbs_aux->Jac[s->hu[i]][s->qv[j]];
}
}
// Dcomposition LU de la matrice -Jv
ludcmp(mbs_aux->mJv,s->nqv,mbs_aux->ind_mJv,&d);
......@@ -128,7 +131,6 @@ void mbs_calc_hJ(MbsData *s, MbsAux *mbs_aux)
// contraintes de fermeture de boucles
mbs_cons_hJ(mbs_aux->h,mbs_aux->Jac,s,s->tsim);
// contraintes user
if (s->Nuserc>0) {
user_cons_hJ(mbs_aux->huserc, mbs_aux->Juserc, s, s->tsim);
......
......@@ -167,7 +167,7 @@ void mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
Simu_realtime *realtime;
Realtime_java *java;
#endif
// INITIALIZATION
// - - - - - - - -
......@@ -203,7 +203,7 @@ void mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
dd->rk4->dyt = (double*) malloc(dd->nState*sizeof(double));
dd->rk4->dym = (double*) malloc(dd->nState*sizeof(double));
}
// real-time modules activated
#ifdef REAL_TIME
if (dd->options->realtime)
......@@ -221,10 +221,9 @@ void mbs_dirdyn_init(MbsDirdyn* dd, MbsData* mbs_data)
// init time
dd->tsim = dd->options->t0;
dd->dt = dd->options->dt0;
// user intialization
user_init(mbs_data, dd);
// Simulation state initialization
for(i=1; i<=mbs_data->nqu; i++)
{
......@@ -427,13 +426,14 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data)
{
while(dd->tsim < dd->options->tf)
{
// user loop
user_loop(mbs_data, dd);
mbs_fct_dirdyn(dd->tsim, dd->y, dd->yd, mbs_data, dd);
rk4(dd->y, dd->yd, dd->nState, dd->tsim, dd->dt, dd->yout, mbs_fct_dirdyn, mbs_data, dd);
dd->tsim += dd->dt;
for (i=0 ; i<dd->nState ; i++)
......@@ -449,7 +449,7 @@ void mbs_dirdyn_loop(MbsDirdyn* dd, MbsData* mbs_data)
{
break;
}
// printf("DIRDYN: current time: %f \n", dd->tsim);
}
}
......@@ -601,7 +601,6 @@ void save_realtime_update(MbsDirdyn* dd, MbsData* mbs_data)
mbs_dirdyn_save(dd, mbs_data, mbs_data->tsim);
}
}
// real-time modules
#ifdef REAL_TIME
if (dd->options->realtime)
......@@ -613,6 +612,7 @@ void save_realtime_update(MbsDirdyn* dd, MbsData* mbs_data)
// real-time loop iteration
mbs_realtime_loop(realtime, mbs_data->tsim);
// quit the simulation
if (realtime->simu_quit)
......
......@@ -30,7 +30,6 @@ extern "C" {
* \param mbs_data the data structure of the model for which
* the time integration will be computed
*/
MbsDirdyn* mbs_new_dirdyn(MbsData* mbs_data);
/**
......@@ -53,7 +52,6 @@ MbsDirdyn* mbs_new_dirdyn_aux(MbsData* mbs_data, MbsAux* mbs_aux);
* @return the MbsDirdyn ...
*
*/
void mbs_run_dirdyn(MbsDirdyn* dirdyn, MbsData* mbs_data);
......@@ -83,7 +81,6 @@ void mbs_dirdyn_finish(MbsDirdyn* dirdyn, MbsData* mbs_data);
* Free memory of the direct dynamic structure
* The options (MbsDirdynOptions) and MbsAux structures are also freed
*/
void mbs_delete_dirdyn(MbsDirdyn* dirdyn, MbsData* mbs_data);
/**
......
......@@ -263,9 +263,8 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
mbs_aux->Fr[i] = -(mbs_aux->Fruc[i] + term);
}
// calcul des accelerations reduites : 'resolution' du systeme ODE = Mr*udd = Fr;
choldc(mbs_aux->Mr,s->nqu,mbs_aux->p_Mr);
choldc(mbs_aux->Mr,s->nqu,mbs_aux->p_Mr);
cholsl(mbs_aux->Mr,s->nqu,mbs_aux->p_Mr,mbs_aux->Fr,s->udd);
nL = s->nqu;
......@@ -346,6 +345,7 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
}
mbs_aux->Qc[i] = term + mbs_aux->Fruc[s->nqu+i];
//mbsdata->Qc[mbs_data->qc[i]] = term + mbs_aux->Fruc[s->nqu + i];
}
}
......
This diff is collapsed.
......@@ -11,14 +11,16 @@
/**
* Allocate the memory for the Equilibrium Options (but not for all the Equilibrium structure)
* Initialize the options structute with default options
* Initialize the options structure with default options
*/
MbsEquil* mbs_new_equil(MbsData* mbs_data);
MbsEquil* mbs_new_equil_aux(MbsData* mbs_data, MbsAux* mbs_aux);
/**
* Free the Equilibrium structure (pointers table, Equibrium options, ...)
*
* The options (MbsEquilOptions) and MbsAux structures are also freed
*
*/
void mbs_delete_equil(MbsEquil* eq, MbsData* mbs_data);
......@@ -27,6 +29,12 @@ void mbs_delete_equil(MbsEquil* eq, MbsData* mbs_data);
* - mbs_equil_init
* - mbs_equil_loop
* - mbs_equil_finish
*
* The MbsData structure associated is modified (equilibrium configuration, q,qd,Qq and possibly other parameters m,I,dpt, ... )
*
* @param eq the MbsEquil to be run
* @return the MbsEquil ...
*
*/
void mbs_run_equil(MbsEquil* mbs_equil,MbsData* mbs_data);
......@@ -34,7 +42,7 @@ void mbs_run_equil(MbsEquil* mbs_equil,MbsData* mbs_data);
* Initialize the Equilibrium structure variables by
* linking the pointers table, x_ptr with:
* - the independent variables : qu
* - the substitution variables : xsub (if defined by the user)
* - the substitution variables : xch (if defined by the user)
* - the extra equilibrium variables : xe (if defined by the user)
* linking the extra equilibrium function pointer, fxe_ptr with the user_equil_fxe (if defined by the user)
* Allocate the memory for x_ptr and x based on Equilibrium Options
......@@ -50,13 +58,14 @@ void mbs_equil_loop(MbsEquil* mbs_equil,MbsData* mbs_data);
/**
* Set equilibrum flag to done in MbsData structure
* Put the MBSysPAD initial velocities and accelerations back in the MbsData structure
* offer the possibility for user to finish operation through the use of user_equil_finish
*
*/
void mbs_equil_finish(MbsEquil* mbs_equil,MbsData* mbs_data);
/**
* Allocate the memory for the substitution equilibrium variables through a table of pointers
* Allocate the memory for the changed equilibrium variables through a table of pointers
*
* \p options Equilibrium options structure (where the pointers table is stored)
*/
......@@ -79,7 +88,7 @@ void mbs_equil_ignorance(MbsEquilOptions *options);
/**
* Q? is that necessary
* Q?? is that necessary
*/
void mbs_equil_save(MbsEquil* eq, MbsData *mbs_data, int n);
......@@ -96,13 +105,13 @@ void mbs_equil_save(MbsEquil* eq, MbsData *mbs_data, int n);
* \p aux ?Q to change for a more general use of function of fsolvepk
* \p s ?Q to change for a more general use of function of fsolvepk
*/
//int mbs_equil_fct(double *x, int nx, double *fx, void *eq, void *aux, void *s) ?Q polymorphisme
int mbs_equil_fct(double x[], int nx, double fx[], MbsEquil *eq, MbsAux *aux, MbsData *s);
//-----------------------------------------------------------------
//---> TO MOVE TO a new user_equil.c (where 3 additional functions have to be described
//---> TO MOVE TO a new user_equil.c (where 3 additional functions have to be described)
// + need for additional test on "user_equil_fxe" i.e. the case when the user nxe>0 but without the definition fxe[] !
//-----------------------------------------------------------------
/**
......@@ -131,23 +140,16 @@ void mbs_print_equil(MbsEquil* eq);
/**
* Compute the Fr vector of size nquc=nqu+nqc for qu and qc variables (Robotran Basics Eq(20))
* Necessary to express equilibrium f(x)=0
*
* \p Fruc the force vector in which the force must be returned
* \p aux the local data structure
* \p s the MbsData of the system
*/
int mbs_compute_Fruc(double Fruc[], MbsAux *aux, MbsData *s);
/**
* 4 functions to move to numerics and change the index !
*/
void cholsl_double_tab(double **a, int n, double *p, double *b, double *x);
void choldc_double_tab(double **a, int n, double *p);
void ludcmp_2(double **a, int n, int *indx, double *d);
void lubksb_2(double **a, int n, int *indx, double b[]);
void cholsl_0(double **a, int n, double *p, double *b, double *x);
void choldc_0(double **a, int n, double *p);
void ludcmp_0(double **a, int n, int *indx, double *d);
void lubksb_0(double **a, int n, int *indx, double b[]);
/**
* Solve equation f(x)=0 based on a given initial value for the variables, x0
......
//---------------------------
// UCL-CEREM-MBS
//
// @version MBsysLab_s 1.7.a
//
// Q.D.
//
// Creation : 14/06/2016
// Last update : 14/06/2016
//---------------------------
#include "mbs_linearipk.h"
int mbs_linearipk(double **GK, MbsLpk *lpk , MbsAux *aux, MbsData *s, int PosOrVel)
{
int i, j, k; // fctlin,qoqp_equi,fctlin_equi,posvit,epsmod,delta_ua,delta_ur,linpa
int ind_x;
int rep, compt, test, li;
int lin_fail = 0;
double max_GK_k, max_GK_prem_div_k;
double increment;
// step 1 : save the mbs_data value !
copy_double_vec(&(s->q[1]),lpk->q_saved , s->njoint);
copy_double_vec(&(s->qd[1]),lpk->qd_saved, s->njoint);
copy_double_vec(&(s->Qq[1]), lpk->Qq_saved, s->njoint);
// step 2 : linearization computation (by modifying s->q and s->qd during the process)
lpk->n_diverge = 0;
lpk->maxcomp = 0.0;
ind_x = 0;
for (i = 1; i <= s->nqu; i++)
{
if (PosOrVel == 1) // check whether the linearization is on the velocities or the positions
{
lpk->x_equil[ind_x] = s->q[s->qu[i]];
}
else
{
lpk->x_equil[ind_x] = s->qd[s->qu[i]];
}
ind_x++;
}
// loop on the matrix column ! (the linearized matrix is build column by column...)
for (k = 0; k<lpk->nx; k++)//
{
for (i = 0; i<lpk->nx; i++)
{
for (j = 0; j<lpk->nx; j++)
{
lpk->GK_comp[i][j] = 100.0;
}
}
rep = 1;
compt = 1;
copy_double_vec(lpk->x_equil, lpk->x_ext, lpk->nx);
// ideal increment computation
if (fabs(lpk->x_ext[k] * lpk->relincr) < lpk->absincr)
{
if (lpk->x_ext[k] == 0.0)
{
increment = lpk->absincr;
}
else
{
increment = sign(lpk->x_ext[k]) * lpk->absincr;
}
}
else
{
increment = lpk->x_ext[k] * lpk->relincr;
}
// Parabolic fitting
// 1. Extreme Point
lpk->x_ext[k] = lpk->x_equil[k] + increment;
copy_double_vec(lpk->q_saved, &(s->q[1]), s->njoint);
copy_double_vec(lpk->qd_saved, &(s->qd[1]), s->njoint);
copy_double_vec(lpk->Qq_saved, &(s->Qq[1]), s->njoint);
lin_fail = mbs_lineari_fct(lpk->x_ext, lpk->nx, lpk->F_ext, aux, s, PosOrVel);
copy_double_vec(lpk->x_ext, lpk->x_mid, lpk->nx);
while (rep)
{
// 2. Middle Point
li = 0;
lpk->x_mid[k] = lpk->x_equil[k] + increment / 2;
lin_fail = mbs_lineari_fct(lpk->x_mid, lpk->nx, lpk->F_mid, aux, s, PosOrVel);
for (i = 0; i<lpk->nx; i++)
{
// if there is no influence on equation Fi(x)=0 , put it to zero...
if (fabs(lpk->F_ext[i]) < lpk->equitol && fabs(lpk->F_mid[i]) < lpk->equitol)
{
li++;
GK[i][k] = 0.0;
}
// otherwise compute the slope
else
{
// slope at point x_equil
GK[i][k] = (-lpk->F_ext[i] + 4.0 * lpk->F_mid[i] - 3.0 * 0.0) / increment; // to demonstrate solve for a centered parabola (then shift it back)
// first iteration in the while loop
if (compt == 1)
{
lpk->GK_prem[i][k] = GK[i][k];
}
}
}
// if all entries are non sensitive, Column k with no influence (i.e. variable non sensitive)
if (li == lpk->nx)
{
if (lpk->verbose)
{
printf(">>LINEARIPK>> Column %d/%d (no influence)\n", k + 1, lpk->nx);
}
rep = 0; // stop the iterations on the column : no influence
}
// otherwise, compute and iterate !
else
{
max_GK_k = 0.0;
for (i = 0; i<lpk->nx; i++)
{
max_GK_k = MAX(max_GK_k, fabs(GK[i][k]));
}
test = 1;
for (i = 0; i<lpk->nx; i++)
{
if (fabs(GK[i][k] / max_GK_k) > 1e-6 && fabs(GK[i][k]) > 1e-8);
{
if (fabs((lpk->GK_comp[i][k] - GK[i][k]) / GK[i][k]) > lpk->lintol)
{
test = 0; // accuracy tolerance for linearization is not reached
}
}
}
// continue to iterate : x_mid->x_ext & increment->increment/2
if (test)
{
for (i = 0; i<lpk->nx; i++)
{
lpk->GK_comp[i][k] = GK[i][k];
}
increment = increment / 2.0;
copy_double_vec(lpk->F_mid, lpk->F_ext, lpk->nx); //// ????????????? 8888
compt++; // The linearization is diverging ...
if (compt > lpk->itermax)
{
if (lpk->verbose)
{
printf(">>LINEARIPK>> Column %d/%d (waiting...)\n", k + 1, lpk->nx);
}
lpk->diverge_ind[lpk->n_diverge] = k;
lpk->n_diverge++;
rep = 0; // stop the iterations on the column : has diverged
}
}
// stop to iterate: linearization tolerance reached
else
{
if (lpk->maxcomp < max_GK_k)
{
lpk->maxcomp = max_GK_k; // save the maximum GK entry value over all columns
}
if (lpk->verbose)
{
printf(">>LINEARIPK>> Column %d/%d (%d iterations)\n", k + 1, lpk->nx, compt);
}
rep = 0; // stop the iterations on the column : has converged
}
}
}
// step 3 : restore the exact value for mbs_data->q and mbs_data->qd and Qq
copy_double_vec(lpk->q_saved, &(s->q[1]), s->njoint);
copy_double_vec(lpk->qd_saved, &(s->qd[1]), s->njoint);
copy_double_vec(lpk->Qq_saved, &(s->Qq[1]), s->njoint);
}
// Processing the waiting columns: based on maxcomp we possibly accept them
if (lpk->n_diverge > 0)
{
if (lpk->verbose)
{
printf(">>LINEARIPK>> Processing the waiting columns\n");
}
// check for all diverging columns (new loop on diverging columns)
for (k = 0; k<lpk->n_diverge; k++)
{
max_GK_prem_div_k = 0.0;
for (i = 0; i<lpk->nx; i++)
{
max_GK_prem_div_k = MAX(max_GK_prem_div_k, fabs(lpk->GK_prem[i][lpk->diverge_ind[k]]));
}
// waiting column k is accepted
if (max_GK_prem_div_k < 1e-5 * lpk->maxcomp)
{
for (i = 0; i<lpk->nx; i++)
{