...
 
Commits (9)
......@@ -32,6 +32,7 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
double** mJv_0;
int* ind_c = NULL;
int* ind_u_des = NULL;
int part_condition;
MbsPart *mbs_part;
......@@ -70,9 +71,15 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
mJv_0 = get_dmat_0(nL, nC);
sub_matrix(mbs_aux->mJv, nL + 1, 0, 0, mJv_0);
//if (fabs(det_dmatrix_0(mJv_0, nL)) < mbs_aux->min_det || (iter >= 10 && iter % 10 == 0)) {
if (fabs(det_dmatrix_0(mJv_0, nL)) < 5e-3 && mbs_aux->online_part){
//if ((iter > 0 && iter % mbs_aux->iter_part == 0) && mbs_aux->online_part) {
//Depending on the user choice, we look if a partitioning is needed
if (mbs_aux->use_det) {
part_condition = fabs(det_dmatrix_0(mJv_0, nL)) < mbs_aux->min_det; // Need of a new partitioning if the determinant of Jv is too small
}
else {
part_condition = iter > 0 && iter % mbs_aux->iter_part == 0; // Need of a new partitioning if the number of iteration on Newton-Raphson is too large
}
if (part_condition && mbs_aux->online_part){
mbs_part = mbs_new_part(s);
mbs_part->options->rowperm = 1;
......@@ -84,9 +91,9 @@ int mbs_close_geo(MbsData *s, MbsAux *mbs_aux)
}
s->DonePart = 0;
s->nqu = 0;
s->nqu = 0; // In order to use all the variable which are not driven
err = PART_coord_part(s, mbs_part, ind_u_des, 0, ind_c, s->nqc, 0);
err = PART_coord_part(s, mbs_part, ind_u_des, 0, ind_c, s->nqc, 0); //Re-partition
if (err < 0) // Error management
{
......
This diff is collapsed.
......@@ -124,13 +124,25 @@ void mbs_delete_dirdyn(MbsDirdyn* dirdyn, MbsData* mbs_data);
* python.
*
* \p dd the MbsDirdyn struct for which the buffers must be written
* \return error (0 if no error)
*/
int mbs_dirdyn_write_buffers(MbsDirdyn* dd);
void mbs_dirdyn_write_buffers(MbsDirdyn* dd);
/*! \brief direct dynamics derivative computation
*
* \param[in] tsim current simulation time [s]
* \param[in] y state vector of size n
* \param[out] dydt derivative
* \param[in,out] s Robotran main structure
* \param[in,out] dd direct dynamic main module structure
*/
void mbs_fct_dirdyn(double t, double y[], double dydt[], MbsData *s, MbsDirdyn *dd);
/*! \brief update the real-time and saving modules if requested
*
* \param[in] dd direct dynamics module
* \param[in] mbs_data Robotran main structure
*/
void save_realtime_update(MbsDirdyn* dd, MbsData* mbs_data);
// double loop_dopri5(double cur_t0, double cur_tf, double dt0, double **dopri5_alloc_tab, MbsData *s, MbsDirdyn *dd, FILE *fileout_dopri5); old stuff
// double loop_rosenbrock(double t0, double tf, double dt0, MbsData *s, MbsDirdyn *dd, FILE *fileout_dopri5); old stuff
#endif
......@@ -26,10 +26,9 @@
int dirdynared(MbsAux *mbs_aux,MbsData *s)
{
int i, j, k;
int iter = 0;
int nL, nC, nk;
int err;
int err = 0;
double term;
......@@ -42,13 +41,18 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
if (s->Ncons > 0)
{
// Geometric resolution
iter = mbs_close_geo(s, mbs_aux);
err = mbs_close_geo(s, mbs_aux);
if (iter >= mbs_aux->MAX_NR_ITER)
if (mbs_aux->permu) {
return err;
}
if (err<0) // Error management
{
return -1;
mbs_msg("\t \t in >DIRDYNARED> \n");
return err;
}
// Kinematic resolution
mbs_close_kin(s, mbs_aux);
......@@ -57,12 +61,9 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
}//if(s->Ncons > 0)
// compute forces on the system
mbs_calc_force(s);
// calcul de la matrice de masse et du vecteur des forces non lineaires
mbs_dirdyna(mbs_aux->M, mbs_aux->c, s, s->tsim);
......@@ -251,17 +252,17 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
for (j = 1; j <= nC; j++)
{
mbs_aux->Mr[i][j] = mbs_aux->Mruc[i][j]; // Muu
if (mbs_aux->Mr[i][j] > 1e-16)
has_a_line_of_zeros = 0;
if (mbs_aux->Mr[i][j] > 1e-16)
has_a_line_of_zeros = 0;
}
if (has_a_line_of_zeros)
if (has_a_line_of_zeros) // Error management
{
mbs_msg("The line %d of the reduced mass matrix, associated to q(%d), is full of zeros\n", i, mbs_aux->iquc[i]);
mbs_msg("\t >DIRDYNARED> The line %d of the reduced mass matrix, associated to q(%d), is full of zeros\n", i, mbs_aux->iquc[i]);
for (k = 1; k <= nC; k++)
{
mbs_msg("mbs_aux->Mr[%d][%d] = %e;\n", i, k, mbs_aux->Mr[i][k]);
mbs_msg("\t >DIRDYNARED> mbs_aux->Mr[%d][%d] = %e;\n", i, k, mbs_aux->Mr[i][k]);
}
return -2;// temporary solution //mbs_error_msg("The reduced mass matrix has a line of zeros\n");
return -70;
}
term = 0.0;
......@@ -273,16 +274,28 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
}
// calcul des accelerations reduites : 'resolution' du systeme ODE = Mr*udd = Fr;
err=choldc(mbs_aux->Mr, s->nqu, mbs_aux->p_Mr);
if (err < 0) { return err;} // temporary
cholsl(mbs_aux->Mr, s->nqu, mbs_aux->p_Mr, mbs_aux->Fr, s->udd);
err = choldc(mbs_aux->Mr, s->nqu, mbs_aux->p_Mr);
if (err < 0) // Error management
{
mbs_msg("\t >DIRDYNARED> Matrix Mr is not positive definite \n");
return (-70+err);
}
cholsl(mbs_aux->Mr, s->nqu, mbs_aux->p_Mr, mbs_aux->Fr, s->udd);
nL = s->nqu;
for (i = 1; i <= nL; i++)
{
s->qdd[s->qu[i]] = s->udd[i];
if (isnan(s->qdd[s->qu[i]])) // Error management
{
mbs_msg("\t >DIRDYNARED> qddu[%d] is Nan \n", i);
return -77;
}
}
if (s->nqv > 0)
{
double d;
......@@ -330,16 +343,13 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
}
// compute lambda
ludcmp(mbs_aux->Jvt, s->nqv, mbs_aux->ind_Jvt, &d);
lubksb(mbs_aux->Jvt, s->nqv, mbs_aux->ind_Jvt, s->lambda);
}
for (i = 1; i <= s->nqu; i++)
{
if (isnan(s->qdd[s->qu[i]])) {
mbs_msg(">>DIRDYN>> qdd[%d] is Not a number (Nan) \n", s->qu[i]);
return -4;
err = ludcmp(mbs_aux->Jvt, s->nqv, mbs_aux->ind_Jvt, &d);
if (err < 0) // Error management
{
mbs_msg("\t >DIRDYNARED> Error in dirdynared during LU decomposition of Jv matrix for lambda computation\n");
return (-70+err);
}
lubksb(mbs_aux->Jvt, s->nqv, mbs_aux->ind_Jvt, s->lambda);
}
// Qc = Mruc_cu * udd + Mruc_cc * qddc + Fruc_c
......@@ -369,6 +379,6 @@ int dirdynared(MbsAux *mbs_aux,MbsData *s)
}
}
return iter;
return err;
}
#endif
This diff is collapsed.
......@@ -28,9 +28,11 @@ void initialize_rk4(MbsData *mbs_data, MbsDirdyn *mbs_dd) {
}
void loop_rk4(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd, FILE *fileout) {
void loop_rk4(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd) {
int i;
double h = tf - t0;
int count = 0;
FILE* file = NULL;
// Check remaining time
if (h > mbs_dd->dt) {
......@@ -41,26 +43,114 @@ void loop_rk4(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd, FILE *
user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
if (mbs_dd->options->saveTimePart) {
file = fopen("Time_partitioning.txt", "w");
}
while (mbs_dd->tsim <= tf)
{
user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
{
mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
if (count == 1) {
if (mbs_dd->mbs_aux->use_det) {
mbs_dd->mbs_aux->iter_part = mbs_dd->mbs_aux->min_det / 10.0;
}
else {
mbs_dd->mbs_aux->iter_part = 2 * mbs_dd->mbs_aux->iter_part;
}
}
if (count >= 5 * mbs_data->njoint) {
printf("mJv matrix \n");
print_dmat_0(mbs_dd->mbs_aux->mJv, mbs_data->nhu+1, mbs_data->nqv+1);
if (mbs_dd->mbs_aux->use_det) {
mbs_msg("\t \t >CLOSE GEO> Error in loop_rk4 can't find a configuration in which the determinant of Jv is > %lf \n", mbs_dd->mbs_aux->min_det);
}
else {
mbs_msg("\t \t >CLOSE GEO> Error in loop_rk4 can't find a configuration in which the loop is close after %d \n", mbs_dd->mbs_aux->iter_part);
}
return;
}
rk4(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, mbs_dd->tsim, h, mbs_dd->yout, mbs_fct_dirdyn, mbs_data, mbs_dd);
mbs_dd->tsim += h;
user_dirdyn_loop(mbs_data, mbs_dd); ///user loop
for (i = 0; i < mbs_dd->nState; i++)
{
mbs_dd->y[i] = mbs_dd->yout[i];
if (mbs_dd->options->flag_precise_dynamics) // Compute f' if asked
{
mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd);
}
mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd); // next f'
if (!mbs_dd->mbs_aux->permu) {
rk4(mbs_dd->y, mbs_dd->yd, mbs_dd->nState, mbs_dd->tsim, h, mbs_dd->yout, mbs_fct_dirdyn, mbs_data, mbs_dd);
}
// -------------------- ALEX start ------------------
if (mbs_dd->mbs_aux->permu) {
if (mbs_dd->options->saveTimePart && file != NULL) {
fprintf(file, "%lf \n", mbs_dd->tsim);
}
for (i = 1; i <= mbs_data->nqu; i++)
{
mbs_dd->y[i - 1] = mbs_data->q[mbs_data->qu[i]];
mbs_dd->y[i - 1 + mbs_data->nqu] = mbs_data->qd[mbs_data->qu[i]];
}
for (i = 1; i <= mbs_data->Nux; i++)
{
mbs_dd->y[i - 1 + 2 * mbs_data->nqu] = mbs_data->ux[i];
}
for (i = 1; i <= mbs_data->nqu; i++)
{
mbs_dd->yd[i - 1] = mbs_data->qd[mbs_data->qu[i]];
mbs_dd->yd[i + mbs_data->nqu - 1] = mbs_data->qdd[mbs_data->qu[i]];
}
mbs_dd->mbs_aux->permu = 0;
count++;
}
//------------------ Alex stop ------------------------
else {
mbs_dd->tsim += h;
for (i = 0; i < mbs_dd->nState; i++)
{
mbs_dd->y[i] = mbs_dd->yout[i];
}
mbs_fct_dirdyn(mbs_dd->tsim, mbs_dd->y, mbs_dd->yd, mbs_data, mbs_dd); // next f'
// -------------------- ALEX start ------------------
if (mbs_dd->mbs_aux->permu) {
if (mbs_dd->options->saveTimePart && file != NULL) {
fprintf(file, "%lf \n", mbs_dd->tsim);
}
for (i = 1; i <= mbs_data->nqu; i++)
{
mbs_dd->y[i - 1] = mbs_data->q[mbs_data->qu[i]];
mbs_dd->y[i - 1 + mbs_data->nqu] = mbs_data->qd[mbs_data->qu[i]];
}
for (i = 1; i <= mbs_data->Nux; i++)
{
mbs_dd->y[i - 1 + 2 * mbs_data->nqu] = mbs_data->ux[i];
}
for (i = 1; i <= mbs_data->nqu; i++)
{
mbs_dd->yd[i - 1] = mbs_data->qd[mbs_data->qu[i]];
mbs_dd->yd[i + mbs_data->nqu - 1] = mbs_data->qdd[mbs_data->qu[i]];
}
mbs_dd->mbs_aux->permu = 0;
}
else {
count = 0;
mbs_dd->mbs_aux->iter_part = mbs_dd->options->iter_part;
mbs_dd->mbs_aux->min_det = mbs_dd->options->det_precision;
}
//------------------ Alex stop ------------------------
}
// save results if always asked or if waypoint reached
if ((!mbs_dd->options->flag_solout_wp) || (mbs_dd->options->flag_solout_wp && mbs_dd->tsim == tf)) {
save_realtime_update(mbs_dd, mbs_data);
......@@ -68,6 +158,10 @@ void loop_rk4(double t0, double tf, MbsData *mbs_data, MbsDirdyn *mbs_dd, FILE *
}
if (mbs_dd->options->saveTimePart && file != NULL) {
fclose(file);
}
}
void finish_rk4(MbsData *mbs_data, MbsDirdyn *dd) {
......
......@@ -26,6 +26,11 @@ typedef struct MbsAux_tag
double norm_h;
double NRerr;
int MAX_NR_ITER;
int use_det;
int iter_part;
int Jv_verbose;
int online_part;
double min_det;
double *h, **Jac;
double *huserc, **Juserc;
......@@ -53,6 +58,8 @@ typedef struct MbsAux_tag
double * bp;
// double **bp;
int permu; //equal to 1 if a coordiante permutation has been execute
MbsSensor psens[1]; // temporary sensor pointer
#elif defined SENSORKIN
......
......@@ -121,6 +121,7 @@ typedef struct{
mbs_cons_hJ_ptr mbs_cons_hJ;
mbs_cons_jdqd_ptr mbs_cons_jdqd;
mbs_sensor_ptr mbs_sensor;
mbs_gensensor_ptr mbs_gensensor;
} symb;
} Functions;
......@@ -248,7 +249,7 @@ struct MbsData
int DoneEquil; ///< Flag that indicates if the equilibrium module has been executed (default: 0=not done; 1=done).
int DoneModal; ///< Flag that indicates if the modal module has been executed (default: 0=not done; 1=done).
int process; ///< Flag that indicate which module is currently running (1=partitioning, 2=equilibrium, 3=direct dynamic, 4=modal)
int process; ///< Flag that indicate which module is currently running (1=partitioning, 2=equilibrium, 3=direct dynamic, 4=modal, 5=inverse kinematic)
int simu_end; ///< DO NOT USE: Deprecated?
int flag_stop; ///< stop the simulation. For dirdyn and invdyn only.
......
......@@ -120,6 +120,33 @@ 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;
/*---------------------------------------------------*/
/* Options related to online coordinate partitioning */
/*---------------------------------------------------*/
/** Determine if you use the determinant or the number of iterartion of Newton-Raphson as condtion on the repartitioning
* 1: Use the determinant
* 0: Use the number of iteration of Newton-Raphson
* default = 0
**/
int use_det;
/** Determine if time of partitioning should be save
* 1: time of partitioning is save
* 0: time of partitioning is not save
* default = 1
*/
int saveTimePart;
//minimum value of Jv's determinant, default = 1e-3
double det_precision;
//maximium number of Newton-Raphson iterration before re-partitioning, default = 5
int iter_part;
/** Determine if Jv before online partitioning and after should be display
* 1: Should dispaly Jv
* 0: Should not display Jv
* default = 1;
**/
int Jv_verbose;
} MbsDirdynOptions;
/**
......