int PART_run_part(MDS_gen_strct* mds_gen_strct, MBSdataStruct* MBSdata, PART_gen_strct* part_gen_strct)
{
int i;
int err, nbiter;
int* ind_c = NULL;
int* ind_u_des = NULL;
LocalDataStruct *lds; //change it !
if(MBSdata->Ncons == 0) // change it !
{
if(part_gen_strct->options->verbose)
{
printf(">>PART>> no coordinate partitioning required for this system !\n");
}
MBSdata->DonePart = 1;
return MBSdata->DonePart;
}
if(part_gen_strct->options->verbose)
{
printf(">>PART>> PART_run start\n");
}
// sort qu and qc
ind_c = get_int_vec(mds_gen_strct->bodytree->n_qc);
ind_u_des = get_int_vec(mds_gen_strct->bodytree->n_qu);
sort_int_vec(mds_gen_strct->bodytree->qc, ind_c, mds_gen_strct->bodytree->n_qc);
sort_int_vec(mds_gen_strct->bodytree->qu, ind_u_des, mds_gen_strct->bodytree->n_qu);
// call user_drivenJoints
user_DrivenJoints(MBSdata, MBSdata->tsim);
/*
for ( i=0; i<100; i++)
{
printf("%f\n",frand());
}*/
/*
print_int_vec(mds_gen_strct->bodytree->qu, s->nqu);
print_int_vec(ind_u_des, s->nqu);
*/
// First partitioning
if (part_gen_strct->options->verbose)
{
printf(">>PART>> ***** Coordinate partitioning *****\n");
}
PART_coord_part(mds_gen_strct, MBSdata, part_gen_strct, ind_u_des, mds_gen_strct->bodytree->n_qu, ind_c, mds_gen_strct->bodytree->n_qc, 1, &err); // d'abord on cherche un choix valable
if( err == -1)
{
printf(">>PART>>\n");
printf(">>PART>> ***** mbs_run_part : Coordinate partitioning interrupted : *****\n");
printf(">>PART>>\n");
//myproject_part = MBS_part;
MBSdata->DonePart = 0;
return MBSdata->DonePart;
}
// Fermeture
if (part_gen_strct->options->verbose)
{
printf(">>PART>> ***** Geometrical Constraint solution *****\n");
}
lds = initLocalDataStruct(MBSdata);
nbiter = mbs_close_geo(MBSdata, lds); // on essaie de fermer
freeLocalDataStruct(lds, MBSdata);
if (nbiter == -1)
{
printf(">>PART>>\n");
printf(">>PART>> ***** mbs_run_part : impossible to close the MBS : *****\n");
printf(">>PART>> -> try with other desired independent variables u\n");
printf(">>PART>> and/or\n");
printf(">>PART>> -> try with other initial values for q (u, v)\n");
printf(">>PART>> -> ... but have a look at the proposed {u,v} partition\n");
printf(">>PART>>\n");
}
else
{
if (part_gen_strct->options->verbose)
{
printf("***** Constaints solved after %d iterations *****\n", nbiter);
}
copy_double_vec(MBSdata->q, part_gen_strct->q_closed ,mds_gen_strct->bodytree->n_joint);
// Second partitioning sur la structure fermée (peaufinement éventuel toujours sur base des ind_u_des)
PART_coord_part(mds_gen_strct, MBSdata, part_gen_strct, ind_u_des, mds_gen_strct->bodytree->n_qu, ind_c, mds_gen_strct->bodytree->n_qc, 0, &err);
if (err == -1) // peu probable mais à envisager ...
{
printf(">>PART>>\n");
printf(">>PART>> ***** mbs_run_part : Second coordinate partitioning interrupted : *****\n");
printf(">>PART>>\n");
//myproject_part = MBS_part;
MBSdata->DonePart = 0;
return MBSdata->DonePart;
}
else
{
MBSdata->DonePart = 1;
}
}
// Stockage des résultats dans la structure part_gen_strct et s//tockage
/*
part_gen_strct.nu = length(ind_u);
part_gen_strct.ind_u = ind_u;
part_gen_strct.nv = length(ind_v);
part_gen_strct.ind_v = ind_v;
part_gen_strct.nhu = length(ind_hu); // JFC : 15/01/2008 : ajout
part_gen_strct.hu = ind_hu;
part_gen_strct.nhv = length(ind_hv); // PF : 09/04/2009 : ajout
part_gen_strct.hv = ind_hv;
*/
// Affectation global=>Workspace
//myproject_part = MBS_part;
if (1)
{
MBSdata->nqu = part_gen_strct->n_qu;
for(i=0; i<part_gen_strct->n_qu; i++)
{
MBSdata->qu[i+1] = part_gen_strct->ind_qu[i] +1;
}
MBSdata->nqv = part_gen_strct->n_qv;
for(i=0; i<part_gen_strct->n_qv; i++)
{
MBSdata->qv[i+1] = part_gen_strct->ind_qv[i] +1;
}
MBSdata->nhu = part_gen_strct->n_hu;
for(i=0; i<part_gen_strct->n_hu; i++)
{
MBSdata->hu[i+1] = part_gen_strct->ind_hu[i] +1;
}
/*MBSdata->nhv = part_gen_strct->n_hv;
for(i=0; i<part_gen_strct->n_hv; i++)
{
MBSdata->hv[i+1] = part_gen_strct->ind_hv[i] +1;
}*/
}
if (part_gen_strct->options->verbose)
{
printf(">>PART>> ***** mbs_run_part end *****\n");
}
//system("pause");
return MBSdata->DonePart;
}
/*
* Simulation initialization
*/
Loop_inputs* init_simulation()
{
// -- Variables declaration -- //
int i;
int model_state_size;
double *ystart;
MBSdataStruct *MBSdata = NULL;
LocalDataStruct *lds = NULL;
Loop_inputs *loop_inputs = NULL;
#if defined(JNI) & defined (REAL_TIME)
JNI_struct* jni_struct = NULL;
#endif
#ifdef WRITE_FILES
Write_files *write_files;
#endif
#ifdef YARP
void* RobotranYarp_interface = NULL;
#endif
struct timeval seed_time;
// -- Seed for random -- //
gettimeofday(&seed_time, NULL);
srand(seed_time.tv_usec * seed_time.tv_sec);
// -- Variables initialization -- //
MBSdata = loadMBSdata_xml(MBS_FILE);
if(MBSdata == NULL)
{
printf("error while loading MBSdata \n");
}
#ifdef PRINT_REPORT
else
{
printf("MBSdata successfully loaded \n");
}
#endif
// LocalDataStruct initialization
#if !defined ACCELRED
lds = initLocalDataStruct(MBSdata);
#else
lds = NULL;
#endif
if(lds == NULL)
{
printf("error while initializing LocalDataStruct\n");
}
#ifdef PRINT_REPORT
else
{
printf("LocalDataStruct successfully initialized\n");
}
#endif
// Yarp Initialization
#ifdef YARP
RobotranYarp_interface = yarp_init();
if(RobotranYarp_interface == NULL)
{
printf("******************\nSomething went wrong during YARP initialization... what to do here?\n******************\n");
}
#endif
// Model initialization
if(user_initialization(MBSdata, lds))
{
printf("error in user_initialization\n");
}
#ifdef PRINT_REPORT
else
{
printf("Model successfully initialized\n");
}
#endif
// Integrator parameters initialization
model_state_size = 2*MBSdata->nqu + MBSdata->Nux;
#ifdef PRINT_REPORT
printf("model_state_size: %d\n", model_state_size);
#endif
ystart = dvector(1,model_state_size);
#ifdef WRITE_FILES
write_files = init_write_files(NB_SIMU_STEPS, MBSdata->njoint);
#endif
// Simulation state initialization
for(i=1; i<=MBSdata->nqu; i++)
{
ystart[i] = MBSdata->q[MBSdata->qu[i]];
ystart[i+MBSdata->nqu] = MBSdata->qd[MBSdata->qu[i]];
}
for(i=1; i<=MBSdata->Nux; i++)
{
ystart[i+2*MBSdata->nqu] = MBSdata->ux[i];
}
// JNI visualization
#if defined(JNI) & defined (REAL_TIME)
jni_struct = init_jni(MBSdata);
#endif
// initialize the inputs of the simulation loop
loop_inputs = (Loop_inputs*) malloc(sizeof(Loop_inputs));
// absolute initial time of the simulation
time_get(&(loop_inputs->init_t_sec), &(loop_inputs->init_t_usec));
loop_inputs->nvar = model_state_size;
loop_inputs->nstep = NB_SIMU_STEPS;
loop_inputs->x1 = TSIM_INIT;
loop_inputs->x2 = TSIM_END;
loop_inputs->ystart = ystart;
loop_inputs->lds = lds;
loop_inputs->MBSdata = MBSdata;
#ifdef WRITE_FILES
loop_inputs->write_files = write_files;
#endif
// Real-time constraiints
#ifdef REAL_TIME
loop_inputs->real_time = init_real_time(loop_inputs->init_t_sec, loop_inputs->init_t_usec);
#endif
// SDL window
#if defined(SDL) & defined (REAL_TIME)
loop_inputs->screen_sdl = configure_screen_sdl(loop_inputs->init_t_sec, loop_inputs->init_t_usec);
#endif
#if defined(JNI) & defined (REAL_TIME)
loop_inputs->jni_struct = jni_struct;
update_jni(loop_inputs->jni_struct, MBSdata, loop_inputs->real_time, MBSdata->q+1);
#endif
#ifdef YARP
loop_inputs->RobotranYarp_interface = RobotranYarp_interface;
#endif
// Running model integration
#ifdef PRINT_REPORT
printf("Running integration of the model...\n");
#endif
return loop_inputs;
}
